#include "common.hpp"

#include <cstdio>
#include <cstdlib>
#include <string>
#include <random>
#include <map>
#include <cassert>

#include <sys/stat.h>

// eigen
#include <Eigen/Core>

#define rep(i,n) for(int (i)=0;(i)<(n);(i)++)

using namespace Eigen;
using namespace std;

constexpr int BUF_SIZE = 1000;

//using p_t = pair<int, int>;
//using r_t = vector<map<p_t, double> >;
using mat_t = MatrixXd;
//using vec_t = VectorXd;
//using vec_mat_t = vector<mat_t>;

std::mt19937_64 engine;//(seed_gen());
std::uniform_real_distribution<> unifrom_real_dist(0.0, 1.0);

using init_prob_t = vector<double>;

double uniform_real_rand(){
  return unifrom_real_dist(engine);
}

void seeding(unsigned seed = 0){
  //std::random_device seed_gen;
  //engine = std::mt19937_64(seed_gen());
  engine = std::mt19937_64(seed);
}

map<int, string> read_poi_dic(const string& infile){
  map<int, string> res;
  ifstream ifs(infile);
  string line;
  getline(ifs, line); // header
  while(getline(ifs, line)){
    auto row = split(line);
    assert(row.size() >= 3);
    int index;
    sscanf(row[1].c_str(), "%d", &index);
    res[index] = row[2];
  }
  return res;
}

bool dir_exists(const string& path){
  struct stat sb;
  return stat(path.c_str(), &sb) == 0 && S_ISDIR(sb.st_mode);
}

int str2i(const string& s){
  int x;
  sscanf(s.c_str(), "%d", &x);
  return x;
}

double str2f(const string& s){
  double x;
  sscanf(s.c_str(), "%lf", &x);
  return x;
}

template<typename T>
struct sparse_mat{
  int m_, n_;
  map<pair<int,int>, T> mat_;
  map<int, T> row_sum_;
  sparse_mat(int m, int n):m_(m),n_(n),mat_(),row_sum_(){}
  void set(int i, int j, T val){
    auto key = make_pair(i,j);
    if(row_sum_.find(i) == row_sum_.end()){
      row_sum_[i] = 0;
    }else{
      row_sum_[i] -= mat_[key];
    }
    mat_[key] = val;
    row_sum_[i] += val;
  }
  T get(int i, int j){
    auto key = make_pair(i, j);
    if(mat_.find(key) == mat_.end()){return 0;}
    return mat_[key];
  }
  T row_sum(int i){
    if(row_sum_.find(i) == row_sum_.end()){return 0;}
    return row_sum_[i];
  }
};

using smat_t = sparse_mat<double>;

/*
################## Train parameters in the generative model ###################
# [output1]: init_prob (M-dim vector)
# [output2]: trans_count ({(time_slot, poi_index_from, poi_index_to): count})
# [output3]: trans_prob (T x (M x M matrix))
# [output4]: copy_train_data (N x CopyNum matrix)
 */
tuple<init_prob_t, map<tuple<int,int,int>, int>, vector<smat_t>, map<pair<int,int>, int>>
TrainParam(int M, int T, int TimeType, int CopyNum, int MaxTimInt,
		const string& TrainTraceFile){
  // # Read a training trace file
  vector<int> init_count(M, 0);
  int init_count_sum = 0;
  map<tuple<int,int,int>, int> trans_count;
  map<pair<int,int>, int> copy_train_data;
  {
    string infile = TrainTraceFile;
    ifstream ifs(infile);
    string line;
    getline(ifs, line); // header
    int user_index_prev = -1;
    int poi_index_prev = 0;
    int unixtime_prev = 0;
    int time_ins_prev = 0;
    while(getline(ifs, line)){
      auto lst = split(line);
      int user_index = str2i(lst[0]);
      int poi_index = str2i(lst[1]);
      int unixtime = str2f(lst[3]);
      int ho = str2i(lst[5]);
      // # Time slot and time instant --> time_slot, time_ins
      int time_slot, time_ins;
      if(TimeType == 1){
	int x = str2i(lst[6]);
	int mi = 2;
	if(x >= 40){
	}else if(x >= 20){
	  mi = 1;
	}else{
	  mi = 0;
	}
	time_slot = 3 * (ho - 9) + mi;
	time_ins = time_slot;
      }else if(TimeType == 2){
	time_slot = ho/2;
	time_ins = ho;
      }else{
	fprintf(stderr, "Wrong TimeType.\n");
	exit(1);
      }
      // # Update copy_train_data for the first CopyNum time instants
      if(time_ins < CopyNum){
	copy_train_data[make_pair(user_index, time_ins)] = poi_index;
      }
      // # Update trans_count if the event and the previous event are from the same user
      if(user_index == user_index_prev){
	// # Consider only temporally-continuous locations within MaxTimInt for a transition
	if( (MaxTimInt == -1 || (unixtime - unixtime_prev <= MaxTimInt) ) && (time_ins - time_ins_prev == 1) ){
	  auto key = make_tuple(time_slot, poi_index_prev, poi_index);
	  if(trans_count.find(key) == trans_count.end()){trans_count[key] = 0;}
	  trans_count[key] += 1;
	}
      }
      // # Update init_count if the event is in the first time slot
      if(time_slot == 0){init_count[poi_index] += 1; ++init_count_sum;}
      user_index_prev = user_index;
      poi_index_prev = poi_index;
      unixtime_prev = unixtime;
      time_ins_prev = time_ins;
    }
  }

  // # Make an init probability vector --> init_prob
  init_prob_t init_prob(M, 0);
  rep(i, M){
    init_prob[i] = static_cast<double>(init_count[i]) / init_count_sum;
  }

  // # Make a count sum vector --> count_sum
  mat_t count_sum = mat_t::Zero(T, M);
  for(const auto& it : trans_count){
    const auto& key = it.first;
    auto& counts = it.second;
    auto& time_slot = get<0>(key);
    auto& poi_index_prev = get<1>(key);
    count_sum(time_slot, poi_index_prev) += counts;
  }

  // # Make a transition probability matrix --> trans_prob
  vector<smat_t> trans_prob;
  rep(i, T){trans_prob.push_back(smat_t(M, M));}
  for(const auto& it : trans_count){
    const auto& key = it.first;
    auto& counts = it.second;
    auto& time_slot = get<0>(key);
    auto& poi_index_prev = get<1>(key);
    auto& poi_index = get<2>(key);
    auto cs = count_sum(time_slot, poi_index_prev);
    if(cs > 0){
      trans_prob[time_slot].set(poi_index_prev, poi_index, counts / cs);
    }
  }

  // # Assign a uniform distribution for a row of zero-values --> trans_prob
  rep(time_slot, T){
    rep(i, M){
      if(trans_prob[time_slot].row_sum(i) == 0){
	rep(j, M){
	  trans_prob[time_slot].set(i, j, 1.0 / M);
	}
      }
    }
  }
  return forward_as_tuple(init_prob, trans_count, trans_prob, copy_train_data);
}

/*
############################## Synthesize traces ##############################
# [input1]: init_prob (M-dim vector)
# [input2]: trans_prob (T x (M x M matrix))
# [input3]: copy_train_data (N x CopyNum matrix)
# [input4]: poi_dic ({poi_index: category})
 */
void SynTraces(init_prob_t& init_prob,
	       vector<smat_t>& trans_prob,
	       map<pair<int,int>, int>& copy_train_data,
	       map<int, string>& poi_dic,
	       const string& SynTraceFile, int CopyNum,
	       int N, int M, int T, int TraceNum, int TimInsNum){
  char buf[BUF_SIZE];
  sprintf(buf, "%s_cn%d.csv", SynTraceFile.c_str(), CopyNum);
  // # Output header information
  string outfile = buf;
  FILE* fp = fopen(outfile.c_str(), "w");
  fprintf(fp, "user,trace_no,time_slot,time_instant,poi_index,category\n");
  // # For each user
  rep(n, N){
    if(n % 1000 == 0){printf("Synthesized traces of %d users.\n", n);}
    // ########################## Synthesize traces ##########################
    int poi_index_pre = 0;
    // # For each trace
    rep(trace_no, TraceNum){
      // # For each time slot
      rep(t, T){
	// # For each time instant
	rep(ins, TimInsNum){
	  int tim = t * TimInsNum + ins;
	  int poi_index = -1;
	  if(CopyNum == 0 && tim == 0){
	    // # Randomly sample POI from init_prob
	    double rnd = uniform_real_rand();
	    double prob_sum = 0;
	    rep(i, M){
	      prob_sum += init_prob[i];
	      if(prob_sum >= rnd){poi_index = i; break;}
	    }
	  }else if(tim < CopyNum && copy_train_data.find(make_pair(n,tim)) != copy_train_data.end()){
	    // # Copy the POI from the training data
	    poi_index = copy_train_data[make_pair(n,tim)];
	  }else{
	    // # Randomly sample POI from trans_prob
	    // # Transform poi_index_pre into poi_index via trans_vec
	    double rnd = uniform_real_rand();
	    double prob_sum = 0;
	    rep(j, M){
	      prob_sum += trans_prob[t].get(poi_index_pre,j);
	      if(prob_sum >= rnd){poi_index = j; break;}
	    }
	  }
	  assert(poi_index >= 0);
	  // # Output an initial location ([user, trace_no, time_slot, time_instant, poi_index, category])
	  fprintf(fp, "%d,%d,%d,%d,%d,%s\n", n, trace_no, t, ins, poi_index, poi_dic[poi_index].c_str());
	  // # Save the previous poi_index
	  poi_index_pre = poi_index;
	}
      }
    }
  }
  fclose(fp);
}

int main(int argc, char* argv[]){
  if(argc < 3){
    fprintf(stderr, "Usage: %s [Dataset] [City] ([TraceNum (default:10)] [CopyNum (default:0)])\n", argv[0]);
    exit(1);
  }
  // # Dataset (PF/FS)
  string DataSet = argv[1];
  // # City
  string City = argv[2];
  // # Number of traces per user
  int TraceNum = 10;
  if(argc >= 4){sscanf(argv[3], "%d", &TraceNum);}
  // # Number of locations copied from the training trace (DataSet = PF)
  int CopyNum = 0;
  if(DataSet == "PF" && argc >= 5){sscanf(argv[4], "%d", &CopyNum);}

  // # Data directory
  string DataDir = "../data/" + DataSet + "/";
  // # Training user index file (input)
  string TUserIndexFile = DataDir + "tuserindex_%s.csv";
  // # POI index file (input)
  string POIIndexFile = DataDir + "POIindex_%s.csv";
  // # Training trace file (input)
  string TrainTraceFile = DataDir + "traintraces_%s.csv";
  // # Type of time slots (1: 9-19h, 20min, 2: 2 hours, )
  int TimeType = 1;
  if(DataSet.find("PF") == 0){
  }else if(DataSet.find("FS") == 0){
    TimeType = 2;
  }else{
    fprintf(stderr, "Wrong Dataset\n");
    exit(1);
  }
  // # Output directory
  string OutDir = DataDir + "SGD_" + City + "/";
  if(!dir_exists(OutDir)){
    //fprintf(stderr, "%s does not exist.\n", OutDir.c_str());
    mode_t mode = S_IRWXU | S_IRGRP | S_IXGRP | S_IROTH | S_IXOTH;
    int res = mkdir(OutDir.c_str(), mode);
  }

  // # Prefix of the synthesized trace file (output)
  string SynTraceFile = OutDir + "syntraces";

  // # Number of time instants per time slot
  int TimInsNum = 1;
  if(DataSet.find("PF") == 0){
  }else if(DataSet.find("FS") == 0){
    TimInsNum = 2;
  }else{
    exit(1);
  }

  // # Maximum time interval between two temporally-continuous locations (sec) (-1: none)
  int MaxTimInt = -1;
  if(DataSet.find("PF") == 0){
  }else if(DataSet.find("FS") == 0){
    MaxTimInt = 7200;
  }else{
    fprintf(stderr, "Wrong Dataset\n");
    exit(1);
  }

  // # Fix a seed
  seeding(1);
  // # Replace %s with City
  TUserIndexFile = string_replace(TUserIndexFile, City);
  POIIndexFile = string_replace(POIIndexFile, City);
  TrainTraceFile = string_replace(TrainTraceFile, City);
  // # Number of training users --> N
  int N = line_num(TUserIndexFile) - 1;
  // # Number of POIs --> M
  int M = line_num(POIIndexFile) - 1;
  // # Number of time slots --> T
  int T = 30;
  if(TimeType == 1){
  }else if(TimeType == 2){
    T = 12;
  }else{
    fprintf(stderr, "Wrong TimeType.\n");
    exit(1);
  }
  // # Read the POI index file --> poi_dic ({poi_index: category})
  auto poi_dic = read_poi_dic(POIIndexFile);

  // # Train parameters in the generative model
  init_prob_t init_prob;
  map<tuple<int,int,int>, int> trans_count;
  vector<smat_t> trans_prob;
  map<pair<int,int>, int> copy_train_data;
  //fprintf(stderr, "M =%d, T = %d\n", M, T);
  tie(init_prob, trans_count, trans_prob, copy_train_data) = TrainParam(M, T, TimeType, CopyNum, MaxTimInt, TrainTraceFile);

  // # Synthesize traces
  SynTraces(init_prob, trans_prob, copy_train_data, poi_dic,
	    SynTraceFile, CopyNum, N, M, T, TraceNum, TimInsNum);
  return 0;
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/bitinteger.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/bitincrement.hpp>
#include <boost/simd/constant/valmax.hpp>

STF_CASE_TPL (" bit integerreal",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::bitinteger;
  using r_t = decltype(bitinteger(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bd::as_integer_t<T>);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(bitinteger(bs::Nan<T>()), -bs::Valmax<r_t>());
#endif
#if !defined(BOOST_SIMD_NO_DENORMALS)
  STF_EQUAL(bitinteger(bs::Bitincrement<T>()), bs::One<r_t>());
  STF_EQUAL(bitinteger(-bs::Bitincrement<T>()), bs::Mone<r_t>());
#endif
  STF_EQUAL(bitinteger(bs::Zero<T>()), bs::Zero<r_t>());
}


#include <iostream>
#include <memory>
#include <tuple>
#include <stdlib.h>

#include <boost/optional.hpp>

// lvr2 includes
#include "lvr2/registration/TransformUtils.hpp"
#include "lvr2/types/MatrixTypes.hpp"

int main(int argc, char** argv)
{
    std::cout << "Coordinate Example" << std::endl;
    // Overview Coordinate Systems

    /** LVR / ROS
    *        z  x
    *        | /
    *   y ___|/
    *
    *  - x: front
    *  - y: left
    *  - z: up
    *  - scale: m
    */

    /** OpenCV
     *
     *        z
     *       /
     *      /___ x
     *     |
     *     |
     *     y
     *
     * - x: right
     * - y: down
     * - z: front
     * - scale: m
     */

    // x: 2.0, y: 0.5, z: 1.0
    // front: 1.0, right: 2.0, down: 0.5
    lvr2::Vector3d cv_point = {2.0, 0.5, 1.0};
    std::cout << "cv point: " << cv_point.transpose() << std::endl;

    // convert to lvr
    // should be x(front): 1.0, y(left): -2.0, z(up): -0.5
    lvr2::Vector3d lvr_point = lvr2::openCvToLvr(cv_point);
    std::cout << "lvr point: " << lvr_point.transpose() << std::endl;

    if(lvr2::lvrToOpenCv(lvr_point) == cv_point)
    {
        std::cout << "LVR <-> OpenCV - Point: Success" << std::endl;
    }

    // check opencv transformation

    lvr2::Rotationd cv_rot, lvr_rot;

    double roll = -0.25*M_PI; // cv: z, lvr: x
    double pitch = 1.6*M_PI; // cv: -x, lvr: y
    double yaw = -0.06*M_PI; // cv: -y, lvr: z

    // cv rotate x: pitch
    cv_rot = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitZ())
        * Eigen::AngleAxisd(-pitch, Eigen::Vector3d::UnitX())
        * Eigen::AngleAxisd(-yaw, Eigen::Vector3d::UnitY());

    // cv -> lvr
    lvr_rot = lvr2::openCvToLvr(cv_rot);

    // cv_rot

    lvr2::Vector3d cv_point_rotated = cv_rot * cv_point;
    lvr2::Vector3d lvr_point_rotated = lvr_rot * lvr_point;

    // lvr2::openCvToLvr(cv_point_rotated) - lvr_point_rotated
    if((lvr2::openCvToLvr(cv_point_rotated) - lvr_point_rotated).norm() < 0.000001)
    {
        std::cout << "LVR <-> OpenCV - Rotation Matrix: Success" << std::endl;
    } else {
        std::cout << "LVR <-> OpenCV - Rotation Matrix: Wrong" << std::endl;
    }

    // transformation
    lvr2::Transformd cv_transform, lvr_transform;

    cv_transform = lvr2::Transformd::Identity();
    cv_transform.block<3,3>(0,0) = cv_rot;
    cv_transform(0,2) = 2.0;
    cv_transform(1,2) = 5.0;
    cv_transform(2,2) = -1.0;

    lvr_transform = lvr2::openCvToLvr(cv_transform);

    lvr2::Vector3d cv_point_transformed = cv_transform * cv_point;
    lvr2::Vector3d lvr_point_transformed = lvr_transform * lvr_point;

    if((lvr2::openCvToLvr(cv_point_transformed)-lvr_point_transformed).norm() < 0.000001)
    {
        std::cout << "LVR <-> OpenCV - Transformation Matrix: Success" << std::endl;
    } else {
        std::cout << "LVR <-> OpenCV - Transformation Matrix: Wrong" << std::endl;
    }

    return 0;
}


#pragma once

// deal.II includes ------------------------------------------------------------
#include <deal.II/base/timer.h>
#include <deal.II/base/utilities.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/trilinos_sparsity_pattern.h>
// system includes -------------------------------------------------------------
#include <algorithm>
#include <fstream>
#include <set>
#include <utility>
#include <vector>
// own includes ---------------------------------------------------------------
#include "aux/message.hpp"
#include "helpers.hpp"
#include "spectral/basis/global_indexer.hpp"
//#include "spectral/basis/spectral_function.hpp"
#include "matrix/assembly/velocity_var_form.hpp"
#include "matrix/dofs/dofindex_sets.hpp"
#include "matrix/sparsity_pattern/sparsity_pattern_base.hpp"

namespace boltzmann {

class SparsityPattern : public SparsityPatternBase
{
 private:
  typedef dealii::DoFHandler<2> dofhandler_t;
  typedef dealii::types::global_dof_index size_type;
  using SparsityPatternBase::sparsity_pattern_t;

 public:
  template <typename SPECTRAL_BASIS, typename INDEXER>
  void init(const SPECTRAL_BASIS& spectral_basis,
            const dofhandler_t& dof_handler,
            const sparsity_t& vsparsity,
            const INDEXER& indexer,
            const DoFIndexSetsBase& dof_map,
            const bool has_bc = true);

  virtual const sparsity_pattern_t& get_sparsity() const { return sparsity_pattern_; }

  virtual const dealii::SparsityPattern& get_L_sparsity() const { return L_sparsity_; }

  void init_trilinos_phys_sparsity(sparsity_pattern_t& sp, const DoFIndexSets& dof_map) const
  {
    unsigned int pid = dealii::Utilities::MPI::this_mpi_process(MPI_COMM_WORLD);
    const auto& local_idx = dof_map.locally_owned_phys_dofs(pid);
    sp.reinit(local_idx, phys_sparsity_);
  }

  const std::vector<bool>& get_boundary_vertex_indicator() const { return is_at_boundary; }

 private:
  sparsity_pattern_t sparsity_pattern_;
  //@{
  /// sparsity patterns in velocity dofs
  /// velocity nnz entries from differential operator L, stores (j1,j2)
  sparsity_t L_sparsity_;
  //@}
  dealii::SparsityPattern phys_sparsity_;
  // indicates boundary vertices
  std::vector<bool> is_at_boundary;
};

// ----------------------------------------------------------------------
template <typename SPECTRAL_BASIS, typename INDEXER>
void
SparsityPattern::init(const SPECTRAL_BASIS& polar_basis,
                      const dofhandler_t& dof_handler,
                      const sparsity_t& vsparsity,
                      const INDEXER& indexer,
                      const DoFIndexSetsBase& dof_partition,
                      const bool has_bc)
{
  unsigned int mpi_this_process = dealii::Utilities::MPI::this_mpi_process(MPI_COMM_WORLD);

  BAssertThrow(vsparsity.is_compressed(), "sparsity pattern must be compressed");

  // pepare sparsity patterns in velocity space
  L_sparsity_.copy_from(vsparsity);
  L_sparsity_.compress();

  // init_l_sparsity(polar_basis, velocity_var_form);

  const size_t N = polar_basis.n_dofs();
  const size_type L = dof_handler.n_dofs();
  // the sparsity pattern for the physical dofs
  dealii::DynamicSparsityPattern phys_block_sparsity(L, L);
  dealii::DoFTools::make_sparsity_pattern(dof_handler, phys_block_sparsity);
  // and now include the velocity degrees of freedom in the sparsity pattern
  phys_sparsity_.copy_from(phys_block_sparsity);
  phys_sparsity_.compress();

  // find vertices at boundary
  is_at_boundary = std::vector<bool>(dof_handler.n_dofs(), false);
  // build list of vertices at the boundary
  for (auto cell = dof_handler.begin_active(); cell != dof_handler.end(); ++cell) {
    if (cell->has_boundary_lines()) {
      for (unsigned int face_index = 0; face_index < dealii::GeometryInfo<2>::faces_per_cell;
           ++face_index) {  // iterate over faces of this cell
        if (cell->face(face_index)->at_boundary()) {
          for (unsigned int face_vertex = 0; face_vertex < 2; ++face_vertex) {
            is_at_boundary[cell->face(face_index)->vertex_dof_index(face_vertex, 0)] = true;
          }
        }
      }
    }
  }

  // count how many at boundary?
  unsigned int n_boundary = 0;
  std::for_each(is_at_boundary.begin(), is_at_boundary.end(), [&](bool v) {
    if (v) n_boundary += 1;
  });

  const auto& owned_dofs = dof_partition.locally_owned_dofs(mpi_this_process);
  const auto& relevant_x_dofs = dof_partition.locally_relevant_phys_dofs();
  const auto& relevant_dofs = dof_partition.locally_relevant_dofs();

  sparsity_pattern_.reinit(owned_dofs,    /* row parallel partitioning */
                           owned_dofs,    /* col parallel partitioning, dummy set */
                           relevant_dofs, /* writeable rows */
                           MPI_COMM_WORLD);
  std::vector<unsigned int> col_indices(N * phys_block_sparsity.max_entries_per_row());
  // Add nonzero entries from NON-boundary-DoFs
  for (size_t ix1 = 0; ix1 < L; ++ix1) {
    if (!relevant_x_dofs.is_element(indexer.to_restricted(ix1))) continue;
    //    if (subdomain_ids[ix1] != mpi_this_process) continue;
    for (size_t j1 = 0; j1 < N; ++j1) {
      // row index
      unsigned int ig1 = indexer.to_global(ix1, j1);
      // store nonzero col indices from current row into `col_indices`
      unsigned int rnnz = 0;
      for (auto ix2 = phys_sparsity_.begin(ix1); ix2 != phys_sparsity_.end(ix1); ++ix2) {
        if (has_bc && is_at_boundary[ix1] && is_at_boundary[ix2->column()]) {
          unsigned int col_idx = indexer.to_global(ix2->column(), 0);
          for (size_t j2 = 0; j2 < N; ++j2) {
            col_indices[rnnz + j2] = col_idx + j2;
          }
          rnnz += N;
        } else {
          for (auto j2 = L_sparsity_.begin(j1); j2 != L_sparsity_.end(j1); ++j2) {
            col_indices[rnnz++] = indexer.to_global(ix2->column(), j2->column());
          }
        }
      }
      sparsity_pattern_.add_entries(ig1, col_indices.data(), col_indices.data() + rnnz, true);
    }
  }
  sparsity_pattern_.compress();

  if (mpi_this_process == 0)
    std::cout << "\nglobal sparsity pattern #nnz: " << sparsity_pattern_.n_nonzero_elements()
              << std::endl;
}

}  // end namespace boltzmann


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_UNIT_ROLL_TEST_HPP_INCLUDED
#define BOOST_SIMD_UNIT_ROLL_TEST_HPP_INCLUDED

#include <boost/simd/swar/include/functions/shuffle.hpp>
#include <nt2/sdk/unit/tests.hpp>

//==============================================================================
template<class T, int IA, int IB>
struct roll_unary_test_2
{
  static void call( T a )
  {
    using boost::simd::shuffle;
    T ref, res;
    ref[0] = (IA == -1) ? 0 : a[IA];
    ref[1] = (IB == -1) ? 0 : a[IB];
    res = shuffle<IA,IB>(a);
    NT2_TEST_EQUAL(res,ref);
    roll_unary_test_2<T,IA+1,IB>::call(a);
  }
};

template<class T, int IB>
struct roll_unary_test_2<T,2,IB>
{
  static void call( T a ) { roll_unary_test_2<T,-1,IB+1>::call(a); }
};

template<class T>
struct roll_unary_test_2<T,-1,2> { static void call( T a ){} };

//==============================================================================
template<class T, int I0, int I1, int I2, int I3>
struct roll_unary_test_4
{
  static void call( T a )
  {
    using boost::simd::shuffle;
    T ref, res;
    ref[0] = (I0 == -1) ? 0 : a[I0];
    ref[1] = (I1 == -1) ? 0 : a[I1];
    ref[2] = (I2 == -1) ? 0 : a[I2];
    ref[3] = (I3 == -1) ? 0 : a[I3];
    res = shuffle<I0,I1,I2,I3>(a);
    NT2_TEST_EQUAL(res,ref);
    roll_unary_test_4<T,I0+1,I1,I2,I3>::call(a);
  }
};

template<class T, int I1, int I2, int I3>
struct roll_unary_test_4<T,4,I1,I2,I3>
{
  static void call( T a ) { roll_unary_test_4<T,-1,I1+1,I2,I3>::call(a); }
};

template<class T, int I2, int I3>
struct roll_unary_test_4<T,-1,4,I2,I3>
{
  static void call( T a ) { roll_unary_test_4<T,-1,-1,I2+1,I3>::call(a); }
};

template<class T, int I3>
struct roll_unary_test_4<T,-1,-1,4,I3>
{
  static void call( T a ) { roll_unary_test_4<T,-1,-1,-1,I3+1>::call(a); }
};

template<class T>
struct roll_unary_test_4<T,-1,-1,-1,4> { static void call( T a ){} };

//==============================================================================
template<class T, int IA, int IB>
struct roll_binary_test_2
{
  static void call( T a, T b)
  {
    using boost::simd::shuffle;
    T ref, res;
    ref[0] = (IA == -1) ? 0 : (IA<2 ? a[IA] : b[IA-2]);
    ref[1] = (IB == -1) ? 0 : (IB<2 ? a[IB] : b[IB-2]);
    res = shuffle<IA,IB>(a,b);
    NT2_TEST_EQUAL(res,ref);
    roll_binary_test_2<T,IA+1,IB>::call(a,b);
  }
};

template<class T, int IB>
struct roll_binary_test_2<T,4,IB>
{
  static void call( T a, T b)
  {
     roll_binary_test_2<T,-1,IB+1>::call(a,b);
  }
};

template<class T>
struct roll_binary_test_2<T,-1,4>
{
  static void call( T a, T b) {}
};

//==============================================================================
template<class T, int I0, int I1, int I2, int I3>
struct roll_binary_test_4
{
  static void call( T a, T b )
  {
    using boost::simd::shuffle;
    T ref, res;
    ref[0] = (I0 == -1) ? 0 : ((I0<4)?a[I0]:b[I0-4]);
    ref[1] = (I1 == -1) ? 0 : ((I1<4)?a[I1]:b[I1-4]);
    ref[2] = (I2 == -1) ? 0 : ((I2<4)?a[I2]:b[I2-4]);
    ref[3] = (I3 == -1) ? 0 : ((I3<4)?a[I3]:b[I3-4]);
    res = shuffle<I0,I1,I2,I3>(a,b);
    NT2_TEST_EQUAL(res,ref);
    roll_binary_test_4<T,I0+1,I1,I2,I3>::call(a,b);
  }
};

template<class T, int I1, int I2, int I3>
struct roll_binary_test_4<T,8,I1,I2,I3>
{
  static void call( T a, T b ) { roll_binary_test_4<T,-1,I1+1,I2,I3>::call(a,b); }
};

template<class T, int I2, int I3>
struct roll_binary_test_4<T,-1,8,I2,I3>
{
  static void call( T a, T b ) { roll_binary_test_4<T,-1,-1,I2+1,I3>::call(a,b); }
};

template<class T, int I3>
struct roll_binary_test_4<T,-1,-1,8,I3>
{
  static void call( T a, T b ) { roll_binary_test_4<T,-1,-1,-1,I3+1>::call(a,b); }
};

template<class T>
struct roll_binary_test_4<T,-1,-1,-1,8> { static void call( T a, T b ){} };

#endif


#include <I3Test.h>

#include <phys-services/I3SPRNGRandomService.h>
#include <icetray/I3Frame.h>
#include <dataclasses/I3Double.h>
#include <boost/foreach.hpp>
#include <fstream>
#include <stdio.h>

TEST_GROUP(RandomServiceState);

TEST(SPRNGInMemory)
{
	{
	I3SPRNGRandomServicePtr rng(new I3SPRNGRandomService(1,1,0, "", "sprng_outstate"));

	typedef std::pair<double, I3FrameObjectPtr> sample_t;
	std::vector<sample_t> states;

	for (int i = 0; i < 1000; i++) {
		I3FrameObjectPtr state = rng->GetState();
		double v = rng->Exp(M_PI);
		states.push_back(std::make_pair(v, state));
	}

	double chance = 0.1;
	BOOST_FOREACH(const sample_t &sample, states) {
		// Discard random intermediate points, mixing up
		// the RNG state in the process
		if (rng->Uniform() > chance)
			continue;
		rng->RestoreState(sample.second);
		ENSURE_EQUAL(sample.first, rng->Exp(M_PI), "Restored RNG returns the same sample");
	}

	// Now, restore from the state from a file, and make sure things still work
	// if we take more samples
	rng.reset();
	rng = I3SPRNGRandomServicePtr(new I3SPRNGRandomService(1,1,0, "sprng_outstate", "sprng_outstate"));

	for (int i = 0; i < 1000; i++) {
		I3FrameObjectPtr state = rng->GetState();
		double v = rng->Exp(M_PI);
		states.push_back(std::make_pair(v, state));
	}

	BOOST_FOREACH(const sample_t &sample, states) {
		// Discard random intermediate points, mixing up
		// the RNG state in the process
		if (rng->Uniform() > chance)
			continue;
		rng->RestoreState(sample.second);
		ENSURE_EQUAL(sample.first, rng->Exp(M_PI), "Restored RNG returns the same sample");
	}

	}
	std::remove("sprng_outstate");
}

TEST(SPRNGToFile)
{
	I3SPRNGRandomServicePtr rng(new I3SPRNGRandomService(1,1,0));

	typedef std::pair<double, I3FrameObjectPtr> sample_t;
	std::vector<sample_t> states;

	I3FramePtr frame(new I3Frame);

	{
		std::ofstream outfile("sprng.i3");
		for (int i = 0; i < 100; i++) {
			frame->clear();
			frame->Put("RNGState", rng->GetState());
			frame->Put("Variate", I3DoublePtr(new I3Double(rng->Exp(M_PI))));
			frame->save(outfile);
		}
	}

	{
		std::ifstream infile("sprng.i3");
		double chance = 0.1;
		for (int i = 0; i < 100; i++) {
			frame->clear();
			frame->load(infile);
			// Discard random intermediate points, mixing up
			// the RNG state in the process
			if (rng->Uniform() > chance)
				continue;
			rng->RestoreState(frame->Get<I3FrameObjectConstPtr>("RNGState"));
			I3DoubleConstPtr v = frame->Get<I3DoubleConstPtr>("Variate");
			ENSURE_EQUAL(v->value, rng->Exp(M_PI), "Restored RNG returns the same sample");
		}

	}

	std::remove("sprng.i3");
}


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

#ifndef CBR_CONTROL__MPC__MPC_TRACKING_HPP_
#define CBR_CONTROL__MPC__MPC_TRACKING_HPP_

#include <Eigen/Dense>
#include <cbr_utils/thread_pool.hpp>
#include <cbr_utils/utils.hpp>
#include <cbr_math/interp.hpp>

#include <utility>
#include <future>
#include <mutex>
#include <thread>

#include "cltv_ocp.hpp"
#include "cltv_ocp_lie.hpp"
#include "dltv_ocp.hpp"
#include "dltv_ocp_solver.hpp"
#include "ocp_common.hpp"

using std::chrono::duration_cast, std::chrono::duration, std::chrono::nanoseconds;

namespace cbr
{

/**
 * @brief Helper type for MPC trajectory tracking
 *
 * CltvOcp requires an interface with get_xxx(tau) methods where tau is
 * the time elapsed from the initial condition of the problem.
 *
 * This class takes functions of absolute time t and implements the required
 * interfaces as functions of relative time.
 */
template<typename _problem_t>
struct MPCTrackingProblem : public _problem_t
{
public:
  using typename _problem_t::state_t;
  using typename _problem_t::deriv_t;
  using typename _problem_t::input_t;

  explicit MPCTrackingProblem(double T, const _problem_t & p)
  : _problem_t(p), T_(T) {}

  explicit MPCTrackingProblem(double T, _problem_t && p)
  : _problem_t(std::move(p)), T_(T) {}

  /**
   * @brief Return the time horizon of the problem
   * @param[out] T result
   */
  double get_T() const {return T_;}

  /**
   * @brief Return the problem initial condition
   * @param[out] x0 result
   */
  const state_t & get_x0() const {return x0;}

  /**
   * @brief Get the desired state
   * @param tau relative problem time tau = t - t0
   * @return xd(t0 + tau)
   */
  state_t get_xd(double tau) const
  {
    return xd(t0 + duration_cast<nanoseconds>(duration<double>(tau)));
  }
  /**
   * @brief Get the desired input
   * @param tau relative problem time tau = t - t0
   * @return ud(t0 + tau)
   */
  input_t get_ud(double tau) const
  {
    return ud(t0 + duration_cast<nanoseconds>(duration<double>(tau)));
  }

  /**
   * @brief Get the state linearization
   * @param tau relative problem time tau = t - t0
   * @return xl(t0 + tau)
   */
  state_t get_xl(double tau) const
  {
    return xl(t0 + duration_cast<nanoseconds>(duration<double>(tau)));
  }
  /**
   * @brief Get the state derivative linearization
   * @param tau relative problem time tau = t - t0
   * @return xldot(t0 + tau)
   */
  deriv_t get_xldot(double tau) const
  {
    return xldot(t0 + duration_cast<nanoseconds>(duration<double>(tau)));
  }
  /**
   * @brief Get the input linearization
   * @param tau relative problem time tau = t - t0
   * @return ul(t0 + tau)
   */
  input_t get_ul(double tau) const
  {
    return ul(t0 + duration_cast<nanoseconds>(duration<double>(tau)));
  }

private:
  // These functions return are defined in absolute time
  std::function<state_t(nanoseconds)> xd = [](nanoseconds) {return state_t{};};
  std::function<input_t(nanoseconds)> ud = [](nanoseconds) {return input_t{};};
  std::function<state_t(nanoseconds)> xl = [](nanoseconds) {return state_t{};};
  std::function<deriv_t(nanoseconds)> xldot = [](nanoseconds) {return deriv_t{};};
  std::function<input_t(nanoseconds)> ul = [](nanoseconds) {return input_t{};};

  nanoseconds t0 = nanoseconds(0);
  state_t x0{};
  double T_{0};

  // MPCTracking modifies the private variables from outside (except T_)
  template<typename problem_t__, std::size_t nPts__>
  friend class MPCTracking;
};

struct MPCTrackingParams
{
  double T{10};  // MPC horizon
  DltvOcpSolverParams solver_params{};
};

// Trait to detect whether a problem is defined on a lie group
template<typename, typename = void>
struct has_tangent : std::false_type {};

template<typename T>
struct has_tangent<T, std::void_t<typename T::Tangent>>: std::true_type {};

/**
 * @brief Trajectory tracking with nonlinear MPC
 *
 * @tparam problem_t stationary problem definition with dynamics, weights, and state bounds
 * @tparam nPts_ number of time discretization points
 *
 * NOTE: all set_xxx methods must be called before solving for the first time
 */
template<typename _problem_t, std::size_t nPts_>
class MPCTracking
{
public:
  // Linearization order used for system linearization
  static constexpr std::size_t LinOrder = 4;

  static constexpr bool is_lie = has_tangent<typename _problem_t::state_t>::value;

  using problem_t = _problem_t;
  using state_t = typename problem_t::state_t;
  using deriv_t = typename problem_t::deriv_t;
  using input_t = typename problem_t::input_t;  // must be eigen type

  // Use CltvOcpLie or CltvOcp depending on the type of problem we have
  using cltv_t = typename std::conditional<is_lie,
      CltvOcpLie<MPCTrackingProblem<_problem_t>>,
      CltvOcp<MPCTrackingProblem<_problem_t>>
    >::type;

  using dltv_t = DltvOcp<cltv_t, nPts_, LinOrder>;
  using solver_t = DltvOcpSolver<dltv_t>;

  using state_traj_t = typename std::conditional<is_lie,
      std::array<state_t, nPts_>,
      Eigen::Matrix<double, problem_t::nx, nPts_>
    >::type;

  struct Solution
  {
    DltvOcpSolverCode rc;
    Eigen::Matrix<double, 1, nPts_> t;
    state_traj_t x;
    Eigen::Matrix<double, input_t::SizeAtCompileTime, nPts_> u;
  };

  MPCTracking() = delete;
  MPCTracking(const MPCTracking &) = default;
  MPCTracking(MPCTracking &&) = default;
  MPCTracking & operator=(const MPCTracking &) = default;
  MPCTracking & operator=(MPCTracking &&) = default;

  explicit MPCTracking(const _problem_t & problem, MPCTrackingParams param = MPCTrackingParams{})
  : solver_(
      DltvOcp<cltv_t, nPts_, LinOrder>(cltv_t(MPCTrackingProblem<_problem_t>(param.T, problem))),
      param.solver_params
  ),
    uspline_(
      (Eigen::Matrix<double, 1, 2>() << 0., 1.).finished(),
      input_t::Zero()
    ),
    tp_(1)
  {
    mpcSol_.rc = DltvOcpSolverCode::no_run;

    lock_problem();

    if constexpr (is_lie) {
      // lie types default-initialize to identity
      set_xd([](std::chrono::nanoseconds) {return state_t{};});
      set_xl([](std::chrono::nanoseconds) {return state_t{};});
      set_xldot([](std::chrono::nanoseconds) {return deriv_t{};});
    } else {
      // Eigen types must be explicitly zero-initialized
      set_xd([](std::chrono::nanoseconds) {return state_t::Zero();});
      set_xl([](std::chrono::nanoseconds) {return state_t::Zero();});
      set_xldot([](std::chrono::nanoseconds) {return deriv_t::Zero();});
    }

    // zero-initialize linearization and desired inputs
    set_ud([](std::chrono::nanoseconds) {return input_t::Zero();});
    set_ul([](std::chrono::nanoseconds) {return input_t::Zero();});

    unlock_problem();
  }

  /**
   * @brief Block solving to safely update xd/ud/xl/xldot/ul
   *
   * Use before calling the set_xxx functions, then call unlock_problem()
   */
  void lock_problem() {problemsMtx_.lock();}

  /**
   * @brief Unblock solving
   *
   * Use after calling lock_problem()
   */
  void unlock_problem() {problemsMtx_.unlock();}

  /**
   * @brief Specify desired state trajectory as a function t -> xd(t) [absolute time]
   *
   * Only use after calling lock_problem()
   *
   * @param f xd as a function of t
   */
  template<typename T>
  void set_xd(T && f) {solver_.problem().problem().problem().xd = std::forward<T>(f);}

  /**
   * @brief Specify desired input trajectory as a function t -> ud(t) [absolute time]
   *
   * Only use after calling lock_problem()
   *
   * @param f ud as a function of t
   */

  template<typename T>
  void set_ud(T && f) {solver_.problem().problem().problem().ud = std::forward<T>(f);}

  /**
   * @brief Specify state linearization trajectory as a function t -> xd(t) [absolute time]
   *
   * Only use after calling lock_problem()
   *
   * May be required if xd/ud is changed to be far from current linearization
   */
  template<typename T>
  void set_xl(T && f) {solver_.problem().problem().problem().xl = std::forward<T>(f);}

  /**
   * @brief Specify state linearization trajectory as a function t -> \dot xd(t) [absolute time]
   *
   * Only use after calling lock_problem()
   *
   * May be required if xd/ud is changed to be far from current linearization
   */
  template<typename T>
  void set_xldot(T && f) {solver_.problem().problem().problem().xldot = std::forward<T>(f);}

  /**
   * @brief Specify state linearization trajectory as a function t -> xd(t) [absolute time]
   *
   * Only use after calling lock_problem()
   *
   * May be required if xd/ud is changed to be far from current linearization
   */
  template<typename T>
  void set_ul(T && f) {solver_.problem().problem().problem().ul = std::forward<T>(f);}

  /**
   * @brief Asynchronous update of MPC
   *
   * @param t current absolute time
   * @param xt state at time t
   * @return future to solver status
   *
   * If optimization is already running this function returns without without effect.
   */
  std::shared_future<DltvOcpSolverCode>
  update(const nanoseconds t, const state_t & xt)
  {
    std::lock_guard lock(futMtx_);
    if (fut_.valid()) {
      if (fut_.wait_for(nanoseconds(0)) != std::future_status::ready) {
        return {};
      }
    }

    fut_ = tp_.enqueue(&MPCTracking::update_, this, t, xt, 1);
    return fut_.share();
  }

  /**
   * @brief Blocking update of MPC
   *
   * @param t current absolute time
   * @param xt state at time t
   * @param iter number of solve/linearaze iterations to run
   * @return solver status
   */
  DltvOcpSolverCode update_sync(const nanoseconds t, const state_t & xt, std::size_t iter = 1)
  {
    std::lock_guard lock(futMtx_);
    if (fut_.valid()) {
      fut_.wait();
    }
    return update_(t, xt, iter);
  }

  /**
   * @brief Obtain input at given time for most recent solution
   */
  input_t get_u(nanoseconds t)
  {
    std::lock_guard lock(solutionMtx_);
    return uspline_.val(duration_cast<duration<double>>(t - uspline_t0_).count());
  }

  /**
   * @brief Obtain most recent solution
   *
   * NOTE: solution times are defined on [0, T]
   */
  Solution solution()
  {
    std::lock_guard lock(solutionMtx_);
    return mpcSol_;
  }

private:
  /**
   * @brief Internal helper function to update mpc solution
   */
  DltvOcpSolverCode update_(std::chrono::nanoseconds t0, const state_t & x0, std::size_t iter)
  {
    std::lock_guard lock(problemsMtx_);  // lock for the duration to ensure consistency
    DltvOcpSolverCode rc = DltvOcpSolverCode::no_run;

    solver_.problem().problem().problem().t0 = t0;
    solver_.problem().problem().problem().x0 = x0;

    for (std::size_t i = 0; i != iter; ++i) {
      solver_.init();

      auto sol = solver_.solve();
      rc = sol.rc;

      if (sol.rc != DltvOcpSolverCode::success) {
        break;
      } else {
        Solution abs_sol;
        abs_sol.rc = sol.rc;

        for (auto i = 0u; i < nPts_; i++) {
          // solution is a trajectory around the linearization, we add back
          // the linearization to get a trajectory in global coordinates
          const double tt = solver_.problem().indexToTime(i);
          abs_sol.t[i] = tt;
          if constexpr (is_lie) {
            abs_sol.x[i] =
              solver_.problem().problem().problem().get_xl(tt) * state_t::exp(sol.x.col(i));
          } else {
            abs_sol.x.col(i) = solver_.problem().problem().problem().get_xl(tt) + sol.x.col(i);
          }
          abs_sol.u.col(i) = solver_.problem().problem().problem().get_ul(tt) + sol.u.col(i);
        }

        auto ulin = cbr::PiecewiseLinear::fitND(abs_sol.t, abs_sol.u);

        {
          // store solution so that get_u() can be called
          std::lock_guard lock(solutionMtx_);
          mpcSol_ = abs_sol;

          uspline_t0_ = t0;
          uspline_ = ulin;
        }

        // Set desired input for future solving to current input
        solver_.problem().problem().problem().ud =
          [u0 = abs_sol.u.col(0).eval()](nanoseconds) -> input_t {
            return u0;
          };

        // Update linearization points (functions defined in absolute time)
        solver_.problem().problem().problem().ul =
          [t0 = t0, ulin = std::move(ulin)](nanoseconds t) -> input_t {
            return ulin.val(duration_cast<duration<double>>(t - t0).count());
          };

        if constexpr (is_lie) {
          // fit splines: first need to copy to a vector
          vector_aligned<state_t> sol_vec(abs_sol.x.begin(), abs_sol.x.end());
          auto xlin = cbr::Spline::fitLie(abs_sol.t, std::move(sol_vec));
          xlin.set_extrap(PiecewisePoly::EXTRAP::CLAMP);

          solver_.problem().problem().problem().xl =
            [t0 = t0, xlin = xlin](nanoseconds t) -> state_t {
              const auto t_spline = duration_cast<duration<double>>(t - t0).count();
              return xlin.val(t_spline);
            };
          solver_.problem().problem().problem().xldot =
            [t0 = t0, xlin = std::move(xlin)](nanoseconds t) -> deriv_t {
              const auto t_spline = duration_cast<duration<double>>(t - t0).count();
              return xlin.der(t_spline);
            };
        } else {
          // fit splines
          auto xlin = cbr::PiecewiseLinear::fitND(abs_sol.t, abs_sol.x);

          solver_.problem().problem().problem().xl =
            [t0 = t0, xlin = xlin](nanoseconds t) -> state_t {
              return xlin.val(duration_cast<duration<double>>(t - t0).count());
            };
          solver_.problem().problem().problem().xldot =
            [t0 = t0, xlin = xlin](nanoseconds t) -> deriv_t {
              return xlin.der(duration_cast<duration<double>>(t - t0).count());
            };
        }

        // reset warm-start solution to zeros
        solver_.set_ic(decltype(sol.x)::Zero(), decltype(sol.u)::Zero());
      }
    }

    return rc;
  }

private:
  solver_t solver_{};

  Solution mpcSol_;
  std::mutex solutionMtx_, problemsMtx_, futMtx_;
  std::future<DltvOcpSolverCode> fut_;

  nanoseconds uspline_t0_;
  cbr::PiecewisePolyND uspline_;

  ThreadPool tp_;
};

}  // namespace cbr

#endif  // CBR_CONTROL__MPC__MPC_TRACKING_HPP_


// Copyright (c) 2015-2021 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#ifndef individual_caller_hpp
#define individual_caller_hpp

#include <vector>
#include <string>
#include <memory>

#include <boost/optional.hpp>

#include "config/common.hpp"
#include "basics/phred.hpp"
#include "core/types/haplotype.hpp"
#include "core/types/genotype.hpp"
#include "core/models/mutation/coalescent_model.hpp"
#include "core/models/genotype/genotype_prior_model.hpp"
#include "core/models/genotype/individual_model.hpp"
#include "caller.hpp"

namespace octopus {

class GenomicRegion;
class ReadPipe;
class Variant;
class HaplotypeLikelihoodArray;
class VariantCall;

class IndividualCaller : public Caller
{
public:
    using Caller::CallTypeSet;

    struct Parameters
    {
        unsigned ploidy;
        boost::optional<CoalescentModel::Parameters> prior_model_params;
        Phred<double> min_variant_posterior, min_refcall_posterior;
        bool deduplicate_haplotypes_with_germline_model = false;
        boost::optional<std::size_t> max_genotypes = boost::none;
    };

    IndividualCaller() = delete;

    IndividualCaller(Caller::Components&& components,
                     Caller::Parameters general_parameters,
                     Parameters specific_parameters);

    IndividualCaller(const IndividualCaller&)            = delete;
    IndividualCaller& operator=(const IndividualCaller&) = delete;
    IndividualCaller(IndividualCaller&&)                 = delete;
    IndividualCaller& operator=(IndividualCaller&&)      = delete;

    ~IndividualCaller() = default;

private:
    class Latents;

    using IndexedHaplotypeBlock = MappableBlock<IndexedHaplotype<>>;
    using GenotypeBlock = MappableBlock<Genotype<IndexedHaplotype<>>>;

    Parameters parameters_;

    std::string do_name() const override;
    CallTypeSet do_call_types() const override;
    unsigned do_min_callable_ploidy() const override;

    std::size_t do_remove_duplicates(HaplotypeBlock& haplotypes) const override;

    std::unique_ptr<Caller::Latents>
    infer_latents(const HaplotypeBlock& haplotypes,
                  const HaplotypeLikelihoodArray& haplotype_likelihoods) const override;

    boost::optional<double>
    calculate_model_posterior(const HaplotypeBlock& haplotypes,
                              const HaplotypeLikelihoodArray& haplotype_likelihoods,
                              const Caller::Latents& latents) const override;

    boost::optional<double>
    calculate_model_posterior(const HaplotypeBlock& haplotypes,
                              const HaplotypeLikelihoodArray& haplotype_likelihoods,
                              const Latents& latents) const;

    std::vector<std::unique_ptr<VariantCall>>
    call_variants(const std::vector<Variant>& candidates, const Caller::Latents& latents) const override;

    std::vector<std::unique_ptr<VariantCall>>
    call_variants(const std::vector<Variant>& candidates, const Latents& latents) const;

    std::vector<std::unique_ptr<ReferenceCall>>
    call_reference(const std::vector<Allele>& alleles, const Caller::Latents& latents,
                   const ReadPileupMap& pileups) const override;

    std::vector<std::unique_ptr<ReferenceCall>>
    call_reference(const std::vector<Allele>& alleles, const Latents& latents,
                   const ReadPileupMap& pileups) const;

    const SampleName& sample() const noexcept;

    std::unique_ptr<GenotypePriorModel> make_prior_model(const HaplotypeBlock& haplotypes) const;
    GenotypeBlock
    propose_genotypes(const HaplotypeBlock& haplotypes,
                      const IndexedHaplotypeBlock& indexed_haplotypes,
                      const HaplotypeLikelihoodArray& haplotype_likelihoods) const;
    GenotypeBlock
    propose_model_check_genotypes(const HaplotypeBlock& haplotypes,
                                  const IndexedHaplotypeBlock& indexed_haplotypes,
                                  const Latents& latents) const;
};

class IndividualCaller::Latents : public Caller::Latents
{
public:
    using IndexedHaplotypeBlock = MappableBlock<IndexedHaplotype<>>;

    using ModelInferences = model::IndividualModel::InferredLatents;

    using Caller::Latents::HaplotypeProbabilityMap;
    using Caller::Latents::GenotypeProbabilityMap;

    friend IndividualCaller;

    Latents() = delete;

    Latents(const SampleName& sample,
            const IndexedHaplotypeBlock& haplotypes,
            IndividualCaller::GenotypeBlock genotypes,
            ModelInferences&& latents);

    std::shared_ptr<HaplotypeProbabilityMap> haplotype_posteriors() const override;
    std::shared_ptr<GenotypeProbabilityMap> genotype_posteriors() const override;
    std::shared_ptr<GenotypeProbabilityMap> genotype_log_posteriors() const;

private:
    mutable std::shared_ptr<GenotypeProbabilityMap> genotype_log_posteriors_, genotype_posteriors_;
    mutable std::shared_ptr<HaplotypeProbabilityMap> haplotype_posteriors_;
    IndexedHaplotypeBlock haplotypes_;
    IndividualCaller::GenotypeBlock genotypes_;
    ModelInferences latents_;
    SampleName samples_;

    HaplotypeProbabilityMap calculate_haplotype_posteriors(const IndexedHaplotypeBlock& haplotypes) const;
};

} // namespace octopus

#endif


#include <cstdlib>
#include <ctime>

#include <chrono>
#include <iostream>
#include <unordered_set>

#include <boost/program_options.hpp>

#include "../yche_refactor/bprw_yche.h"
#include "../yche_refactor/simrank.h"

using namespace std;
using namespace std::chrono;
using namespace boost::program_options;

void test_bp(string data_name, double c, double epsilon, double delta, int x, int y) {
    string path = get_edge_list_path(data_name);
    GraphYche g(path);

    // 1st: init
    BackPush bprw(data_name, g, c, epsilon, delta);
    size_t n = static_cast<size_t>(g.n);
    NodePair q{x, y};

    TruthSim ts(data_name, g, c, epsilon);
    auto max_err = 0.0;
    auto min_err = 1.0;
    // 2nd: query sim(x,y)
    auto failure_count = 0;
    auto start = std::chrono::high_resolution_clock::now();
    for (auto i = 0; i < 2000; i++) {
        double result = bprw.query_one2one(q);
        auto cur_err = abs(result - ts.sim(x, y));
        max_err = max(max_err, cur_err);
        min_err = min(min_err, cur_err);
        if (cur_err > 0.01) {
            cout << result << " ," << ts.sim(x, y) << endl;
            failure_count++;
        }
    }
    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = end - start;

    cout << "failure count:" << failure_count << endl;
    cout << "max err:" << max_err << endl;
    cout << "min err:" << min_err << endl;
    cout << format("memory:%s KB") % getValue() << endl;
    cout << format("total query cost: %s s") % elapsed.count() << endl; // record the pre-processing time
}

int main(int args, char *argv[]) {
    string data_name(argv[1]);
    double c = 0.6;
    double epsilon = 0.01;
    double delta = 0.01;

    int x = atoi(argv[2]), y = atoi(argv[3]);
    test_bp(data_name, c, epsilon, delta, x, y);
}


#include <iostream>

#include <bs/ewma.hpp>
#include <bs/frame_range.hpp>
#include <bs/utils.hpp>
#include <bs/temporal_median.hpp>

#include <boost/make_shared.hpp>
#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <range/v3/front.hpp>
#include <range/v3/action/take.hpp>
#include <range/v3/view/take.hpp>

#include <options.hpp>
#include <run.hpp>

//
// options_t::options_t is specific to each example:
//
options_t::options_t (int argc, char** argv) {
    {
        auto tmp = std::make_pair (
                       "program", po::variable_value (std::string (argv [0]), false));
        map_.insert (tmp);
    }

    po::options_description generic ("Generic options");
    po::options_description config ("Configuration options");

    generic.add_options ()
    ("version", "version")
    ("help", "this");

    config.add_options ()
    ("display,d", "display frames.")

    ("input,i",   po::value< std::string > ()->default_value ("0"),
     "input (file or stream index).")

    ("lambda", po::value< double > ()->default_value (7),
     "a threshold multiplier (2)")

    ("history-size", po::value< size_t > ()->default_value (9),
     "length of frame history buffer (9)")

    ("frame-interval", po::value< size_t > ()->default_value (16),
     "interval between frames sampled for history buffer (16)")

    ("lo", po::value< size_t > ()->default_value (2),
     "distance from median for low threshold(2)")

    ("hi", po::value< size_t > ()->default_value (4),
     "distance from median for high threshold (4)");

    desc_ = boost::make_shared< po::options_description > ();

    desc_->add (generic);
    desc_->add (config);

    store (po::command_line_parser (argc, argv).options (*desc_).run (), map_);

    notify (map_);
}

////////////////////////////////////////////////////////////////////////

static void
program_options_from (int& argc, char** argv) {
    bool complete_invocation = false;

    options_t program_options (argc, argv);

    if (program_options.have ("version")) {
        std::cout << "OpenCV v3.1\n";
        complete_invocation = true;
    }

    if (program_options.have ("help")) {
        std::cout << program_options.description () << std::endl;
        complete_invocation = true;
    }

    if (complete_invocation)
        exit (0);

    global_options (program_options);
}

////////////////////////////////////////////////////////////////////////

static cv::Mat
bootstrap (cv::VideoCapture& cap) {
    using namespace ranges;

    //
    // The bootstrapping function:
    //
    bs::ewma_t f;

    //
    // Learn the background over 15 frames:
    //
    auto frames = bs::getframes_from (cap);

    for (auto frame : (frames | view::take (15))) {
        f (bs::scale_frame (frame));
    }

    return f.value ();
}

static void
process_temporal_median_background (cv::VideoCapture& cap, const options_t& opts) {
    const bool display = opts.have ("display");

    auto background_model = bootstrap (cap);

    bs::temporal_median_t temporal_median (
        background_model,
        opts ["history-size" ].as< size_t > (),
        opts ["frame-interval" ].as< size_t > (),
        opts ["lo" ].as< size_t > (),
        opts ["hi" ].as< size_t > ());

    for (auto& frame : bs::getframes_from (cap)) {
        bs::frame_delay temp { 10 };

        auto mask = temporal_median (bs::scale_frame (frame));

        if (display)
            imshow ("Temporal median", mask);

        if (temp.wait_for_key (27))
            break;
    }
}

////////////////////////////////////////////////////////////////////////

int main (int argc, char** argv) {
    program_options_from (argc, argv);
    return run_with (process_temporal_median_background, global_options ()), 0;
}


/*=============================================================================
    Copyright (c) 2001-2010 Joel de Guzman
    Copyright (c) 2001-2010 Hartmut Kaiser
    http://spirit.sourceforge.net/

    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)
=============================================================================*/
#if !defined(SPIRIT_REAL_IMPL_APRIL_18_2006_0901AM)
#define SPIRIT_REAL_IMPL_APRIL_18_2006_0901AM

#if defined(_MSC_VER)
#pragma once
#endif

#include <cmath>
#include <limits>
#include <boost/type_traits/is_same.hpp>
#include <boost/spirit/home/support/unused.hpp>
#include <boost/spirit/home/support/attributes.hpp>
#include <boost/spirit/home/support/detail/pow10.hpp>
#include <boost/spirit/home/support/detail/sign.hpp>
#include <boost/assert.hpp>

#if BOOST_WORKAROUND(BOOST_MSVC, >= 1400)
# pragma warning(push)
# pragma warning(disable: 4100)   // 'p': unreferenced formal parameter
# pragma warning(disable: 4127)   // conditional expression is constant
#endif

namespace boost { namespace spirit { namespace traits
{
    using spirit::detail::pow10;

    template <typename T>
    inline void
    scale(int exp, T& n)
    {
        if (exp >= 0)
        {
            // $$$ Why is this failing for boost.math.concepts ? $$$
            //~ int nn = std::numeric_limits<T>::max_exponent10;
            //~ BOOST_ASSERT(exp <= std::numeric_limits<T>::max_exponent10);
            n *= pow10<T>(exp);
        }
        else
        {
            if (exp < std::numeric_limits<T>::min_exponent10)
            {
                n /= pow10<T>(-std::numeric_limits<T>::min_exponent10);
                n /= pow10<T>(-exp + std::numeric_limits<T>::min_exponent10);
            }
            else
            {
                n /= pow10<T>(-exp);
            }
        }
    }

    inline void
    scale(int /*exp*/, unused_type /*n*/)
    {
        // no-op for unused_type
    }

    template <typename T>
    inline void
    scale(int exp, int frac, T& n)
    {
        scale(exp - frac, n);
    }

    inline void
    scale(int /*exp*/, int /*frac*/, unused_type /*n*/)
    {
        // no-op for unused_type
    }

      inline float
    negate(bool neg, float n)
    {
        return neg ? spirit::detail::changesign(n) : n;
    }

    inline double
    negate(bool neg, double n)
    {
        return neg ? spirit::detail::changesign(n) : n;
    }

    inline long double
    negate(bool neg, long double n)
    {
        return neg ? spirit::detail::changesign(n) : n;
    }

    template <typename T>
    inline T
    negate(bool neg, T const& n)
    {
        return neg ? -n : n;
    }

    inline unused_type
    negate(bool /*neg*/, unused_type n)
    {
        // no-op for unused_type
        return n;
    }

    template <typename T>
    inline bool
    is_equal_to_one(T const& value)
    {
        return value == 1.0;
    }

    inline bool
    is_equal_to_one(unused_type)
    {
        // no-op for unused_type
        return false;
    }
}}}

namespace boost { namespace spirit { namespace qi  { namespace detail
{
    template <typename T, typename RealPolicies>
    struct real_impl
    {
        template <typename Iterator, typename Attribute>
        static bool
        parse(Iterator& first, Iterator const& last, Attribute& attr,
            RealPolicies const& p)
        {
            if (first == last)
                return false;
            Iterator save = first;

            // Start by parsing the sign. neg will be true if
            // we got a "-" sign, false otherwise.
            bool neg = p.parse_sign(first, last);

            // Now attempt to parse an integer
            T n = 0;
            bool got_a_number = p.parse_n(first, last, n);

            // If we did not get a number it might be a NaN, Inf or a leading
            // dot.
            if (!got_a_number)
            {
                // Check whether the number to parse is a NaN or Inf
                if (p.parse_nan(first, last, n) ||
                    p.parse_inf(first, last, n))
                {
                    // If we got a negative sign, negate the number
                    traits::assign_to(traits::negate(neg, n), attr);
                    return true;    // got a NaN or Inf, return early
                }

                // If we did not get a number and our policies do not
                // allow a leading dot, fail and return early (no-match)
                if (!p.allow_leading_dot)
                {
                    first = save;
                    return false;
                }
            }

            bool e_hit = false;
            int frac_digits = 0;

            // Try to parse the dot ('.' decimal point)
            if (p.parse_dot(first, last))
            {
                // We got the decimal point. Now we will try to parse
                // the fraction if it is there. If not, it defaults
                // to zero (0) only if we already got a number.
                Iterator savef = first;
                if (p.parse_frac_n(first, last, n))
                {
                    // Optimization note: don't compute frac_digits if T is
                    // an unused_type. This should be optimized away by the compiler.
                    if (!is_same<T, unused_type>::value)
                        frac_digits =
                            static_cast<int>(std::distance(savef, first));
                }
                else if (!got_a_number || !p.allow_trailing_dot)
                {
                    // We did not get a fraction. If we still haven't got a
                    // number and our policies do not allow a trailing dot,
                    // return no-match.
                    first = save;
                    return false;
                }

                // Now, let's see if we can parse the exponent prefix
                e_hit = p.parse_exp(first, last);
            }
            else
            {
                // No dot and no number! Return no-match.
                if (!got_a_number)
                {
                    first = save;
                    return false;
                }

                // If we must expect a dot and we didn't see an exponent
                // prefix, return no-match.
                e_hit = p.parse_exp(first, last);
                if (p.expect_dot && !e_hit)
                {
                    first = save;
                    return false;
                }
            }

            if (e_hit)
            {
                // We got the exponent prefix. Now we will try to parse the
                // actual exponent. It is an error if it is not there.
                int exp = 0;
                if (p.parse_exp_n(first, last, exp))
                {
                    // Got the exponent value. Scale the number by
                    // exp-frac_digits.
                    traits::scale(exp, frac_digits, n);
                }
                else
                {
                    // Oops, no exponent, return no-match.
                    first = save;
                    return false;
                }
            }
            else if (frac_digits)
            {
                // No exponent found. Scale the number by -frac_digits.
                traits::scale(-frac_digits, n);
            }
            else if (traits::is_equal_to_one(n))
            {
                // There is a chance of having to parse one of the 1.0#...
                // styles some implementations use for representing NaN or Inf.

                // Check whether the number to parse is a NaN or Inf
                if (p.parse_nan(first, last, n) ||
                    p.parse_inf(first, last, n))
                {
                    // If we got a negative sign, negate the number
                    traits::assign_to(traits::negate(neg, n), attr);
                    return true;    // got a NaN or Inf, return immediately
                }
            }

            // If we got a negative sign, negate the number
            traits::assign_to(traits::negate(neg, n), attr);

            // Success!!!
            return true;
        }
    };

#if BOOST_WORKAROUND(BOOST_MSVC, >= 1400)
# pragma warning(pop)
#endif

}}}}

#endif


#include "plots.h"
#include "cws/component/lfo.h"
#include "utility/arithmetic.h"
#include "utility/types.h"
#include <boost/lexical_cast.hpp>
#include <getopt.h>
#include <memory>
#include <vector>
#include <algorithm>
#include <cstdio>
#include <cstdlib>
#include <cstring>
using namespace cws80;

Lfo::Param Lfop;
f64 FS = 44100;
std::vector<f64> RT;  // reset times
f64 D = 1;  // duration
uint B = 64;  // block size
bool Plot = false;

static void parse_level_params(const char *arg);
static void process();

//
static const char usage[] =
    "Usage: test-lfo [options]\n"
    "   -f <sample-rate>           Set the sample rate\n"
    "   -l <l1,l2>                 Set the level parameters (in 0..63)\n"
    "   -w <wave>                  Set the waveform (0-3=TRI,SAW,SQR,NOI)\n"
    "   -d <duration>              Set the duration (in s)\n"
    "   -b <block-size>            Set the block size\n"
    "   -R <time>                  Reset at the given time\n"
    "   -p                         Plot the results\n";

int main(int argc, char *argv[])
{
    Lfop.FREQ = Lfo::freqidx(3.0);
    Lfop.WAV = (uint)LfoWave::TRI;
    Lfop.L1 = 0;
    Lfop.L2 = 63;
    Lfop.DELAY = 63;
    Lfop.HUMAN = false;

    for (int c; (c = getopt(argc, argv, "hf:l:w:d:b:R:p")) != -1;) {
        switch (c) {
        case 'h':
            fputs(usage, stderr);
            return 1;
        case 'f':
            FS = boost::lexical_cast<f64>(optarg);
            break;
        case 'l':
            parse_level_params(optarg);
            break;
        case 'w': {
            uint wav = boost::lexical_cast<uint>(optarg);
            if (wav >= 4)
                throw std::logic_error("invalid waveform parameter");
            Lfop.WAV = wav;
            break;
        }
        case 'd':
            D = boost::lexical_cast<f64>(optarg);
            if (D <= 0)
                throw std::logic_error("invalid duration parameter");
            break;
        case 'b':
            B = boost::lexical_cast<uint>(optarg);
            if (B <= 0)
                throw std::logic_error("invalid block size parameter");
            break;
        case 'p':
            Plot = true;
            break;
        case 'R':
            RT.push_back(boost::lexical_cast<f64>(optarg));
            break;
        default:
            return 1;
        }
    }

    if (argc != optind)
        exit(1);

    process();
    return 0;
}

static void process()
{
    f64 fs = FS;
    uint nsamples = (uint)ceil(D * fs);
    std::unique_ptr<i8[]> samples(new i8[nsamples]);

    Lfo lfo;
    lfo.setparam(&Lfop);
    lfo.initialize(fs, B);

    std::sort(RT.begin(), RT.end());
    uint nT = 0;

    std::unique_ptr<i8[]> modulation(new i8[B]);
    for (uint i = 0; i < B; ++i) {
        modulation[i] = 0;  // TODO modulation?
    }

    for (uint i = 0; i < nsamples;) {
        uint bs = std::min(B, nsamples - i);
        lfo.generate(&samples[i], modulation.get(), bs);
        i += bs;
        // resetting
        f64 t = (f64)i / fs;
        for (; nT < RT.size() && t >= RT[nT]; ++nT) {
            lfo.reset();
        }
    }

    if (Plot) {
        FILE *gp = popen("gnuplot -d", "w");
        if (!gp)
            fprintf(stderr, "Cannot run gnuplot.\n");
        else {
            SCOPE(exit) { pclose(gp); };

            const char title[] = "LFO";

            fputs("set terminal wxt size 1024,480\n", gp);

            fprintf(gp, "set xrange [0:%f]\n", (f64)(nsamples - 1) / fs);

            for (f64 t : RT)
                fprintf(gp, "set arrow from %.17e,graph(0,0)"
                        " to %.17e,graph(1,1) nohead lc rgb 'red'\n", t, t);

            fprintf(gp, "plot '-' using 1:2 title \"%s\"\n", gp_escape(title).c_str());
            for (uint i = 0; i < nsamples; ++i) {
                f64 t = (f64)i / fs;
                fprintf(gp, "%.17e %d\n", t, samples[i]);
            }
            fputs("e\n", gp);
            fputs("pause mouse close\n", gp);
            fflush(gp);
        }
    }
    else {
        for (uint i = 0; i < nsamples; ++i) {
            f64 t = (f64)i / fs;
            printf("%f %d\n", t, samples[i]);
        }
    }
}

static void parse_level_params(const char *arg)
{
    size_t arglen = strlen(arg);
    uint n;

    uint L1, L2;
    if (sscanf(arg, "%u,%u%n", &L1, &L2, (int *)&n) != 2 || n != arglen ||
        Lfop.L1 >= 64 || Lfop.L2 >= 64)
        throw std::logic_error("invalid level parameters");

    Lfop.L1 = L1;
    Lfop.L2 = L2;
}


/**

Copyright (c) 2016, Allgeyer Tobias, Aumann Florian, Borella Jocelyn, Meißner Pascal, Qattan Mohamad
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

2. 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.

3. Neither the name of the copyright holder 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 HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include "error_simulation.h"
#include <random>
#include <iostream>
#include <ros/console.h>
#include <Eigen/Dense>

namespace fake_object_recognition {

ErrorSimulation::ErrorSimulation() : ErrorSimulation(false, false, false) { }

ErrorSimulation::ErrorSimulation(bool use_pose_invalidation, bool use_position_noise, bool use_orientation_noise) :
    use_pose_invalidation_(use_pose_invalidation),
    use_position_noise_(use_position_noise),
    use_orientation_noise_(use_orientation_noise),
    prob_pose_inval_(DEFAULT_PROB_POSE_INVAL),
    pos_noise_dist_mean_(DEFAULT_POS_NOISE_MEAN),
    pos_noise_dist_dev_(DEFAULT_POS_NOISE_DEV),
    or_x_noise_dist_mean_(DEFAULT_OR_X_NOISE_MEAN),
    or_x_noise_dist_dev_(DEFAULT_OR_X_NOISE_DEV),
    or_y_noise_dist_mean_(DEFAULT_OR_Y_NOISE_MEAN),
    or_y_noise_dist_dev_(DEFAULT_OR_Y_NOISE_DEV),
    or_z_noise_dist_mean_(DEFAULT_OR_Z_NOISE_MEAN),
    or_z_noise_dist_dev_(DEFAULT_OR_Z_NOISE_DEV) { }

double ErrorSimulation::genUniformNumber(double interval_start, double interval_end) {
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<double> dis(interval_start, interval_end);
    return dis(gen);
}

double ErrorSimulation::genNormalDistNumber(double mean, double std_deviation) {
    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<double> dis(mean, std_deviation);
    return dis(gen);
}

bool ErrorSimulation::poseInvalidation() {
    if (use_pose_invalidation_) {
        return genUniformNumber(0.0, 1.0) >= prob_pose_inval_;
    }
    return true;
}

geometry_msgs::Pose ErrorSimulation::addNoiseToPosition(const geometry_msgs::Pose &pose) {
    if (use_position_noise_) {
        geometry_msgs::Pose result(pose);
        result.position.x += genNormalDistNumber(pos_noise_dist_mean_, pos_noise_dist_dev_);
        result.position.y += genNormalDistNumber(pos_noise_dist_mean_, pos_noise_dist_dev_);
        result.position.z += genNormalDistNumber(pos_noise_dist_mean_, pos_noise_dist_dev_);
        return result;
    }
    return pose;
}

geometry_msgs::Pose ErrorSimulation::addNoiseToOrientation(const geometry_msgs::Pose &pose) {
    if (use_orientation_noise_) {
        geometry_msgs::Pose result(pose);
        Eigen::Quaternion<double> orientation(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
        Eigen::Quaternion<double> quat_x(Eigen::AngleAxis<double>(genNormalDistNumber(or_x_noise_dist_mean_, or_x_noise_dist_dev_), Eigen::Vector3d(1.0, 0.0, 0.0)));
        Eigen::Quaternion<double> quat_y(Eigen::AngleAxis<double>(genNormalDistNumber(or_y_noise_dist_mean_, or_y_noise_dist_dev_), Eigen::Vector3d(0.0, 1.0, 0.0)));
        Eigen::Quaternion<double> quat_z(Eigen::AngleAxis<double>(genNormalDistNumber(or_z_noise_dist_mean_, or_z_noise_dist_dev_), Eigen::Vector3d(0.0, 0.0, 1.0)));

        orientation = quat_x * quat_y * quat_z * orientation;
        result.orientation.w = orientation.w();
        result.orientation.x = orientation.x();
        result.orientation.y = orientation.y();
        result.orientation.z = orientation.z();
        return result;
    }
    return pose;
}

void ErrorSimulation::setProbPoseInval(double prob_pose_inval) {
    prob_pose_inval_ = prob_pose_inval;
}
void ErrorSimulation::setPoseNoiseDistMean(double pos_noise_dist_mean) {
    pos_noise_dist_mean_ = pos_noise_dist_mean;
}

void ErrorSimulation::setPoseNoiseDistDev(double pos_noise_dist_dev) {
    pos_noise_dist_dev_ = pos_noise_dist_dev;
}

void ErrorSimulation::setOrXNoiseDistMean(double or_x_noise_dist_mean) {
    or_x_noise_dist_mean_ = or_x_noise_dist_mean;
}

void ErrorSimulation::setOrXNoiseDistDev(double or_x_noise_dist_dev) {
    or_x_noise_dist_dev_ = or_x_noise_dist_dev;
}

void ErrorSimulation::setOrYNoiseDistMean(double or_y_noise_dist_mean) {
    or_y_noise_dist_mean_ = or_y_noise_dist_mean;
}

void ErrorSimulation::setOrYNoiseDistDev(double or_y_noise_dist_dev) {
    or_y_noise_dist_dev_ = or_y_noise_dist_dev;
}

void ErrorSimulation::setOrZNoiseDistMean(double or_z_noise_dist_mean) {
    or_z_noise_dist_mean_ = or_z_noise_dist_mean;
}

void ErrorSimulation::setOrZNoiseDistDev(double or_z_noise_dist_dev) {
    or_z_noise_dist_dev_ = or_z_noise_dist_dev;
}

}


// doMatching2D

// Program for 2D LIDAR alignment
// using the FRSM library. It uses pretty brute force
// settings.
//
// Options: - take 2 scans from user and give transformation between them (-a reference.csv, -b input.csv)
//          - loop over a dataset and give transformation between all combinations (write to file)

#include <icp-registration/icp_utils.h>

#include <zlib.h>
#include <lcm/lcm-cpp.hpp>

#include <lcmtypes/bot_core.hpp>
#include <boost/shared_ptr.hpp>
#include <lidar-odom/lidar-odometry.hpp>
#include <lcmtypes/bot_core.hpp>
#include <ConciseArgs>

#include <iterator>
#include <sstream>
#include <iostream>

void parseTransf(string& transform, float& x, float& y, float& theta)
{
  transform.erase(std::remove(transform.begin(), transform.end(), '['),
            transform.end());
  transform.erase(std::remove(transform.begin(), transform.end(), ']'),
            transform.end());
  std::replace( transform.begin(), transform.end(), ',', ' ');
  std::replace( transform.begin(), transform.end(), ';', ' ');

  float transValues[3] = {0};
  stringstream transStringStream(transform);
  for( int i = 0; i < 3; i++) {
    if(!(transStringStream >> transValues[i])) {
      cerr << "An error occured while trying to parse the initial "
         << "transformation." << endl
         << "No initial transformation will be used" << endl;
      return;
    }
  }

  x = transValues[0];
  y = transValues[1];
  theta = transValues[2];
}

struct CommandLineConfig
{
  std::string filenameA;
  std::string filenameB;
};

Eigen::Isometry3d getXYThetaAsIsometry3d(Eigen::Vector3d transl, Eigen::Vector3d rpy){
  Eigen::Isometry3d tf_out;
  tf_out.setIdentity();
  tf_out.translation()  << transl[0], transl[1], transl[2];

  double rpy_d[3] = { rpy[0], rpy[1], rpy[2] };
  double quat[4];
  bot_roll_pitch_yaw_to_quat(rpy_d, quat);
  Eigen::Quaterniond q(quat[0], quat[1],quat[2],quat[3]);
  tf_out.rotate(q);

  return tf_out;
}

class App{
  public:
    App(boost::shared_ptr<lcm::LCM> &lcm_, const CommandLineConfig& cl_cfg_, int indTransfFile);

    ~App(){
    }

    const char *homedir;

    int num_clouds; //number of clouds to combine

    string cloudA;
    string cloudB;

    string initialT;  // We store the transform used to initialize the
                      // pose of cloud B. The lidar odometry class outputs the complete
                      // transformation from the starting pose (without initialization)
                      // of cloud B to cloud A. Instead, we want to store just the remaining
                      // x,y,theta after initialization.
    ScanTransform initInput;
    ScanTransform initReference;

    void doWork(Eigen::MatrixXf &transf_matrix, int transf_index);
    void doWork();

  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    const CommandLineConfig cl_cfg_;
    LidarOdom* lidarOdom_;
};

void readCSVFile(std::string filename, std::vector<float> &x, std::vector<float> &y){

    std::vector<std::vector<double> > values;
    std::ifstream fin(filename.c_str());
    for (std::string line; std::getline(fin, line); )
    {
        std::replace(line.begin(), line.end(), ',', ' ');
        std::istringstream in(line);
        values.push_back(
            std::vector<double>(std::istream_iterator<double>(in),
                                std::istream_iterator<double>()));
    }

   for (size_t i=0; i<values.size(); i++){
     x.push_back(values[i][0]);
     y.push_back(values[i][1]);
   }
}

App::App(boost::shared_ptr<lcm::LCM> &lcm_, const CommandLineConfig& cl_cfg_, int indTransfFile) :
       lcm_(lcm_), cl_cfg_(cl_cfg_){
  if ((homedir = getenv("HOME")) == NULL) {
    homedir = getpwuid(getuid())->pw_dir;
  }

  LidarOdomConfig lo_cfg = LidarOdomConfig();

  // Overwrite default config values with more brute force search:
  lo_cfg.matchingMode = FRSM_GRID_COORD;
  lo_cfg.initialSearchRangeXY = 1.7;
  lo_cfg.initialSearchRangeTheta = 1.7;
  lo_cfg.maxSearchRangeXY = 2.0;
  lo_cfg.maxSearchRangeTheta = 2.0;

  string fileinitname;
  fileinitname.clear();
  fileinitname.append(homedir);
  fileinitname.append("/logs/multisenselog__2015-11-16/initialization/initTransf.txt");

  initialT = readLineFromFile(fileinitname, indTransfFile);

  float tmpx, tmpy, tmptheta;
  parseTransf(initialT, tmpx, tmpy, tmptheta);
  initInput.x = tmpx;
  initInput.y = tmpy;
  initInput.theta = (tmptheta * M_PI)/180;

  cout << "initInput: " << tmpx << "," << tmpy << "," << tmptheta << endl;

  initReference.x = 0;
  initReference.y = 0;
  initReference.theta = (0 * M_PI)/180;

  num_clouds = 10;

  lidarOdom_ = new LidarOdom(lcm_, lo_cfg);
}

void App::doWork(Eigen::MatrixXf &transf_matrix, int transf_index){
  std::vector<float> xA;
  std::vector<float> yA;
  readCSVFile(cloudA, xA, yA);
  std::cout << xA.size() << " points in File A\n";
  lidarOdom_->doOdometry(xA, yA, xA.size(), 0, &initReference);

  std::vector<float> xB;
  std::vector<float> yB;
  readCSVFile(cloudB, xB, yB);
  std::cout << xB.size() << " points in File B\n";
  lidarOdom_->doOdometry(xB, yB, xB.size(), 1, &initInput);

  // 2. Determine the body position using the LIDAR motion estimate:
  /*
  Eigen::Isometry3d tf_01_fixed = lidarOdom_->getCurrentPose();
  Eigen::Isometry3d head_to_fixscan, fixscan_to_head;
  head_to_fixscan = getXYThetaAsIsometry3d(transl_head_to_fixed, rpy_head_to_fixed);
  fixscan_to_head = head_to_fixscan.inverse();
  Eigen::Isometry3d tf_01_head = fixscan_to_head * tf_01_fixed * head_to_fixscan;
  */
  Eigen::Isometry3d tf_01_head = lidarOdom_->getCurrentPose();

  Eigen::Quaterniond orientation(tf_01_head.rotation());
  Eigen::Vector3d rpy = orientation.matrix().eulerAngles(0,1,2);

  float xres,yres,thetares;
  xres = tf_01_head.translation().x();
  yres = tf_01_head.translation().y();
  thetares = rpy[2] * 180 / M_PI;

  transf_matrix(0, transf_index-1) = xres;
  transf_matrix(1, transf_index-1) = yres;
  transf_matrix(2, transf_index-1) = thetares;

  cout << "x,y,yaw (m,m,deg): "<< xres << ", " << yres << ", " << thetares << "\n";
}

int main(int argc, char **argv){
  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good()){
    std::cerr <<"ERROR: lcm is not good()" <<std::endl;
  }

  CommandLineConfig cl_cfg;
  ConciseArgs parser(argc, argv, "simple-fusion");
  parser.add(cl_cfg.filenameA, "a", "filenameA", "FilenameA.csv");
  parser.add(cl_cfg.filenameB, "b", "filenameB", "FilenameB.csv");
  parser.parse();

  App* app = new App(lcm, cl_cfg, 0);

  if (cl_cfg.filenameA.empty())
  {
    int transf_index = 0;
    //-------------------------
    // To store...
    int cols = 0;
    for (int i = app->num_clouds-1; i > 0; i--)
    {
      cols = cols + i;
    }
    Eigen::MatrixXf truth_res = Eigen::MatrixXf::Zero(3, cols);
    //-------------------------
    for (int i = 0; i < app->num_clouds-1; i++)
    {
      for (int j = 1+i; j < app->num_clouds; j++)
      {
        // Set index to load inital transformations from file
        transf_index++;

        // Name reference cloud from file
        app->cloudA.clear();
        app->cloudA.append(app->homedir);
        app->cloudA.append("/logs/multisenselog__2015-11-16/planar_scans/scan_0");
        app->cloudA.append(to_string(i));
        app->cloudA.append(".csv");

        // Name input cloud from file
        app->cloudB.clear();
        app->cloudB.append(app->homedir);
        app->cloudB.append("/logs/multisenselog__2015-11-16/planar_scans/scan_0");
        app->cloudB.append(to_string(j));
        app->cloudB.append(".csv");

        app->doWork(truth_res, transf_index);

        // Uncomment for visualization of matching results one by one:
        //cout << "Press ENTER to continue..." << endl;
        //cin.get();

        app->~App();
        app = new App(lcm, cl_cfg, transf_index);
      }
    }
    cout << "Final matrix:" << endl << truth_res.block(0,0,3,6) << endl;

    string out_file;
    out_file.append(app->homedir);
    out_file.append("/logs/multisenselog__2015-11-16/results/truth_transf.txt");
    writeTransformToFile(truth_res, out_file, app->num_clouds);
  }
  else
  {
    app->~App();
    app = new App(lcm, cl_cfg, 0);

    app->cloudA = cl_cfg.filenameA;
    app->cloudB = cl_cfg.filenameB;

    Eigen::MatrixXf truth_res = Eigen::MatrixXf::Zero(3, 1);
    app->doWork(truth_res, 0);
  }
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_MINDENORMAL_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_MINDENORMAL_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/meta/float.hpp>
#include <boost/simd/meta/int_c.hpp>
#include <boost/simd/meta/double.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/sdk/config.hpp>

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

     Represents the Mindenormal constant in generic contexts.

     @par Models:
        Hierarchy
   **/

    #ifdef BOOST_SIMD_NO_DENORMALS
    struct Smallestposval;
    typedef Smallestposval Mindenormal;
    #else
    struct Mindenormal : ext::pure_constant_<Mindenormal>
    {
      typedef double default_type;
      typedef ext::pure_constant_<Mindenormal> parent;

      /// INTERNAL ONLY
      template<class Target, class Dummy=void>
      struct apply : meta::int_c<typename Target::type,1> {};
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_Mindenormal( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Mindenormal::apply<boost::dispatch::meta::single_<T>,Dummy>
          : meta::single_<1> {};

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Mindenormal::apply<boost::dispatch::meta::double_<T>,Dummy>
          : meta::double_<1> {};
    #endif
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Mindenormal, Site> dispatching_Mindenormal(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Mindenormal, Site>();
   }
   template<class... Args>
   struct impl_Mindenormal;
  }
  /*!
    Constant Mindenormal : the least of all non zero positive value

    @par Semantic:

    @code
    T r = Mindenormal<T>();
    @endcode

    is similar to:

    @code
    if T is integral
      r = T(1)
    else if T is double
      r =  4.940656458412465e-324;
    else if T is float
      r = 1.4012985e-45;
    @endcode
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Mindenormal, Mindenormal)
} }

#ifdef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/constant/constants/smallestposval.hpp>
#endif

#include <boost/simd/constant/common.hpp>

#endif


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-predicates
    Function object implementing is_not_greater_equal capabilities

    Returns @ref True if x is not greater or equal to y else returns @ref False.

    @par Semantic:

    @code
    auto r = is_not_greater_equal(x,y);
    @endcode

    is similar to:

    @code
    auto r = !(x >= y);
    @endcode

    @par Note:

    Due to existence of nan, this is not equivalent to is_less(x)
    for floating types
  **/
  as_logical_t<Value> is_not_greater_equal(Value const& x, Value const& y);
} }
#endif

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

#endif


/*
 * 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.
 *
 *
 */

#include <string>
#include <vector>
#include <map>
#include <iostream>
#include <fstream>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <graphlab.hpp>

#include <graphlab/util/fs_util.hpp>
#include <boost/filesystem.hpp>

//remove assigned options from arguments
std::string get_arg_str_without(int argc, char** argv,
    std::vector<std::string> remove_opts) {
  std::stringstream strm;
  bool skip_next = false;
  for (int i = 1; i < argc; ++i) {
    bool skip = false;
    for (size_t j = 0; j < remove_opts.size(); ++j) {
      std::string with_equal = remove_opts[j] + "=";
      if (strncmp(with_equal.c_str(), argv[i], with_equal.size()) == 0) {
        skip = true;
      } else if (strncmp(remove_opts[j].c_str(), argv[i], remove_opts[j].size())
          == 0) {
        skip = true;
        skip_next = true;
      }
    }
    if (skip == false && skip_next == false) {
      strm << argv[i] << " ";
    } else if (skip == false && skip_next == true) {
      skip_next = false;
    }
  }
  return strm.str();
}

bool call_graph_laplacian_construction(const std::string& mpi_args,
    const std::string& filename, const float sigma, const float epsilon,
    const size_t num_nearests, const std::string& args) {
  std::stringstream strm;
  if (mpi_args.length() > 0)
    strm << "mpiexec " << mpi_args << " ";
  strm << "./graph_laplacian_for_sc ";
  strm << " --data=" << filename;
  strm << " --sigma=" << sigma;
  strm << " --similarity-thres=" << epsilon;
  strm << " --t-nearest=" << num_nearests;
  strm << " " << args;
  std::cout << "CALLING >" << strm.str() << std::endl;
  int sys_ret = system(strm.str().c_str());
  if (sys_ret != 0) {
    std::cout << "system call fails" << std::endl;
    return false;
  }

  return true;
}

void make_initial_vector_file(const std::string& filename, const size_t num_data){
  std::ofstream ofs((filename + ".init").c_str());
  for(size_t i=0;i<num_data;++i){
    ofs << 0.1*((i+1)%10)/10.0 << "\n";
  }
  ofs.close();
}

bool call_svd(const std::string& mpi_args, const std::string& filename,
    const std::string& svd_dir, const size_t num_clusters, const size_t rank,
    const size_t num_data, const std::string& args) {
  make_initial_vector_file(filename, num_data+1);
  std::stringstream strm;
  if (mpi_args.length() > 0)
    strm << "mpiexec " << mpi_args << " ";
  strm << svd_dir << "svd " + filename + ".glap";
  strm << " --rows=" << num_data+1;
  strm << " --cols=" << num_data;
  strm << " --nsv=" << num_clusters;
  strm << " --nv=" << rank;
//  strm << " --tol=1e-10";
//  strm << " --max_iter=20";
  strm << " --quiet=1";
  strm << " --input_file_offset=1";
  strm << " --save_vectors=1";
  strm << " --ortho_repeats=3";
//  strm << " --id=1";
  strm << " --prediction=" << filename;
  strm << " --initial_vector=" << filename + ".init";
  strm << " " << args;
  std::cout << "CALLING >" << strm.str() << std::endl;
  int sys_ret = system(strm.str().c_str());
  if (sys_ret != 0) {
    std::cout << "system call fails" << std::endl;
    return false;
  }

  return true;
}

bool call_eigen_vector_normalization(const std::string& mpi_args,
    const std::string& filename, const std::string& graph_analytics_dir,
    const size_t num_clusters, const size_t rank, const size_t num_data,
    const std::string& args) {
  std::stringstream strm;
  if (mpi_args.length() > 0)
    strm << "mpiexec " << mpi_args << " ";
  strm << graph_analytics_dir << "eigen_vector_normalization";
  strm << " --data=" << filename;
  strm << " --clusters=" << num_clusters;
  strm << " --rank=" << rank;
  strm << " --data-num=" << num_data;
  strm << " " << args;
  std::cout << "CALLING >" << strm.str() << std::endl;
  int sys_ret = system(strm.str().c_str());
  if (sys_ret != 0) {
    std::cout << "system call fails" << std::endl;
    return false;
  }

  return true;
}

bool call_kmeans(const std::string& mpi_args, const std::string& filename,
    const std::string& kmeans_dir, const size_t num_clusters,
    const std::string& args) {
  //call svd
  std::stringstream strm;
  if (mpi_args.length() > 0)
    strm << "mpiexec " << mpi_args << " ";
  strm << kmeans_dir << "kmeans ";
  strm << " --data " << filename << ".compressed";
  strm << " --clusters " << num_clusters;
  strm << " --output-data " << filename << ".result";
  strm << " --id=1";
  strm << " " << args;
  std::cout << "CALLING >" << strm.str() << std::endl;
  int sys_ret = system(strm.str().c_str());
  if (sys_ret != 0) {
    std::cout << "system call fails" << std::endl;
    return false;
  }
  return true;
}

bool call_kmeans_as_preprocess(const std::string& mpi_args, const std::string& filename,
    const std::string& kmeans_dir, const size_t num_clusters,
    const std::string& args) {
  //call svd
  std::stringstream strm;
  if (mpi_args.length() > 0)
    strm << "mpiexec " << mpi_args << " ";
  strm << kmeans_dir << "kmeans ";
  strm << " --data " << filename;
  strm << " --clusters " << num_clusters;
  strm << " --output-data " << filename << ".pre.labels";
  strm << " --output-clusters " << filename << ".pre.centers";
  strm << " --id=1";
  strm << " " << args;
  std::cout << "CALLING >" << strm.str() << std::endl;
  int sys_ret = system(strm.str().c_str());
  if (sys_ret != 0) {
    std::cout << "system call fails" << std::endl;
    return false;
  }
  return true;
}

//select good value of rank (TODO)
int get_lanczos_rank(const size_t num_clusters, const size_t num_data) {
  size_t rank = 1;
  if (num_data < 1000) {
    if (num_clusters + 10 <= num_data)
      rank = num_clusters + 10;
    else
      rank = num_data;
  } else if (num_data < 10000) {
    rank = num_clusters + 100;
  } else if (num_data < 100000) {
    rank = num_clusters + 150;
  } else if (num_data < 1000000) {
    rank = num_clusters + 200;
  } else {
    rank = num_clusters + 300;
  }
  return rank;
//  return num_clusters+2;
}

void read_pairs_with_prefix(std::vector<std::vector<size_t> >& ret, const std::string& prefix){
  std::string directory_name;
  std::string original_path(prefix);
  boost::filesystem::path path(prefix);
  std::string search_prefix;
  if (boost::filesystem::is_directory(path)) {
    // if this is a directory
    // force a "/" at the end of the path
    // make sure to check that the path is non-empty. (you do not
    // want to make the empty path "" the root path "/" )
    directory_name = path.native();
  }
  else {
    directory_name = path.parent_path().native();
    search_prefix = path.filename().native();
    directory_name = (directory_name.empty() ? "." : directory_name);
  }
  std::vector<std::string> files;
  graphlab::fs_util::list_files_with_prefix(directory_name, search_prefix, files);
  if (files.size() == 0) {
    logstream(LOG_WARNING) << "No files found matching " << original_path << std::endl;
  }
  for(size_t i = 0; i < files.size(); ++i) {
    std::ifstream ifs(files[i].c_str());
    if (!ifs) {
      std::cout << "can't read " << files[i] << std::endl;
      return;
    }
    while( !ifs.eof() ) {
      std::vector<size_t> pair;
      size_t id = 0;
      size_t label = 0;
      ifs >> id;
//      ifs.ignore(1);
      ifs >> label;
      if(id > 0 && label > 0){
        pair.push_back(id);
        pair.push_back(label);
        ret.push_back(pair);
      }
    }
  }
}

int recover_labels(const std::string& prefix){
  const std::string kmeans_result_prefix = prefix + ".pre.labels";
  const std::string spectral_result_prefix = prefix + ".pre.centers.result";
  const std::string outfile = prefix + ".result_1_of_1";

  std::vector<std::vector<size_t> > kmeans_result;
  read_pairs_with_prefix(kmeans_result, kmeans_result_prefix);
  std::vector<std::vector<size_t> > spectral_result;
  read_pairs_with_prefix(spectral_result, spectral_result_prefix);

  std::map<size_t, size_t> label_map;
  for(size_t i=0;i<spectral_result.size();++i){
    label_map.insert(std::make_pair(spectral_result[i][0], spectral_result[i][1]));
  }

  std::ofstream ofs(outfile.c_str());
  for(size_t i=0;i<kmeans_result.size();++i){
    ofs << kmeans_result[i][0] << "\t";
    ofs << label_map[kmeans_result[i][1]] << "\n";
  }

  return 0;
}

int main(int argc, char** argv) {
  std::cout << "Spectral clustering\n\n";
  time_t start, end, mid;
  std::vector<std::pair<std::string,time_t> > times;
  time(&start);

  std::string datafile;
  std::string graph_analytics_dir = "../graph_analytics/";
  std::string svd_dir = "../collaborative_filtering/";
  std::string kmeans_dir = "./";
  std::string mpi_args;
  size_t num_clusters = 0;
  size_t num_nearests = 30;
  float sigma = 1.0;
  float epsilon = 0.0;
  size_t pre_kmeans_clusters = 0;
  size_t sv = 0;
  //parse command line
  graphlab::command_line_options clopts(
          "Spectral clustering. The input data file is provided by the "
          "--data argument which is non-optional. The format of the data file is a "
          "collection of lines, where each line contains a data id followed by a "
          "comma or white-space separated list of numeric values representing a vector. "
          "Every line must have the same number of values. The required --clusters=N "
          "argument denotes the number of clusters to generate.");
  clopts.attach_option("data", datafile,
          "Input file. Each line holds a data id followed by a white-space "
          "or comma separated numeric vector");
  clopts.attach_option("clusters", num_clusters,
          "The number of clusters to create");
  clopts.attach_option("sigma", sigma,
          "Scale parameter for Gaussian kernel");
  clopts.attach_option("t-nearest", num_nearests,
          "Number of nearest neighbors (=t). Will use only the t-nearest similarities "
          "for each datapoint. If set at 0, will use all similarities.");
  clopts.attach_option("similarity-thres", epsilon,
          "Threshold to discard small similarities");
  clopts.attach_option("svd-dir", svd_dir,
          "Path to the directory where Graphlab svd is located");
  clopts.attach_option("kmeans-dir", kmeans_dir,
          "Path to the directory where Graphlab kmeans is located");
  clopts.attach_option("graph-analytics-dir", graph_analytics_dir,
          "Path to the directory where Graphlab eigen_vector_normalization is located");
  clopts.attach_option("pre-kmeans-clusters", pre_kmeans_clusters,
          "If set, will perform kmeans as a preprocess with the given cluster number.");
  clopts.attach_option("mpi-args", mpi_args,
          "If set, will execute mipexec with the given arguments. "
          "For example, --mpi-args=\"-n [N machines] --hostfile [host file]\"");
  clopts.attach_option("sv", sv,
          "Number of vectors in each iteration in the Lanczos svd.");
  if (!clopts.parse(argc, argv))
    return EXIT_FAILURE;
  if (datafile == "") {
    std::cout << "--data is not optional\n";
    return EXIT_FAILURE;
  }
  if (num_clusters == 0) {
    std::cout << "--cluster is not optional\n";
    return EXIT_FAILURE;
  }

  std::vector<std::string> remove_opts;
  remove_opts.push_back("--data");
  remove_opts.push_back("--svd-dir");
  remove_opts.push_back("--graph-analytics-dir");
  remove_opts.push_back("--kmeans-dir");
  remove_opts.push_back("--clusters");
  remove_opts.push_back("--sigma");
  remove_opts.push_back("--similarity-thres");
  remove_opts.push_back("--mpi-args");
  remove_opts.push_back("--t-nearest");
  remove_opts.push_back("--pre-kmeans-clusters");
  remove_opts.push_back("--sv");
  std::string other_args = get_arg_str_without(argc, argv, remove_opts);

  //preprocess by kmeans for fast clustering
  if(pre_kmeans_clusters > 0){
    if(pre_kmeans_clusters < num_clusters){
      std::cout << "the number of --pre-kmeans-clusters must be bigger than the number of clusters\n";
      return EXIT_FAILURE;
    }
    time(&mid);
    if(call_kmeans_as_preprocess(mpi_args, datafile, kmeans_dir, pre_kmeans_clusters, other_args) == false)
      return EXIT_FAILURE;
    //modify settings
    datafile = datafile + ".pre.centers";
    num_nearests = 0;
    time(&end);
    times.push_back(std::pair<std::string, time_t>("kmeans preprocess",(end - mid)));
  }

  //construct graph laplacian
  time(&mid);
  if (call_graph_laplacian_construction(mpi_args, datafile, sigma, epsilon,
      num_nearests, other_args) == false) {
    return EXIT_FAILURE;
  }
  time(&end);
  times.push_back(std::pair<std::string, time_t>("graph laplacian",(end - mid)));

  //eigen value decomposition
  //read number of data
  size_t num_data = 0;
  const std::string datanum_filename = datafile + ".datanum";
  std::ifstream ifs(datanum_filename.c_str());
  if (!ifs) {
    std::cout << "can't read number of data." << std::endl;
    return EXIT_FAILURE;
  }
  ifs >> num_data;
  //determine the sv of Lanczos method
  if(sv == 0){
    sv = get_lanczos_rank(num_clusters, num_data);
  }else{
    if(sv < num_clusters)
      sv = num_clusters;
  }
  time(&mid);
  if (call_svd(mpi_args, datafile, svd_dir, num_clusters, sv, num_data,
      other_args) == false) {
    return EXIT_FAILURE;
  }
  if (call_eigen_vector_normalization(mpi_args, datafile, graph_analytics_dir,
      num_clusters, sv, num_data, other_args) == false) {
    return EXIT_FAILURE;
  }
  time(&end);
  times.push_back(std::pair<std::string, time_t>("eigen decomposition",(end - mid)));

  //run kmeans
  time(&mid);
  if (call_kmeans(mpi_args, datafile, kmeans_dir, num_clusters, other_args)
      == false) {
    return EXIT_FAILURE;
  }
  time(&end);
  times.push_back(std::pair<std::string, time_t>("kmeans",(end - mid)));

  //recover cluster membership if preprocess with kmeans was done
  if(pre_kmeans_clusters > 0){
    //remove ".pre.centers"
    datafile = datafile.substr(0, datafile.size() - 12);
    recover_labels(datafile);
  }

  time(&end);

  std::cout << "computation times:\n";
  for(size_t i=0;i<times.size();++i){
    std::cout << "process " << i+1 << "\t" << times[i].first << "\t" << times[i].second << " sec\n";
  }
  std::cout << "Overall processing time of spectral clustering is " << (end - start) << " sec\n";

  return EXIT_SUCCESS;
}