// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef CBR_CONTROL__ASIF___HPP_
#define CBR_CONTROL__ASIF___HPP_

#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>

#include <boost/numeric/odeint.hpp>
#include <boost/hana/adapt_struct.hpp>

#include <cbr_math/lie/odeint.hpp>
#include <cbr_math/lie/common.hpp>
#include <cbr_math/math.hpp>

#include <vector>

#include "cbr_control/osqp-cpp.hpp"

using autodiff::forward::dual, autodiff::forward::jacobian,
autodiff::forward::wrt, autodiff::forward::at;

using Eigen::Matrix;

namespace cbr
{

struct ASIFParams
{
  // integration time step and number of steps
  double dt{1};
  std::size_t steps{100};

  // spacing between added constraints (1 adds a constraint for each time step)
  std::size_t constr_dist{1};

  // cost multiplier of bounds relaxation
  double relax_cost{100};

  // barrier function constant
  double alpha{1};

  // solver tolerances
  double osqp_eps_abs{1e-4};
  double osqp_eps_rel{1e-4};
  bool osqp_polish{false};
  double osqp_timelimit{1e-2};
  std::size_t osqp_maxiter{5000};

  bool debug{false};
};

/**
 * @brief Active Set Invariance Filter
 *
 * @tparam Dyn: type with following functions
 *  Matrix<T, nx, 1> f(const State<T> &) const;
 *  Matrix<T, nx, nu> g(const State<T> &) const;
 * @tparam BackupContr: type with function
 *  Input<T> operator()(const State<T> &) const;
 * @tparam SafetySet: type with function
 *  Matrix<T, nh, 1> operator()(cosnt State<T> &) const;
 */
template<typename Dyn, typename BackupContr, typename SafetySet>
class ASIF
{
public:
  template<typename T>
  using State = typename Dyn::template State<T>;

  template<typename T>
  using Input = typename Dyn::template Input<T>;

  static constexpr std::size_t nx = State<double>::DoF;
  static constexpr std::size_t nu = Input<double>::SizeAtCompileTime;
  static constexpr std::size_t nh = decltype(SafetySet{} (State<double>{}))::RowsAtCompileTime;

  /**
   * @brief Construct a new ASIF
   *
   * @param dyn dynamics
   * @param bc backup controller
   * @param ss safety set described by level sets
   * @param params parameters
   */
  ASIF(const Dyn & dyn, const BackupContr & bc, const SafetySet & ss, const ASIFParams & params)
  : dyn_(dyn),
    bc_(bc),
    ss_(ss),
    prm_(params)
  {
    const std::size_t num_constr = prm_.steps / prm_.constr_dist;

    // QP constraints qp_l <= qp_A [u ; eps] <= qp_u

    // qp_A = [ B  1
    //          I  0 ]
    //
    // where B are barrier constraints

    // upper part of constraint matrix is dense, lower part has one nonzero per line
    Eigen::Matrix<std::size_t, -1, 1> qp_spA_sparsity(nu + num_constr * nh, 1);
    qp_spA_sparsity.head(nu + num_constr * nh).setConstant(nu + 1);
    qp_spA_sparsity.tail(nu).setOnes();

    qp_objvec_.setZero();

    qp_l_.resize(num_constr * nh + nu);
    qp_u_.resize(num_constr * nh + nu);

    qp_l_.setConstant(-OSQP_INFTY);
    qp_u_.setConstant(OSQP_INFTY);

    qp_A_.resize(num_constr * nh + nu, nu + 1);
    qp_A_.reserve(qp_spA_sparsity);

    for (auto i = 0u; i != num_constr * nh; ++i) {
      for (auto k = 0u; k != nu + 1; ++k) {
        qp_A_.insert(i, k) = 1;
      }
    }
    for (auto i = 0u; i != nu; ++i) {
      qp_A_.insert(num_constr * nh + i, i) = 1;
    }

    qp_A_.makeCompressed();

    Eigen::Matrix<std::size_t, -1, 1> objective_sparsity(nu + 1, 1);
    objective_sparsity.setOnes();

    osqp::OsqpInstance instance;

    // objective matrix does not change
    instance.objective_matrix.resize(nu + 1, nu + 1);
    instance.objective_matrix.reserve(objective_sparsity);
    for (auto i = 0u; i != nu; ++i) {instance.objective_matrix.insert(i, i) = 1;}
    instance.objective_matrix.insert(nu, nu) = prm_.relax_cost;
    instance.objective_matrix.makeCompressed();

    // objective vector changes
    instance.objective_vector = qp_objvec_;

    // constraint matrix changes
    instance.constraint_matrix = qp_A_;

    // constraint bounds change
    instance.lower_bounds = qp_l_;
    instance.upper_bounds = qp_u_;

    osqp::OsqpSettings settings;
    settings.verbose = prm_.debug;
    settings.eps_abs = prm_.osqp_eps_abs;
    settings.eps_rel = prm_.osqp_eps_rel;
    settings.polish = prm_.osqp_polish;
    settings.max_iter = prm_.osqp_maxiter;
    settings.time_limit = prm_.osqp_timelimit;

    osqp_solver_.Init(instance, settings, false);
  }

  /**
   * @brief Add input bounds to ASIF
   *
   * @param lb lower bounds
   * @param ub upper bounds
   */
  void setBounds(const Input<double> & lb, const Input<double> & ub)
  {
    qp_l_.template tail<nu>() = lb;
    qp_u_.template tail<nu>() = ub;
  }

  /**
   * @brief Filter an input with ASIF
   *
   * @param x[in] current state
   * @param u[in, out] desired input is modified in-place to satisfy barrier constraint
   * @param backup_traj[out] ouput backup trajectory (value for each QP constraint)
   * @param min_h[out] smallest h values along backup trajectory
   *
   * @return solver return code
   */
  osqp::OsqpExitCode filter(
    const State<double> & x,
    Input<double> & u,
    std::optional<
      std::reference_wrapper<
        std::vector<
          State<double>,
          Eigen::aligned_allocator<State<double>>
        >
      >
    > backup_traj = {},
    std::optional<Eigen::Ref<Eigen::Matrix<double, nh, 1>>> min_h = {}
  )
  {
    // initial f and g
    const Eigen::Matrix<double, nx, 1> f0 = dyn_.f(x);
    const Matrix<double, nx, nu> g0 = dyn_.g(x);

    // AD variable
    Matrix<dual, nx, 1> dx_ad = Matrix<dual, nx, 1>::Zero();

    // loop variables
    double ti = 0;
    State<double> xi = x;
    Matrix<double, nx, nx> dxi_dx0 = Matrix<double, nx, nx>::Identity();

    if (backup_traj.has_value()) {
      backup_traj.value().get().resize(prm_.steps / prm_.constr_dist);
    }

    for (auto i = 0u, j = 0u; i != prm_.steps; ++i, j = (j + 1) % prm_.constr_dist) {
      const auto xi_ad = xi.template cast<dual>();

      if (j == 0) {
        // evaluate h(x) and its derivative
        Matrix<dual, nh, 1> h_ad;
        const Matrix<double, nh, nx> dh_dxi = jacobian(
          [&](const auto & var) -> Matrix<dual, nh, 1> {return ss_(xi_ad * State<dual>::exp(var));},
          wrt(dx_ad), at(dx_ad), h_ad
        );
        const Matrix<double, nh, 1> h = h_ad.template cast<double>();

        // insert asif constraint in A and l
        const Matrix<double, nh, nx> dh_dx0 = dh_dxi * dxi_dx0;
        const Matrix<double, nh, nu> A_block = dh_dx0 * g0;

        if (prm_.debug) {
          std::cout << "dh_dxi" << std::endl << dh_dxi << std::endl;
          std::cout << "dxi_dx0" << std::endl << dxi_dx0 << std::endl;
          std::cout << "dh_dx0" << std::endl << dh_dx0 << std::endl;
          std::cout << "A_block" << std::endl << A_block << std::endl;
        }

        for (auto c = 0u; c != nu; ++c) {
          for (auto r = 0u; r != nh; ++r) {
            qp_A_.coeffRef((i / prm_.constr_dist) * nh + r, c) = A_block(r, c);
          }
        }
        qp_l_.template segment<nh>((i / prm_.constr_dist) * nh) = -prm_.alpha * h - dh_dx0 * f0;

        // store in backup trajectory
        if (backup_traj.has_value()) {
          backup_traj.value().get()[i / prm_.constr_dist] = xi;
        }

        if (min_h.has_value()) {
          min_h.value() = min_h.value().cwiseMin(h);
        }
      }

      // evaluate closed-loop dynamics fcl(x) and its derivative
      Matrix<dual, nx, 1> fcl_ad;
      const Matrix<double, nx, nx> dfcl_dxi = jacobian(
        [&](const auto & var) -> Matrix<dual, nx, 1> {
          const State<dual> x_dx = xi_ad * State<dual>::exp(var);
          const Matrix<dual, nu, 1> u = bc_(x_dx);
          const Matrix<dual, nu, 1> u_smooth = Matrix<dual, nu, 1>::NullaryExpr(
            [&](int i) {return smoothSat(u(i), qp_l_.tail<nu>()(i), qp_u_.tail<nu>()(i));}
          );
          return dyn_.f(x_dx) + dyn_.g(x_dx) * u_smooth;
        },
        wrt(dx_ad), at(dx_ad), fcl_ad
      );
      const Matrix<double, nx, 1> fcl = fcl_ad.template cast<double>();

      if (prm_.debug) {
        std::cout << "dfcl_dxi" << std::endl << dfcl_dxi << std::endl;
      }

      // NOTE should use ad() with lie library that supports it
      Matrix<double, nx, nx> adjoint_fcl;
      for (auto i = 0u; i != nx; ++i) {
        adjoint_fcl.col(i) = State<double>::lieBracket(fcl, Eigen::Matrix<double, nx, 1>::Unit(i));
      }

      // integrate system and sensitivity forward
      state_stepper.do_step(
        [&fcl](const State<double> &, Matrix<double, nx, 1> & dx_dt, double) {dx_dt = fcl;},
        xi, ti, prm_.dt
      );
      sensitivity_stepper.do_step(
        [&adjoint_fcl, &dfcl_dxi](const Matrix<double, nx, nx> & Dx,
        Matrix<double, nx, nx> & dDx_dt, double) {
          dDx_dt = (-adjoint_fcl + dfcl_dxi) * Dx;
        },
        dxi_dx0, ti, prm_.dt
      );

      ti += prm_.dt;
    }

    qp_objvec_.template head<nu>() = -u;

    if (prm_.debug) {
      std::cout << "Number of constraints " << qp_A_.rows() << std::endl;
      std::cout << "Setting constraint matrix to" << std::endl << qp_A_.toDense() << std::endl;
      std::cout << "Setting lb = " << qp_l_.transpose() << std::endl;
      std::cout << "Setting ub = " << qp_u_.transpose() << std::endl;
      std::cout << "Setting obj_vec = " << qp_objvec_.transpose() << std::endl;
    }

    osqp_solver_.UpdateConstraintMatrix(qp_A_);
    osqp_solver_.SetBounds(qp_l_, qp_u_);
    osqp_solver_.SetObjectiveVector(qp_objvec_);

    const auto rc = osqp_solver_.Solve();

    if (rc == osqp::OsqpExitCode::kOptimal || rc == osqp::OsqpExitCode::kOptimalInaccurate) {
      // return optimal solution
      u = osqp_solver_.primal_solution().template head<nu>();
    } else {
      // return saturated backup trajectory if solving failed
      const Matrix<double, nu, 1> u_backup = bc_(x);
      u = Matrix<double, nu, 1>::NullaryExpr(
        [&](int i) {return smoothSat(u_backup(i), qp_l_.tail<nu>()(i), qp_u_.tail<nu>()(i));}
      );
    }

    return rc;
  }

private:
  // problem definition
  Dyn dyn_;
  BackupContr bc_;
  SafetySet ss_;
  ASIFParams prm_;

  // steppers
  cbr::lie::odeint::euler<State<double>, double, Eigen::Matrix<double, nx, 1>> state_stepper{};
  boost::numeric::odeint::euler<
    Matrix<double, nx, nx>, double, Matrix<double, nx, nx>, double,
    boost::numeric::odeint::vector_space_algebra
  > sensitivity_stepper{};

  // qp matrices
  Eigen::SparseMatrix<double, Eigen::RowMajor> qp_A_;
  Eigen::Matrix<double, -1, 1> qp_l_, qp_u_;
  Eigen::Matrix<double, nu + 1, 1> qp_objvec_;

  // qp solver
  osqp::OsqpSolver osqp_solver_;
};

}  // namespace cbr

// cppcheck-suppres unknownMacro
BOOST_HANA_ADAPT_STRUCT(
  cbr::ASIFParams,
  dt,
  steps,
  constr_dist,
  relax_cost,
  alpha,
  osqp_eps_abs,
  osqp_eps_rel,
  osqp_polish,
  osqp_timelimit,
  osqp_maxiter,
  debug
);

#endif  // CBR_CONTROL__ASIF___HPP_


#ifdef MEX
#ifdef WITH_EMBREE
#  include "winding_number_ray.h"
#endif
#include "parse_rhs.h"
#include "prepare_lhs.h"

#include <igl/winding_number.h>
#include <igl/WindingNumberAABB.h>
#include <igl/matlab/MexStream.h>
#include <igl/matlab/mexErrMsgTxt.h>
#include <igl/writeOBJ.h>
#include <igl/get_seconds.h>
#include <igl/per_face_normals.h>
#ifdef WITH_EMBREE
#  include <igl/embree/EmbreeIntersector.h>
#endif
#include <igl/random_dir.h>

#include <mex.h>
#include <Eigen/Dense>
#include <iostream>

void mexFunction(
  int nlhs, mxArray *plhs[],
  int nrhs, const mxArray *prhs[])
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;

  igl::matlab::MexStream mout;
  std::streambuf *outbuf = cout.rdbuf(&mout);
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);

  // vertex position list
  double * V;
  // face list
  double * Ftemp;
  // origin list
  double * O;
  // number of dimensions (and simplex size in F)
  int dim;
  // number of mesh vertices
  int n;
  // number of mesh faces
  int m;
  // number of origins
  int no;
  winding_number_params params;
  parse_rhs(nrhs,prhs,dim,V,n,Ftemp,m,O,no,params);

  double * F = new double[dim*m];
  // convert to 0-index
  //transform(Ftemp,Ftemp+m*dim,F,bind2nd(plus<double>(),-1.0));
  for(int i = 0;i<m;i++)
  {
    for(int d = 0;d<dim;d++)
    {
      F[d*m + i]=Ftemp[d*m+i]-1;
    }
  }

  //// Set up openmp
  //int nProcessors=omp_get_max_threads();
  ////std::cout<<nProcessors<<std::endl;
  //omp_set_num_threads(min(nProcessors,2));

  // List of winding numbers
  double * W = NULL;
  if(nlhs == 0)
  {
    return;
  }
  prepare_lhs(nlhs,plhs,no,W);

  double start_sec = igl::get_seconds();
  if(params.ray_cast)
  {
#ifdef WITH_EMBREE
    // Prepare eigen versions of input
    MatrixXd MV(n,dim);
    copy(V,V+n*dim,MV.data());
    MatrixXi MF(m,dim);
    copy(F,F+m*dim,MF.data());

    // Initialize Embree
    //cout<<"Flipping faces..."<<endl;
    MatrixXi FF;
    FF.resize(MF.rows()*2,MF.cols());
    FF << MF, MF.rowwise().reverse().eval();
    //cout<<"Computing normals..."<<endl;
    Eigen::MatrixXd N;
    per_face_normals(MV,MF,N);
    //cout<<"Initializing Embree..."<<endl;
    // Initialize intersector
    igl::embree::EmbreeIntersector ei;
    ei.init(MV.cast<float>(),FF);

    // loop over origins
#   pragma omp parallel for if (no>IGL_WINDING_NUMBER_OMP_MIN_VALUE)
    for(int o = 0;o<no;o++)
    {
      Vector3d p(O[0*no+o], O[1*no+o], O[2*no+o]);
      for(int r = 0;r<params.num_rays;r++)
      {
        int num_rays_shot = -1;
        Vector3d dir = random_dir();
        if(params.twod_rays)
        {
          dir[2] = 0;
          dir.normalize();
        }
        double w = 0;
        if(params.ray_parity)
        {
          winding_number_ray_parity(ei,N,p,dir,w,num_rays_shot);
        }else
        {
          winding_number_ray(ei,N,p,dir,w,num_rays_shot);
        }
        W[o] += w;
      }
      W[o] /= (double)params.num_rays;
    }

#else
    igl::matlab::mexErrMsgTxt(false,"Recompile with WITH_EMBREE defined");
#endif
  }else
  {
    if(params.hierarchical && dim == 3)
    {
      // Prepare eigen versions of input
      MatrixXd MV(n,dim);
      copy(V,V+n*dim,MV.data());
      MatrixXi MF(m,dim);
      copy(F,F+m*dim,MF.data());
      // Initialize hierarchy
      WindingNumberAABB<Vector3d> hier(MV,MF);
      // Build hierarchy
      hier.grow();
      // loop over origins
#     pragma omp parallel for if (no>IGL_WINDING_NUMBER_OMP_MIN_VALUE)
      for(int o = 0;o<no;o++)
      {
        Vector3d p(O[0*no+o], O[1*no+o], O[2*no+o]);
        W[o] = hier.winding_number(p);
      }
    }else
    {
      switch(dim)
      {
        case 3:
          winding_number_3(V,n,F,m,O,no,W);
          break;
        case 2:
          winding_number_2(V,n,F,m,O,no,W);
          break;
      }
    }
  }
  //mexPrintf("Elapsed time is %g seconds\n",igl::get_seconds()-start_sec);

  // Restore the std stream buffer Important!
  delete[] F;
  std::cout.rdbuf(outbuf);
}

#endif


#include "IntegratorRect.hh"

#include <Eigen/Dense>
#include <iterator>

#include "TypesFunctions.hh"

using namespace Eigen;
using namespace std;

IntegratorRect::IntegratorRect(int order, const std::string& mode) : IntegratorBase(order)
{
  init(mode);
}

IntegratorRect::IntegratorRect(size_t bins, int orders, double* edges, const std::string& mode) : IntegratorBase(bins, orders, edges)
{
  init(mode);
}

IntegratorRect::IntegratorRect(size_t bins, int* orders, double* edges, const std::string& mode) : IntegratorBase(bins, orders, edges)
{
  init(mode);
}

void IntegratorRect::init(const std::string& mode) {
  m_mode = mode;

  if(m_mode=="left"){
    m_rect_offset=-1;
  }
  else if(m_mode=="center"){
    m_rect_offset=0;
  }
  else if(m_mode=="right"){
    m_rect_offset=1;
  }
  else{
    throw std::runtime_error("invalid rectangular integration mode");
  }

  init_sampler();
}

void IntegratorRect::sample(FunctionArgs& fargs){
  auto& rets=fargs.rets;
  rets[1].x = m_edges.cast<double>();
  rets[2].x = 0.0;
  auto npoints=m_edges.size()-1;
  rets[3].x = 0.5*(m_edges.tail(npoints)+m_edges.head(npoints));

  auto& abscissa=rets[0].x;

  auto nbins=m_edges.size()-1;
  auto& binwidths=m_edges.tail(nbins) - m_edges.head(nbins);
  ArrayXd samplewidths=binwidths/m_orders.cast<double>();

  ArrayXd low, high;
  switch(m_rect_offset){
    case -1: {
      low=m_edges.head(nbins);
      high=m_edges.tail(nbins)-samplewidths;
      break;
      }
    case 0: {
      ArrayXd offsetwidth=samplewidths*0.5;
      low=m_edges.head(nbins)+offsetwidth;
      high=m_edges.tail(nbins)-offsetwidth;
      break;
      }
    case 1: {
      samplewidths=binwidths/m_orders.cast<double>();
      low=m_edges.head(nbins)+samplewidths;
      high=m_edges.tail(nbins);
      break;
      }
  }

  size_t offset=0;
  for (size_t i = 0; i < static_cast<size_t>(m_orders.size()); ++i) {
    auto n=m_orders[i];
    if(n>1){
      abscissa.segment(offset, n)=ArrayXd::LinSpaced(n, low[i], high[i]);
      m_weights.segment(offset, n)=samplewidths[i];
    }
    else{
      abscissa[i]=low[i];
      m_weights[i]=binwidths[i];
    }
    offset+=n;
  }
  rets.untaint();
  rets.freeze();
}


#ifndef IMGEN_HSL_HPP_
#define IMGEN_HSL_HPP_

/**
 * The HSL implementation from the Boost GIL Contributions
 * (http://gil-contributions.googlecode.com/svn/sandbox/boost/gil/extension/toolbox/hsl.hpp)
 * doesn't correctly convert from HSL to RGB so this is my own implementation.
 *
 * You can test this by the below code:
 *
 * hsl32f_pixel_t hsl(0.33, 0.56, 0.64);
 * rgb8_pixel_t rgb;
 *
 * color_convert(hsl, rgb);
 *
 * rgb will erroneously be (163, 163, 163) when it should infact be (114, 215,
 * 112)
 *
 * The algorithms implemented here have been taken from:
 * http://axonflux.com/handy-rgb-to-hsl-and-rgb-to-hsv-color-model-c
 */

#include <boost/gil/gil_all.hpp>
#include <boost/mpl/vector_c.hpp>

#include <cstdlib>

namespace boost { namespace gil {

struct hue_t {};
struct saturation_t {};
struct lightness_t {};

typedef mpl::vector3<hue_t, saturation_t, lightness_t> hsl_t;
typedef layout<hsl_t> hsl_layout_t;

GIL_DEFINE_ALL_TYPEDEFS(32f, hsl)

namespace detail {

bits32f hue_to_rgb(bits32f p, bits32f q, bits32f t);

} // namespace detail

template <>
struct default_color_converter_impl<rgb_t, hsl_t> {
    template <typename P1>
    void operator()(const P1& src, hsl32f_pixel_t& dst) const {
        bits32f red = channel_convert<bits32f>(get_color(src, red_t()));
        bits32f green = channel_convert<bits32f>(get_color(src, green_t()));
        bits32f blue = channel_convert<bits32f>(get_color(src, blue_t()));
        bits32f hue, saturation, lightness;

        auto max = std::max(red, std::max(green, blue));
        auto min = std::min(red, std::min(green, blue));
        lightness = (max + min) / 2;

        if(max == min) {
            hue = 0.0f;
            saturation = 0.0f;
        } else {
            auto diff = max - min;

            if(lightness > 0.5f) {
                saturation = diff / (2 - max - min);
            } else {
                saturation = diff / (max + min);
            }

            if(red == max) {
                hue = (green - blue) / diff + (green < blue ? 6.0f : 0.0f);
            } else if(green == max) {
                hue = (blue - red) / diff + 2.0f;
            } else { // blue == max
                hue = (red - green) / diff + 4.0f;
            }

            hue /= 6.0f;
        }

        get_color(dst, hue_t()) = hue;
        get_color(dst, saturation_t()) = saturation;
        get_color(dst, lightness_t()) = lightness;
    }
};

template <>
struct default_color_converter_impl<hsl_t, rgb_t> {
    template <typename P2>
    void operator()(const hsl32f_pixel_t& src, P2& dst) const {
        bits32f hue = get_color(src, hue_t());
        bits32f saturation = get_color(src, saturation_t());
        bits32f lightness = get_color(src, lightness_t());
        bits32f red, green, blue;

        auto q = lightness < 0.5 ? lightness * (1 + saturation)
                                 : lightness + saturation - lightness * saturation;
        auto p = 2 * lightness - q;

        if(saturation == 0) {
            red = lightness;
            green = lightness;
            blue = lightness;
        } else {
            red   = detail::hue_to_rgb(p, q, hue + 1.0f/3.0f);
            green = detail::hue_to_rgb(p, q, hue);
            blue  = detail::hue_to_rgb(p, q, hue - 1.0/3.0);
        }

        get_color(dst, red_t()) =
            channel_convert<typename color_element_type<P2, red_t>::type>(red);

        get_color(dst, green_t()) =
            channel_convert<typename color_element_type<P2, green_t>::type>(green);

        get_color(dst, blue_t()) =
            channel_convert<typename color_element_type<P2, blue_t>::type>(blue);
    }
};

} } //namespace boost::gil

#endif // IMGEN_HSL_HPP_


#include <vector>
#include <iostream>
#include <Eigen/Core>
#include <pcl/common/time.h>
#include "pcl/point_types.h"
#include "pcl/point_cloud.h"
#include "pcl/io/pcd_io.h"
#include <pcl/search/kdtree.h>
#include "pcl/kdtree/kdtree_flann.h"
#include "pcl/features/normal_3d.h"
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/vfh.h>
#include <pcl/visualization/histogram_visualizer.h>
#include <pcl/visualization/pcl_plotter.h>
#include <pcl/console/parse.h>

void
downsample (pcl::PointCloud<pcl::PointXYZ>::Ptr &points, float leaf_size,
            pcl::PointCloud<pcl::PointXYZ>::Ptr &downsampled_out)
{
  pcl::VoxelGrid<pcl::PointXYZ> vox_grid;
  vox_grid.setLeafSize (leaf_size, leaf_size, leaf_size);
  vox_grid.setInputCloud (points);
  vox_grid.filter (*downsampled_out);
}

void
compute_surface_normals (pcl::PointCloud<pcl::PointXYZ>::Ptr &points, float normal_radius,
                         pcl::PointCloud<pcl::Normal>::Ptr &normals_out)
{
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> norm_est;

  // Use a FLANN-based KdTree to perform neighborhood searches
  //norm_est.setSearchMethod (pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr (new pcl::KdTreeFLANN<pcl::PointXYZ>));
  norm_est.setSearchMethod (pcl::search::KdTree<pcl::PointXYZ>::Ptr (new pcl::search::KdTree<pcl::PointXYZ>));

  // Specify the size of the local neighborhood to use when computing the surface normals
  norm_est.setRadiusSearch (normal_radius);

  // Set the input points
  norm_est.setInputCloud (points);

  // Estimate the surface normals and store the result in "normals_out"
  norm_est.compute (*normals_out);
}

void
compute_VFH_features (pcl::PointCloud<pcl::PointXYZ>::Ptr &points,
                      pcl::PointCloud<pcl::Normal>::Ptr &normals,
                      pcl::PointCloud<pcl::VFHSignature308>::Ptr &descriptors_out)
{
  // Create a PFHEstimation object
  pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh_est;

  // Set the input points and surface normals
  vfh_est.setInputCloud (points);
  vfh_est.setInputNormals(normals);
  vfh_est.setSearchMethod(pcl::search::KdTree<pcl::PointXYZ>::Ptr (new pcl::search::KdTree<pcl::PointXYZ>));
  // Optionally, we can normalize the bins of the resulting histogram,
  // using the total number of points.
  vfh_est.setNormalizeBins(true);
  // Also, we can normalize the SDC with the maximum size found between
  // the centroid and any of the cluster's points.
  vfh_est.setNormalizeDistance(false);

  std::cout << "start compute VFH features descriptors" << std::endl;
  // Compute the features
  vfh_est.compute (*descriptors_out);
  std::cout << "Compute VFH descriptors successfully" << std::endl;
/*
  pcl::visualization::PCLPlotter plotter;
  // We need to set the size of the descriptor beforehand.
  plotter.addFeatureHistogram(*descriptors_out, 308);
  plotter.plot();

  // Plotter object.
  pcl::visualization::PCLHistogramVisualizer viewer;
  // We need to set the size of the descriptor beforehand.
  viewer.addFeatureHistogram(*descriptors_out, 308);
  viewer.spin();
*/
}

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

  for(int i=0; i< 143; i++){

    pcl::PointCloud<pcl::PointXYZ>::Ptr points1 (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled1 (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::Normal>::Ptr normals1 (new pcl::PointCloud<pcl::Normal>);
    pcl::PointCloud<pcl::VFHSignature308>::Ptr descriptors_vfh1 (new pcl::PointCloud<pcl::VFHSignature308>);

    std::stringstream ss1, ss2;
    ss1 << "training_model_"<<i<<".pcd";
    pcl::io::loadPCDFile (ss1.str(), *points1);
    const float voxel_grid_leaf_size1 = 4.0;
    downsample (points1, voxel_grid_leaf_size1, downsampled1);
    const float normal_radius1 = 4.5;
    compute_surface_normals (downsampled1, normal_radius1, normals1);
    compute_VFH_features (downsampled1, normals1, descriptors_vfh1);
    ss2 << "training_VFH_features_"<<i<<".pcd";
    pcl::io::savePCDFileBinary(ss2.str(), *descriptors_vfh1 );

  }

  return (0);
}


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

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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_IDIVCEIL_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_IDIVCEIL_HPP_INCLUDED

#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/iceil.hpp>
#include <boost/simd/function/ceil.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::iceil_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( bd::functor<bs::tag::iceil_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return div(ceil, a0, a1);
    }
  };

#ifdef BOOST_MSVC
  #pragma warning(push)
  #pragma warning(disable: 4723) // potential divide by 0
#endif

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::iceil_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<A0> operator() ( bd::functor<bs::tag::iceil_> const&
                                                      , A0 const & a0
                                                      , A0 const & a1) const BOOST_NOEXCEPT
    {
      return iceil(a0/a1);
    }
  };

#ifdef BOOST_MSVC
  #pragma warning(pop)
#endif
} } }

#endif


//
// Created by Hamza El-Kebir on 6/21/21.
//

#ifndef LODESTAR_SINGLETON_HPP
#define LODESTAR_SINGLETON_HPP

#include <type_traits>
#include <Eigen/Dense>
#include "Lodestar/aux/CompileTimeQualifiers.hpp"

#include "SetExpression.hpp"
#include "SetUnion.hpp"
#include "EmptySet.hpp"
#include "SetComplement.hpp"

namespace ls {
    namespace primitives {
        namespace sets {
            /**
             * @brief A singleton set.
             *
             * @brief A singleton contains a single element, represented by the vector \c value_.
             *
             * @tparam TDimension The dimension of the singleton; if -1, it is dynamically sized.
             * @tparam TScalarType The scalar type.
             */
            template<int TDimension = -1, typename TScalarType = double>
            class Singleton : public SetExpression<Singleton<TDimension, TScalarType>> {
            public:
                template<typename, typename>
                friend
                class SetUnion;

                using Base = SetExpression<Singleton<TDimension, TScalarType>>; //! Base class.
                using type = Singleton<TDimension, TScalarType>; //! Expression type.

                typedef Eigen::Matrix<TScalarType, LS_STATIC_UNLESS_DYNAMIC(TDimension), 1> TDValue; //! Value typedef.

                /**
                 * @brief Default constructor.
                 */
                Singleton() : value_{TDValue::Zero()}
                {
                    this->sEnum_ = SetEnum::Singleton;
                }

                /**
                 * @brief Constructs a Singleton instance from a vector.
                 *
                 * @tparam TDerived Derived EigenBase class.
                 *
                 * @param value Singleton value.
                 */
                template<typename TDerived>
                Singleton(const Eigen::EigenBase<TDerived> &value) : value_(value)
                {
                    this->sEnum_ = SetEnum::Singleton;
                }

                /**
                 * @brief Returns the singleton dimension.
                 *
                 * @details This specialization deals with the static case.
                 *
                 * @tparam T_TDimension Copy of \c TDimension.
                 *
                 * @return Dimension of the singleton instance.
                 */
                template<int T_TDimension = TDimension>
                int
                dimension(typename std::enable_if<(T_TDimension < 0)>::type * = nullptr) const
                {
                    return T_TDimension;
                }

                /**
                 * @brief Returns the singleton dimension.
                 *
                 * @details This specialization deals with the dynamic case.
                 *
                 * @tparam T_TDimension Copy of \c TDimension.
                 *
                 * @return Dimension of the singleton instance.
                 */
                template<int T_TDimension = TDimension>
                int
                dimension(typename std::enable_if<(T_TDimension >= 0)>::type * = nullptr) const
                {
                    return value_.rows();
                }

                /**
                 * @brief Changes the dimension of the singleton.
                 *
                 * @details This specialization raises a static assert since statically type Singleton instances cannot
                 * be resized.
                 *
                 * @tparam T_TDimension Copy of \c TDimension.
                 *
                 * @param dim New dimension.
                 */
                template<int T_TDimension = TDimension>
                void
                setDimension(size_t dim, typename std::enable_if<(T_TDimension < 0)>::type * = nullptr)
                {
                    static_assert(TDimension > -1, "Cannot set dimension of statically sized singleton.");
                }

                /**
                 * @brief Changes the dimension of the singleton.
                 *
                 * @details This specialization conservatively resizes \c value_.
                 *
                 * @tparam T_TDimension Copy of \c TDimension.
                 *
                 * @param dim New dimension.
                 */
                template<int T_TDimension = TDimension>
                void
                setDimension(size_t dim, typename std::enable_if<(T_TDimension >= 0)>::type * = nullptr)
                {
                    value_.conservativeResize(dim);
                }

                /**
                 * @brief Returns true if this expression contains \c expr.
                 *
                 * @details This specialization handles a singleton expression of the same type.
                 *
                 * @tparam TExpression Type of the other expression.
                 *
                 * @param expr Expression to check containment of.
                 * @param tol Numerical tolerance.
                 *
                 * @return True if this expression contains \c expr, false otherwise.
                 */
                template<typename TExpression>
                typename std::enable_if<std::is_same<TExpression, type>::value, bool>::type
                contains(const SetExpression<TExpression> &expr, double tol = 1e-6) const
                {
                    return (value_ - static_cast<const TExpression *>(&expr)->value_).isMuchSmallerThan(tol);
                }

                /**
                 * @brief Returns true if this expression contains \c expr.
                 *
                 * @details This specialization handles all other cases.
                 *
                 * @tparam TExpression Type of the other expression.
                 *
                 * @param expr Expression to check containment of.
                 * @param tol Numerical tolerance.
                 *
                 * @return False.
                 */
                template<typename TExpression>
                typename std::enable_if<!std::is_same<TExpression, type>::value, bool>::type
                contains(const SetExpression<TExpression> &expr, double tol = 1e-6) const
                {
                    return false;
                }

                /**
                 * @brief Checks if this expression is equal to \expr.
                 *
                 * @details This specialization deals with singletons of the same type.
                 *
                 * @tparam TExpression Type of the other expression.
                 *
                 * @param expr Expression to check equality with.
                 *
                 * @return True if this expression is equal to \c expr, false otherwise.
                 */
                template<typename TExpression>
                typename std::enable_if<std::is_same<TExpression, type>::value, bool>::type
                operator==(const SetExpression<TExpression> &expr) const
                {
                    return (value_ - static_cast<const TExpression *>(&expr)->value_).isMuchSmallerThan(1e-6);
                }

                // TODO: Add dynamic case.

                /**
                 * @brief Checks if this expression is equal to \expr.
                 *
                 * @details This specialization deals with all other cases.
                 *
                 * @tparam TExpression Type of the other expression.
                 *
                 * @param expr Expression to check equality with.
                 *
                 * @return False.
                 */
                template<typename TExpression>
                typename std::enable_if<!std::is_same<TExpression, type>::value, bool>::type
                operator==(const SetExpression<TExpression> &expr) const
                {
                    return false;
                }

                /**
                 * @brief Checks if this expression is not equal to \expr.
                 *
                 * @details This specialization deals with singletons of the same type.
                 *
                 * @tparam TExpression Type of the other expression.
                 *
                 * @param expr Expression to check inequality with.
                 *
                 * @return True if this expression is not equal to \c expr, false otherwise.
                 */
                template<typename TExpression>
                typename std::enable_if<std::is_same<TExpression, type>::value, bool>::type
                operator!=(const SetExpression<TExpression> &expr) const
                {
                    return !((value_ - static_cast<const TExpression *>(&expr)->value_).isMuchSmallerThan(1e-6));
                }

                /**
                 * @brief Checks if this expression is not equal to \expr.
                 *
                 * @details This specialization deals with all other cases.
                 *
                 * @tparam TExpression Type of the other expression.
                 *
                 * @param expr Expression to check inequality with.
                 *
                 * @return True.
                 */
                template<typename TExpression>
                typename std::enable_if<!std::is_same<TExpression, type>::value, bool>::type
                operator!=(const SetExpression<TExpression> &expr) const
                {
                    return true;
                }

                /**
                 * @brief Creates a union between this expression and another SetExpression.
                 *
                 * @tparam TExpression Type of the other expression.
                 *
                 * @param expr Expression to create a union with.
                 *
                 * @return Union.
                 */
                template<typename TExpression>
                SetUnion<type, TExpression> unionize(const SetExpression<TExpression> &expr)
                {
                    return SetUnion<type, TExpression>(*this, *static_cast<const TExpression *>(&expr));
                }

                /**
                 * @brief Checks if this expression is a subset of \c expr.
                 *
                 * @tparam TExpression Type of the other expression.
                 *
                 * @param expr Expression to check inclusion on.
                 *
                 * @return True if this expression is a subset of \c expr, false otherwise.
                 */
                template<typename TExpression>
                bool isSubset(const SetExpression<TExpression> &expr)
                {
                    return static_cast<const TExpression *>(&expr)->contains(*this);
                }

                /**
                 * @brief Checks if this expression is a superset of \c expr.
                 *
                 * @tparam TExpression Type of the other expression.
                 *
                 * @param expr Expression to check subsumption on.
                 *
                 * @return True if this expression is a superset of \c expr, false otherwise.
                 */
                template<typename TExpression>
                bool isSuperset(const SetExpression<TExpression> &expr)
                {
                    return contains(expr);
                }

                /**
                 * @brief Returns true if the expression is the empty set.
                 *
                 * @note This always returns false, since a singleton always contains one element.
                 *
                 * @return False.
                 */
                bool isEmpty() const
                {
                    return false;
                }

                /**
                 * @brief Computes the relative complement of this expression and \c expr.
                 *
                 * @details The syntax is as follows:
                 * \code
                 * // A \ B
                 * A.relComplement(B);
                 * \endcode
                 *
                 * @tparam TExpression Type of the other expression.
                 *
                 * @param expr Expression to take relative complement with.
                 *
                 * @return Relative complement.
                 */
                template<typename TExpression>
                SetComplement<type, TExpression> relComplement(const SetExpression<TExpression> &expr)
                {
                    return SetComplement<type, TExpression>(*this, *static_cast<const TExpression *>(&expr),
                                                            isSubset(expr));
                }

                /**
                 * @brief Returns signed distance to \c p.
                 *
                 * @tparam TDerived Derived MatrixBase class.
                 *
                 * @param p A point.
                 *
                 * @return Signed distance.
                 */
                template<typename TDerived>
                double sdf(Eigen::MatrixBase<TDerived> &p) const
                {
                    p.resize(dimension());
                    return (value_ - p).norm();
                }

            protected:
                TDValue value_; //! Singleton value.
            };
        }
    }
}

#endif //LODESTAR_SINGLETON_HPP


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2013.
// 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 <mpllibs/metamonad/eval_let_c.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/plus.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/assert.hpp>

#include "common.hpp"

BOOST_AUTO_TEST_CASE(test_eval_let_c)
{
  using boost::mpl::equal_to;
  using boost::mpl::plus;

  using mpllibs::metamonad::eval_let_c;

  // test_evaluation_of_expression
  BOOST_MPL_ASSERT((equal_to<int13, eval_let_c<x,int11,plus<int2, x> >::type>));
}


//  (C) Copyright Jeremy Siek 2004
//  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)

// From Louis Lavery <Louis@devilsChimney.co.uk>
/*Expected Output:-
A:   0 A
B:  11 A

Actual Output:-
A:   0 A
B: 2147483647 B
*/

#include <iostream>
#include <iomanip>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <boost/cstdlib.hpp>
#include <boost/test/minimal.hpp>

int test_main(int, char*[])
{
  using namespace boost;

  enum { A, B, Z };
  char const name[] = "ABZ";
  int const numVertex = static_cast<int>(Z) + 1;
  typedef std::pair<int,int> Edge;
  Edge edge_array[] = {Edge(B,A)};
  int const numEdges = sizeof(edge_array) / sizeof(Edge);
  int const weight[numEdges] = {11};

  typedef adjacency_list<vecS,vecS,undirectedS,
    no_property,property<edge_weight_t,int> > Graph;

  Graph g(edge_array, edge_array + numEdges, numVertex);

  Graph::edge_iterator ei, ei_end;
  property_map<Graph,edge_weight_t>::type
    weight_pmap = get(edge_weight, g);

  int i = 0;
  for(boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei, ++i)
    weight_pmap[*ei] = weight[i];

  std::vector<int> parent(numVertex);
  for(i = 0; i < numVertex; ++i)
    parent[i] = i;

  int inf = (std::numeric_limits<int>::max)();
  std::vector<int> distance(numVertex, inf);
  distance[A] = 0; // Set source distance to zero

  bool const r = bellman_ford_shortest_paths
    (g, int (numVertex),
     weight_pmap,
     &parent[0],
     &distance[0],
     closed_plus<int>(),
     std::less<int>(),
     default_bellman_visitor());

  if (r) {
    for(int i = 0; i < numVertex; ++i) {
      std::cout << name[i] << ": ";
      if (distance[i] == inf)
        std::cout  << std::setw(3) << "inf";
      else
        std::cout << std::setw(3) << distance[i];
      std::cout << " " << name[parent[i]] << std::endl;
    }
  } else {
    std::cout << "negative cycle" << std::endl;
  }

#if !(defined(__INTEL_COMPILER) && __INTEL_COMPILER <= 700) && !(defined(BOOST_MSVC) && BOOST_MSVC <= 1300)
  graph_traits<Graph>::vertex_descriptor s = vertex(A, g);
  std::vector<int> parent2(numVertex);
  std::vector<int> distance2(numVertex, 17);
  bool const r2 = bellman_ford_shortest_paths
                    (g,
                     weight_map(weight_pmap).distance_map(&distance2[0]).
                     predecessor_map(&parent2[0]).root_vertex(s));
  if (r2) {
    for(int i = 0; i < numVertex; ++i) {
      std::cout << name[i] << ": ";
      if (distance2[i] == inf)
        std::cout  << std::setw(3) << "inf";
      else
        std::cout << std::setw(3) << distance2[i];
      std::cout << " " << name[parent2[i]] << std::endl;
    }
  } else {
    std::cout << "negative cycle" << std::endl;
  }

  BOOST_CHECK(r == r2);
  if (r && r2) {
    BOOST_CHECK(parent == parent2);
    BOOST_CHECK(distance == distance2);
  }
#endif

  return boost::exit_success;
}


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <core/util/test_macros.hpp>
#include <core/storage/sgraph_data/sgraph.hpp>
#include <core/storage/sgraph_data/sgraph_vertex_apply.hpp>
#include "sgraph_test_util.hpp"

using namespace turi;

struct sgraph_vertex_apply_test {

public:

void check_vertex_apply_result(std::vector<std::shared_ptr<sarray<flexible_type>>>& val) {
  for(auto& iter: val) {
    std::vector<flexible_type> ret;
    turi::copy(*iter, std::inserter(ret, ret.end()));
    for (auto& retval: ret) {
      TS_ASSERT_EQUALS((int)retval.get_type(), (int)flex_type_enum::FLOAT);
      TS_ASSERT_EQUALS(retval.get<double>(), 2.0);
    }
  }
}

void test_vertex_apply() {
  // there are 4 overloads
  size_t n_vertex = 10;
  size_t n_partition = 2;
  sgraph ring_graph  = create_ring_graph(n_vertex, n_partition, false);
  size_t data_index = ring_graph.vertex_group(0)[0].column_index("vdata");
  // map data + 1 = 2.0
  auto ret = sgraph_compute::vertex_apply(ring_graph,
                                          flex_type_enum::FLOAT,
                                          [=](const std::vector<flexible_type>& val){
                                            TS_ASSERT_LESS_THAN(data_index, val.size());
                                            return val[data_index] + 1.0;
                                          });
  check_vertex_apply_result(ret);

  // map data + prevret / 2 = 2.0
  ret = sgraph_compute::vertex_apply(ring_graph,
                                     ret,
                                     flex_type_enum::FLOAT,
                                     [=](const std::vector<flexible_type>& val, flexible_type prev_ret){
                                       TS_ASSERT_LESS_THAN(data_index, val.size());
                                       return val[data_index] + prev_ret / 2;
                                     });
  check_vertex_apply_result(ret);

  // map data + prevret / 2 = 2.0
  ret = sgraph_compute::vertex_apply(ring_graph,
                                     "vdata",
                                     ret,
                                     flex_type_enum::FLOAT,
                                     [=](const flexible_type& val, flexible_type prev_ret){
                                       return val + prev_ret / 2;
                                     });
  check_vertex_apply_result(ret);

  // map data + 1 = 2.0
  ret = sgraph_compute::vertex_apply(ring_graph,
                                     "vdata",
                                     flex_type_enum::FLOAT,
                                     [=](const flexible_type& val){
                                       return val + 1.0;
                                     });
  check_vertex_apply_result(ret);

  double vsum = sgraph_compute::vertex_reduce<double>(ring_graph,
                                      [=](const std::vector<flexible_type>& val, double& sum) {
                                        TS_ASSERT_LESS_THAN(data_index, val.size());
                                        sum += (double)val[data_index];
                                      },
                                      [=](const double& val, double& sum) {
                                        sum += val;
                                      });
  TS_ASSERT_EQUALS(vsum, n_vertex);

  flexible_type vsum2 = sgraph_compute::vertex_reduce<flexible_type>(ring_graph,
                                               "vdata",
                                               [=](const flexible_type& val, flexible_type& sum) {
                                                 sum += val;
                                               },
                                               [=](const flexible_type& val, flexible_type& sum) {
                                                 sum += val;
                                               });
  TS_ASSERT_EQUALS(vsum2, n_vertex);

}
};

BOOST_FIXTURE_TEST_SUITE(_sgraph_vertex_apply_test, sgraph_vertex_apply_test)
BOOST_AUTO_TEST_CASE(test_vertex_apply) {
  sgraph_vertex_apply_test::test_vertex_apply();
}
BOOST_AUTO_TEST_SUITE_END()


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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)
//=======================================================================
//
//  Sample output
//  DFS parenthesis:
//  (0(2(3(4(11)4)3)2)0)

#include <boost/config.hpp>
#include <assert.h>
#include <iostream>

#include <vector>
#include <algorithm>
#include <utility>

#include "boost/graph/visitors.hpp"
#include "boost/graph/adjacency_list.hpp"
#include "boost/graph/breadth_first_search.hpp"
#include "boost/graph/depth_first_search.hpp"

using namespace boost;
using namespace std;

struct open_paren : public base_visitor<open_paren> {
  typedef on_discover_vertex event_filter;
  template <class Vertex, class Graph>
  void operator()(Vertex v, Graph&) {
    std::cout << "(" << v;
  }
};
struct close_paren : public base_visitor<close_paren> {
  typedef on_finish_vertex event_filter;
  template <class Vertex, class Graph>
  void operator()(Vertex v, Graph&) {
    std::cout << v << ")";
  }
};

int
main(int, char*[])
{

  using namespace boost;

  typedef adjacency_list<> Graph;
  typedef std::pair<int,int> E;
  E edge_array[] = { E(0, 2),
                E(1, 1), E(1, 3),
                E(2, 1), E(2, 3),
                E(3, 1), E(3, 4),
                E(4, 0), E(4, 1) };
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  Graph G(5);
  for (std::size_t j = 0; j < sizeof(edge_array) / sizeof(E); ++j)
    add_edge(edge_array[j].first, edge_array[j].second, G);
#else
  Graph G(edge_array, edge_array + sizeof(edge_array)/sizeof(E), 5);
#endif

  std::cout << "DFS parenthesis:" << std::endl;
  depth_first_search(G, visitor(make_dfs_visitor(std::make_pair(open_paren(),
                                                        close_paren()))));
  std::cout << std::endl;
  return 0;
}


// Copyright (C) 2013 The Regents of the University of California (Regents).
// 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 Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "gtest/gtest.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <math.h>

#include "theia/math/util.h"
#include "theia/sfm/pose/perspective_three_point.h"
#include "theia/sfm/pose/test_util.h"
#include "theia/sfm/types.h"
#include "theia/test/test_utils.h"
#include "theia/util/random.h"

namespace theia {

using Eigen::Map;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;

RandomNumberGenerator rng(55);

void PoseFromThreeCalibratedTest(const double noise) {
  // Projection matrix.
  const Matrix3d gt_rotation =
      (Eigen::AngleAxisd(DegToRad(15.0), Vector3d(1.0, 0.0, 0.0)) *
       Eigen::AngleAxisd(DegToRad(-10.0), Vector3d(0.0, 1.0, 0.0)))
          .toRotationMatrix();
  const Vector3d gt_translation(0.3, -1.7, 1.15);
  Matrix3x4d projection_mat;
  projection_mat << gt_rotation, gt_translation;

  // Points in the 3D scene.
  const Vector3d kPoints3d[3] = {Vector3d(-0.3001, -0.5840, 1.2271),
                                 Vector3d(-1.4487, 0.6965, 0.3889),
                                 Vector3d(-0.7815, 0.7642, 0.1257)};

  // Points in the camera view.
  Vector2d kPoints2d[3];
  for (int i = 0; i < 3; i++) {
    kPoints2d[i] =
        (projection_mat * kPoints3d[i].homogeneous()).eval().hnormalized();
    if (noise) {
      AddNoiseToProjection(noise, &rng, &kPoints2d[i]);
    }
  }

  std::vector<Matrix3d> rotations;
  std::vector<Vector3d> translations;
  CHECK(PoseFromThreePoints(kPoints2d, kPoints3d, &rotations, &translations));

  bool matched_transform = false;
  for (int i = 0; i < rotations.size(); ++i) {
    // Check that the rotation and translation are close.
    double angular_diff =
        RadToDeg(Eigen::Quaterniond(rotations[i])
                     .angularDistance(Eigen::Quaterniond(gt_rotation)));
    double trans_diff =
        ((-gt_rotation * gt_translation) - (-rotations[i] * translations[i]))
            .norm();
    bool rot_match = angular_diff < 1.0;
    bool trans_match = trans_diff < 0.1;
    if (rot_match && trans_match) {
      matched_transform = true;

      Matrix3x4d soln_proj;
      soln_proj << rotations[i], translations[i];
      // Check the reprojection error.
      for (int j = 0; j < 3; j++) {
        const Vector3d projected_pt = soln_proj * kPoints3d[j].homogeneous();
        EXPECT_LT((kPoints2d[j] - projected_pt.hnormalized()).norm() * 800.0,
                  2.0);
      }
    }
  }
  EXPECT_TRUE(matched_transform);
}

TEST(P3P, PoseFromThreeCalibrated) { PoseFromThreeCalibratedTest(0.0 / 800.0); }

TEST(P3P, PoseFromThreeCalibratedNoise) {
  PoseFromThreeCalibratedTest(1.0 / 800.0);
}

}  // namespace theia


/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include "Tree30Dof.h"

#define BOOST_TEST_MODULE Coriolis
#include <RBDyn/Coriolis.h>
#include <RBDyn/EulerIntegration.h>
#include <RBDyn/FD.h>
#include <RBDyn/FK.h>
#include <RBDyn/FV.h>

#include <boost/test/unit_test.hpp>

void setRandomFreeFlyer(rbd::MultiBodyConfig & mbc)
{
  Eigen::Vector3d axis = Eigen::Vector3d::Random();
  Eigen::AngleAxisd aa(0.5, axis / axis.norm());
  Eigen::Quaterniond qd(aa);
  mbc.q[0][0] = qd.w();
  mbc.q[0][1] = qd.x();
  mbc.q[0][2] = qd.y();
  mbc.q[0][3] = qd.z();
}

BOOST_AUTO_TEST_CASE(CoriolisTest)
{
  std::srand(133757348);

  constexpr double TOL = 1e-6;
  constexpr double BIGTOL = 50 * TOL;

  rbd::MultiBody mb;
  rbd::MultiBodyConfig mbc;
  rbd::MultiBodyGraph mbg;

  std::tie(mb, mbc, mbg) = makeTree30Dof(false);

  rbd::ForwardDynamics fd(mb);
  rbd::Coriolis coriolis(mb);

  const static int ROUNDS = 100;

  for(int i = 0; i < ROUNDS; ++i)
  {
    mbc.zero(mb);

    Eigen::VectorXd q = Eigen::VectorXd::Random(mb.nrParams());
    mbc.q = rbd::vectorToParam(mb, q);
    setRandomFreeFlyer(mbc);

    rbd::forwardKinematics(mb, mbc);
    rbd::forwardVelocity(mb, mbc);

    fd.computeC(mb, mbc);
    Eigen::VectorXd gravity = fd.C();

    Eigen::VectorXd qd = Eigen::VectorXd::Random(mb.nrDof());
    mbc.alpha = rbd::vectorToDof(mb, qd);

    rbd::forwardVelocity(mb, mbc);

    Eigen::MatrixXd C = coriolis.coriolis(mb, mbc);

    fd.computeC(mb, mbc);
    Eigen::MatrixXd N = fd.C();

    BOOST_CHECK_SMALL((C * qd + gravity - N).norm(), TOL);

    fd.computeH(mb, mbc);
    Eigen::MatrixXd m1 = fd.H();

    double dt = 1e-8;

    rbd::eulerIntegration(mb, mbc, dt);

    rbd::forwardKinematics(mb, mbc);
    rbd::forwardVelocity(mb, mbc);
    fd.computeH(mb, mbc);

    Eigen::MatrixXd m2 = fd.H();

    Eigen::MatrixXd diff = (m2 - m1) / dt - (C + C.transpose());

    // Because we are using finite differences, the error is larger
    // on this test
    BOOST_CHECK_SMALL(diff.norm(), BIGTOL);
  }
}


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

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_LOG10_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_LOG10_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-exponential
    This function object returns the base ten logarithm of its argument.

    For integer input types @c log10 return the truncation of the real result.

    @par Header <boost/simd/function/log10.hpp>

    @par Decorators

      - std_ for floating entries calls @c std::log10

    @see log, log2, log1p

    @par Example:

      @snippet log10.cpp log10

    @par Possible output:

      @snippet log10.txt log10

  **/
  IEEEValue log10(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/log10.hpp>
#include <boost/simd/function/simd/log10.hpp>

#endif


/*! \file svg_test.cpp
   \brief demonstrate boxplot of common functions.
   \details This shows the distribution of the function from the median, quartiles etc.
    This is now obselete - see demo_boxplot.cpp.

  \author Jake Voytko
  \date Mar 2009
*/

// Copyright Jake Voytko 2007
// Copyright Paul A. Bristow 2009

//  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)

#ifdef _MSC_VER
#  pragma warning (disable : 4224) // warning C4224: nonstandard extension used : formal parameter 'function_ptr' was previously defined as a type
// in boost/function/function_template.hpp.  Should be fixed eventually.
#endif

#include <boost/svg_plot/svg.hpp>
  using boost::svg::svg;

#include <boost/svg_plot/svg_1d_plot.hpp>
  using boost::svg::svg_1d_plot;

#include <boost/svg_plot/svg_boxplot.hpp>
  using boost::svg::svg_boxplot;

#include <vector>
  using std::vector;
#include <iostream>
  using std::cout;
  using std::cin;
  using std::endl;
#include <cmath>

double h(double x)
{
    return 5 / (x);
}

double f(double x)
{
    return 40 + 25 * sin(x * 50);
}

int main()
{
    using namespace boost::svg;
    std::vector<double> data1, data2;
    for(double i= 0.1; i < 3; i += .1)
    {
      data1.push_back(h(i));
      data2.push_back(f(i));
    }

    svg_1d_plot my_1d_plot;

    my_1d_plot.title("Boxplots of Common Functions")
      .x_autoscale(data1);
    my_1d_plot.plot(data1, "[50 / x]");
    my_1d_plot.plot(data2, "[40 + 25 * sin(50x)]").stroke_color(red);

    my_1d_plot.write("./svg_test.svg");

    svg_boxplot my_boxplot;

    //my_boxplot.y_range(0, 100)  // Y-Axis information.
    //.y_minor_tick_length(10)
    //.y_major_interval(20);

    my_boxplot.y_autoscale(data2);

    my_boxplot.plot(data1, "[50 / x]");
    my_boxplot.plot(data2, "[40 + 25 * sin(50x)]");
    my_boxplot.write("./svg_test_box.svg");

    return 0;
} // int main()

/*

Output:

svg_test.cpp
Linking...
Embedding manifest...
Autorun "j:\Cpp\SVG\Debug\svg_test.exe

*/


#include <problem_description.h>
#include <simd.template.h>

#include <deal.II/base/vectorization.h>

#include <iostream>
#include <iomanip>

using namespace ryujin;
using namespace dealii;

template <int dim, typename Number>
void test()
{
  std::cout << std::setprecision(10);

  ProblemDescription problem_description;
  problem_description.parse_parameters_callback();

  using state_type = typename ProblemDescription::state_type<dim, Number>;

  const auto from_1d_state =
      [&problem_description](
          const dealii::Tensor<1, 3, Number> &state_1d) -> state_type {
    const auto &rho = state_1d[0];
    const auto &u = state_1d[1];
    const auto &p = state_1d[2];

    state_type state;

    state[0] = rho;
    state[1] = rho * u;
    state[dim + 1] = p / (problem_description.gamma() - 1.) + 0.5 * rho * u * u;

    return state;
  };

  dealii::Tensor<1, 3, Number> state_1d;
  state_1d[0] = problem_description.gamma();
  state_1d[1] = 3.;
  state_1d[2] = 1.;
  const auto U = from_1d_state(state_1d);

  std::cout << "dim = " << dim << std::endl;
  std::cout << "momentum = "                                          //
            << problem_description.momentum(U)                        //
            << std::endl;                                             //
  std::cout << "internal_energy = "                                   //
            << problem_description.internal_energy(U)                 //
            << std::endl;                                             //
  std::cout << "internal_energy_derivative = "                        //
            << problem_description.internal_energy_derivative(U)      //
            << std::endl;                                             //
  std::cout << "pressure = "                                          //
            << problem_description.pressure(U)                        //
            << std::endl;                                             //
  std::cout << "specific_entropy = "                                  //
            << problem_description.specific_entropy(U)                //
            << std::endl;                                             //
  std::cout << "harten entropy = "                                    //
            << problem_description.harten_entropy(U)                  //
            << std::endl;                                             //
  std::cout << "harten_entropy_derivative = "                         //
            << problem_description.harten_entropy_derivative(U)       //
            << std::endl;                                             //
  std::cout << "mathematical entropy = "                              //
            << problem_description.mathematical_entropy(U)            //
            << std::endl;                                             //
  std::cout << "mathematical_entropy_derivative = "                   //
            << problem_description.mathematical_entropy_derivative(U) //
            << std::endl;                                             //
  std::cout << "f = "                                                 //
            << problem_description.f(U)                               //
            << std::endl;                                             //
}

int main()
{
  std::cout << "\ndouble:\n" << std::endl;
  test<1, double>();
  test<2, double>();
  test<3, double>();

  std::cout << "float:\n" << std::endl;
  test<1, float>();
  test<2, float>();
  test<3, float>();

  std::cout << "\nVectorizedArray<double>\n" << std::endl;
  test<1, VectorizedArray<double>>();
  test<2, VectorizedArray<double>>();
  test<3, VectorizedArray<double>>();

  std::cout << "\nVectorizedArray<float>\n" << std::endl;
  test<1, VectorizedArray<float>>();
  test<2, VectorizedArray<float>>();
  test<3, VectorizedArray<float>>();

  return 0;
}


/*
 [auto_generated]
 boost/numeric/odeint/stepper/explicit_error_generic_rk.hpp

 [begin_description]
 Implementation of the generic Runge Kutta error stepper. Base class for many RK error steppers.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

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

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_EXPLICIT_ERROR_GENERIC_RK_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_EXPLICIT_ERROR_GENERIC_RK_HPP_INCLUDED

#include <boost/numeric/odeint/stepper/base/explicit_error_stepper_base.hpp>

#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>
#include <boost/numeric/odeint/stepper/detail/generic_rk_algorithm.hpp>
#include <boost/numeric/odeint/stepper/detail/generic_rk_call_algebra.hpp>
#include <boost/numeric/odeint/stepper/detail/generic_rk_operations.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template<
size_t StageCount,
size_t Order,
size_t StepperOrder ,
size_t ErrorOrder ,
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = typename algebra_dispatcher< State >::algebra_type ,
class Operations = typename operations_dispatcher< State >::operations_type ,
class Resizer = initially_resizer
>
#ifndef DOXYGEN_SKIP
class explicit_error_generic_rk
: public explicit_error_stepper_base<
  explicit_error_generic_rk< StageCount , Order , StepperOrder , ErrorOrder , State ,
  Value , Deriv , Time , Algebra , Operations , Resizer > ,
  Order , StepperOrder , ErrorOrder , State , Value , Deriv , Time , Algebra ,
  Operations , Resizer >
#else
class explicit_error_generic_rk : public explicit_error_stepper_base
#endif
{

public:
#ifndef DOXYGEN_SKIP
    typedef explicit_error_stepper_base<
            explicit_error_generic_rk< StageCount , Order , StepperOrder , ErrorOrder , State ,
            Value , Deriv , Time , Algebra , Operations , Resizer > ,
            Order , StepperOrder , ErrorOrder , State , Value , Deriv , Time , Algebra ,
            Operations , Resizer > stepper_base_type;
#else
    typedef explicit_stepper_base< ... > stepper_base_type;
#endif
    typedef typename stepper_base_type::state_type state_type;
    typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_base_type::value_type value_type;
    typedef typename stepper_base_type::deriv_type deriv_type;
    typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
    typedef typename stepper_base_type::time_type time_type;
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::operations_type operations_type;
    typedef typename stepper_base_type::resizer_type resizer_type;
#ifndef DOXYGEN_SKIP
    typedef explicit_error_generic_rk< StageCount , Order , StepperOrder , ErrorOrder , State ,
            Value , Deriv , Time , Algebra , Operations , Resizer > stepper_type;
#endif
    typedef detail::generic_rk_algorithm< StageCount , Value , Algebra , Operations > rk_algorithm_type;

    typedef typename rk_algorithm_type::coef_a_type coef_a_type;
    typedef typename rk_algorithm_type::coef_b_type coef_b_type;
    typedef typename rk_algorithm_type::coef_c_type coef_c_type;

    static const size_t stage_count = StageCount;

private:

public:

    // we use an explicit_generic_rk to do the normal rk step
    // and add a separate calculation of the error estimate afterwards
    explicit_error_generic_rk( const coef_a_type &a ,
            const coef_b_type &b ,
            const coef_b_type &b2 ,
            const coef_c_type &c ,
            const algebra_type &algebra = algebra_type() )
    : stepper_base_type( algebra ) , m_rk_algorithm( a , b , c ) , m_b2( b2 )
    { }

    template< class System , class StateIn , class DerivIn , class StateOut , class Err >
    void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt ,
            time_type t , StateOut &out , time_type dt , Err &xerr )
    {
        // normal step
        do_step_impl( system , in , dxdt , t , out , dt );

        // additionally, perform the error calculation
        detail::template generic_rk_call_algebra< StageCount , algebra_type >()( stepper_base_type::m_algebra ,
                xerr , dxdt , m_F , detail::generic_rk_scale_sum_err< StageCount , operations_type , value_type , time_type >( m_b2 , dt) );
    }

    template< class System , class StateIn , class DerivIn , class StateOut >
    void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt ,
            time_type t , StateOut &out , time_type dt )
    {
        m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );

        // actual calculation done in generic_rk.hpp
        m_rk_algorithm.do_step( stepper_base_type::m_algebra , system , in , dxdt , t , out , dt , m_x_tmp.m_v , m_F );
    }

    template< class StateIn >
    void adjust_size( const StateIn &x )
    {
        resize_impl( x );
        stepper_base_type::adjust_size( x );
    }

private:

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized( false );
        resized |= adjust_size_by_resizeability( m_x_tmp , x , typename is_resizeable<state_type>::type() );
        for( size_t i = 0 ; i < StageCount-1 ; ++i )
        {
            resized |= adjust_size_by_resizeability( m_F[i] , x , typename is_resizeable<deriv_type>::type() );
        }
        return resized;
    }

    rk_algorithm_type m_rk_algorithm;
    coef_b_type m_b2;

    resizer_type m_resizer;

    wrapped_state_type m_x_tmp;
    wrapped_deriv_type m_F[StageCount-1];

};

/********* DOXYGEN *********/

/**
 * \class explicit_error_generic_rk
 * \brief A generic implementation of explicit Runge-Kutta algorithms with error estimation. This class is as a
 * base class for all explicit Runge-Kutta steppers with error estimation.
 *
 * This class implements the explicit Runge-Kutta algorithms with error estimation in a generic way.
 * The Butcher tableau is passed to the stepper which constructs the stepper scheme with the help of a
 * template-metaprogramming algorithm. ToDo : Add example!
 *
 * This class derives explicit_error_stepper_base which provides the stepper interface.
 *
 * \tparam StageCount The number of stages of the Runge-Kutta algorithm.
 * \tparam Order The order of a stepper if the stepper is used without error estimation.
 * \tparam StepperOrder The order of a step if the stepper is used with error estimation. Usually Order and StepperOrder have
 * the same value.
 * \tparam ErrorOrder The order of the error step if the stepper is used with error estimation.
 * \tparam State The type representing the state of the ODE.
 * \tparam Value The floating point type which is used in the computations.
 * \tparam Time The type representing the independent variable - the time - of the ODE.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

    /**
     * \fn explicit_error_generic_rk::explicit_error_generic_rk( const coef_a_type &a , const coef_b_type &b , const coef_b_type &b2 , const coef_c_type &c , const algebra_type &algebra )
     * \brief Constructs the explicit_error_generik_rk class with the given parameters a, b, b2 and c. See examples section for details on the coefficients.
     *
     * \param a Triangular matrix of parameters b in the Butcher tableau.
     * \param b Last row of the butcher tableau.
     * \param b2 Parameters for lower-order evaluation to estimate the error.
     * \param c Parameters to calculate the time points in the Butcher tableau.
     * \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
     */

    /**
     * \fn explicit_error_generic_rk::do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt , Err &xerr )
     * \brief This method performs one step. The derivative `dxdt` of `in` at the time `t` is passed to the method.
     * The result is updated out-of-place, hence the input is in `in` and the output in `out`. Futhermore, an
     * estimation of the error is stored in `xerr`. `do_step_impl` is used by explicit_error_stepper_base.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved. in is not modified in this method
     * \param dxdt The derivative of x at t.
     * \param t The value of the time, at which the step should be performed.
     * \param out The result of the step is written in out.
     * \param dt The step size.
     * \param xerr The result of the error estimation is written in xerr.
     */

    /**
     * \fn explicit_error_generic_rk::do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
     * \brief This method performs one step. The derivative `dxdt` of `in` at the time `t` is passed to the method.
     * The result is updated out-of-place, hence the input is in `in` and the output in `out`.
     * Access to this step functionality is provided by explicit_stepper_base and
     * `do_step_impl` should not be called directly.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved. in is not modified in this method
     * \param dxdt The derivative of x at t.
     * \param t The value of the time, at which the step should be performed.
     * \param out The result of the step is written in out.
     * \param dt The step size.
     */

    /**
     * \fn explicit_error_generic_rk::adjust_size( const StateIn &x )
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */

}
}
}

#endif // BOOST_NUMERIC_ODEINT_STEPPER_EXPLICIT_ERROR_GENERIC_RK_HPP_INCLUDED


//          Copyright Erik Lundin 2016.
// 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 <iomanip>
#include <cstdlib>
#include <cstring>
#include <vector>
#include <boost/program_options.hpp>

#include "gausskruger.h"

using namespace gausskruger;
namespace po = boost::program_options;

#define EXE_NAME "gausskruger"

class ParameterProjection : public Projection
{
public:
    ParameterProjection(double flattening, double equatorialRadius, double centralMeridian,
            double scale, double falseNorthing, double falseEasting) :
        mFlattening(flattening),
        mEquatorialRadius(equatorialRadius),
        mCentralMeridian(centralMeridian),
        mScale(scale),
        mFalseNorthing(falseNorthing),
        mFalseEasting(falseEasting) {}
    double flattening() { return mFlattening; }
    double equatorialRadius() { return mEquatorialRadius; }
    double centralMeridian() { return mCentralMeridian; }
    double scale() { return mScale; }
    double falseNorthing() { return mFalseNorthing; }
    double falseEasting() { return mFalseEasting; }
private:
    double mFlattening;
    double mEquatorialRadius;
    double mCentralMeridian;
    double mScale;
    double mFalseNorthing;
    double mFalseEasting;
};

int main(int argc, char *argv[])
{
    double inverseFlattening;
    double equatorialRadius;
    double centralMeridian;
    double scale;
    double falseNorthing;
    double falseEasting;
    std::vector<double> coords;
    int nDecimals;

    try {
        // Projection parameters
        po::options_description projection_parameters("Projection parameters (mandatory)");
        projection_parameters.add_options()
                ("invflattening,i", po::value<double>(&inverseFlattening),
                        "inverse flattening of the ellipsoid")
                ("radius,a", po::value<double>(&equatorialRadius),
                        "equatorial radius, a.k.a. semi-major axis of the ellipsoid")
                ("meridian,m", po::value<double>(&centralMeridian),
                        "longitude of the central meridian")
                ("scale,s", po::value<double>(&scale),
                        "scale factor along the central meridian")
                ("falsenorthing,n", po::value<double>(&falseNorthing),
                        "false northing")
                ("falseeasting,e", po::value<double>(&falseEasting),
                        "false easting")
                ;

        // Options
        po::options_description options("Options");
        options.add_options()
                ("help,h", "print this help")
                ("decimals,d", po::value<int>(&nDecimals)->default_value(3), "number of decimals")
                ("reverse,r",
                        "reverse transformation (grid to geodetic), default is geodetic to grid")
                ;

        // Positional parameters (input coordinates)
        po::options_description hidden_parameters;
        hidden_parameters.add_options()
                ("coords", po::value<std::vector<double> >(&coords))
                ;
        po::positional_options_description positional_parameters;
        positional_parameters.add("coords", 2);

        // Store the parameters
        po::options_description all_parameters;
        all_parameters.add(options).add(projection_parameters).add(hidden_parameters);
        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).options(all_parameters).positional(positional_parameters).run(), vm);
        po::notify(vm);

        // Print help (if requested or no other options is given)
        po::options_description visible_parameters;
        visible_parameters.add(projection_parameters).add(options);
        if (argc == 1 || vm.count("help")) {
            std::cout << "Usage: " << EXE_NAME << " <projection> [options] latitude longitude\n"
                      << "       " << EXE_NAME << " <projection> [options] -r northing easting\n"
                      << visible_parameters << std::endl;
            return 1;
        }

        // Check for all mandatory parameters
        if (!(vm.count("invflattening") && vm.count("radius") && vm.count("meridian") && vm.count("scale")
                && vm.count("falsenorthing") && vm.count("falseeasting"))) {
            std::cout << "Missing mandatory projection parameter(s)" << std::endl;
            return 2;
        }
        if (coords.size() != 2) {
            std::cerr << "Exactly two coordinate values have to be entered" << std::endl;
            return 3;
        }

        // Set number of decimals in output
        std::cout.setf(std::ios::fixed);
        std::cout.precision(nDecimals);

        // Do the actual transformation
        ParameterProjection projection(1 / inverseFlattening, equatorialRadius,
                centralMeridian, scale, falseNorthing, falseEasting);
        if (vm.count("reverse")) {
            double lat, lon;
            projection.gridToGeodetic(coords.at(0), coords.at(1), lat, lon);
            std::cout << std::fixed << "Latitude: " << lat << "\nLongitude: " << lon << std::endl;
        } else {
            double northing, easting;
            projection.geodeticToGrid(coords.at(0), coords.at(1), northing, easting);
            std::cout << std::fixed << "Northing: " << northing << "\nEasting: " << easting << std::endl;
        }
    } catch (std::exception& e) {
        std::cerr << "Error: " << e.what() << std::endl;
        return 4;
    }

    return 0;
}


#ifndef BOOST_MOVE_TEST_RANDOM_SHUFFLE_HPP
#define BOOST_MOVE_TEST_RANDOM_SHUFFLE_HPP

#include <boost/move/adl_move_swap.hpp>
#include <boost/move/detail/iterator_traits.hpp>
#include <stdlib.h>

inline unsigned long long rand_15_bit()
{
   //Many rand implementation only use 15 bits
   //so make sure we have only 15 bits
   return (unsigned long long)((std::rand()) & 0x7fff);
}

inline unsigned long long ullrand()
{
   return  (rand_15_bit() << 54u) ^ (rand_15_bit() << 39u)
         ^ (rand_15_bit() << 26u) ^ (rand_15_bit() << 13u)
         ^ rand_15_bit();
}

template< class RandomIt >
void random_shuffle( RandomIt first, RandomIt last )
{
   std::size_t n = std::size_t (last - first);
   for (std::size_t i = n-1; i > 0; --i) {
      std::size_t j = static_cast<std::size_t >(ullrand() % (unsigned long long)(i+1));
      if(j != i) {
         boost::adl_move_swap(first[i], first[j]);
      }
   }
}

#endif// BOOST_MOVE_TEST_RANDOM_SHUFFLE_HPP


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/type.hpp>

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

int main() {
    //! [liftable]
    BOOST_HANA_CONSTANT_ASSERT(trait<std::is_integral>(type<int>));
    BOOST_HANA_CONSTANT_ASSERT(not_(trait<std::is_integral>(type<float>)));
    //! [liftable]

    //! [nonliftable]
    BOOST_HANA_CONSTEXPR_LAMBDA auto extent = [](auto t, auto n) {
        return std::extent<typename decltype(t)::type, n()>{};
    };
    BOOST_HANA_CONSTANT_ASSERT(extent(type<char>, int_<1>) == int_<0>);
    BOOST_HANA_CONSTANT_ASSERT(extent(type<char[1][2]>, int_<1>) == int_<2>);
    //! [nonliftable]
}


#include "catch.hpp"

#include <cmath>
#include <cstdio>
#include <iostream>
#include <sstream>
#include <vector>

#include <Eigen/Core>
#include <Eigen/SparseCore>

#include <SmurffCpp/Utils/MatrixUtils.h>
#include <SmurffCpp/Utils/TensorUtils.h>
#include <SmurffCpp/Configs/TensorConfig.h>

using namespace smurff;

static NoiseConfig fixed_ncfg(NoiseTypes::fixed);

TEST_CASE("TensorConfig(const std::vector<std::uint64_t>& dims, const std::vector<double> values, const NoiseConfig& noiseConfig)")
{
   std::vector<std::uint64_t> tensorConfigDims = { 3, 4 };
   std::vector<double> tensorConfigValues = { 1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12 };
   TensorConfig tensorConfig(tensorConfigDims, tensorConfigValues, fixed_ncfg);
   MatrixConfig matrixConfig = tensor_utils::tensor_to_matrix(tensorConfig);

   Eigen::MatrixXd actualMatrix0 = tensor_utils::dense_to_eigen(tensorConfig);
   Eigen::MatrixXd actualMatrix1 = matrix_utils::dense_to_eigen(matrixConfig);
   Eigen::MatrixXd expectedMatrix(3, 4);
   expectedMatrix << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;

   REQUIRE(matrix_utils::equals(actualMatrix0, expectedMatrix));
   REQUIRE(matrix_utils::equals(actualMatrix1, expectedMatrix));
}

TEST_CASE("TensorConfig(std::vector<std::uint64_t>&& dims, std::vector<double>&& values, const NoiseConfig& noiseConfig)")
{
   std::vector<std::uint64_t> tensorConfigDims = { 3, 4 };
   std::vector<double> tensorConfigValues = { 1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12 };
   TensorConfig tensorConfig(std::move(tensorConfigDims), std::move(tensorConfigValues), fixed_ncfg);
   MatrixConfig matrixConfig = tensor_utils::tensor_to_matrix(tensorConfig);

   Eigen::MatrixXd actualMatrix0 = tensor_utils::dense_to_eigen(tensorConfig);
   Eigen::MatrixXd actualMatrix1 = matrix_utils::dense_to_eigen(matrixConfig);
   Eigen::MatrixXd expectedMatrix(3, 4);
   expectedMatrix << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;

   REQUIRE(tensorConfigDims.data() == NULL);
   REQUIRE(tensorConfigValues.data() == NULL);

   REQUIRE(matrix_utils::equals(actualMatrix0, expectedMatrix));
   REQUIRE(matrix_utils::equals(actualMatrix1, expectedMatrix));
}

TEST_CASE("TensorConfig(std::shared_ptr<std::vector<std::uint64_t> > dims, std::shared_ptr<std::vector<double> > values, const NoiseConfig& noiseConfig)")
{
   std::shared_ptr<std::vector<std::uint64_t> > tensorConfigDims =
      std::make_shared<std::vector<std::uint64_t> >(
         std::initializer_list<std::uint64_t>({ 3, 4 })
      );
   std::shared_ptr<std::vector<double> > tensorConfigValues =
      std::make_shared<std::vector<double> >(
         std::initializer_list<double>({ 1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12 })
      );
   TensorConfig tensorConfig(tensorConfigDims, tensorConfigValues, fixed_ncfg);
   MatrixConfig matrixConfig = tensor_utils::tensor_to_matrix(tensorConfig);

   Eigen::MatrixXd actualMatrix0 = tensor_utils::dense_to_eigen(tensorConfig);
   Eigen::MatrixXd actualMatrix1 = matrix_utils::dense_to_eigen(matrixConfig);
   Eigen::MatrixXd expectedMatrix(3, 4);
   expectedMatrix << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;

   REQUIRE(matrix_utils::equals(actualMatrix0, expectedMatrix));
   REQUIRE(matrix_utils::equals(actualMatrix1, expectedMatrix));
}

TEST_CASE("TensorConfig(const std::vector<std::uint64_t>& dims, const std::vector<std::uint32_t>& columns, const std::vector<double>& values, const NoiseConfig& noiseConfig)")
{
   std::vector<std::uint64_t> tensorConfigDims = { 3, 4 };
   std::vector<std::uint32_t> tensorConfigColumns = { 0, 0, 0, 0, 2, 2, 2, 2,
                                                      0, 1, 2, 3, 0, 1, 2, 3
                                                    };
   std::vector<double> tensorConfigValues = { 1, 2, 3, 4, 9, 10, 11, 12 };
   TensorConfig tensorConfig(tensorConfigDims, tensorConfigColumns, tensorConfigValues, fixed_ncfg, false);
   MatrixConfig matrixConfig = tensor_utils::tensor_to_matrix(tensorConfig);

   Eigen::SparseMatrix<double> actualMatrix0 = tensor_utils::sparse_to_eigen(tensorConfig);
   Eigen::SparseMatrix<double> actualMatrix1 = matrix_utils::sparse_to_eigen(matrixConfig);
   Eigen::SparseMatrix<double> expectedMatrix(3, 4);
   std::vector<Eigen::Triplet<double> > expectedMatrixTriplets;
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 1, 2));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 2, 3));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 3, 4));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 0, 9));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 1, 10));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 2, 11));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 3, 12));
   expectedMatrix.setFromTriplets(expectedMatrixTriplets.begin(), expectedMatrixTriplets.end());

   REQUIRE(matrix_utils::equals(actualMatrix0, expectedMatrix));
   REQUIRE(matrix_utils::equals(actualMatrix1, expectedMatrix));
}

TEST_CASE("TensorConfig(std::vector<std::uint64_t>&& dims, std::vector<std::uint32_t>&& columns, std::vector<double>&& values, const NoiseConfig& noiseConfig)")
{
   std::vector<std::uint64_t> tensorConfigDims = { 3, 4 };
   std::vector<std::uint32_t> tensorConfigColumns = { 0, 0, 0, 0, 2, 2, 2, 2,
                                                      0, 1, 2, 3, 0, 1, 2, 3
                                                    };
   std::vector<double> tensorConfigValues = { 1, 2, 3, 4, 9, 10, 11, 12 };
   TensorConfig tensorConfig( std::move(tensorConfigDims)
                            , std::move(tensorConfigColumns)
                            , std::move(tensorConfigValues)
                            , fixed_ncfg
                            , false
                            );
   MatrixConfig matrixConfig = tensor_utils::tensor_to_matrix(tensorConfig);

   Eigen::SparseMatrix<double> actualMatrix0 = tensor_utils::sparse_to_eigen(tensorConfig);
   Eigen::SparseMatrix<double> actualMatrix1 = matrix_utils::sparse_to_eigen(matrixConfig);
   Eigen::SparseMatrix<double> expectedMatrix(3, 4);
   std::vector<Eigen::Triplet<double> > expectedMatrixTriplets;
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 1, 2));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 2, 3));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 3, 4));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 0, 9));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 1, 10));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 2, 11));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 3, 12));
   expectedMatrix.setFromTriplets(expectedMatrixTriplets.begin(), expectedMatrixTriplets.end());

   REQUIRE(tensorConfigDims.data() == NULL);
   REQUIRE(tensorConfigColumns.data() == NULL);
   REQUIRE(tensorConfigValues.data() == NULL);

   REQUIRE(matrix_utils::equals(actualMatrix0, expectedMatrix));
   REQUIRE(matrix_utils::equals(actualMatrix1, expectedMatrix));
}

TEST_CASE("TensorConfig(std::shared_ptr<std::vector<std::uint64_t> > dims, std::shared_ptr<std::vector<std::uint32_t> > columns, std::shared_ptr<std::vector<double> > values, const NoiseConfig& noiseConfig)")
{
   std::shared_ptr<std::vector<std::uint64_t> > tensorConfigDims =
      std::make_shared<std::vector<std::uint64_t> >(
         std::initializer_list<std::uint64_t>({ 3, 4 })
      );
   std::shared_ptr<std::vector<std::uint32_t> > tensorConfigColumns =
      std::make_shared<std::vector<std::uint32_t> >(
         std::initializer_list<std::uint32_t>({
            0, 0, 0, 0, 2, 2, 2, 2,
            0, 1, 2, 3, 0, 1, 2, 3
         })
      );
   std::shared_ptr<std::vector<double> > tensorConfigValues =
      std::make_shared<std::vector<double> >(
         std::initializer_list<double>({ 1, 2, 3, 4, 9, 10, 11, 12 })
      );
   TensorConfig tensorConfig(tensorConfigDims, tensorConfigColumns, tensorConfigValues, fixed_ncfg, false);
   MatrixConfig matrixConfig = tensor_utils::tensor_to_matrix(tensorConfig);

   Eigen::SparseMatrix<double> actualMatrix0 = tensor_utils::sparse_to_eigen(tensorConfig);
   Eigen::SparseMatrix<double> actualMatrix1 = matrix_utils::sparse_to_eigen(matrixConfig);
   Eigen::SparseMatrix<double> expectedMatrix(3, 4);
   std::vector<Eigen::Triplet<double> > expectedMatrixTriplets;
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 1, 2));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 2, 3));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 3, 4));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 0, 9));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 1, 10));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 2, 11));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 3, 12));
   expectedMatrix.setFromTriplets(expectedMatrixTriplets.begin(), expectedMatrixTriplets.end());

   REQUIRE(matrix_utils::equals(actualMatrix0, expectedMatrix));
   REQUIRE(matrix_utils::equals(actualMatrix1, expectedMatrix));
}

TEST_CASE("TensorConfig(const std::vector<std::uint64_t>& dims, const std::vector<std::uint32_t>& columns, const NoiseConfig& noiseConfig)")
{
   std::vector<std::uint64_t> tensorConfigDims = { 3, 4 };
   std::vector<std::uint32_t> tensorConfigColumns = { 0, 0, 0, 0, 2, 2, 2, 2,
                                                      0, 1, 2, 3, 0, 1, 2, 3
                                                    };
   TensorConfig tensorConfig(tensorConfigDims, tensorConfigColumns, fixed_ncfg, false);
   MatrixConfig matrixConfig = tensor_utils::tensor_to_matrix(tensorConfig);

   Eigen::SparseMatrix<double> actualMatrix0 = tensor_utils::sparse_to_eigen(tensorConfig);
   Eigen::SparseMatrix<double> actualMatrix1 = matrix_utils::sparse_to_eigen(matrixConfig);
   Eigen::SparseMatrix<double> expectedMatrix(3, 4);
   std::vector<Eigen::Triplet<double> > expectedMatrixTriplets;
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 1, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 2, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 3, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 1, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 2, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 3, 1));
   expectedMatrix.setFromTriplets(expectedMatrixTriplets.begin(), expectedMatrixTriplets.end());

   REQUIRE(matrix_utils::equals(actualMatrix0, expectedMatrix));
   REQUIRE(matrix_utils::equals(actualMatrix1, expectedMatrix));
}

TEST_CASE("TensorConfig(std::vector<std::uint64_t>&& dims, std::vector<std::uint32_t>&& columns, const NoiseConfig& noiseConfig)")
{
   std::vector<std::uint64_t> tensorConfigDims = { 3, 4 };
   std::vector<std::uint32_t> tensorConfigColumns = { 0, 0, 0, 0, 2, 2, 2, 2,
                                                      0, 1, 2, 3, 0, 1, 2, 3
                                                    };
   TensorConfig tensorConfig(std::move(tensorConfigDims), std::move(tensorConfigColumns), fixed_ncfg, false);
   MatrixConfig matrixConfig = tensor_utils::tensor_to_matrix(tensorConfig);

   Eigen::SparseMatrix<double> actualMatrix0 = tensor_utils::sparse_to_eigen(tensorConfig);
   Eigen::SparseMatrix<double> actualMatrix1 = matrix_utils::sparse_to_eigen(matrixConfig);
   Eigen::SparseMatrix<double> expectedMatrix(3, 4);
   std::vector<Eigen::Triplet<double> > expectedMatrixTriplets;
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 1, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 2, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 3, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 1, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 2, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 3, 1));
   expectedMatrix.setFromTriplets(expectedMatrixTriplets.begin(), expectedMatrixTriplets.end());

   REQUIRE(tensorConfigDims.data() == NULL);
   REQUIRE(tensorConfigColumns.data() == NULL);

   REQUIRE(matrix_utils::equals(actualMatrix0, expectedMatrix));
   REQUIRE(matrix_utils::equals(actualMatrix1, expectedMatrix));
}

TEST_CASE("TensorConfig(std::shared_ptr<std::vector<std::uint64_t> > dims, std::shared_ptr<std::vector<std::uint32_t> > columns, const NoiseConfig& noiseConfig)")
{
   std::shared_ptr<std::vector<std::uint64_t> > tensorConfigDims =
      std::make_shared<std::vector<std::uint64_t> >(
         std::initializer_list<std::uint64_t>({ 3, 4 })
      );
   std::shared_ptr<std::vector<std::uint32_t> > tensorConfigColumns =
      std::make_shared<std::vector<std::uint32_t> >(
         std::initializer_list<std::uint32_t>({
            0, 0, 0, 0, 2, 2, 2, 2,
            0, 1, 2, 3, 0, 1, 2, 3
         })
      );
   TensorConfig tensorConfig(tensorConfigDims, tensorConfigColumns, fixed_ncfg, false);
   MatrixConfig matrixConfig = tensor_utils::tensor_to_matrix(tensorConfig);

   Eigen::SparseMatrix<double> actualMatrix0 = tensor_utils::sparse_to_eigen(tensorConfig);
   Eigen::SparseMatrix<double> actualMatrix1 = matrix_utils::sparse_to_eigen(matrixConfig);
   Eigen::SparseMatrix<double> expectedMatrix(3, 4);
   std::vector<Eigen::Triplet<double> > expectedMatrixTriplets;
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 1, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 2, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 3, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 1, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 2, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 3, 1));
   expectedMatrix.setFromTriplets(expectedMatrixTriplets.begin(), expectedMatrixTriplets.end());

   REQUIRE(matrix_utils::equals(actualMatrix0, expectedMatrix));
   REQUIRE(matrix_utils::equals(actualMatrix1, expectedMatrix));
}


#include <acrolib/sampling.h>
#include <acrolib/test_utils.h>

#include <cassert>
#include <iostream>
#include <Eigen/Core>

using namespace acro;
using namespace std;

void testRangeCalculation()
{
  vector<vector<double>> expected_ranges{ { 0.0, 0.5, 1.0, 1.5, 2.0 }, { 0.0, 0.5, 1.0 } };
  vector<Limits> limits{ { 0, 2 }, { 0, 1 } };
  auto sample_ranges = calculateRangesFromLimits(limits, 0.5);

  assert(sample_ranges.size() == expected_ranges.size());
  for (size_t dim{ 0 }; dim < sample_ranges.size(); ++dim)
  {
    assert(sample_ranges[dim].size() == expected_ranges[dim].size());
    for (size_t i{ 0 }; i < sample_ranges[dim].size(); ++i)
    {
      ASSERT_EQ(sample_ranges[dim][i], expected_ranges[dim][i]);
    }
  }
}

void test1()
{
  vector<vector<double>> ranges{ { 1.1, 2.2 }, { 0.5, 1.5, 2.5 } };

  GridSamples<Eigen::VectorXd> sampler(ranges);

  for (size_t k{ 0 }; k < sampler.size(); ++k)
  {
    cout << sampler[k].transpose() << endl;
  }
}

void test2()
{
  vector<vector<double>> ranges{ { 1.1, 2.2 }, { 0.5, 1.5, 2.5 } };

  GridSamples<vector<double>> sampler(ranges);

  for (size_t k{ 0 }; k < sampler.size(); ++k)
  {
    auto sample = sampler[k];
    cout << sample[0] << " " << sample[1] << endl;
  }
}

int main()
{
  test1();
  test2();
}