// #define R_BUILD
// #define TEST
#ifdef R_BUILD

#include <Rcpp.h>
#include <RcppEigen.h>
//[[Rcpp::depends(RcppEigen)]]
using namespace Rcpp;

#else

#include <Eigen/Eigen>
#include "List.h"

#endif

#include <iostream>
#include "Data.h"
#include "Algorithm.h"
#include "Metric.h"
#include "path.h"
// #include "path.cpp"
#include "utilities.h"
#include "abess.h"
#include "screening.h"
#include <vector>

typedef Eigen::Triplet<double> triplet;

#ifdef _OPENMP
#include <omp.h>
// [[Rcpp::plugins(openmp)]]
#else
#ifndef DISABLE_OPENMP
// use pragma message instead of warning
#pragma message("Warning: OpenMP is not available, "                 \
                "project will be compiled into single-thread code. " \
                "Use OpenMP-enabled compiler to get benefit of multi-threading.")
#endif
inline int omp_get_thread_num()
{
  return 0;
}
inline int omp_get_num_threads() { return 1; }
inline int omp_get_num_procs() { return 1; }
inline void omp_set_num_threads(int nthread) {}
inline void omp_set_dynamic(int flag) {}
#endif

// #ifdef OTHER_ALGORITHM2
// #include "PrincipalBallAlgorithm.h"
// #endif

using namespace Eigen;
using namespace std;

// [[Rcpp::export]]
List abessCpp2(Eigen::MatrixXd x, Eigen::MatrixXd y, int n, int p,
               int data_type, Eigen::VectorXd weight, Eigen::MatrixXd sigma,
               bool is_normal,
               int algorithm_type, int model_type, int max_iter, int exchange_num,
               int path_type, bool is_warm_start,
               int ic_type, double ic_coef, bool is_cv, int Kfold,
               Eigen::VectorXi status,
               Eigen::VectorXi sequence,
               Eigen::VectorXd lambda_seq,
               int s_min, int s_max, int K_max, double epsilon,
               double lambda_min, double lambda_max, int nlambda,
               bool is_screening, int screening_size, int powell_path,
               Eigen::VectorXi g_index,
               Eigen::VectorXi always_select,
               double tau,
               int primary_model_fit_max_iter, double primary_model_fit_epsilon,
               bool early_stop, bool approximate_Newton,
               int thread,
               bool covariance_update,
               bool sparse_matrix,
               int splicing_type)
{
  bool is_parallel = thread != 1;

#ifdef _OPENMP
  // Eigen::initParallel();
  if (thread == 0)
  {
    thread = omp_get_max_threads();
  }
  Eigen::setNbThreads(thread);
  omp_set_num_threads(thread);
#ifdef TEST
  cout << Eigen::nbThreads() << " Threads for eigen." << endl;
  cout << omp_get_num_threads() << " Threads for omp." << endl;
#endif
#endif

  Algorithm<Eigen::VectorXd, Eigen::VectorXd, double, Eigen::MatrixXd> *algorithm_uni_dense = nullptr;
  Algorithm<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::VectorXd, Eigen::MatrixXd> *algorithm_mul_dense = nullptr;
  Algorithm<Eigen::VectorXd, Eigen::VectorXd, double, Eigen::SparseMatrix<double>> *algorithm_uni_sparse = nullptr;
  Algorithm<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::VectorXd, Eigen::SparseMatrix<double>> *algorithm_mul_sparse = nullptr;

  //////////////////// function generate_algorithm_pointer() ////////////////////////////
  // to do
  if (!sparse_matrix)
  {
    if (algorithm_type == 6)
    {
      if (model_type == 1)
      {
        algorithm_uni_dense = new abessLm<Eigen::MatrixXd>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, covariance_update, splicing_type);
      }
      else if (model_type == 2)
      {
        algorithm_uni_dense = new abessLogistic<Eigen::MatrixXd>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
      }
      else if (model_type == 3)
      {
        algorithm_uni_dense = new abessPoisson<Eigen::MatrixXd>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
      }
      else if (model_type == 4)
      {
        algorithm_uni_dense = new abessCox<Eigen::MatrixXd>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
      }
      else if (model_type == 5)
      {
        algorithm_mul_dense = new abessMLm<Eigen::MatrixXd>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, covariance_update, splicing_type);
      }
      else if (model_type == 6)
      {
        algorithm_mul_dense = new abessMultinomial<Eigen::MatrixXd>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
      }
      else if (model_type == 7)
      {
        algorithm_uni_dense = new abessPCA<Eigen::MatrixXd>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
      }
    }
  }
  else
  {
    if (algorithm_type == 6)
    {
      if (model_type == 1)
      {
        algorithm_uni_sparse = new abessLm<Eigen::SparseMatrix<double>>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, covariance_update, splicing_type);
      }
      else if (model_type == 2)
      {
        algorithm_uni_sparse = new abessLogistic<Eigen::SparseMatrix<double>>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
      }
      else if (model_type == 3)
      {
        algorithm_uni_sparse = new abessPoisson<Eigen::SparseMatrix<double>>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
      }
      else if (model_type == 4)
      {
        algorithm_uni_sparse = new abessCox<Eigen::SparseMatrix<double>>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
      }
      else if (model_type == 5)
      {
        algorithm_mul_sparse = new abessMLm<Eigen::SparseMatrix<double>>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, covariance_update, splicing_type);
      }
      else if (model_type == 6)
      {
        algorithm_mul_sparse = new abessMultinomial<Eigen::SparseMatrix<double>>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
      }
      else if (model_type == 7)
      {
        algorithm_uni_sparse = new abessPCA<Eigen::SparseMatrix<double>>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
      }
    }
  }

#ifdef TEST
  cout << "abesscpp2 1" << endl;
#endif

  vector<Algorithm<Eigen::VectorXd, Eigen::VectorXd, double, Eigen::MatrixXd> *> algorithm_list_uni_dense(max(Kfold, thread));
  vector<Algorithm<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::VectorXd, Eigen::MatrixXd> *> algorithm_list_mul_dense(max(Kfold, thread));
  vector<Algorithm<Eigen::VectorXd, Eigen::VectorXd, double, Eigen::SparseMatrix<double>> *> algorithm_list_uni_sparse(max(Kfold, thread));
  vector<Algorithm<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::VectorXd, Eigen::SparseMatrix<double>> *> algorithm_list_mul_sparse(max(Kfold, thread));
  if (is_cv)
  {
    for (int i = 0; i < max(Kfold, thread); i++)
    {
      if (!sparse_matrix)
      {
        if (algorithm_type == 6)
        {
          if (model_type == 1)
          {
            algorithm_list_uni_dense[i] = new abessLm<Eigen::MatrixXd>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, covariance_update, splicing_type);
          }
          else if (model_type == 2)
          {
            algorithm_list_uni_dense[i] = new abessLogistic<Eigen::MatrixXd>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
          }
          else if (model_type == 3)
          {
            algorithm_list_uni_dense[i] = new abessPoisson<Eigen::MatrixXd>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
          }
          else if (model_type == 4)
          {
            algorithm_list_uni_dense[i] = new abessCox<Eigen::MatrixXd>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
          }
          else if (model_type == 5)
          {
            algorithm_list_mul_dense[i] = new abessMLm<Eigen::MatrixXd>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, covariance_update, splicing_type);
          }
          else if (model_type == 6)
          {
            algorithm_list_mul_dense[i] = new abessMultinomial<Eigen::MatrixXd>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
          }
          else if (model_type == 7)
          {
            algorithm_list_uni_dense[i] = new abessPCA<Eigen::MatrixXd>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
          }
        }
      }
      else
      {
        if (algorithm_type == 6)
        {
          if (model_type == 1)
          {
            algorithm_list_uni_sparse[i] = new abessLm<Eigen::SparseMatrix<double>>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, covariance_update, splicing_type);
          }
          else if (model_type == 2)
          {
            algorithm_list_uni_sparse[i] = new abessLogistic<Eigen::SparseMatrix<double>>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
          }
          else if (model_type == 3)
          {
            algorithm_list_uni_sparse[i] = new abessPoisson<Eigen::SparseMatrix<double>>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
          }
          else if (model_type == 4)
          {
            algorithm_list_uni_sparse[i] = new abessCox<Eigen::SparseMatrix<double>>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
          }
          else if (model_type == 5)
          {
            algorithm_list_mul_sparse[i] = new abessMLm<Eigen::SparseMatrix<double>>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, covariance_update, splicing_type);
          }
          else if (model_type == 6)
          {
            algorithm_list_mul_sparse[i] = new abessMultinomial<Eigen::SparseMatrix<double>>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
          }
          else if (model_type == 7)
          {
            algorithm_list_uni_sparse[i] = new abessPCA<Eigen::SparseMatrix<double>>(algorithm_type, model_type, max_iter, primary_model_fit_max_iter, primary_model_fit_epsilon, is_warm_start, exchange_num, approximate_Newton, always_select, splicing_type);
          }
        }
      }
    }
  }
#ifdef TEST
  cout << "abesscpp2 2" << endl;
#endif

  List out_result;
  if (!sparse_matrix)
  {
    if (y.cols() == 1)
    {
#ifdef TEST
      cout << "abesscpp2 4" << endl;
      cout << "uni_dense" << endl;
#endif
      Eigen::VectorXd y_vec = y.col(0).eval();

      out_result = abessCpp<Eigen::VectorXd, Eigen::VectorXd, double, Eigen::MatrixXd>(x, y_vec, n, p,
                                                                                data_type, weight, sigma,
                                                                                is_normal,
                                                                                algorithm_type, model_type, max_iter, exchange_num,
                                                                                path_type, is_warm_start,
                                                                                ic_type, ic_coef, is_cv, Kfold,
                                                                                status,
                                                                                sequence,
                                                                                lambda_seq,
                                                                                s_min, s_max, K_max, epsilon,
                                                                                lambda_min, lambda_max, nlambda,
                                                                                is_screening, screening_size, powell_path,
                                                                                g_index,
                                                                                always_select,
                                                                                tau,
                                                                                primary_model_fit_max_iter, primary_model_fit_epsilon,
                                                                                early_stop, approximate_Newton,
                                                                                thread,
                                                                                covariance_update,
                                                                                sparse_matrix,
                                                                                algorithm_uni_dense, algorithm_list_uni_dense);
#ifdef TEST
      cout << "abesscpp2 5" << endl;
#endif
    }
    else
    {
#ifdef TEST
      cout << "abesscpp2 5" << endl;
      cout << "mul_dense" << endl;
#endif
      out_result = abessCpp<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::VectorXd, Eigen::MatrixXd>(x, y, n, p,
                                                                                data_type, weight, sigma,
                                                                                is_normal,
                                                                                algorithm_type, model_type, max_iter, exchange_num,
                                                                                path_type, is_warm_start,
                                                                                ic_type, ic_coef, is_cv, Kfold,
                                                                                status,
                                                                                sequence,
                                                                                lambda_seq,
                                                                                s_min, s_max, K_max, epsilon,
                                                                                lambda_min, lambda_max, nlambda,
                                                                                is_screening, screening_size, powell_path,
                                                                                g_index,
                                                                                always_select,
                                                                                tau,
                                                                                primary_model_fit_max_iter, primary_model_fit_epsilon,
                                                                                early_stop, approximate_Newton,
                                                                                thread,
                                                                                covariance_update,
                                                                                sparse_matrix,
                                                                                algorithm_mul_dense, algorithm_list_mul_dense);
#ifdef TEST
      cout << "abesscpp2 6" << endl;
#endif
    }
  }
  else
  {
#ifdef TEST
    clock_t t1 = clock();
#endif
    Eigen::SparseMatrix<double> sparse_x(n, p);

    // std::vector<triplet> tripletList;
    // tripletList.reserve(x.rows());
    // for (int i = 0; i < x.rows(); i++)
    // {
    //   tripletList.push_back(triplet(int(x(i, 1)), int(x(i, 2)), x(i, 0)));
    // }
    // sparse_x.setFromTriplets(tripletList.begin(), tripletList.end());

    sparse_x.reserve(x.rows());
    for (int i = 0; i < x.rows(); i++)
    {
      sparse_x.insert(int(x(i, 1)), int(x(i, 2))) = x(i, 0);
    }
    sparse_x.makeCompressed();

#ifdef TEST
    cout << "sparse x nonZeros: " << sparse_x.nonZeros() << endl;
    clock_t t2 = clock();
    std::cout << "sparse X time: " << ((double)(t2 - t1) / CLOCKS_PER_SEC) << endl;
#endif

    if (y.cols() == 1)
    {
#ifdef TEST
      cout << "abesscpp2 3" << endl;
      cout << "uni_sparse" << endl;
#endif
      Eigen::VectorXd y_vec = y.col(0).eval();

      out_result = abessCpp<Eigen::VectorXd, Eigen::VectorXd, double, Eigen::SparseMatrix<double>>(sparse_x, y_vec, n, p,
                                                                                data_type, weight, sigma,
                                                                                is_normal,
                                                                                algorithm_type, model_type, max_iter, exchange_num,
                                                                                path_type, is_warm_start,
                                                                                ic_type, ic_coef, is_cv, Kfold,
                                                                                status,
                                                                                sequence,
                                                                                lambda_seq,
                                                                                s_min, s_max, K_max, epsilon,
                                                                                lambda_min, lambda_max, nlambda,
                                                                                is_screening, screening_size, powell_path,
                                                                                g_index,
                                                                                always_select,
                                                                                tau,
                                                                                primary_model_fit_max_iter, primary_model_fit_epsilon,
                                                                                early_stop, approximate_Newton,
                                                                                thread,
                                                                                covariance_update,
                                                                                sparse_matrix,
                                                                                algorithm_uni_sparse, algorithm_list_uni_sparse);
#ifdef TEST
      cout << "abesscpp2 5" << endl;
#endif
    }
    else
    {
#ifdef TEST
      cout << "abesscpp2 5" << endl;
      cout << "mul_sparse" << endl;
#endif

      out_result = abessCpp<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::VectorXd, Eigen::SparseMatrix<double>>(sparse_x, y, n, p,
                                                                                data_type, weight, sigma,
                                                                                is_normal,
                                                                                algorithm_type, model_type, max_iter, exchange_num,
                                                                                path_type, is_warm_start,
                                                                                ic_type, ic_coef, is_cv, Kfold,
                                                                                status,
                                                                                sequence,
                                                                                lambda_seq,
                                                                                s_min, s_max, K_max, epsilon,
                                                                                lambda_min, lambda_max, nlambda,
                                                                                is_screening, screening_size, powell_path,
                                                                                g_index,
                                                                                always_select,
                                                                                tau,
                                                                                primary_model_fit_max_iter, primary_model_fit_epsilon,
                                                                                early_stop, approximate_Newton,
                                                                                thread,
                                                                                covariance_update,
                                                                                sparse_matrix,
                                                                                algorithm_mul_sparse, algorithm_list_mul_sparse);
#ifdef TEST
      cout << "abesscpp2 6" << endl;
#endif
    }
  }

  delete algorithm_uni_dense;
  delete algorithm_mul_dense;
  delete algorithm_uni_sparse;
  delete algorithm_mul_sparse;
  for (unsigned int i = 0; i < algorithm_list_uni_dense.size(); i++)
  {
    delete algorithm_list_uni_dense[i];
  }
  for (unsigned int i = 0; i < algorithm_list_mul_dense.size(); i++)
  {
    delete algorithm_list_mul_dense[i];
  }
  for (unsigned int i = 0; i < algorithm_list_uni_sparse.size(); i++)
  {
    delete algorithm_list_uni_sparse[i];
  }
  for (unsigned int i = 0; i < algorithm_list_mul_sparse.size(); i++)
  {
    delete algorithm_list_mul_sparse[i];
  }
  return out_result;
};

//  T1 for y, XTy, XTone
//  T2 for beta
//  T3 for coef0
//  T4 for X
//  <Eigen::VectorXd, Eigen::VectorXd, double, Eigen::MatrixXd> for Univariate Dense
//  <Eigen::VectorXd, Eigen::VectorXd, double, Eigen::SparseMatrix<double> > for Univariate Sparse
//  <Eigen::MatrixXd, Eigen::MatrixXd, Eigen::VectorXd, Eigen::MatrixXd> for Multivariable Dense
//  <Eigen::MatrixXd, Eigen::MatrixXd, Eigen::VectorXd, Eigen::SparseMatrix<double> > for Multivariable Sparse
template <class T1, class T2, class T3, class T4>
List abessCpp(T4 &x, T1 &y, int n, int p,
              int data_type, Eigen::VectorXd weight, Eigen::MatrixXd sigma,
              bool is_normal,
              int algorithm_type, int model_type, int max_iter, int exchange_num,
              int path_type, bool is_warm_start,
              int ic_type, double ic_coef, bool is_cv, int Kfold,
              Eigen::VectorXi status,
              Eigen::VectorXi sequence,
              Eigen::VectorXd lambda_seq,
              int s_min, int s_max, int K_max, double epsilon,
              double lambda_min, double lambda_max, int nlambda,
              bool is_screening, int screening_size, int powell_path,
              Eigen::VectorXi g_index,
              Eigen::VectorXi always_select,
              double tau,
              int primary_model_fit_max_iter, double primary_model_fit_epsilon,
              bool early_stop, bool approximate_Newton,
              int thread,
              bool covariance_update,
              bool sparse_matrix,
              Algorithm<T1, T2, T3, T4> *algorithm, vector<Algorithm<T1, T2, T3, T4> *> algorithm_list)
{
  // to do: -openmp
#ifdef TEST
  clock_t t1, t2;
  cout << "abessCpp 1" << endl;
#endif

#ifndef R_BUILD
  std::srand(123);
#endif

  bool is_parallel = thread != 1;

  Data<T1, T2, T3, T4> data(x, y, data_type, weight, is_normal, g_index, status, sparse_matrix);

  Eigen::VectorXi screening_A;
  if (is_screening)
  {
    screening_A = screening(data, model_type, screening_size, always_select, approximate_Newton, primary_model_fit_max_iter, primary_model_fit_epsilon);
  }

  int M = data.y.cols();

  Metric<T1, T2, T3, T4> *metric = new Metric<T1, T2, T3, T4>(ic_type, ic_coef, is_cv, Kfold);

  // For CV:
  // 1:mask
  // 2:warm start save
  // 3:group_XTX
  if (is_cv)
  {
    metric->set_cv_train_test_mask(data, data.get_n());
    metric->set_cv_init_fit_arg(data.p, data.M);
    // metric->set_cv_initial_model_param(Kfold, data.get_p());
    // metric->set_cv_initial_A(Kfold, data.get_p());
    // metric->set_cv_initial_coef0(Kfold, data.get_p());
    // if (model_type == 1)
    //   metric->cal_cv_group_XTX(data);
  }

  // calculate loss for each parameter parameter combination
#ifdef TEST
  t1 = clock();
  cout << "abessCpp 2" << endl;
#endif
  Result<T2, T3> result;
  vector<Result<T2, T3>> result_list(Kfold);
  if (path_type == 1)
  {
    if (is_cv)
    {
      //////////////////////////////////can parallel///////////////////////////////////
      if (is_parallel)
      {
#pragma omp parallel for
        for (int i = 0; i < Kfold; i++)
        {
          sequential_path_cv<T1, T2, T3, T4>(data, sigma, algorithm_list[i], metric, sequence, lambda_seq, early_stop, i, result_list[i]);
        }
      }
      else
      {
        for (int i = 0; i < Kfold; i++)
        {
          sequential_path_cv<T1, T2, T3, T4>(data, sigma, algorithm, metric, sequence, lambda_seq, early_stop, i, result_list[i]);
        }
      }
    }
    else
    {
      sequential_path_cv<T1, T2, T3, T4>(data, sigma, algorithm, metric, sequence, lambda_seq, early_stop, -1, result);
    }
  }
  else
  {
    // if (algorithm_type == 5 || algorithm_type == 3)
    // {
    //     double log_lambda_min = log(max(lambda_min, 1e-5));
    //     double log_lambda_max = log(max(lambda_max, 1e-5));

    //     result = pgs_path(data, algorithm, metric, s_min, s_max, log_lambda_min, log_lambda_max, powell_path, nlambda);
    // }
    gs_path(data, algorithm, algorithm_list, metric, s_min, s_max, sequence, lambda_seq, K_max, epsilon, is_parallel, result);
  }

#ifdef TEST
  t2 = clock();
  std::cout << "path time : " << ((double)(t2 - t1) / CLOCKS_PER_SEC) << endl;
#endif

  // Get bestmodel index && fit bestmodel
  int min_loss_index_row = 0, min_loss_index_col = 0, s_size = sequence.size(), lambda_size = lambda_seq.size();
  Eigen::Matrix<T2, Dynamic, Dynamic> beta_matrix(s_size, lambda_size);
  Eigen::Matrix<T3, Dynamic, Dynamic> coef0_matrix(s_size, lambda_size);
  Eigen::Matrix<VectorXd, Dynamic, Dynamic> bd_matrix(s_size, lambda_size);
  Eigen::MatrixXd ic_matrix(s_size, lambda_size);
  Eigen::MatrixXd test_loss_sum = Eigen::MatrixXd::Zero(s_size, lambda_size);
  Eigen::MatrixXd train_loss_matrix(s_size, lambda_size);

  if (path_type == 1)
  {
    if (is_cv)
    {
      Eigen::MatrixXd test_loss_tmp;
      for (int i = 0; i < Kfold; i++)
      {
        test_loss_tmp = result_list[i].test_loss_matrix;
        test_loss_sum = test_loss_sum + test_loss_tmp / Kfold;
      }
      test_loss_sum.minCoeff(&min_loss_index_row, &min_loss_index_col);

      Eigen::Matrix<T4, -1, -1> full_group_XTX = group_XTX(data.x, data.g_index, data.g_size, data.n, data.p, data.g_num, model_type);

      Eigen::MatrixXd covariance;
      T1 XTy;
      T1 XTone;
      if (covariance_update)
      {
        XTy = data.x.transpose() * data.y;
        XTone = data.x.transpose() * Eigen::MatrixXd::Ones(data.n, data.M);
      }

      if (is_parallel)
      {
        // cout << "cv parallel" << endl;
        for (int i = 0; i < max(Kfold, thread); i++)
        {
          if (covariance_update)
          {
            algorithm_list[i]->covariance_update_flag = Eigen::VectorXi::Zero(data.p);
            algorithm_list[i]->XTy = XTy;
            algorithm_list[i]->XTone = XTone;
          }

          algorithm_list[i]->update_group_XTX(full_group_XTX);
          algorithm_list[i]->PhiG = Eigen::Matrix<Eigen::MatrixXd, -1, -1>(0, 0);
        }
#pragma omp parallel for
        for (int i = 0; i < sequence.size() * lambda_seq.size(); i++)
        {
          int s_index = i / lambda_seq.size();
          int lambda_index = i % lambda_seq.size();
          int algorithm_index = omp_get_thread_num();

          T2 beta_init;
          T3 coef0_init;
          coef_set_zero(data.p, M, beta_init, coef0_init);
          Eigen::VectorXd bd_init = Eigen::VectorXd::Zero(data.p);

          for (int j = 0; j < Kfold; j++)
          {
            beta_init = beta_init + result_list[j].beta_matrix(s_index, lambda_index) / Kfold;
            coef0_init = coef0_init + result_list[j].coef0_matrix(s_index, lambda_index) / Kfold;
            bd_init = bd_init + result_list[j].bd_matrix(s_index, lambda_index) / Kfold;
          }

          algorithm_list[algorithm_index]->update_sparsity_level(sequence(s_index));
          algorithm_list[algorithm_index]->update_lambda_level(lambda_seq(lambda_index));
          algorithm_list[algorithm_index]->update_beta_init(beta_init);
          algorithm_list[algorithm_index]->update_coef0_init(coef0_init);
          algorithm_list[algorithm_index]->update_bd_init(bd_init);

          algorithm_list[algorithm_index]->fit(data.x, data.y, data.weight, data.g_index, data.g_size, data.n, data.p, data.g_num, data.status, sigma);

          beta_matrix(s_index, lambda_index) = algorithm_list[algorithm_index]->get_beta();
          coef0_matrix(s_index, lambda_index) = algorithm_list[algorithm_index]->get_coef0();
          train_loss_matrix(s_index, lambda_index) = algorithm_list[algorithm_index]->get_train_loss();
          ic_matrix(s_index, lambda_index) = metric->ic(data.n, data.M, data.g_num, algorithm_list[algorithm_index]);
        }
#ifdef TEST
        std::cout << "parallel cv 2 end--------" << std::endl;
#endif
      }
      else
      {
        if (covariance_update)
        {
          algorithm->covariance_update_flag = Eigen::VectorXi::Zero(data.p);
          algorithm->XTy = XTy;
          algorithm->XTone = XTone;
        }

        algorithm->update_group_XTX(full_group_XTX);

        algorithm->PhiG = Eigen::Matrix<Eigen::MatrixXd, -1, -1>(0, 0);
        for (int i = 0; i < sequence.size() * lambda_seq.size(); i++)
        {
          int s_index = i / lambda_seq.size();
          int lambda_index = i % lambda_seq.size();

          T2 beta_init;
          T3 coef0_init;
          coef_set_zero(data.p, M, beta_init, coef0_init);
          Eigen::VectorXd bd_init = Eigen::VectorXd::Zero(data.p);

          for (int j = 0; j < Kfold; j++)
          {
            beta_init = beta_init + result_list[j].beta_matrix(s_index, lambda_index) / Kfold;
            coef0_init = coef0_init + result_list[j].coef0_matrix(s_index, lambda_index) / Kfold;
            bd_init = bd_init + result_list[j].bd_matrix(s_index, lambda_index) / Kfold;
          }

          algorithm->update_sparsity_level(sequence(s_index));
          algorithm->update_lambda_level(lambda_seq(lambda_index));
          algorithm->update_beta_init(beta_init);
          algorithm->update_coef0_init(coef0_init);
          algorithm->update_bd_init(bd_init);

          algorithm->fit(data.x, data.y, data.weight, data.g_index, data.g_size, data.n, data.p, data.g_num, data.status, sigma);

          beta_matrix(s_index, lambda_index) = algorithm->get_beta();
          coef0_matrix(s_index, lambda_index) = algorithm->get_coef0();
          train_loss_matrix(s_index, lambda_index) = algorithm->get_train_loss();
          ic_matrix(s_index, lambda_index) = metric->ic(data.n, data.M, data.g_num, algorithm);
        }
      }
#ifdef TEST
      cout << "test_loss: " << test_loss_sum << endl;
#endif
    }
    else
    {
      beta_matrix = result.beta_matrix;
      coef0_matrix = result.coef0_matrix;
      ic_matrix = result.ic_matrix;
      train_loss_matrix = result.train_loss_matrix;
      ic_matrix.minCoeff(&min_loss_index_row, &min_loss_index_col);
#ifdef TEST
      std::cout << "train_loss: " << std::endl;
      std::cout << train_loss_matrix << std::endl;
      std::cout << "ic: " << std::endl;
      std::cout << ic_matrix << std::endl;
#endif
    }
  }
  else
  {
    beta_matrix = result.beta_matrix;
    coef0_matrix = result.coef0_matrix;
    ic_matrix = result.ic_matrix;
    train_loss_matrix = result.train_loss_matrix;
    Eigen::MatrixXd test_loss_matrix = result.test_loss_matrix;
    if (is_cv)
    {
      test_loss_matrix.minCoeff(&min_loss_index_row, &min_loss_index_col);
    }
    else
    {
      ic_matrix.minCoeff(&min_loss_index_row, &min_loss_index_col);
    }

#ifdef TEST
    std::cout << "test_loss: " << std::endl;
    std::cout << test_loss_matrix << std::endl;
    std::cout << "train_loss: " << std::endl;
    std::cout << train_loss_matrix << std::endl;
    std::cout << "ic: " << std::endl;
    std::cout << ic_matrix << std::endl;
#endif
  }

#ifdef TEST
  std::cout << "abesscpp 3 end --------" << std::endl;
#endif
  // fit best model
  // int best_s = sequence(min_loss_index_row);
  double best_lambda = lambda_seq(min_loss_index_col);

  T2 best_beta;
  T3 best_coef0;
  double best_train_loss, best_ic, best_test_loss;

  best_beta = beta_matrix(min_loss_index_row, min_loss_index_col);
  best_coef0 = coef0_matrix(min_loss_index_row, min_loss_index_col);
  best_train_loss = train_loss_matrix(min_loss_index_row, min_loss_index_col);
  best_ic = ic_matrix(min_loss_index_row, min_loss_index_col);
  best_test_loss = test_loss_sum(min_loss_index_row, min_loss_index_col);

  //////////////Restore best_fit_result for normal//////////////
  // to do
  if (data.is_normal && !sparse_matrix)
  {
    if (data.data_type == 1)
    {
      array_quotient(best_beta, data.x_norm, 1);
      best_beta = best_beta * sqrt(double(data.n));
      best_coef0 = data.y_mean - matrix_dot(best_beta, data.x_mean);
    }
    else if (data.data_type == 2)
    {
      array_quotient(best_beta, data.x_norm, 1);
      best_beta = best_beta * sqrt(double(data.n));
      best_coef0 = best_coef0 - matrix_dot(best_beta, data.x_mean);
    }
    else
    {
      array_quotient(best_beta, data.x_norm, 1);
      best_beta = best_beta * sqrt(double(data.n));
    }
  }

  ////////////// Restore all_fit_result for normal ////////////////////////
  // to do
  if (data.is_normal && !sparse_matrix)
  {
    if (data.data_type == 1)
    {
      for (int j = 0; j < beta_matrix.cols(); j++)
      {
        for (int i = 0; i < beta_matrix.rows(); i++)
        {
          array_quotient(beta_matrix(i, j), data.x_norm, 1);
          beta_matrix(i, j) = beta_matrix(i, j) * sqrt(double(data.n));
          coef0_matrix(i, j) = data.y_mean - matrix_dot(beta_matrix(i, j), data.x_mean);
        }
      }
    }
    else if (data.data_type == 2)
    {
      for (int j = 0; j < beta_matrix.cols(); j++)
      {
        for (int i = 0; i < beta_matrix.rows(); i++)
        {
          array_quotient(beta_matrix(i, j), data.x_norm, 1);
          beta_matrix(i, j) = beta_matrix(i, j) * sqrt(double(data.n));
          coef0_matrix(i, j) = coef0_matrix(i, j) - matrix_dot(beta_matrix(i, j), data.x_mean);
        }
      }
    }
    else
    {
      for (int j = 0; j < beta_matrix.cols(); j++)
      {
        for (int i = 0; i < beta_matrix.rows(); i++)
        {
          array_quotient(beta_matrix(i, j), data.x_norm, 1);
          beta_matrix(i, j) = beta_matrix(i, j) * sqrt(double(data.n));
        }
      }
    }
  }
  // cout << "abess 7" << endl;

  // List result;
  List out_result;
#ifdef R_BUILD
  out_result = List::create(Named("beta") = best_beta,
                            Named("coef0") = best_coef0,
                            Named("train_loss") = best_train_loss,
                            Named("ic") = best_ic,
                            Named("lambda") = best_lambda,
                            Named("beta_all") = beta_matrix,
                            Named("coef0_all") = coef0_matrix,
                            Named("train_loss_all") = train_loss_matrix,
                            Named("ic_all") = ic_matrix,
                            Named("test_loss_all") = test_loss_sum);
  if (path_type == 2) {
    out_result.push_back(sequence, "sequence");
  }
#else
  out_result.add("beta", best_beta);
  out_result.add("coef0", best_coef0);
  out_result.add("train_loss", best_train_loss);
  out_result.add("test_loss", best_test_loss);
  out_result.add("ic", best_ic);
  out_result.add("lambda", best_lambda);
  // out_result.add("beta_all", beta_matrix);
  // out_result.add("coef0_all", coef0_matrix);
  // out_result.add("train_loss_all", train_loss_matrix);
  // out_result.add("ic_all", ic_matrix);
  // out_result.add("test_loss_all", test_loss_sum);
#endif

  // Restore best_fit_result for screening
  if (is_screening)
  {
#ifdef TEST
    cout << "screening_A: " << screening_A << endl;
    cout << "p: " << x.cols() << endl;
#endif
    T2 beta_screening_A;
    T2 beta;
    T3 coef0;
    coef_set_zero(x.cols(), M, beta, coef0);

#ifndef R_BUILD
    out_result.get_value_by_name("beta", beta_screening_A);
    slice_restore(beta_screening_A, screening_A, beta);
    out_result.add("beta", beta);
    out_result.add("screening_A", screening_A);
#else
    beta_screening_A = out_result["beta"];
    slice_restore(beta_screening_A, screening_A, beta);
    out_result["beta"] = beta;
    out_result.push_back(screening_A, "screening_A");
#endif
  }

  delete metric;
  return out_result;
}

#ifndef R_BUILD

void pywrap_abess(double *x, int x_row, int x_col, double *y, int y_row, int y_col, int n, int p, int data_type, double *weight, int weight_len, double *sigma, int sigma_row, int sigma_col,
                  bool is_normal,
                  int algorithm_type, int model_type, int max_iter, int exchange_num,
                  int path_type, bool is_warm_start,
                  int ic_type, double ic_coef, bool is_cv, int Kfold,
                  int *gindex, int gindex_len,
                  int *status, int status_len,
                  int *sequence, int sequence_len,
                  double *lambda_sequence, int lambda_sequence_len,
                  int s_min, int s_max, int K_max, double epsilon,
                  double lambda_min, double lambda_max, int n_lambda,
                  bool is_screening, int screening_size, int powell_path,
                  int *always_select, int always_select_len, double tau,
                  int primary_model_fit_max_iter, double primary_model_fit_epsilon,
                  bool early_stop, bool approximate_Newton,
                  int thread,
                  bool covariance_update,
                  bool sparse_matrix,
                  int splicing_type,
                  double *beta_out, int beta_out_len, double *coef0_out, int coef0_out_len, double *train_loss_out,
                  int train_loss_out_len, double *ic_out, int ic_out_len, double *nullloss_out, double *aic_out,
                  int aic_out_len, double *bic_out, int bic_out_len, double *gic_out, int gic_out_len, int *A_out,
                  int A_out_len, int *l_out)
{
  Eigen::MatrixXd x_Mat;
  Eigen::MatrixXd y_Mat;
  Eigen::MatrixXd sigma_Mat;
  Eigen::VectorXd weight_Vec;
  Eigen::VectorXi gindex_Vec;
  Eigen::VectorXi status_Vec;
  Eigen::VectorXi sequence_Vec;
  Eigen::VectorXd lambda_sequence_Vec;
  Eigen::VectorXi always_select_Vec;

#ifdef TEST
  clock_t t1, t2;
#endif
  // t1 = clock();
  x_Mat = Pointer2MatrixXd(x, x_row, x_col);
  y_Mat = Pointer2MatrixXd(y, y_row, y_col);
  sigma_Mat = Pointer2MatrixXd(sigma, sigma_row, sigma_col);
  weight_Vec = Pointer2VectorXd(weight, weight_len);
  status_Vec = Pointer2VectorXi(status, status_len);
  gindex_Vec = Pointer2VectorXi(gindex, gindex_len);
  sequence_Vec = Pointer2VectorXi(sequence, sequence_len);
  lambda_sequence_Vec = Pointer2VectorXd(lambda_sequence, lambda_sequence_len);
  always_select_Vec = Pointer2VectorXi(always_select, always_select_len);
  // t2 = clock();
  // std::cout << "pointer to data: " << ((double)(t2 - t1) / CLOCKS_PER_SEC) << endl;
#ifdef TEST
  t1 = clock();
#endif
  List mylist = abessCpp2(x_Mat, y_Mat, n, p, data_type, weight_Vec, sigma_Mat,
                          is_normal,
                          algorithm_type, model_type, max_iter, exchange_num,
                          path_type, is_warm_start,
                          ic_type, ic_coef, is_cv, Kfold,
                          status_Vec,
                          sequence_Vec,
                          lambda_sequence_Vec,
                          s_min, s_max, K_max, epsilon,
                          lambda_min, lambda_max, n_lambda,
                          is_screening, screening_size, powell_path,
                          gindex_Vec,
                          always_select_Vec, tau,
                          primary_model_fit_max_iter, primary_model_fit_epsilon,
                          early_stop, approximate_Newton,
                          thread,
                          covariance_update,
                          sparse_matrix,
                          splicing_type);

#ifdef TEST
  t2 = clock();
  std::cout << "get result : " << ((double)(t2 - t1) / CLOCKS_PER_SEC) << endl;
#endif

  // t1 = clock();
  if (y_col == 1)
  {
    Eigen::VectorXd beta;
    double coef0 = 0;
    double train_loss = 0;
    double ic = 0;
    mylist.get_value_by_name("beta", beta);
    mylist.get_value_by_name("coef0", coef0);
    mylist.get_value_by_name("train_loss", train_loss);
    mylist.get_value_by_name("ic", ic);

    VectorXd2Pointer(beta, beta_out);
    *coef0_out = coef0;
    *train_loss_out = train_loss;
    *ic_out = ic;
  }
  else
  {
    Eigen::MatrixXd beta;
    Eigen::VectorXd coef0;
    double train_loss = 0;
    double ic = 0;
    mylist.get_value_by_name("beta", beta);
    mylist.get_value_by_name("coef0", coef0);
    mylist.get_value_by_name("train_loss", train_loss);
    mylist.get_value_by_name("ic", ic);
    // cout << "beta" << endl;
    // cout << beta << endl;
    // cout << "coef0" << endl;
    // cout << coef0 << endl;
    // cout << "train_loss" << endl;
    // cout << train_loss << endl;
    // cout << "ic" << endl;
    // cout << ic << endl;

    MatrixXd2Pointer(beta, beta_out);
    VectorXd2Pointer(coef0, coef0_out);
    train_loss_out[0] = train_loss;
    ic_out[0] = ic;
  }

  // t2 = clock();
  // std::cout << "result to pointer: " << ((double)(t2 - t1) / CLOCKS_PER_SEC) << endl;
}
#endif


#include <vector>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

int
main() {
  typedef bg::model::point<float, 2, bg::cs::cartesian> point;
  typedef bg::model::box<point> box;
  typedef std::pair<box, unsigned> value;
  bgi::rtree< value, bgi::quadratic<16> > rtree;
  for (unsigned i = 0 ; i < 10 ; ++i) {
    box b(point(i + 0.0f, i + 0.0f), point(i + 0.5f, i + 0.5f));
    rtree.insert(std::make_pair(b, i));
  }
  box query_box(point(0, 0), point(5, 5));
  std::vector<value> result_s;
  rtree.query(bgi::intersects(query_box), std::back_inserter(result_s));
  return result_s.size() == 5;
}


/**
 * \file boost/numeric/ublasx/operation/relational_ops.hpp
 *
 * \brief Relational operators defined over matrix and vector expressions.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_RELATIONAL_OPS_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_RELATIONAL_OPS_HPP

#include <boost/numeric/ublas/detail/config.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>

/// Equality operator for vector expressions.
template <typename VectorExpr1T, typename VectorExpr2T>
BOOST_UBLAS_INLINE
bool operator==(::boost::numeric::ublas::vector_expression<VectorExpr1T> const& ve1,
				::boost::numeric::ublas::vector_expression<VectorExpr2T> const& ve2)
{
	typedef typename ::boost::numeric::ublas::promote_traits<
			typename ::boost::numeric::ublas::vector_traits<VectorExpr1T>::size_type,
			typename ::boost::numeric::ublas::vector_traits<VectorExpr2T>::size_type
		>::promote_type size_type;
	typedef typename ::boost::numeric::ublas::promote_traits<
			typename ::boost::numeric::ublas::vector_traits<VectorExpr1T>::value_type,
			typename ::boost::numeric::ublas::vector_traits<VectorExpr2T>::value_type
		>::promote_type value_type;

	size_type n1(::boost::numeric::ublasx::size(ve1));
	size_type n2(::boost::numeric::ublasx::size(ve2));

	if (n1 != n2)
	{
		return false;
	}

	for (size_type i = 0; i < n1; ++i)
	{
		if (static_cast<value_type>(ve1()(i)) != static_cast<value_type>(ve2()(i)))
		{
			return false;
		}
	}

	return true;
}

/// Inequality operator for vector expressions.
template <typename VectorExpr1T, typename VectorExpr2T>
BOOST_UBLAS_INLINE
bool operator!=(::boost::numeric::ublas::vector_expression<VectorExpr1T> const& ve1,
				::boost::numeric::ublas::vector_expression<VectorExpr2T> const& ve2)
{
	return !(ve1 == ve2);
}

/// Equality operator for matrix expressions.
template <typename MatrixExpr1T, typename MatrixExpr2T>
BOOST_UBLAS_INLINE
bool operator==(::boost::numeric::ublas::matrix_expression<MatrixExpr1T> const& me1,
				::boost::numeric::ublas::matrix_expression<MatrixExpr2T> const& me2)
{
	typedef typename ::boost::numeric::ublas::promote_traits<
			typename ::boost::numeric::ublas::matrix_traits<MatrixExpr1T>::size_type,
			typename ::boost::numeric::ublas::matrix_traits<MatrixExpr2T>::size_type
		>::promote_type size_type;
	typedef typename ::boost::numeric::ublas::promote_traits<
			typename ::boost::numeric::ublas::matrix_traits<MatrixExpr1T>::value_type,
			typename ::boost::numeric::ublas::matrix_traits<MatrixExpr2T>::value_type
		>::promote_type value_type;

	size_type nr1(::boost::numeric::ublasx::num_rows(me1));
	size_type nc1(::boost::numeric::ublasx::num_columns(me1));
	size_type nr2(::boost::numeric::ublasx::num_rows(me2));
	size_type nc2(::boost::numeric::ublasx::num_columns(me2));

	if (nr1 != nr2 || nc1 != nc2)
	{
		return false;
	}

	for (size_type r = 0; r < nr1; ++r)
	{
		for (size_type c = 0; c < nc1; ++c)
		{
			if (static_cast<value_type>(me1()(r,c)) != static_cast<value_type>(me2()(r,c)))
			{
				return false;
			}
		}
	}

	return true;
}

/// Inequality operator for matrix expressions.
template <typename MatrixExpr1T, typename MatrixExpr2T>
BOOST_UBLAS_INLINE
bool operator!=(::boost::numeric::ublas::matrix_expression<MatrixExpr1T> const& me1,
				::boost::numeric::ublas::matrix_expression<MatrixExpr2T> const& me2)
{
	return !(me1 == me2);
}

#endif // BOOST_NUMERIC_UBLASX_OPERATION_RELATIONAL_OPS_HPP


#include "moja/flint/configuration/configcell.h"

#include "moja/flint/configuration/configblock.h"

#include <boost/format.hpp>
#include <boost/format/group.hpp>

#include <cmath>
#include <iomanip>

namespace moja {
namespace flint {
namespace configuration {

ConfigCell::ConfigCell(const ConfigBlock& block, int row, int col) : _block(block) {
   if (row < 0) {
      throw std::invalid_argument("Error in config cell row < 0");
   }

   if (col < 0) {
      throw std::invalid_argument("Error in config cell col < 0");
   }

   _row = row;
   _col = col;
   _id = row * block.xPixels() + col + 1;
}

double ConfigCell::area() const {
   double x = _block.yPixelSize() * DiameterOfEarthInMetersPerDeg;
   return x * x * std::cos(lat() * DegToRadMultiplier) * 0.0001;
}

double ConfigCell::lon() const { return _block.left() + _block.xPixelSize() * _col; }

double ConfigCell::lat() const { return _block.top() - _block.yPixelSize() * (_row + 1); }

std::string ConfigCell::toString() const {
   return (boost::format("%1%,%2% (%3%,%4%)") % _row % _col % boost::io::group(std::setprecision(15), lon()) %
           boost::io::group(std::setprecision(15), lat()))
       .str();
}

}  // namespace configuration
}  // namespace flint
}  // namespace moja


#include <boost/date_time/gregorian/gregorian.hpp>
#include <iostream>
#include <cstdlib>

int main( int argc , char* argv[ ] ) {
   using namespace boost::gregorian ;

   greg_month months[ ] = { Jan , Feb , Mar , Apr , May , Jun , Jul ,
      Aug , Sep , Oct , Nov , Dec } ;
   greg_year gy = atoi( argv[ 1 ] ) ;
   for ( int i = 0 ; i < 12 ; i++ ) {
      last_day_of_the_week_in_month lwdm ( Friday , months[ i ] ) ;
      date d = lwdm.get_date( gy ) ;
      std::cout << d << std::endl ;
   }
   return 0 ;
}


/*
@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/bool.hpp>
#include <boost/hana/comparable/comparable.hpp>
#include <boost/hana/core/is_a.hpp>
#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/ext/std/tuple.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/iterable/iterable.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/logical/logical.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/monad/monad.hpp>
#include <boost/hana/range.hpp>

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

int main() {
    //! [standard]
    BOOST_HANA_CONSTANT_ASSERT( is_a<Foldable, Maybe>);
    BOOST_HANA_CONSTANT_ASSERT(!is_a<Logical, Maybe>);
    BOOST_HANA_CONSTANT_ASSERT(!is_a<Foldable, int>);
    BOOST_HANA_CONSTANT_ASSERT( are<Comparable, int, long>);
    //! [standard]

    //! [alternate]
    BOOST_HANA_CONSTEXPR_LAMBDA auto row = list;
    BOOST_HANA_CONSTEXPR_LAMBDA auto check_table = [](auto ...headers) {
        return [=](auto ...rows) {
            auto row_is_correct = [=](auto row) {
                return list(headers(head(row))...) == tail(row);
            };
            BOOST_HANA_CONSTANT_ASSERT(all(list(rows...), row_is_correct));
        };
    };

    check_table(                      is_a<Logical>, is_an<Iterable>, is_a<Monad>   )(
    row(just(1),                         false_,         false_,         true_      ),
    row(list(1, '2', 3.3),               false_,         true_,          true_      ),
    row(std::make_tuple("abc", 'd'),     false_,         true_,          true_      ),
    row(long_<12>,                       true_,          false_,         false_     ),
    row(range(int_<-4>, int_<15>),       false_,         true_,          false_     )
    );
    //! [alternate]
}


#include <boost/test/unit_test.hpp>
#include <array>
#include "L4/Utils/Math.h"

namespace L4
{
namespace UnitTests
{

using namespace Utils;

BOOST_AUTO_TEST_CASE(MathTest)
{
    // RoundUp tests.
    BOOST_CHECK_EQUAL(Math::RoundUp(5, 10), 10);
    BOOST_CHECK_EQUAL(Math::RoundUp(10, 10), 10);
    BOOST_CHECK_EQUAL(Math::RoundUp(11, 10), 20);
    BOOST_CHECK_EQUAL(Math::RoundUp(5, 0), 5);

    // RoundDown tests.
    BOOST_CHECK_EQUAL(Math::RoundDown(5, 10), 0);
    BOOST_CHECK_EQUAL(Math::RoundDown(10, 10), 10);
    BOOST_CHECK_EQUAL(Math::RoundDown(11, 10), 10);
    BOOST_CHECK_EQUAL(Math::RoundDown(5, 0), 5);

    // IsPowerOfTwo tests.
    BOOST_CHECK(Math::IsPowerOfTwo(2));
    BOOST_CHECK(Math::IsPowerOfTwo(4));
    BOOST_CHECK(!Math::IsPowerOfTwo(3));
    BOOST_CHECK(!Math::IsPowerOfTwo(0));

    // NextHighestPowerOfTwo tests.
    BOOST_CHECK_EQUAL(Math::NextHighestPowerOfTwo(0), 0U);
    BOOST_CHECK_EQUAL(Math::NextHighestPowerOfTwo(1), 1U);
    BOOST_CHECK_EQUAL(Math::NextHighestPowerOfTwo(2), 2U);
    BOOST_CHECK_EQUAL(Math::NextHighestPowerOfTwo(3), 4U);
    BOOST_CHECK_EQUAL(Math::NextHighestPowerOfTwo(4), 4U);
    BOOST_CHECK_EQUAL(Math::NextHighestPowerOfTwo(5), 8U);
    BOOST_CHECK_EQUAL(Math::NextHighestPowerOfTwo(200), 256U);
}

BOOST_AUTO_TEST_CASE(PointerArithmeticTest)
{
    std::array<int, 3> elements;

    BOOST_CHECK(reinterpret_cast<int*>(Math::PointerArithmetic::Add(&elements[0], sizeof(int))) == &elements[1]);
    BOOST_CHECK(reinterpret_cast<int*>(Math::PointerArithmetic::Subtract(&elements[1], sizeof(int))) == &elements[0]);
    BOOST_CHECK(Math::PointerArithmetic::Distance(&elements[2], &elements[0]) == sizeof(int) * 2U);
    BOOST_CHECK(Math::PointerArithmetic::Distance(&elements[0], &elements[2]) == sizeof(int) * 2U);
}

} // namespace UnitTests
} // namespace L4


// Copyright (C) 2013 The Regents of the University of California (Regents)
// and Google, Inc. All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California, Google,
//       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), John Flynn (jflynn@google.com)

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <vector>

#include "gtest/gtest.h"

#include "theia/math/util.h"
#include "theia/test/benchmark.h"
#include "theia/util/random.h"
#include "theia/util/util.h"
#include "theia/vision/sfm/triangulation/triangulation.h"
#include "theia/vision/sfm/pose/util.h"

namespace theia {
using Eigen::MatrixXd;
using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Vector4d;

double ReprojectionError(const TransformationMatrix& pose,
                         const Vector3d& world_point,
                         const Vector2d& image_point) {
  const Vector2d reprojected_point = (pose * world_point).hnormalized();
  const double sq_reproj_error =
      (reprojected_point - image_point).squaredNorm();
  return sq_reproj_error;
}

void TestTriangulationBasic(const Vector3d& point_3d,
                            const Quaterniond& rel_rotation,
                            const Vector3d& rel_translation,
                            const double projection_noise,
                            const double max_reprojection_error) {
  InitRandomGenerator();

  TransformationMatrix pose_left = TransformationMatrix::Identity();
  TransformationMatrix pose_right = TransformationMatrixFromRt(
      rel_rotation.toRotationMatrix(), rel_translation);

  // Reproject point into both image 2, assume image 1 is identity rotation at
  // the origin.
  Vector2d image_point_1 = (pose_left * point_3d).hnormalized();
  Vector2d image_point_2 = (pose_right * point_3d).hnormalized();

  // Add projection noise if required.
  if (projection_noise) {
    AddNoiseToProjection(projection_noise, &image_point_1);
    AddNoiseToProjection(projection_noise, &image_point_2);
  }

  // Triangulate.
  Vector3d triangulated_point;
  EXPECT_TRUE(Triangulate(pose_left.matrix(), pose_right.matrix(),
                          image_point_1, image_point_2, &triangulated_point));

  // Check the reprojection error.
  EXPECT_LE(
      ReprojectionError(pose_left, triangulated_point, image_point_1),
      max_reprojection_error);
  EXPECT_LE(
      ReprojectionError(pose_right, triangulated_point, image_point_2),
      max_reprojection_error);
}

void TestTriangulationManyPoints(const double projection_noise,
                                 const double max_reprojection_error) {
  using Eigen::AngleAxisd;

  static const int num_views = 8;

  // Sets some test rotations and translations.
  static const Quaterniond kRotations[num_views] = {
    Quaterniond(AngleAxisd(Radians(7.0), Vector3d(0.0, 0.0, 1.0).normalized())),
    Quaterniond(
        AngleAxisd(Radians(12.0), Vector3d(0.0, 1.0, 0.0).normalized())),
    Quaterniond(
        AngleAxisd(Radians(15.0), Vector3d(1.0, 0.0, 0.0).normalized())),
    Quaterniond(
        AngleAxisd(Radians(20.0), Vector3d(1.0, 0.0, 1.0).normalized())),
    Quaterniond(
        AngleAxisd(Radians(11.0), Vector3d(0.0, 1.0, 1.0).normalized())),
    Quaterniond(AngleAxisd(Radians(0.0), Vector3d(1.0, 1.0, 1.0).normalized())),
    Quaterniond(AngleAxisd(Radians(5.0), Vector3d(0.0, 1.0, 1.0).normalized())),
    Quaterniond(AngleAxisd(Radians(0.0), Vector3d(1.0, 1.0, 1.0).normalized()))
  };

  static const Vector3d kTranslations[num_views] = {
    Vector3d(1.0, 1.0, 1.0),
    Vector3d(3.0, 2.0, 13.0),
    Vector3d(4.0, 5.0, 11.0),
    Vector3d(1.0, 2.0, 15.0),
    Vector3d(3.0, 1.5, 91.0),
    Vector3d(1.0, 7.0, 11.0),
    Vector3d(0.0, 0.0, 0.0),  // Tests no translation.
    Vector3d(0.0, 0.0, 0.0)  // Tests no translation and no rotation.
  };

  // Set up model points.
  static const double kTestPoints[][3] = {
    { -1.62, -2.99, 6.12 }, { 4.42, -1.53, 9.83 }, { 1.45, -0.59, 5.29 },
    { 1.89, -1.10, 8.22 }, { -0.21, 2.38, 5.63 }, { 0.61, -0.97, 7.49 },
    { 0.48, 0.70, 8.94 }, { 1.65, -2.56, 8.63 }, { 2.44, -0.20, 7.78 },
    { 2.84, -2.58, 7.35 }, { -1.35, -2.84, 7.33 }, { -0.42, 1.54, 8.86 },
    { 2.56, 1.72, 7.86 }, { 1.75, -1.39, 5.73 }, { 2.08, -3.91, 8.37 },
    { -0.91, 1.36, 9.16 }, { 2.84, 1.54, 8.74 }, { -1.01, 3.02, 8.18 },
    { -3.73, -0.62, 7.81 }, { -2.98, -1.88, 6.23 }, { 2.39, -0.19, 6.47 },
    { -0.63, -1.05, 7.11 }, { -1.76, -0.55, 5.18 }, { -3.19, 3.27, 8.18 },
    { 0.31, -2.77, 7.54 }, { 0.54, -3.77, 9.77 },
  };

  // Set up pose matrices.
  std::vector<ProjectionMatrix> poses(num_views);
  for (int i = 0; i < num_views; i++) {
    poses[i] = TransformationMatrixFromRt(kRotations[i].toRotationMatrix(),
                                          kTranslations[i]).matrix();
  }

  for (int j = 0; j < ARRAYSIZE(kTestPoints); j++) {
    // Reproject model point into the images.
    std::vector<Vector2d> image_points(num_views);
    const Vector3d model_point(kTestPoints[j][0], kTestPoints[j][1],
                               kTestPoints[j][2]);
    for (int i = 0; i < num_views; i++) {
      image_points[i] =
          (poses[i] * model_point.homogeneous()).eval().hnormalized();
    }

    // Add projection noise if required.
    if (projection_noise) {
      for (int i = 0; i < num_views; i++) {
        AddNoiseToProjection(projection_noise, &image_points[i]);
      }
    }

    Vector3d triangulated_point;
    ASSERT_TRUE(TriangulateNView(poses, image_points, &triangulated_point));

    // Check the reprojection error.
    for (int i = 0; i < num_views; i++) {
      EXPECT_LE(ReprojectionError(TransformationMatrix(poses[i]),
                                  triangulated_point, image_points[i]),
                max_reprojection_error);
    }
  }
}

TEST(Triangluation, BasicTest) {
  static const double kProjectionNoise = 0.0;
  static const double kReprojectionTolerance = 1e-12;

  // Set up model points.
  const Vector3d points_3d[2] = { Vector3d(5.0, 20.0, 23.0),
                                  Vector3d(-6.0, 16.0, 33.0) };

  // Set up rotations.
  const Quaterniond kRotation(Eigen::AngleAxisd(0.15, Vector3d(0.0, 1.0, 0.0)));

  // Set up translations.
  const Vector3d kTranslation(-3.0, 1.5, 11.0);

  // Run the test.
  for (int i = 0; i < 2; i++) {
    TestTriangulationBasic(points_3d[i],
                           kRotation,
                           kTranslation,
                           kProjectionNoise,
                           kReprojectionTolerance);
  }
}

TEST(Triangluation, NoiseTest) {
  static const double kProjectionNoise = 1.0 / 512.0;
  static const double kReprojectionTolerance = 1e-5;

  // Set up model points.
  const Vector3d points_3d[2] = { Vector3d(5.0, 20.0, 23.0),
                                  Vector3d(-6.0, 16.0, 33.0) };

  // Set up rotations.
  const Quaterniond kRotation(Eigen::AngleAxisd(0.15, Vector3d(0.0, 1.0, 0.0)));

  // Set up translations.
  const Vector3d kTranslation(-3.0, 1.5, 11.0);

  // Run the test.
  for (int i = 0; i < 2; i++) {
    TestTriangulationBasic(points_3d[i],
                           kRotation,
                           kTranslation,
                           kProjectionNoise,
                           kReprojectionTolerance);
  }
}

TEST(TriangluationNView, BasicTest) {
  static const double kProjectionNoise = 0.0;
  static const double kReprojectionTolerance = 1e-12;

  // Run the test.
  TestTriangulationManyPoints(kProjectionNoise, kReprojectionTolerance);
}

TEST(TriangluationNView, NoiseTest) {
  static const double kProjectionNoise = 1.0 / 512.0;
  static const double kReprojectionTolerance = 5e-4;

  // Run the test.
  TestTriangulationManyPoints(kProjectionNoise, kReprojectionTolerance);
}

BENCHMARK(TriangulationNView, Benchmark, 100, 1000) {
  static const double kProjectionNoise = 0.0;
  static const double kReprojectionTolerance = 1e-12;

  // Run the test.
  TestTriangulationManyPoints(kProjectionNoise, kReprojectionTolerance);
}

}  // namespace theia


#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <boost/thread/thread.hpp>
#include "strands_movebase/noise_approximate_voxel_grid.h"

ros::Publisher pub;

void callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromROSMsg(*msg, *cloud);
	pcl::PointCloud<pcl::PointXYZ> voxel_cloud;

    noise_approximate_voxel_grid sor(5, 20); // a bit faster than nvg, not as accurate
	sor.setInputCloud(cloud);
    sor.setLeafSize(0.05f, 0.05f, 0.05f);
	sor.filter(voxel_cloud);

	/*pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; // another possibility, slow
	sor.setInputCloud(new_cloud);
	sor.setMeanK(20);
	sor.setStddevMulThresh(1.0);
	sor.filter(voxel_cloud);*/

	sensor_msgs::PointCloud2 msg_cloud;
    pcl::toROSMsg(voxel_cloud, msg_cloud);
	msg_cloud.header = msg->header;

	pub.publish(msg_cloud);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "subsample_cloud");
	ros::NodeHandle n;

    // topic of input cloud
    if (!n.hasParam("/subsample_cloud/input")) {
        ROS_ERROR("Could not find parameter input.");
        return -1;
    }
    std::string input;
    n.getParam("/subsample_cloud/input", input);

    // topic of output cloud
    if (!n.hasParam("/subsample_cloud/output")) {
        ROS_ERROR("Could not find parameter output.");
        return -1;
    }
    std::string output;
    n.getParam("/subsample_cloud/output", output);

	ros::Subscriber sub = n.subscribe(input, 1, callback);
    pub = n.advertise<sensor_msgs::PointCloud2>(output, 1);

    ros::Rate rate(5); // updating at 5 hz, slightly faster than move_base
    while (n.ok()) {
        rate.sleep();
        ros::spinOnce();
    }

	return 0;
}


#include <iostream>
#include <boost/functional/hash.hpp>
#include <boost/unordered_set.hpp>
#include <boost/foreach.hpp>
#include <string>

using namespace std;

namespace lib{
struct book
{
    int id;
    string author;
    string title;
    book(int _id, string _author, string _title)
    {
        id = _id;
        author = _author;
        title = _title;
    }
};

bool operator==(const book& a, const book& b)
{
    return a.id == b.id;
}

std::size_t hash_value(book const&a)
{
    boost::hash<int>hasher;
    return hasher(a.id);
}
}
int main()
{
    boost::hash<string> string_hash;
    cout << string_hash("WO CAO") << endl;
    boost::hash< vector<int> > vector_hash;
    int a[3] = {2,3,1};
    vector<int>v(a, a + 3);
    cout << vector_hash(v) << endl;

    lib::book knife(3458, "Zane Grey", "The Hash Knife Outfit");
    lib::book danger(1354, "Paul J. Shanley", "HAHAHAHAHAH");
    boost::hash< lib::book >hasher;
    cout << hasher(knife) << "\t" << hasher(danger) << endl;

    boost::unordered_set< lib::book > oset;
    oset.insert(knife);
    oset.insert(danger);
    oset.insert(lib::book(123, "London", "Strong Man"));

    BOOST_FOREACH(lib::book book, oset)
    {
        cout << book.id << "\t" << book.author << "\t" << book.title << endl;
    }
    return 0;
}


#include <dmatrix/buffer_model.h>
#include <dmatrix/display_model.h>
#include <dmatrix/driver.h>
#include <Eigen/Core>
#include <unsupported/Eigen/CXX11/Tensor>
#include "catch.hpp"

using namespace DMAtrix;

using Image = Eigen::Tensor<unsigned int, 3>;

template <size_t num_pins, size_t num_buffers>
struct DummyDriver {
  using dtype = uint32_t;
  std::array<std::vector<dtype>, num_buffers> buffers;

  struct Config {};

  void setup(std::array<int, num_pins> data_pins, int clk_pin, Config config,
             size_t size) {
    for (auto &buffer : buffers) buffer.resize(size);
  }

  /// decode the image in buffer[buf]
  template <typename D>
  Image decode(size_t buf) {
    Image res((int)D::rows, (int)D::cols, (int)D::colors);
    res.setZero();

    // state of shift register on LE
    std::vector<unsigned int> shift_reg_front(D::data_words);
    // circular buffer with for the non-visible shift register
    std::vector<unsigned int> shift_reg_back(D::data_words);
    size_t shift_reg_ptr = 0;

    unsigned int oe_clocks = 0;
    unsigned int oe_addr = 0;

    for (auto &x : buffers[buf]) {
      bool oe = !(x & 1);
      bool le = (x >> 1) & 1;
      unsigned int addr = (x >> 2) & ((1 << D::addr_bits) - 1);
      unsigned int data = (x >> (2 + D::addr_bits)) & ((1 << D::data_bits) - 1);

      shift_reg_back[shift_reg_ptr] = data;

      if (oe) {
        REQUIRE(!le);

        // addr is consistent curing oe pulse
        if (oe_clocks == 0)
          oe_addr = addr;
        else
          REQUIRE(addr == oe_addr);

        oe_clocks++;
      } else {
        if (oe_clocks) {
          // we've finished an oe pulse, with shift_reg_front loaded into the
          // column drivers (because le was not asserted during the pulse) and
          // the address lines set to oe_addr

          // where the display was lit during this oe pulse, brighten the result
          for (size_t row = 0; row < D::rows; row++)
            for (size_t col = 0; col < D::cols; col++)
              for (size_t color = 0; color < D::colors; color++) {
                DataAddr dataaddr = D::encode(row, col, color);
                if (dataaddr.addr == oe_addr &&
                    ((shift_reg_front[dataaddr.word] >> dataaddr.bit) & 1))
                  res((int)row, (int)col, (int)color) += oe_clocks;
              }

          oe_clocks = 0;
        }
      }

      if (le)
        for (size_t i = 0; i < D::data_words; i++)
          shift_reg_front[i] =
              shift_reg_back[(shift_reg_ptr + D::data_words - i) %
                             D::data_words];

      shift_reg_ptr = (shift_reg_ptr + 1) % D::data_words;
    }

    return res;
  }
};

/// check that if we write image, then that image is displayed correctly.
/// multiplier is the scaling factor from the written value to the number of
/// clocks that oe is held low for while that pixel is lit
template <typename D, typename Driver>
void run_test(Driver &driver, Image &image, int multiplier) {
  for (size_t row = 0; row < D::rows; row++)
    for (size_t col = 0; col < D::cols; col++)
      for (size_t color = 0; color < D::colors; color++) {
        unsigned int r = image((int)row, (int)col, 0);
        unsigned int g = image((int)row, (int)col, 1);
        unsigned int b = image((int)row, (int)col, 2);
        if (r || g || b) driver.write_rgb(row, col, r, g, b);
      }

  auto res = driver.pin_driver.template decode<D>(0);

  for (size_t row = 0; row < D::rows; row++)
    for (size_t col = 0; col < D::cols; col++)
      for (size_t color = 0; color < D::colors; color++) {
        unsigned int expected_brightness =
            multiplier * image((int)row, (int)col, (int)color);
        unsigned int actual_brightness = res((int)row, (int)col, (int)color);
        REQUIRE(actual_brightness == expected_brightness);
      }
}

TEST_CASE("single_pixels") {
  for (int row = 0; row < 2; row++)
    for (int col = 0; col < 2; col++)
      for (int color = 0; color < 3; color++)
        for (int shift = 0; shift < 8; shift++) {
          using D = FullDisplay<32, 64, 4, RGBOrder::RRGGBB>;
          Pins<D> pins{1, 2, 3, {4, 5, 6, 7}, {8, 9, 10, 11, 12, 13}};
          DisplayDriver<D, DummyDriver, false> driver(pins, 2, 8);

          Image im((int)D::rows, (int)D::cols, (int)D::colors);
          im.setZero();
          im(row, col, color) = 1 << shift;

          run_test<D>(driver, im, 2);
        }
}


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

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

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

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

 /*!

    @ingroup group-predicates
    Function object implementing is_le capabilities

    Returns @ref True or @ref False depending on whether x is
    less than or equal to y or not.

    Infix notation can be used with operator '<='.

    This is a convenient alias of @ref is_less_equal
  **/
  const boost::dispatch::functor<tag::is_le_> is_le = {};
} }
#endif

#include <boost/simd/function/scalar/is_less_equal.hpp>
#include <boost/simd/function/simd/is_le.hpp>

#endif


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

#include <boost/test/unit_test.hpp>
#include <boost/array.hpp>
#include <boost/time_series/characteristic_series.hpp>
#include <boost/time_series/constant_series.hpp>
#include <boost/time_series/delta_series.hpp>
#include <boost/time_series/dense_series.hpp>
#include <boost/time_series/heaviside_series.hpp>
#include <boost/time_series/inverse_heaviside_series.hpp>
#include <boost/time_series/piecewise_constant_series.hpp>
#include <boost/time_series/shifted_series.hpp>
#include <boost/time_series/sparse_series.hpp>
#include <boost/time_series/ordered_inserter.hpp>
#include <boost/time_series/numeric/period_sums.hpp>
#include "./is_close.hpp"

using namespace boost;
using namespace unit_test;

///////////////////////////////////////////////////////////////////////////////
// test_period_sums
//
void test_period_sums()
{
    using namespace time_series;

    // expected result for test 1:
    sparse_series<int> result;
    make_ordered_inserter(result)
        (20, 1)
        (20, 11)
        (20, 21)
        (20, 31)
        (20, 41)
        (20, 51)
        (20, 61)
        (20, 71)
        (20, 81)
        (18, 91)
    .commit();

    dense_series<int> d(stop = 100, value = 2);
    sparse_series<int> s = period_sums(d, 1, 10);
    BOOST_CHECK_EQUAL(s, result);

    // expected result for tests 2:
    sparse_series<int> result2;
    make_ordered_inserter(result2)
        (18, 1)
        (2,  81)
        (18, 91)
    .commit();

    piecewise_constant_series<int> p;
    make_ordered_inserter(p)
        (2, 0, 10)
        (2, 90, 100)
    .commit();
    sparse_series<int> s2 = period_sums(p, 1, 10);
    BOOST_CHECK_EQUAL(s2, result2);
}

///////////////////////////////////////////////////////////////////////////////
// test_period_sums_float
//
void test_period_sums_float()
{
    using namespace time_series;

    piecewise_constant_series<double,int,double> pwc;
    make_ordered_inserter(pwc)
        (3., 0.5, 11.5)
        (2., 90., 100.)
    .commit();

    sparse_series<double> expected;
    make_ordered_inserter(expected)
        (30., 1)
        (1.5, 11)
        (2.,  81)
        (18., 91)
    .commit();

    sparse_series<double> actual = period_sums(pwc, 1, 10);
    BOOST_CHECK(is_close(expected, actual, 0.1));
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("period_sums test");

    test->add(BOOST_TEST_CASE(&test_period_sums));
    test->add(BOOST_TEST_CASE(&test_period_sums_float));

    return test;
}


#pragma once
#include <Eigen/Dense>
#include <ensenso/nxLib.h>
#include <cstdint>
#include "util.hpp"

namespace dr {

/// Convert an NxLibItem holding a 3D vector to an Eigen::Vector3d.
Result<Eigen::Vector3d> toEigenVector(NxLibItem const & item);

/// Convert an NxLibItem holding a 3x3 matrix to an Eigen::Matrix3d.
Result<Eigen::Matrix3d> toEigenMatrix(NxLibItem const & item);

/// Convert an NxLibItem holding a 3D vector to an Eigen::Translation3d.
Result<Eigen::Translation3d> toEigenTranslation(NxLibItem const & item);

/// Convert an NxLibItem holding a rotation to an Eigen::AngleAxisd.
Result<Eigen::AngleAxisd> toEigenRotation(NxLibItem const & item);

/// Convert an NxLibItem holding a pose or transformation to an Eigen::Isometry3d.
Result<Eigen::Isometry3d> toEigenIsometry(NxLibItem const & item);

/// Set an Eigen::Vector3d in an NxLibItem.
Result<void> setNx(NxLibItem const & item, Eigen::Vector3d const & vector);

/// Set an Eigen::Vector3d in an NxLibItem.
Result<void> setNx(NxLibItem const & item, Eigen::Translation3d const & translation);

/// Set an Eigen::Vector3d in an NxLibItem.
Result<void> setNx(NxLibItem const & item, Eigen::AngleAxisd const & rotation);

/// Set an Eigen::Vector3d in an NxLibItem.
Result<void> setNx(NxLibItem const & item, Eigen::Isometry3d const & isometry);

template <std::size_t rows, std::size_t cols>
Result<Eigen::Matrix<double, rows, cols>> toEigenMatrix(NxLibItem const & item) {
	Eigen::Matrix<double, rows, cols> result;

	for (std::size_t row = 0; row < rows; ++row) {
		for (std::size_t col = 0; col < cols; ++col) {
			Result<double> value = getNx<double>(item[col][row]);
			if (!value) {
				return value.error().push_description("failed to retrieve matrix: ");
			}
			result(row, col) = *value;
		}
	}

	return result;
}

}


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

#ifndef ALS_VERTEX_PROGRAM_HPP
#define ALS_VERTEX_PROGRAM_HPP

/**
 * \file
 * \ingroup toolkit_matrix_factorization
 *
 * \brief This file describes the vertex program for the alternating
 * least squares (ALS) matrix factorization algorithm.  See
 * \ref als_vertex_program for description of the ALS Algorithm.
 */

#include <Eigen/Dense>

//#include <graphlab.hpp>

//#include "eigen_serialization.hpp"

typedef Eigen::VectorXd vec_type;
typedef Eigen::MatrixXd mat_type;

/**
 * \ingroup toolkit_matrix_factorization
 *
 * \brief the vertex data type which contains the latent factor.
 *
 * Each row and each column in the matrix corresponds to a different
 * vertex in the ALS graph.  Associated with each vertex is a factor
 * (vector) of latent parameters that represent that vertex.  The goal
 * of the ALS algorithm is to find the values for these latent
 * parameters such that the non-zero entries in the matrix can be
 * predicted by taking the dot product of the row and column factors.
 */
struct vertex_data {
  /**
   * \brief A shared "constant" that specifies the number of latent
   * values to use.
   */
  static size_t NLATENT;
  /** \brief The number of times this vertex has been updated. */
  uint32_t nupdates;
  /** \brief The most recent L1 change in the factor value */
  float residual; //! how much the latent value has changed
  /** \brief The latent factor for this vertex */
  vec_type factor;
  /**
   * \brief Simple default constructor which randomizes the vertex
   *  data
   */
  vertex_data() : nupdates(0), residual(1) { randomize(); }
  /** \brief Randomizes the latent factor */
  void randomize() { factor.resize(NLATENT); factor.setRandom(); }
  /** \brief Save the vertex data to a binary archive */
  //void save(graphlab::oarchive& arc) const {
  //  arc << nupdates << residual << factor;
  //}
  /** \brief Load the vertex data from a binary archive */
  //void load(graphlab::iarchive& arc) {
  //  arc >> nupdates >> residual >> factor;
  //}
}; // end of vertex data

/**
 * \brief The edge data stores the entry in the matrix.
 *
 * In addition the edge data also stores the most recent error estimate.
 */
struct edge_data : public graphlab::IS_POD_TYPE {
  /**
   * \brief The type of data on the edge;
   *
   * \li *Train:* the observed value is correct and used in training
   * \li *Validate:* the observed value is correct but not used in training
   * \li *Predict:* The observed value is not correct and should not be
   *        used in training.
   */
  enum data_role_type { TRAIN, VALIDATE, PREDICT  };

  /** \brief the observed value for the edge */
  float obs;

  /** \brief The train/validation/test designation of the edge */
  data_role_type role;

  /** \brief basic initialization */
  edge_data(float obs = 0, data_role_type role = PREDICT) :
    obs(obs), role(role) { }

}; // end of edge data

/**
 * \brief The graph type is defined in terms of the vertex and edge
 * data.
 */
typedef graphlab::distributed_graph<vertex_data, edge_data> graph_type;

/**
 * \brief Given a vertex and an edge return the other vertex in the
 * edge.
 */
inline graph_type::vertex_type
get_other_vertex(graph_type::edge_type& edge,
                 const graph_type::vertex_type& vertex) {
  return vertex.id() == edge.source().id()? edge.target() : edge.source();
}; // end of get_other_vertex

/**
 * \brief Given an edge compute the error associated with that edge
 */
double extract_l2_error(const graph_type::edge_type & edge) {
  const double pred =
    edge.source().data().factor.dot(edge.target().data().factor);
  return (edge.data().obs - pred) * (edge.data().obs - pred);
} // end of extract_l2_error

/**
 * \brief The graph loader function is a line parser used for
 * distributed graph construction.
 */
// Commented out for graphchi
/*
inline bool graph_loader(graph_type& graph,
                         const std::string& filename,
                         const std::string& line) {
  ASSERT_FALSE(line.empty());
  // Determine the role of the data
  edge_data::data_role_type role = edge_data::TRAIN;
  if(boost::ends_with(filename,".validate")) role = edge_data::VALIDATE;
  else if(boost::ends_with(filename, ".predict")) role = edge_data::PREDICT;
  // Parse the line
  std::stringstream strm(line);
  graph_type::vertex_id_type source_id(-1), target_id(-1);
  float obs(0);
  strm >> source_id >> target_id;
  if(role == edge_data::TRAIN || role == edge_data::VALIDATE) strm >> obs;
  // Create an edge and add it to the graph
  graph.add_edge(source_id, target_id+1000000, edge_data(obs, role));
  return true; // successful load
} // end of graph_loader

*/

/**
 * \brief The gather type used to construct XtX and Xty needed for the ALS
 * update
 *
 * To compute the ALS update we need to compute the sum of
 * \code
 *  sum: XtX = nbr.factor.transpose() * nbr.factor
 *  sum: Xy  = nbr.factor * edge.obs
 * \endcode
 * For each of the neighbors of a vertex.
 *
 * To do this in the Gather-Apply-Scatter model the gather function
 * computes and returns a pair consisting of XtX and Xy which are then
 * added. The gather type represents that tuple and provides the
 * necessary gather_type::operator+= operation.
 *
 */
class gather_type {
public:
  /**
   * \brief Stores the current sum of nbr.factor.transpose() *
   * nbr.factor
   */
  mat_type XtX;

  /**
   * \brief Stores the current sum of nbr.factor * edge.obs
   */
  vec_type Xy;

  /** \brief basic default constructor */
  gather_type() { }

  /**
   * \brief This constructor computes XtX and Xy and stores the result
   * in XtX and Xy
   */
  gather_type(const vec_type& X, const double y) :
    XtX(X.size(), X.size()), Xy(X.size()) {
    XtX.triangularView<Eigen::Upper>() = X * X.transpose();
    Xy = X * y;
  } // end of constructor for gather type

  /** \brief Save the values to a binary archive */
//  void save(graphlab::oarchive& arc) const { arc << XtX << Xy; }

  /** \brief Read the values from a binary archive */
 // void load(graphlab::iarchive& arc) { arc >> XtX >> Xy; }

  /**
   * \brief Computes XtX += other.XtX and Xy += other.Xy updating this
   * tuples value
   */
  gather_type& operator+=(const gather_type& other) {
    if(other.Xy.size() == 0) {
      ASSERT_EQ(other.XtX.rows(), 0);
      ASSERT_EQ(other.XtX.cols(), 0);
    } else {
      if(Xy.size() == 0) {
        ASSERT_EQ(XtX.rows(), 0);
        ASSERT_EQ(XtX.cols(), 0);
        XtX = other.XtX; Xy = other.Xy;
      } else {
        XtX.triangularView<Eigen::Upper>() += other.XtX;
        Xy += other.Xy;
      }
    }
    return *this;
  } // end of operator+=

}; // end of gather type

/**
 * ALS vertex program type
 */
class als_vertex_program :
  public graphlab::ivertex_program<graph_type, gather_type,
                                   graphlab::messages::sum_priority>,
  public graphlab::IS_POD_TYPE {
public:
  /** The convergence tolerance */
  static double TOLERANCE;
  static double LAMBDA;
  static size_t MAX_UPDATES;

  /** The set of edges to gather along */
  edge_dir_type gather_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }; // end of gather_edges

  /** The gather function computes XtX and Xy */
  gather_type gather(icontext_type& context, const vertex_type& vertex,
                     edge_type& edge) const {
    if(edge.data().role == edge_data::TRAIN) {
      const vertex_type other_vertex = get_other_vertex(edge, vertex);
      return gather_type(other_vertex.data().factor, edge.data().obs);
    } else return gather_type();
  } // end of gather function

  /** apply collects the sum of XtX and Xy */
  void apply(icontext_type& context, vertex_type& vertex,
             const gather_type& sum) {
    // Get and reset the vertex data
    vertex_data& vdata = vertex.data();
    // Determine the number of neighbors.  Each vertex has only in or
    // out edges depending on which side of the graph it is located
    if(sum.Xy.size() == 0) { vdata.residual = 0; ++vdata.nupdates; return; }
    mat_type XtX = sum.XtX;
    vec_type Xy = sum.Xy;
    // Add regularization
    for(int i = 0; i < XtX.rows(); ++i) XtX(i,i) += LAMBDA; // /nneighbors;
    // Solve the least squares problem using eigen ----------------------------
    const vec_type old_factor = vdata.factor;
    vdata.factor = XtX.selfadjointView<Eigen::Upper>().ldlt().solve(Xy);
    // Compute the residual change in the factor factor -----------------------
    vdata.residual = (vdata.factor - old_factor).cwiseAbs().sum() / XtX.rows();
    ++vdata.nupdates;
  } // end of apply

  /** The edges to scatter along */
  edge_dir_type scatter_edges(icontext_type& context,
                              const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }; // end of scatter edges

  /** Scatter reschedules neighbors */
  void scatter(icontext_type& context, const vertex_type& vertex,
               edge_type& edge) const {
  /*  edge_data& edata = edge.data();
    if(edata.role == edge_data::TRAIN) {
      const vertex_type other_vertex = get_other_vertex(edge, vertex);
      const vertex_data& vdata = vertex.data();
      const vertex_data& other_vdata = other_vertex.data();
      const double pred = vdata.factor.dot(other_vdata.factor);
      const float error = std::fabs(edata.obs - pred);
      const double priority = (error * vdata.residual);
      // Reschedule neighbors ------------------------------------------------
      if( priority > TOLERANCE && other_vdata.nupdates < MAX_UPDATES)
        context.signal(other_vertex, priority);
    }*/
  } // end of scatter function

  /**
   * \brief Signal all vertices on one side of the bipartite graph
   */
  static graphlab::empty signal_left(icontext_type& context,
                                     vertex_type& vertex) {
    if(vertex.num_out_edges() > 0) context.signal(vertex);
    return graphlab::empty();
  } // end of signal_left

}; // end of als vertex program

struct error_aggregator : public graphlab::IS_POD_TYPE {
  typedef als_vertex_program::icontext_type icontext_type;
  typedef graph_type::edge_type edge_type;
  double train_error, validation_error;
  size_t ntrain, nvalidation;
  error_aggregator() :
    train_error(0), validation_error(0), ntrain(0), nvalidation(0) { }
  error_aggregator& operator+=(const error_aggregator& other) {
    train_error += other.train_error;
    validation_error += other.validation_error;
    ntrain += other.ntrain;
    nvalidation += other.nvalidation;
    return *this;
  }
  static error_aggregator map(icontext_type& context, const graph_type::edge_type& edge) {
    error_aggregator agg;
    if(edge.data().role == edge_data::TRAIN) {
      agg.train_error = extract_l2_error(edge); agg.ntrain = 1;
    } else if(edge.data().role == edge_data::VALIDATE) {
      agg.validation_error = extract_l2_error(edge); agg.nvalidation = 1;
    }
    return agg;
  }
  static void finalize(icontext_type& context, error_aggregator& agg) {
    ASSERT_GT(agg.ntrain, 0);
    agg.train_error = std::sqrt(agg.train_error / agg.ntrain);
    context.cout() << context.elapsed_seconds() << "\t" << agg.train_error;
    if(agg.nvalidation > 0) {
      const double validation_error =
        std::sqrt(agg.validation_error / agg.nvalidation);
      context.cout() << "\t" << validation_error;
    }
    context.cout() << std::endl;
  }
}; // end of error aggregator

struct prediction_saver {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  std::string save_vertex(const vertex_type& vertex) const {
    return ""; //nop
  }
  std::string save_edge(const edge_type& edge) const {
    std::stringstream strm;
    const double prediction =
      edge.source().data().factor.dot(edge.target().data().factor);
    strm << edge.source().id() << '\t'
         << edge.target().id() << '\t'
         << prediction << '\n';
    return strm.str();
  }
}; // end of prediction_saver

#endif


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>
#include <boost/container/flat_map.hpp>
#include <CGAL/Polygon_mesh_processing/corefinement.h>

#include <fstream>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Surface_mesh<K::Point_3>             Mesh;

namespace PMP = CGAL::Polygon_mesh_processing;

struct Visitor :
  public PMP::Corefinement::Default_visitor<Mesh>
{
  typedef Mesh::Face_index face_descriptor;

  boost::container::flat_map<const Mesh*, Mesh::Property_map<Mesh::Face_index, int> > properties;
  int face_id;

  Visitor()
  {
    properties.reserve(3);
    face_id=-1;
  }

// visitor API overloaded
  void before_subface_creations(face_descriptor f_split,Mesh& tm)
  {
    face_id = properties[&tm][f_split];
  }

  void after_subface_created(face_descriptor f_new,Mesh& tm)
  {
    properties[&tm][f_new] = face_id;
  }

  void after_face_copy(face_descriptor f_src, Mesh& tm_src,
                       face_descriptor f_tgt, Mesh& tm_tgt)
  {
    properties[&tm_tgt][f_tgt] = properties[&tm_src][f_src];
  }
};

int main(int argc, char* argv[])
{
  const char* filename1 = (argc > 1) ? argv[1] : "data/blobby.off";
  const char* filename2 = (argc > 2) ? argv[2] : "data/eight.off";
  std::ifstream input(filename1);

  Mesh mesh1, mesh2;
  if (!input || !(input >> mesh1))
  {
    std::cerr << "First mesh is not a valid off file." << std::endl;
    return 1;
  }
  input.close();
  input.open(filename2);
  if (!input || !(input >> mesh2))
  {
    std::cerr << "Second mesh is not a valid off file." << std::endl;
    return 1;
  }

  Mesh out;

  // add a property in each mesh to track the parent mesh for each face of the output
  Mesh::Property_map<Mesh::Face_index, int>
    mesh1_id = mesh1.add_property_map<Mesh::Face_index, int>("f:id", -1).first,
    mesh2_id = mesh2.add_property_map<Mesh::Face_index, int>("f:id", -1).first,
    out_id = out.add_property_map<Mesh::Face_index, int>("f:id", -1).first;

  // init the face ids (for the purpose of the example but choosing 1 (2) as default value of the map would avoid the loop)
  for(Mesh::Face_index f : faces(mesh1))
    mesh1_id[f] = 1;
  for(Mesh::Face_index f : faces(mesh2))
    mesh2_id[f] = 2;

  Visitor visitor;
  visitor.properties[&mesh1] = mesh1_id;
  visitor.properties[&mesh2] = mesh2_id;
  visitor.properties[&out] = out_id;

  bool valid_union = PMP::corefine_and_compute_union(mesh1, mesh2, out, PMP::parameters::visitor(visitor));

  for(Mesh::Face_index f : faces(mesh1))
    assert( mesh1_id[f] == 1 );
  for(Mesh::Face_index f : faces(mesh2))
    assert( mesh2_id[f] == 2 );
  for(Mesh::Face_index f : faces(out))
    assert( out_id[f]==1 || out_id[f]==2);

  if (valid_union)
  {
    std::cout << "Union was successfully computed\n";
    std::ofstream output("union.off");
    output << out;
    return 0;
  }

  std::cout << "Union could not be computed\n";
  return 1;
}


//==============================================================================
//          Copyright 2014    Jean-Thierry Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_CUMMAX_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_CUMMAX_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <boost/simd/operator/functions/plus.hpp>
#include <boost/simd/include/functions/scalar/max.hpp>
#include <boost/simd/constant/constants/minf.hpp>

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief cummax generic tag

      Represents the cummax function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct cummax_ : ext::cumulative_<cummax_, tag::max_, tag::Minf>
    {
      /// @brief Parent hierarchy
      typedef ext::cumulative_<cummax_, tag::max_, tag::Minf> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_cummax_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::cummax_, Site> dispatching_cummax_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::cummax_, Site>();
    }
    template<class... Args>
    struct impl_cummax_;
  }
  /*!
    @brief compute the cumulate maximum of the vector elements

    @par Semantic:

    For every parameter of type T0

    @code
    T0 r = cummax(a0);
    @endcode

    is similar to:

    @code
    T r =x;
    for(int i=0;i < T::static_size; ++i)
      r[i] += max(r[i-1], r[i]);
    @endcode

    @param a0

    @return a value of the same type as the second parameter
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::cummax_, cummax, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::cummax_, cummax, 2)
} }

#endif