// Copyright (c) 2019 Graphcore Ltd. All rights reserved.

#include "popops/NormaliseImage.hpp"
#include "poplibs_test/Util.hpp"
#include "popops/codelets.hpp"
#include "poputil/Util.hpp"
#include <boost/multi_array.hpp>
#include <boost/program_options.hpp>
#include <boost/random.hpp>
#include <poplar/Program.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <random>

using namespace poplar;
using namespace poplar::program;
using namespace poplibs_test::util;
using namespace poplibs_support;

static constexpr float halfTolerance = .005;
static constexpr float floatTolerance = 1e-6;

// Normalise the 3 input channels and pad a 4th zero channel
void refNormalise(const boost::multi_array<float, 4> &input,
                  boost::multi_array<float, 4> &output, float inScale,
                  std::vector<float> &offsets, std::vector<float> &scales) {
  for (std::size_t i = 0; i != input.num_elements() / 3; ++i) {
    for (unsigned c = 0; c != 3; ++c) {
      output.data()[i * 4 + c] =
          (input.data()[i * 3 + c] * inScale - offsets[c]) * scales[c];
    }
    output.data()[i * 4 + 3] = 0;
  }
}

static bool imageNormaliseTest(DeviceType deviceType, unsigned numTiles,
                               Type dType, std::size_t batchSize,
                               std::size_t fieldSize) {
  std::mt19937 randomEngine;
  boost::random::uniform_real_distribution<float> dist(
      0., dType == UNSIGNED_CHAR ? 256 : 10.);

  boost::multi_array<float, 4> input(
      boost::extents[batchSize][fieldSize][fieldSize][3]);
  boost::multi_array<float, 4> output(
      boost::extents[batchSize][fieldSize][fieldSize][4]);
  boost::multi_array<float, 4> refOutput(
      boost::extents[batchSize][fieldSize][fieldSize][4]);

  for (unsigned i = 0; i < input.num_elements(); ++i) {
    auto &val = *(input.data() + i);
    val = dist(randomEngine);
    if (dType == UNSIGNED_CHAR)
      val = floor(val);
  }

  const std::vector<std::size_t> shape = {batchSize, fieldSize, fieldSize, 3};

  auto device = createTestDevice(deviceType, 1, numTiles, true);
  const auto &target = device.getTarget();
  Graph graph(target);

  popops::addCodelets(graph);

  Type outType = dType == FLOAT ? FLOAT : HALF;
  Tensor tOffsets = graph.addVariable(outType, {3}, {"offsets"});
  Tensor tScales = graph.addVariable(outType, {3}, {"scales"});
  graph.setTileMapping(tOffsets, 0);
  graph.setTileMapping(tScales, 0);
  float inScale = 1. / 255;
  std::vector<float> offsets{0.485, 0.456, 0.406};
  std::vector<float> scales{1 / 0.229, 1 / 0.224, 1 / 0.225};
  Sequence seq;
  Tensor tIn =
      popops::createNormaliseImageInput(graph, dType, shape, {"TestIn"});
  Tensor tOut = popops::normaliseImage(graph, seq, tIn, inScale, tOffsets,
                                       tScales, {"Normalise"});

  Sequence uploadProg, downloadProg;
  std::vector<std::pair<std::string, char *>> tmap;

  auto rawHostInput = allocateHostMemoryForTensor(
      tIn, "input", graph, uploadProg, downloadProg, tmap);
  copy(target, input, dType, rawHostInput.get());
  auto rawHostScales = allocateHostMemoryForTensor(
      tScales, "scales", graph, uploadProg, downloadProg, tmap);
  copy(target, scales, outType, rawHostScales.get());
  auto rawHostOffsets = allocateHostMemoryForTensor(
      tOffsets, "offsets", graph, uploadProg, downloadProg, tmap);
  copy(target, offsets, outType, rawHostOffsets.get());

  auto rawHostOutput = allocateHostMemoryForTensor(
      tOut, "out", graph, uploadProg, downloadProg, tmap);

  Engine engine(graph, Sequence{uploadProg, seq, downloadProg});
  device.bind([&](const Device &d) {
    engine.load(d);
    attachStreams(engine, tmap);
    engine.run(0);
  });

  copy(target, outType, rawHostOutput.get(), output);

  refNormalise(input, refOutput, inScale, offsets, scales);
  bool debugging = false;
  auto tol = outType == FLOAT ? floatTolerance : halfTolerance;
  for (unsigned i = 0; i != output.num_elements(); ++i) {
    auto inVal = i % 4 == 3 ? 0 : input.data()[i / 4 * 3 + i % 4];
    auto outVal = output.data()[i];
    auto refVal = refOutput.data()[i];
    if (debugging)
      std::cerr << "i=" << i << ", in=" << inVal << ", ref=" << refVal
                << ", out=" << outVal << "\n";

    if (fabs(refVal - outVal) > tol &&
        (fabs(refVal) > tol && fabs(outVal) > tol &&
         fabs(refVal / outVal - 1.0) > tol)) {
      return false;
    }
  }
  return true;
}

int main(int argc, char **argv) {
  namespace po = boost::program_options;
  DeviceType deviceType;

  std::size_t batchSize = 1, fieldSize = 224;
  Type dType = FLOAT;
  unsigned numTiles = 4;

  po::options_description desc("Options");
  // clang-format off
  desc.add_options() ("help", "Print help")
    ("device-type",
     po::value<DeviceType>(&deviceType)->required(),
     "Device Type")
    ("batch-size",
     po::value<std::size_t>(&batchSize)->default_value(batchSize))
    ("field-size",
     po::value<std::size_t>(&fieldSize)->default_value(fieldSize))
    ("num-tiles",
     po::value<unsigned>(&numTiles),
     "Number of tiles to use (default chosen by heuristics)")
    ("data-type",
     po::value<Type>(&dType)->required(),
     "data type used in the test (uint8/half/float)");

  // clang-format on

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n\n";
      return 1;
    }
    po::notify(vm);
    if (dType != UNSIGNED_CHAR && dType != HALF && dType != FLOAT) {
      std::cout << "Only uchar, half and float are permitted\n";
      return 1;
    }
    if (!vm.count("num-tiles")) {
      // Use enough tiles to keep the footprint below 128K/tile.
      auto numElem = batchSize * fieldSize * fieldSize;
      auto bytes = (3 + 4) * sizeof(float) * numElem; // allow for FLOAT->FLOAT
      numTiles = ceil(bytes + (1024 * 128 - 1)) / (1024 * 128);
      std::cout << "Testing with " << numTiles << " tiles.\n";
    }
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  auto result =
      imageNormaliseTest(deviceType, numTiles, dType, batchSize, fieldSize);

  return !result;
}


#include "common.hpp"
#include <string>
#include <cassert>
#include <map>
#include <vector>
#include <tuple>
#include <algorithm>
#include <random>
#include <math.h>

// eigen
#include <Eigen/Core>

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

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
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 id, index;
    sscanf(row[0].c_str(), "%d", &id);
    sscanf(row[1].c_str(), "%d", &index);
    string category = row[2];
    res[index] = category;
    //printf("<%d><%d><%s>\n", id, index, category.c_str());
  }
  return res;
}

mat_t read_train_tensor(const string& infile, int m, int n){
  mat_t res = mat_t::Zero(m, n);
  ifstream ifs(infile);
  string line;
  while(getline(ifs, line)){
    auto row = split(line);
    assert(row.size() >= 3);
    int from, to;
    sscanf(row[1].c_str(), "%d", &from);
    sscanf(row[2].c_str(), "%d", &to);
    res(from, to) = 1;
  }
  return res;
}

mat_t load_mat(const string& infile){
  vector<vector<double> > mat;
  ifstream ifs(infile);
  string line;
  while(getline(ifs, line)){
    vector<double> row;
    for(const auto& field : split(line)){
      double x;
      sscanf(field.c_str(), "%lf", &x);
      row.push_back(x);
    }
    mat.push_back(row);
  }
  int m = mat.size();
  assert(m > 0);
  int n = mat[0].size();
  mat_t res = mat_t::Zero(m, n);
  for(int i = 0; i < m; ++i){
    assert(n == mat[i].size());
    for(int j = 0; j < n; ++j){
      res(i, j) = mat[i][j];
    }
  }
  return res;
}

mat_t ReadModelParameter(const string& prefix, int K, int ItrNum, const string& name){
  char buf[BUF_SIZE];
  sprintf(buf, "%s_Itr%d_%s.csv", prefix.c_str(), ItrNum, name.c_str());
  //read_file(buf);
  return load_mat(buf);
}

/*
  ########################### Read model parameters #############################
  # [output1]: A (N x K matrix)
  # [output2]: B (M x K matrix)
  # [output3]: C (M x K matrix)
  # [output4]: D (T x K matrix)
*/
tuple<mat_t, mat_t, mat_t, mat_t> ReadModelParameters(const string& prefix, int K, int ItrNum){
  mat_t A = ReadModelParameter(prefix, K, ItrNum, "A");
  mat_t B = ReadModelParameter(prefix, K, ItrNum, "B");
  mat_t C = ReadModelParameter(prefix, K, ItrNum, "C");
  mat_t D = ReadModelParameter(prefix, K, ItrNum, "D");
  return forward_as_tuple(A, B, C, D);
}

int sample_from_dist(const vec_t& df){
  int n = df.size();
  double rnd = uniform_real_rand();
  double prob_sum = 0;
  for(int i = 0; i < n; ++i){
    prob_sum += df(i);
    if(prob_sum >= rnd){return i;}
  }
  assert(0);
}

/*
  ############################## Synthesize traces ##############################
  # [input1]: A (N x K matrix)
  # [input2]: B (M x K matrix)
  # [input3]: D (T x K matrix)
  # [input4]: N -- Number of users
  # [input5]: M -- Number of POIs
  # [input6]: T -- Number of time slots
  # [input7]: poi_dic ({poi_index: category})
 */
void SynTraces(mat_t& A,
	       mat_t& B,
	       mat_t& C, // added
	       mat_t& D,
	       int N, int M, int T,
	       map<int, string>& poi_dic,
	       int K,
	       int ReadTrans,
	       int ReadVisit,
	       double VisThr,
	       double VisitDelta,
	       double TransDelta,
	       int TraceNum,
	       int TimInsNum,
	       const string& SynTraceFile,
	       int ItrNum,
	       const string& TrainTransTensorFile,
	       const string& TrainVisitTensorFile){
  char buf[BUF_SIZE];
  sprintf(buf, "%s_Itr%d.csv", SynTraceFile.c_str(), ItrNum);
  string outfile(buf);
  FILE* fp = fopen(outfile.c_str(), "w");

  // Output header information
  fprintf(fp, "user,trace_no,time_slot,time_instant,poi_index,category,loglikeli\n");

  // Read transitions from TrainTransTensorFile --> trans
  mat_t trans = mat_t::Zero(M, M);
  if(ReadTrans == 1){trans = read_train_tensor(TrainTransTensorFile, M, M);}

  // Read visits from TrainVisitTensorFile --> visit
  mat_t visit = mat_t::Zero(T, M);
  if(ReadVisit == 1){visit = read_train_tensor(TrainVisitTensorFile, T, M);}

  // For each user
  for(int n = 0; n < N; ++n){
    if((n > 0 && n % 1000 == 0) || n == N - 1){printf("Synthesized traces of %d users.\n", n);}
    // Initialization
    mat_t time_poi_dist = mat_t::Zero(T, M);
    mat_t time_poi_dist_sum = mat_t::Zero(T, 1);
    mat_t prop_mat = mat_t::Zero(M, M);
    mat_t trans_vec = mat_t::Zero(M, 1);

    // ################### Calculate the POI distributions ###################
    for(int t = 0; t < T; ++t){
      mat_t ad = A.row(n).array() * D.row(t).array();
      for(int i = 0; i < M; ++i){
	// Elements in a sampled visit tensor --> time_poi_dist
	time_poi_dist(t, i) = (ad.array() * B.row(i).array()).sum();
	// Assign VisitDelta for an element whose value is less than VisThr
	if(time_poi_dist(t, i) < VisThr){
	  time_poi_dist(t, i) = VisitDelta;
	}
	// Assign VisitDelta if there is no visits for time t & user i
	if(ReadVisit == 1 && visit(t,i) == 0){
	  time_poi_dist(t, i) = VisitDelta;
	}
      }
    }
    // Normalize time_poi_dist
    for(int t = 0; t < T; ++t){
      time_poi_dist_sum(t, 0) = time_poi_dist.row(t).sum();
      if(time_poi_dist_sum(t, 0) > 0){
	time_poi_dist.row(t) /= time_poi_dist_sum(t, 0);
      }else{
	fprintf(stderr, "Error: All probabilities are 0 for user %d and time %d\n", n, t);
	exit(1);
      }
    }

    // #################### Calculate the proposal matrix ####################
    //puts("Calculating the proposal matrix ...");
    for(int i = 0; i < M; ++i){
      mat_t ab = A.row(n).array() * B.row(i).array();
      // Elements in a sampled transition tensor (assign TransDelta for a small transition count) --> prop_mat
      for(int j = 0; j < M; ++j){
	// prop_mat[i,j] = max(np.sum(ab * C[j, :]), TransDelta)
	prop_mat(i,j) = max((ab.array() * C.row(j).array()).sum(), TransDelta);
	// Assign TransDelta if there is no transitions between i and j
	if(ReadTrans == 1 && trans(i,j) == 0){
	  prop_mat(i,j) = TransDelta;
	}
      }
      // Normalize prop_mat
      double row_sum = prop_mat.row(i).sum();
      prop_mat.row(i) /= row_sum;
    }

    // ########################## Synthesize traces ##########################
    //puts("Synthesizing traces ...");
    int poi_index_pre = 0;
    // For each trace
    for(int trace_no = 0; trace_no < TraceNum; ++trace_no){
	  double loglikeli = 0.0;
      // For each time slot
      for(int t = 0; t < T; ++t){
	// For each time instant
	for(int ins = 0; ins < TimInsNum; ++ins){
	  int poi_index = -1;
	  // Initial time slot and initial event
	  if(t == 0 && ins == 0){
	    // Randomly sample POI from the POI distribution
	    poi_index = sample_from_dist(time_poi_dist.row(t));
		loglikeli += log(time_poi_dist(t, poi_index));
	  }else{
	    // ##### Transform poi_index_pre into poi_index via MH (Metropolis-Hastings) ######
	    // Calculate the transition vector --> trans_vec
	    trans_vec(poi_index_pre,0) = 0;
	    for(int j = 0; j < M; ++j){
	      if(poi_index_pre != j){
		double alpha = (time_poi_dist(t, j) * prop_mat(j, poi_index_pre)) / (time_poi_dist(t, poi_index_pre) * prop_mat(poi_index_pre, j));
		trans_vec(j,0) = prop_mat(poi_index_pre, j) * min(1.0, alpha);
	      }
	    }
	    double row_sum = trans_vec.sum();
	    trans_vec(poi_index_pre,0) = 1 - row_sum;

	    // Transform poi_index_pre into poi_index via trans_vec
	    poi_index = sample_from_dist(trans_vec);
		loglikeli += log(trans_vec(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,%f\n", n, trace_no, t, ins, poi_index, poi_dic[poi_index].c_str(), loglikeli);

	  // Save the previous poi_index
	  poi_index_pre = poi_index;
	}
      }
    }
  }
  fclose(fp);
}

int main(int argc, char *argv[]){
  if(argc < 3){
      printf("Usage: SynData_PPMTF [Dataset] [City] ([TraceNum (default:10)] [Alp (default:200)] [MaxNumTrans (default:100)] [MaxNumVisit (default:100)] [ItrNum (default:100)])\n");
      return -1;
  }

  // Fix a seed
  seeding();

  // ################################# Parameters ##################################
  // Dataset
  const string Dataset = argv[1];
  // City
//  const string City = "TK";
  const string City = argv[2];

  // Number of traces per user
  int TraceNum = 10;
  if(argc >= 4){
    TraceNum = atoi(argv[3]);
  }
  // Hyper-hyper parameter alpha
  string AlpStr = "200";
  if(argc >= 5){
	  AlpStr = argv[4];
  }
  // Maximum number of transitions per user (-1: infinity)
  int MaxNumTrans = 100;
  if(argc >= 6){
	  MaxNumTrans = atoi(argv[5]);
  }
  // Maximum number of POI visits per user (-1: infinity)
  int MaxNumVisit = 100;
  if(argc >= 7){
	  MaxNumVisit = atoi(argv[6]);
  }
  // Number of iterations in Gibbs sampling
  int ItrNum = 100;
  if(argc >= 8){
	  ItrNum = atoi(argv[7]);
  }

  // Training user index file (input)
  string TUserIndexFile = "../data/" + Dataset + "/tuserindex_%s.csv";
  // POI index file (input)
  string POIIndexFile = "../data/" + Dataset + "/POIindex_%s.csv";
  // Training transition tensor file (input)
  string TrainTransTensorFile = "../data/" + Dataset + "/traintranstensor_%s.csv";
  // Training visit tensor file (input)
  string TrainVisitTensorFile = "../data/" + Dataset + "/trainvisittensor_%s.csv";

  // Prefix of the model parameter file (output)
  const string OutDir = "../data/" + Dataset + "/PPMTF_" + City + "_alp" + AlpStr + "_mnt" + std::to_string(MaxNumTrans) + "_mnv" + std::to_string(MaxNumVisit);
  // Prefix of the model parameter file (output)
  const string ModelParameterFile = OutDir + "/modelparameter";
  // Prefix of the synthesized trace file (output)
  const string SynTraceFile = OutDir + "/syntraces";

  // Name of the model parameter A
  //const string ParamA = "A";

  // Number of time slots
  int T;
  if(Dataset.find("PF") == 0){
	  T = 30;
  }else if(Dataset.find("FS") == 0){
	  T = 12;
  }else{
	  cout << "Wrong Dataset\n";
	  exit(-1);
  }

  // Number of columns in model parameters (A, B, C)
//  const int K = 32;
  const int K = 16;
  // Threshold for a visit count
  const double VisThr = 0;
  // Minimum value of a visit count
  const double VisitDelta = 0.00000001;
  // Minimum value of a transition count
  const double TransDelta = 0.00000001;
  // Read trans from TrainTransTensorFile (1:yes, 0:no)
  const int ReadTrans = 0;
  // Read visits from TrainVisitTensorFile (1:yes, 0:no)
  const int ReadVisit = 0;

  // Number of time instants per time slot
  int TimInsNum;
  if(Dataset.find("PF") == 0){
	  TimInsNum = 1;
  }else if(Dataset.find("FS") == 0){
    	  TimInsNum = 2;
  }else{
	  cout << "Wrong Dataset\n";
	  exit(-1);
  }
//  const int TimInsNum = 1;

  // Replace %s with City
  TUserIndexFile = string_replace(TUserIndexFile, City);
  POIIndexFile = string_replace(POIIndexFile, City);
  TrainTransTensorFile = string_replace(TrainTransTensorFile, City);
  TrainVisitTensorFile = string_replace(TrainVisitTensorFile, City);

  // Number of training users --> N
  int N = line_num(TUserIndexFile) - 1;
  // Number of POIs --> M
  int M = line_num(POIIndexFile) - 1;

  // Read the POI index file --> poi_dic ({poi_index: category})
  auto poi_dic = read_poi_dic(POIIndexFile);

  // Read model parameters
  mat_t A, B, C, D;
  tie(A, B, C, D) = ReadModelParameters(ModelParameterFile, K, ItrNum);

  // Synthesize traces
  SynTraces(A, B, C, D, N, M, T, poi_dic,
	    K, ReadTrans, ReadVisit, VisThr, VisitDelta, TransDelta, TraceNum, TimInsNum, SynTraceFile, ItrNum, TrainTransTensorFile, TrainVisitTensorFile);

  return 0;
}


#pragma once
#include "GriddingReconGadgetBase.h"
#include "mri_core_grappa.h"

#include "vector_td_utilities.h"
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include "cgSolver.h"
#include "hoNDArray_math.h"
#include "hoNDArray_utils.h"
#include <numeric>
#include <random>
#include "NonCartesianTools.h"
#include "NFFTOperator.h"

namespace Gadgetron {

template<template<class> class ARRAY> 	GriddingReconGadgetBase<ARRAY>::GriddingReconGadgetBase() : Gadget1() {}
template<template<class> class ARRAY> 	GriddingReconGadgetBase<ARRAY>::~GriddingReconGadgetBase() {}
template<template<class> class ARRAY> 	int GriddingReconGadgetBase<ARRAY>::process_config(ACE_Message_Block* mb)
	{
		// -------------------------------------------------

		ISMRMRD::IsmrmrdHeader h;
		deserialize(mb->rd_ptr(), h);

		auto matrixsize = h.encoding.front().encodedSpace.matrixSize;

		kernel_width_ = kernel_width.value();
		oversampling_factor_ = gridding_oversampling_factor.value();

		image_dims_.push_back(matrixsize.x);
		image_dims_.push_back(matrixsize.y);

		//Figure out what the oversampled matrix size should be taking the warp size into consideration.
		unsigned int warp_size = 32;
		image_dims_os_ = uint64d2
			(((static_cast<size_t>(std::ceil(image_dims_[0]*oversampling_factor_))+warp_size-1)/warp_size)*warp_size,
			 ((static_cast<size_t>(std::ceil(image_dims_[1]*oversampling_factor_))+warp_size-1)/warp_size)*warp_size);

		// In case the warp_size constraint kicked in
		oversampling_factor_ = float(image_dims_os_[0])/float(image_dims_[0]);
		this->initialize_encoding_space_limits(h);

		return GADGET_OK;
	}

template<template<class> class ARRAY> 	int GriddingReconGadgetBase<ARRAY>::process(Gadgetron::GadgetContainerMessage< IsmrmrdReconData >* m1)
	{

		std::unique_ptr<GadgetronTimer> timer;
		if (perform_timing) { timer = std::make_unique<GadgetronTimer>("Gridding Recon");}
		process_called_times++;

		IsmrmrdReconData* recon_bit_ = m1->getObjectPtr();

		// for every encoding space
		for (size_t e = 0; e < recon_bit_->rbit_.size(); e++)
		{

			IsmrmrdDataBuffered* buffer = &(recon_bit_->rbit_[e].data_);

			size_t RO = buffer->data_.get_size(0);
			size_t E1 = buffer->data_.get_size(1);
			size_t E2 = buffer->data_.get_size(2);
			size_t CHA = buffer->data_.get_size(3);
			size_t N = buffer->data_.get_size(4);
			size_t S = buffer->data_.get_size(5);
			size_t SLC = buffer->data_.get_size(6);

			if (E2 > 1) {
 				GERROR("3D data is not supported in GriddingReconGadgetBase\n");
				m1->release();
				return GADGET_FAIL;
			}

			if (buffer->trajectory_ == Core::none) {
				GERROR("Trajectories not found. Bailing out.\n");
				m1->release();
				return GADGET_FAIL;
			}

			std::vector<size_t> new_order = {0,1,2,4,5,6,3};

			boost::shared_ptr<ARRAY<float>> dcw;
			boost::shared_ptr<ARRAY<floatd2>> traj;

			auto & trajectory = *buffer->trajectory_;

			if (buffer->headers_[0].trajectory_dimensions == 3){
				auto traj_dcw = separate_traj_and_dcw(&trajectory);
				dcw = boost::make_shared<ARRAY<float>>(std::get<1>(traj_dcw).get());
				traj = boost::make_shared<ARRAY<floatd2>>(std::get<0>(traj_dcw).get());
			} else if (buffer->headers_[0].trajectory_dimensions == 2){
				auto old_traj_dims = *trajectory.get_dimensions();
				std::vector<size_t> traj_dims (old_traj_dims.begin()+1,old_traj_dims.end()); //Remove first element
				hoNDArray<floatd2> tmp_traj(traj_dims,(floatd2*)trajectory.get_data_ptr());
				traj = boost::make_shared<ARRAY<floatd2>>(tmp_traj);
			} else {
				throw std::runtime_error("Unsupported number of trajectory dimensions");
			}

			auto permuted = permute(*(hoNDArray<float_complext>*)&buffer->data_,new_order);
			ARRAY<float_complext> data(permuted);

			if (dcw){
				float scale_factor = float(prod(image_dims_os_))/asum(dcw.get());
				*dcw *= scale_factor;
			}

			//Gridding
			auto images = reconstruct(&data,traj.get(),dcw.get(),CHA);

			//Calculate coil sensitivity map
			auto csm = estimate_b1_map<float,2>(*images);

                        //Coil combine
			*images *= *conj(&csm);
			auto combined = sum(images.get(),images->get_number_of_dimensions()-1);

			auto host_img = as_hoNDArray(combined);

			IsmrmrdImageArray imarray;

			auto elements = imarray.data_.get_number_of_elements();
			imarray.data_ = std::move(*boost::reinterpret_pointer_cast<decltype(imarray.data_)>(host_img));
//			memcpy(imarray.data_.get_data_ptr(), host_img->get_data_ptr(), host_img->get_number_of_bytes());

			NonCartesian::append_image_header(imarray,recon_bit_->rbit_[e], e);
			this->prepare_image_array(imarray, e, ((int)e + 1), GADGETRON_IMAGE_REGULAR);

                        this->next()->putq(new GadgetContainerMessage<IsmrmrdImageArray>(std::move(imarray)));

                    //Is this where we measure SNR?
			if (replicas.value() > 0 && snr_frame.value() == process_called_times) {

				pseudo_replica(buffer->data_,*traj,*dcw,csm,recon_bit_->rbit_[e],e,CHA);
			}
		}

		m1->release();

		return GADGET_OK;
	}

template<template<class> class ARRAY> 	boost::shared_ptr<ARRAY<float_complext> > GriddingReconGadgetBase<ARRAY>::reconstruct(
		ARRAY<float_complext>* data,
		ARRAY<floatd2>* traj,
		ARRAY<float>* dcw,
		size_t ncoils ) {
		GadgetronTimer timer("Reconstruct");
		//We have density compensation and iteration is set to false
		if (!iterate.value() && dcw) {

			auto plan = NFFT<ARRAY,float,2>::make_plan(from_std_vector<size_t,2>(image_dims_),image_dims_os_,kernel_width_);
			std::vector<size_t> recon_dims = image_dims_;
			recon_dims.push_back(ncoils);
			auto result = new ARRAY<float_complext>(recon_dims);

			std::vector<size_t> flat_dims = {traj->get_number_of_elements()};
			ARRAY<floatd2> flat_traj(flat_dims,traj->get_data_ptr());

			plan->preprocess(flat_traj,NFFT_prep_mode::NC2C);
			plan->compute(*data,*result,dcw,NFFT_comp_mode::BACKWARDS_NC2C);

			return boost::shared_ptr<ARRAY<float_complext>>(result);

		} else { //No density compensation, we have to do iterative reconstruction.
			std::vector<size_t> recon_dims = image_dims_;
			recon_dims.push_back(ncoils);

			auto E = boost::make_shared<NFFTOperator<ARRAY,float,2>>();

                        auto data_cpy = data;

			E->setup(from_std_vector<size_t,2>(image_dims_),image_dims_os_,kernel_width_);
			if (dcw){
                              auto dcw_sqrt = boost::make_shared<ARRAY<float>>(*dcw);
                              sqrt_inplace(dcw_sqrt.get());
                              E->set_dcw(boost::make_shared<ARRAY<float>>(*dcw_sqrt));
                                data_cpy = new ARRAY<float_complext>(*data);
                                *data_cpy *= *dcw_sqrt;
			}
			std::vector<size_t> flat_dims = {traj->get_number_of_elements()};
			ARRAY<floatd2> flat_traj(flat_dims,traj->get_data_ptr());

			E->set_domain_dimensions(&recon_dims);
			cgSolver<ARRAY<float_complext>> solver;
			solver.set_max_iterations(iteration_max.value());
			solver.set_encoding_operator(E);
			solver.set_tc_tolerance(iteration_tol.value());
			solver.set_output_mode(decltype(solver)::OUTPUT_SILENT);
			E->set_codomain_dimensions(data->get_dimensions().get());
			E->preprocess(flat_traj);
			auto res = solver.solve(data_cpy);

                        if (dcw) delete data_cpy;

			return res;
		}
	}

template<template<class> class ARRAY> 	std::tuple<boost::shared_ptr<hoNDArray<floatd2 > >, boost::shared_ptr<hoNDArray<float >>> GriddingReconGadgetBase<ARRAY>::separate_traj_and_dcw(
		hoNDArray<float >* traj_dcw) {
		std::vector<size_t> dims = *traj_dcw->get_dimensions();
		std::vector<size_t> reduced_dims(dims.begin()+1,dims.end()); //Copy vector, but leave out first dim
		auto  dcw = boost::make_shared<hoNDArray<float>>(reduced_dims);

		auto traj = boost::make_shared<hoNDArray<floatd2>>(reduced_dims);

		auto dcw_ptr = dcw->get_data_ptr();
		auto traj_ptr = traj->get_data_ptr();
		auto ptr = traj_dcw->get_data_ptr();
		for (size_t i = 0; i < traj_dcw->get_number_of_elements()/3; i++){
			traj_ptr[i][0] = ptr[i*3];
			traj_ptr[i][1] = ptr[i*3+1];
			dcw_ptr[i] = ptr[i*3+2];
		}

		return std::make_tuple(traj,dcw);
	}

template<template<class> class ARRAY> 	void GriddingReconGadgetBase<ARRAY>::pseudo_replica(const hoNDArray<std::complex<float>>& data,
	ARRAY<floatd2>& traj,ARRAY<float>& dcw, const ARRAY<float_complext>& csm,
	const IsmrmrdReconBit& recon_bit, size_t encoding, size_t ncoils) {
		hoNDArray<std::complex<float> > rep_array(image_dims_[0], image_dims_[1], replicas.value());

		std::mt19937 engine;
		std::normal_distribution<float> distribution;

		for (size_t r = 0; r < replicas.value(); ++r) {

			if (r % 10 == 0) {
				GDEBUG("Running pseudo replics %d of %d\n", r, replicas.value());
			}
			hoNDArray<std::complex<float> > dtmp = data;
			std::vector<size_t> new_order = {0,1,2,4,5,6,3};
			auto permuted_rep = permute(*(hoNDArray<float_complext>*)&dtmp,new_order);
			auto dataptr = permuted_rep.get_data_ptr();

			for (size_t k =0; k <  permuted_rep.get_number_of_elements(); k++){
				dataptr[k] += std::complex<float>(distribution(engine),distribution(engine));
			}

			ARRAY<float_complext> data_rep(permuted_rep);

			auto images = reconstruct(&data_rep,&traj,&dcw,ncoils);

			//Coil combine
			*images *= *conj(&csm);
			auto combined = sum(images.get(),images->get_number_of_dimensions()-1);

			auto host_img = as_hoNDArray(combined);

			size_t offset = image_dims_[0]*image_dims_[1]*r;

			memcpy(rep_array.get_data_ptr()+offset, host_img->get_data_ptr(), host_img->get_number_of_bytes());
		}

		hoNDArray<float> mag(rep_array.dimensions());
		hoNDArray<float> mean(image_dims_[0],image_dims_[1]);
		hoNDArray<float> std(image_dims_[0],image_dims_[1]);

		Gadgetron::abs(rep_array, mag);
		Gadgetron::sum_over_dimension(mag,mean,2);
		Gadgetron::scal(1.0f/replicas.value(), mean);

		mag -= mean;
		mag *= mag;

		Gadgetron::sum_over_dimension(mag,std,2);
		Gadgetron::scal(1.0f/(replicas.value()-1), std);
		Gadgetron::sqrt_inplace(&std);

		//SNR image
		mean /= std;
		IsmrmrdImageArray imarray;
		imarray.data_ = *real_to_complex< std::complex<float> >(&mean);

		NonCartesian::append_image_header(imarray,recon_bit, encoding);
		this->prepare_image_array(imarray, encoding, image_series + 100 * ((int)encoding + 3), GADGETRON_IMAGE_SNR_MAP);
		this->next()->putq(new GadgetContainerMessage<IsmrmrdImageArray>(imarray));
	}

}


//
// A demonstration of using the KDTREE2 C++ routines, and timing.
// This file is in the public domain.
//

#include "kdtree2.hpp"

#include <boost/multi_array.hpp>
#include <boost/random.hpp>

static boost::minstd_rand generator(42u);
static boost::uniform_real<> uni_dist(0,1);
boost::variate_generator<boost::minstd_rand&,boost::uniform_real<> > uni(generator,uni_dist);

kdtree2::KDFloat random_variate() {
  // between [0,1)
  return(uni());
}

//
// define, for convenience a 2d array of floats.
//
typedef boost::multi_array<kdtree2::KDFloat,2> array2dfloat;

#include <ctime>

kdtree2::KDFloat time_a_search(kdtree2::KDTree* tree, int nn, int nsearch) {
  int dim = tree->dim;
  std::vector<kdtree2::KDFloat> query(dim);
  kdtree2::KDTreeResultVector result;

  clock_t t0, t1;

  t0 = clock();

  for (int i=0; i<nsearch;i++) {
      for (int j=0; j<dim; j++) query[j] = random_variate();
      tree->n_nearest(query,nn,result);
  }

  t1 = clock();

  return(static_cast<kdtree2::KDFloat>
	 (static_cast<double> (t1-t0) / static_cast<double> (CLOCKS_PER_SEC) ));
}

void time_random_searches(kdtree2::KDTree* tree, int nn) {
  // emit the number of searches per second.
  int nsearch;

  nsearch = 50;
  for(;;) {
    kdtree2::KDFloat t = time_a_search(tree,nn,nsearch);
    if (t < 1.0) {
      nsearch *= 5;
      continue;
    } else {
      kdtree2::KDFloat sps = kdtree2::KDFloat(nsearch) / t ;
      std::cout << "C++ impl, for nn=" << nn << " searches/sec = " << sps << "\n";
      return;
    }
  }
}

int main() {
  array2dfloat data(boost::extents[10][3]);  // declare a 10000 x 3 array.
  array2dfloat realdata;

  // notice it is in C-standard layout.
  kdtree2::KDTree* tree;
  kdtree2::KDTreeResultVector res;
  int N, dim;

  if (false) {
    for (int i=0; i<10; i++) {
      for (int j=0; j<3; j++)
	data[i][j] = static_cast<kdtree2::KDFloat> (3*i+j);
    }
    tree = new kdtree2::KDTree(data,true);

    //    tree->dump_data();
    //data[0][0]=666.0;  // mutate it underneath.  DO NOT DO THIS IN REAL USE
    //tree->dump_data(); // test to see it change there.

    tree->n_nearest_around_point(1,1,1,res);
    for (unsigned int i=0; i<res.size(); i++) {
      printf("result[%d]= (%d,%f)\n",i,res[i].idx,res[i].dis);
    }

    delete tree;
  }
  printf("Give me N, and dim (e.g. '1000 3').  No commas!");
  scanf("%d %d",&N,&dim);
  printf("I found N=%d,dim=%d\n",N,dim);
  realdata.resize(boost::extents[N][dim]);

  for (int i=0; i<N; i++) {
    for (int j=0; j<dim; j++)
      realdata[i][j] = random_variate();
  }

  tree = new kdtree2::KDTree(realdata,true);
  tree->sort_results = true;
  std::cout << "Tree created, now testing against brute force...";
  {
    std::vector<kdtree2::KDFloat> query(dim);
    kdtree2::KDTreeResultVector result, resultbrute;
    int nn = 10;

    for (int i=0; i<50; i++) {
      for (int j=0; j<dim; j++) query[j] = random_variate();

      tree->n_nearest_brute_force(query,nn,resultbrute);
      tree->n_nearest(query,nn,result); // search for 10 of them.

      for (int k=0; k<nn; k++) {
	if ((resultbrute[k].dis != result[k].dis) ||
	    (resultbrute[k].idx != result[k].idx)) {
	  std::cout << "Mismatch! nn=" << k << " brute=[" <<
	    resultbrute[k].dis << "," << resultbrute[k].idx <<
	    "] tree=[" << result[k].dis << "," << result[k].idx << "]\n";
	}
      }
    }
  }
  std::cout << "\nTesting complete.  Now testing timing...\n";
  tree->sort_results = false;

  {

    int nnarray[] = {1,5,10,25,500} ;
    for (int i=0; i< 5; i++) {
      time_random_searches(tree,nnarray[i]);
    }

  }

}


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

  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/constant/log_2.hpp>
#include <boost/simd/as.hpp>
#include <scalar_test.hpp>

STF_CASE_TPL( "Check log_2 behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log_2;
  using boost::simd::Log_2;
  T ref = T(0);
  STF_TYPE_IS(decltype(Log_2<T>()), T);
  STF_EQUAL(Log_2<T>(), ref);
  STF_EQUAL(log_2( as(T{}) ),ref);
}

STF_CASE_TPL( "Check log_2 behavior for double"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log_2;
  using boost::simd::Log_2;
  T ref = T(0.693147180559945309417232121458);

  STF_TYPE_IS(decltype(Log_2<T>()), T);
  STF_IEEE_EQUAL(Log_2<T>(), ref);
  STF_IEEE_EQUAL(log_2( as(T{}) ), ref);
}


#ifndef GRADIENT_H_
#define GRADIENT_H_

#include <Eigen/Dense>
#include <memory>
#include <string>

class Gradient {
public:
  Gradient(const Eigen::Ref<const Eigen::ArrayXXd> &model);
  virtual ~Gradient();
  virtual Eigen::ArrayXd compute(const double freq, const double c) const = 0;

protected:
  const Eigen::ArrayXXd model_;
  const int nl_;
};

class GradientEval {
public:
  GradientEval(const Eigen::Ref<const Eigen::ArrayXXd> &model,
               const std::string &wave_type);
  Eigen::ArrayXd compute(const double freq, const double c);

private:
  std::unique_ptr<Gradient> grad_;
};
#endif


#include <sstream>
#include <iostream>
#include <fstream>
#include <vector>
#include <cassert>
#include <cmath>

#include <boost/shared_ptr.hpp>
#include <boost/program_options.hpp>
#include <boost/program_options/variables_map.hpp>

#include "optimize.h"
#include "fdict.h"
#include "weights.h"
#include "sparse_vector.h"

using namespace std;
using boost::shared_ptr;
namespace po = boost::program_options;

void SanityCheck(const vector<double>& w) {
  for (int i = 0; i < w.size(); ++i) {
    assert(!isnan(w[i]));
    assert(!isinf(w[i]));
  }
}

struct FComp {
  const vector<double>& w_;
  FComp(const vector<double>& w) : w_(w) {}
  bool operator()(int a, int b) const {
    return fabs(w_[a]) > fabs(w_[b]);
  }
};

void ShowLargestFeatures(const vector<double>& w) {
  vector<int> fnums(w.size());
  for (int i = 0; i < w.size(); ++i)
    fnums[i] = i;
  vector<int>::iterator mid = fnums.begin();
  mid += (w.size() > 10 ? 10 : w.size());
  partial_sort(fnums.begin(), mid, fnums.end(), FComp(w));
  cerr << "TOP FEATURES:";
  for (vector<int>::iterator i = fnums.begin(); i != mid; ++i) {
    cerr << ' ' << FD::Convert(*i) << '=' << w[*i];
  }
  cerr << endl;
}

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("input_weights,i",po::value<string>(),"Input feature weights file")
        ("output_weights,o",po::value<string>()->default_value("-"),"Output feature weights file")
        ("optimization_method,m", po::value<string>()->default_value("lbfgs"), "Optimization method (sgd, lbfgs, rprop)")
        ("state,s",po::value<string>(),"Read (and write if output_state is not set) optimizer state from this state file. In the first iteration, the file should not exist.")
        ("input_format,f",po::value<string>()->default_value("b64"),"Encoding of the input (b64 or text)")
        ("output_state,S", po::value<string>(), "Output state file (optional override)")
	("correction_buffers,M", po::value<int>()->default_value(10), "Number of gradients for LBFGS to maintain in memory")
        ("eta,e", po::value<double>()->default_value(0.1), "Learning rate for SGD (eta)")
        ("gaussian_prior,p","Use a Gaussian prior on the weights")
        ("means,u", po::value<string>(), "File containing the means for Gaussian prior")
        ("sigma_squared", po::value<double>()->default_value(1.0), "Sigma squared term for spherical Gaussian prior");
  po::options_description clo("Command line options");
  clo.add_options()
        ("config", po::value<string>(), "Configuration file")
        ("help,h", "Print this help message and exit");
  po::options_description dconfig_options, dcmdline_options;
  dconfig_options.add(opts);
  dcmdline_options.add(opts).add(clo);

  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("config")) {
    ifstream config((*conf)["config"].as<string>().c_str());
    po::store(po::parse_config_file(config, dconfig_options), *conf);
  }
  po::notify(*conf);

  if (conf->count("help") || !conf->count("input_weights") || !conf->count("state")) {
    cerr << dcmdline_options << endl;
    exit(1);
  }
}

int main(int argc, char** argv) {
  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);

  const bool use_b64 = conf["input_format"].as<string>() == "b64";

  Weights weights;
  weights.InitFromFile(conf["input_weights"].as<string>());
  const string s_obj = "**OBJ**";
  int num_feats = FD::NumFeats();
  cerr << "Number of features: " << num_feats << endl;
  const bool gaussian_prior = conf.count("gaussian_prior");
  vector<double> means(num_feats, 0);
  if (conf.count("means")) {
    if (!gaussian_prior) {
      cerr << "Don't use --means without --gaussian_prior!\n";
      exit(1);
    }
    Weights wm;
    wm.InitFromFile(conf["means"].as<string>());
    if (num_feats != FD::NumFeats()) {
      cerr << "[ERROR] Means file had unexpected features!\n";
      exit(1);
    }
    wm.InitVector(&means);
  }
  shared_ptr<BatchOptimizer> o;
  const string omethod = conf["optimization_method"].as<string>();
  if (omethod == "rprop")
    o.reset(new RPropOptimizer(num_feats));  // TODO add configuration
  else
    o.reset(new LBFGSOptimizer(num_feats, conf["correction_buffers"].as<int>()));
  cerr << "Optimizer: " << o->Name() << endl;
  string state_file = conf["state"].as<string>();
  {
    ifstream in(state_file.c_str(), ios::binary);
    if (in)
      o->Load(&in);
    else
      cerr << "No state file found, assuming ITERATION 1\n";
  }

  vector<double> lambdas(num_feats, 0);
  weights.InitVector(&lambdas);
  double objective = 0;
  vector<double> gradient(num_feats, 0);
  // 0<TAB>**OBJ**=12.2;Feat1=2.3;Feat2=-0.2;
  // 0<TAB>**OBJ**=1.1;Feat1=1.0;
  int total_lines = 0;  // TODO - this should be a count of the
                        // training instances!!
  while(cin) {
    string line;
    getline(cin, line);
    if (line.empty()) continue;
    ++total_lines;
    int feat;
    double val;
    size_t i = line.find("\t");
    assert(i != string::npos);
    ++i;
    if (use_b64) {
      SparseVector<double> g;
      double obj;
      if (!B64::Decode(&obj, &g, &line[i], line.size() - i)) {
        cerr << "B64 decoder returned error, skipping gradient!\n";
	cerr << "  START: " << line.substr(0,line.size() > 200 ? 200 : line.size()) << endl;
	if (line.size() > 200)
	  cerr << "    END: " << line.substr(line.size() - 200, 200) << endl;
        cout << "-1\tRESTART\n";
        exit(99);
      }
      objective += obj;
      const SparseVector<double>& cg = g;
      for (SparseVector<double>::const_iterator it = cg.begin(); it != cg.end(); ++it) {
        if (it->first >= num_feats) {
	  cerr << "Unexpected feature in gradient: " << FD::Convert(it->first) << endl;
	  abort();
        }
        gradient[it->first] -= it->second;
      }
    } else {       // text encoding - your gradients will not be accurate!
      while (i < line.size()) {
        size_t start = i;
        while (line[i] != '=' && i < line.size()) ++i;
        if (i == line.size()) { cerr << "FORMAT ERROR\n"; break; }
        string fname = line.substr(start, i - start);
        if (fname == s_obj) {
          feat = -1;
        } else {
          feat = FD::Convert(line.substr(start, i - start));
          if (feat >= num_feats) {
	    cerr << "Unexpected feature in gradient: " << line.substr(start, i - start) << endl;
	    abort();
	  }
        }
        ++i;
        start = i;
        while (line[i] != ';' && i < line.size()) ++i;
        if (i - start == 0) continue;
        val = atof(line.substr(start, i - start).c_str());
        ++i;
        if (feat == -1) {
          objective += val;
        } else {
          gradient[feat] -= val;
        }
      }
    }
  }

  if (gaussian_prior) {
    const double sigsq = conf["sigma_squared"].as<double>();
    double norm = 0;
    for (int k = 1; k < lambdas.size(); ++k) {
      const double& lambda_k = lambdas[k];
      if (lambda_k) {
        const double param = (lambda_k - means[k]);
        norm += param * param;
        gradient[k] += param / sigsq;
      }
    }
    const double reg = norm / (2.0 * sigsq);
    cerr << "REGULARIZATION TERM: " << reg << endl;
    objective += reg;
  }
  cerr << "EVALUATION #" << o->EvaluationCount() << " OBJECTIVE: " << objective << endl;
  double gnorm = 0;
  for (int i = 0; i < gradient.size(); ++i)
    gnorm += gradient[i] * gradient[i];
  cerr << "  GNORM=" << sqrt(gnorm) << endl;
  vector<double> old = lambdas;
  int c = 0;
  while (old == lambdas) {
    ++c;
    if (c > 1) { cerr << "Same lambdas, repeating optimization\n"; }
    o->Optimize(objective, gradient, &lambdas);
    assert(c < 5);
  }
  old.clear();
  SanityCheck(lambdas);
  ShowLargestFeatures(lambdas);
  weights.InitFromVector(lambdas);
  weights.WriteToFile(conf["output_weights"].as<string>(), false);

  const bool conv = o->HasConverged();
  if (conv) { cerr << "OPTIMIZER REPORTS CONVERGENCE!\n"; }

  if (conf.count("output_state"))
    state_file = conf["output_state"].as<string>();
  ofstream out(state_file.c_str(), ios::binary);
  cerr << "Writing state to: " << state_file << endl;
  o->Save(&out);
  out.close();

  cout << o->EvaluationCount() << "\t" << conv << endl;
  return 0;
}


//
// Created by Lu WJ on 5/7/2017 AD.
//

#ifndef CRYPTCONV_MATRIX_HPP
#define CRYPTCONV_MATRIX_HPP
#include <NTL/matrix.h>
typedef long val_t;
typedef NTL::Mat<val_t> Matrix;
typedef NTL::Vec<val_t> PlainVec;
void randomize(Matrix *mat);
void zeros(Matrix *mat, long N);
void add(Matrix *mat, const Matrix &b);
PlainVec mul(const Matrix &a, const PlainVec &b);
Matrix mul(const Matrix &a, const Matrix &b);
void add(PlainVec *a, const PlainVec &b);
void transpose(Matrix *T, const Matrix &mat);
bool is_same(const Matrix &a, const Matrix &b);
bool is_same(const Matrix &a, const Matrix &b, long modulus);
bool load_matrix(Matrix *mat, const std::string &file);
bool load_matrix(std::istream& in, Matrix *mat);
bool save_matrix(const Matrix &mat, const std::string &file);
bool save_matrix(std::ostream &out, const Matrix &mat);
bool load_vector(PlainVec *vec, const std::string &file);
bool load_vector(std::istream &in, PlainVec *vec);
bool save_vector(const PlainVec &vec, const std::string &file);
bool save_vector(std::ostream &out, const PlainVec &vec);
#endif //CRYPTCONV_MATRIX_HPP


#pragma once

#include <vector>
#include <Eigen/Dense>

namespace cilantro {
    static std::vector<size_t> getPointToClusterIndexMap(const std::vector<std::vector<size_t>> &cluster_to_point,
                                                         size_t num_points)
    {
        std::vector<size_t> point_to_segment(num_points, cluster_to_point.size());
        for (size_t i = 0; i < cluster_to_point.size(); i++) {
            for (size_t j = 0; j < cluster_to_point[i].size(); j++) {
                point_to_segment[cluster_to_point[i][j]] = i;
            }
        }
        return point_to_segment;
    }

    static std::vector<std::vector<size_t>> getClusterToPointIndicesMap(const std::vector<size_t> &point_to_cluster,
                                                                        size_t num_clusters)
    {
        std::vector<std::vector<size_t>> segment_to_point(num_clusters);
        for (size_t i = 0; i < point_to_cluster.size(); i++) {
            segment_to_point[point_to_cluster[i]].emplace_back(i);
        }
        return segment_to_point;
    }

    static std::vector<size_t> getUnlabeledPointIndices(const std::vector<std::vector<size_t>> &cluster_to_point,
                                                        const std::vector<size_t> &point_to_cluster)
    {
        const size_t no_label = cluster_to_point.size();
        std::vector<size_t> res;
        res.reserve(point_to_cluster.size());
        for (size_t i = 0; i < point_to_cluster.size(); i++) {
            if (point_to_cluster[i] == no_label) res.emplace_back(i);
        }
        return res;
    }

    // CRTP base class that holds clustering results and accessors
    template <typename Derived = void>
    class ClusteringBase {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        inline const std::vector<std::vector<size_t>>& getClusterToPointIndicesMap() const { return cluster_to_point_indices_map_; }

        inline const std::vector<size_t>& getPointToClusterIndexMap() const { return point_to_cluster_index_map_; }

        inline size_t getNumberOfClusters() const { return cluster_to_point_indices_map_.size(); }

        inline size_t getNumberOfPoints() const { return point_to_cluster_index_map_.size(); }

        inline std::vector<size_t> getUnlabeledPointIndices() const {
            return cilantro::getUnlabeledPointIndices(cluster_to_point_indices_map_, point_to_cluster_index_map_);
        }

    protected:
        std::vector<std::vector<size_t>> cluster_to_point_indices_map_;
        std::vector<size_t> point_to_cluster_index_map_;
    };
}


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

  Copyright 2015 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/sqr.hpp>
#include <simd_test.hpp>
#include <boost/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>

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

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(sqr(bs::Inf<T>()), bs::Inf<T>());
  STF_EQUAL(sqr(bs::Minf<T>()), bs::Inf<T>());
  STF_IEEE_EQUAL(sqr(bs::Nan<T>()), bs::Nan<T>());
#endif
  STF_EQUAL(sqr(bs::Mone<T>()), bs::One<T>());
  STF_EQUAL(sqr(bs::One<T>()), bs::One<T>());
  STF_EQUAL(sqr(bs::Zero<T>()), bs::Zero<T>());
} // end of test for floating_

STF_CASE_TPL (" sqr unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::sqr;
  using r_t = decltype(sqr(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(sqr(bs::One<T>()), bs::One<T>());
  STF_EQUAL(sqr(bs::Zero<T>()), bs::Zero<T>());
} // end of test for unsigned_int_

STF_CASE_TPL (" sqr signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::sqr;
  using r_t = decltype(sqr(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(sqr(bs::Mone<T>()), bs::One<T>());
  STF_EQUAL(sqr(bs::One<T>()), bs::One<T>());
  STF_EQUAL(sqr(bs::Zero<T>()), bs::Zero<T>());
} // end of test for signed_int_


#include "stdafx.h"
#include "qmath.h"
#include "config.h"

// each test module could contain no more then one 'main' file with init function defined
// alternatively you could define init function yourself
#include <boost/test/unit_test.hpp>

// These are sample tests that show the different features of the framework

using namespace math;

namespace {

BOOST_AUTO_TEST_CASE(RANGE)
{
   {
		float a = 1.0f;
		float b = 2.0f;
		float c = 3.0f;

		BOOST_CHECK(min(a,b) == a);
		BOOST_CHECK(max(a,b) == b);
	}

	{
		double a = 1.0;
		double b = 2.0;
		double c = 3.0;

		BOOST_CHECK(min(a,b) == a);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(clamp(a,b,c) == b);
		BOOST_CHECK(clamp(b,a,c) == b);
		BOOST_CHECK(clamp(c,a,b) == b);
	}

	{
		int8_t a = 1;
		int8_t b = 2;
		int8_t c = 3;

		BOOST_CHECK(min(a,b) == a);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(clamp(a,b,c) == b);
		BOOST_CHECK(clamp(b,a,c) == b);
		BOOST_CHECK(clamp(c,a,b) == b);
	}

	{
		uint8_t a = 1;
		uint8_t b = 2;
		uint8_t c = 3;

		BOOST_CHECK(min(a,b) == a);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(clamp(a,b,c) == b);
		BOOST_CHECK(clamp(b,a,c) == b);
		BOOST_CHECK(clamp(c,a,b) == b);
	}

	{
		int16_t a = 1;
		int16_t b = 2;
	    int16_t c = 3;

		BOOST_CHECK(min(a,b) == a);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(clamp(a,b,c) == b);
		BOOST_CHECK(clamp(b,a,c) == b);
		BOOST_CHECK(clamp(c,a,b) == b);
	}

	{
		uint16_t a = 1;
		uint16_t b = 2;
		uint16_t c = 3;

		BOOST_CHECK(min(a,b) == a);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(clamp(a,b,c) == b);
		BOOST_CHECK(clamp(b,a,c) == b);
		BOOST_CHECK(clamp(c,a,b) == b);
	}

	{
		int32_t a = 1;
		int32_t b = 2;
		int32_t c = 3;

		BOOST_CHECK(min(a,b) == a);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(clamp(a,b,c) == b);
		BOOST_CHECK(clamp(b,a,c) == b);
		BOOST_CHECK(clamp(c,a,b) == b);
	}

	{
		uint32_t a = 1;
		uint32_t b = 2;
		uint32_t c = 3;

		BOOST_CHECK(min(a,b) == a);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(clamp(a,b,c) == b);
		BOOST_CHECK(clamp(b,a,c) == b);
		BOOST_CHECK(clamp(c,a,b) == b);
	}
	{
		int64_t a = 1;
		int64_t b = 2;
		int64_t c = 3;

		BOOST_CHECK(min(a,b) == a);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(clamp(a,b,c) == b);
		BOOST_CHECK(clamp(b,a,c) == b);
		BOOST_CHECK(clamp(c,a,b) == b);
	}
	{
		uint64_t a = 1;
		uint64_t b = 2;
		uint64_t c = 3;

		BOOST_CHECK(min(a,b) == a);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(clamp(a,b,c) == b);
		BOOST_CHECK(clamp(b,a,c) == b);
		BOOST_CHECK(clamp(c,a,b) == b);
	}

	{
		vec2f a(1.f,1.f);
		vec2f b(2.f,2.f);
		vec2f c(3.f,3.f);

		BOOST_CHECK(min(a,b) == a);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(clamp(a,b,c) == b);
		BOOST_CHECK(clamp(b,a,c) == b);
		BOOST_CHECK(clamp(c,a,b) == b);
	}

	{
		vec3f a(1.f, 1.f, 1.f);
		vec3f b(2.f, 2.f, 2.f);
		vec3f c(3.f, 3.f, 3.f);

		BOOST_CHECK(min(a,b) == a);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(clamp(a,b,c) == b);
		BOOST_CHECK(clamp(b,a,c) == b);
		BOOST_CHECK(clamp(c,a,b) == b);
	}
	{
		vec4f a(1.f, 1.f, 1.f, 1.f);
		vec4f b(2.f, 2.f, 2.f, 2.f);
		vec4f c(3.f, 3.f, 3.f, 3.f);

		BOOST_CHECK(min(a,b) == a);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(max(a,b) == b);
		BOOST_CHECK(clamp(a,b,c) == b);
		BOOST_CHECK(clamp(b,a,c) == b);
		BOOST_CHECK(clamp(c,a,b) == b);
	}

	{
		float a = 1.2f;
		float b = 1.0f;
		float c = 1.8f;
		float d = 2.0f;

		BOOST_CHECK(math::floor(a) == b);
		BOOST_CHECK(math::ceil(a) == d);
		BOOST_CHECK(math::round(a) == b);

		BOOST_CHECK(math::floor(c) == b);
		BOOST_CHECK(math::ceil(c) == d);
		BOOST_CHECK(math::round(c) == d);
	}

	{
		double a = 1.2;
		double b = 1.0;
		double c = 1.8;
	    double d = 2.0;

		BOOST_CHECK(floor(a) == b);
		BOOST_CHECK(ceil(a) == d);
		BOOST_CHECK(math::round(a) == b);

		BOOST_CHECK(floor(c) == b);
		BOOST_CHECK(ceil(c) == d);
		BOOST_CHECK(math::round(c) == d);
	}
}

}


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

// Copyright (c) 2020 Digvijay Janartha, Hamirpur, India.

// This file was modified by Oracle on 2020.
// Modifications copyright (c) 2020, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <test_common/test_point.hpp>

template <typename T>
bg::model::d2::point_xy<T> create_point_xy()
{
    T t1 = 1;
    T t2 = 2;
    return bg::model::d2::point_xy<T>(t1, t2);
}

template <typename P, typename T>
void check_point_xy(P& to_check, T x, T y)
{
    BOOST_CHECK_EQUAL(bg::get<0>(to_check), x);
    BOOST_CHECK_EQUAL(bg::get<1>(to_check), y);
    BOOST_CHECK_EQUAL(to_check.x(), x);
    BOOST_CHECK_EQUAL(to_check.y(), y);
}

template <typename T>
void test_default_constructor()
{
    bg::model::d2::point_xy<T> p(create_point_xy<T>());
    check_point_xy(p, 1, 2);
}

template <typename T>
void test_copy_constructor()
{
    bg::model::d2::point_xy<T> p(create_point_xy<T>());
    check_point_xy(p, 1, 2);
}

template <typename T>
void test_copy_assignment()
{
    bg::model::d2::point_xy<T> p(create_point_xy<T>());
    bg::set<0>(p, 4);
    bg::set<1>(p, 5);
    check_point_xy(p, 4, 5);
}

template <typename T>
void test_constexpr()
{
    typedef bg::model::d2::point_xy<T> P;
    constexpr P p = P(1, 2);
    constexpr T c = bg::get<0>(p);
    BOOST_CHECK_EQUAL(c, 1);
    check_point_xy(p, 1, 2);
}

template <typename T>
void test_all()
{
    test_default_constructor<T>();
    test_copy_constructor<T>();
    test_copy_assignment<T>();
    test_constexpr<T>();
}

int test_main(int, char* [])
{
    test_all<int>();
    test_all<float>();
    test_all<double>();

    return 0;
}


/* boost random/uniform_real.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: uniform_real.hpp 12208 2002-01-03 22:21:34Z jmaurer $
 *
 * Revision history
 *  2001-04-08  added min<max assertion (N. Becker)
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_UNIFORM_REAL_HPP
#define BOOST_RANDOM_UNIFORM_REAL_HPP

#include <cassert>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/random/uniform_01.hpp>

namespace boost {

// uniform distribution on a real range
template<class UniformRandomNumberGenerator, class RealType = double>
class uniform_real
{
public:
  typedef UniformRandomNumberGenerator base_type;
  typedef RealType result_type;
  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);

  uniform_real(base_type & rng, RealType min, RealType max)
    : _rng(rng), _min(min), _max(max)
  {
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    BOOST_STATIC_ASSERT(!std::numeric_limits<RealType>::is_integer);
#endif
    assert(min < max);
  }
  // compiler-generated copy ctor is fine
  // uniform_01 cannot be assigned, neither can this class

  result_type operator()() { return _rng() * (_max - _min) + _min; }
  result_type min() const { return _min; }
  result_type max() const { return _max; }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const uniform_real& x, const uniform_real& y)
  { return x._min == y._min && x._max == y._max && x._rng == y._rng; }
#else
  // Use a member function
  bool operator==(const uniform_real& rhs) const
  { return _min == rhs._min && _max == rhs._max && _rng == rhs._rng;  }
#endif
private:
  uniform_01<base_type, result_type> _rng;
  RealType _min, _max;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class UniformRandomNumberGenerator, class RealType>
const bool uniform_real<UniformRandomNumberGenerator, RealType>::has_fixed_range;
#endif

} // namespace boost

#endif // BOOST_RANDOM_UNIFORM_REAL_HPP


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 6, 3> test_double_6_3(const Eigen::Matrix<double, 6, 3> & M)
{
	return M;
}
void export_double_6_3()
{
	boost::python::def("test_double_6_3",test_double_6_3);
}


#ifndef _PROBLEM_HH_
#define _PROBLEM_HH_

#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <fstream>
#include <sstream>
#include <iostream>
#include <unordered_map>

typedef double val;
typedef Eigen::SparseMatrix<val, Eigen::RowMajor> sp_mat;
typedef Eigen::Matrix<val, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> mat;
typedef Eigen::Triplet<val> triplet;

class Entity {
public:
    int n;                                // number of training examples
    sp_mat A;                             // intrinsic graph structure among entities
    std::unordered_map<int, std::string> id_of;     // given the instance index, return id
    std::unordered_map<std::string, int> index_of;  // given the instance id, return index
    /*
     * load the entity graph from sparse representation
     */
    Entity(const char* filename) {
        assert(loadSparseGraph(filename));
    }
    /*
     * @FORMAT
     * As in LibSVM, each line of the input file should consist of
     * `instance_id<string> feature_1<int>:value_1<val> feature_2 value_2`
     *
     * A symmetric k-NN graph will be constructed afterwards
     *
     * Note the full kernel matrix is explicitly computed,
     * which might be inappropriate for large-scale problems
     */
    Entity(const char* filename, int k) {
        sp_mat X = load(filename);
        formCosKNNGraph(X, k);
    }
    bool saveSparseGraph(const char* filename);

private:
    void formCosKNNGraph(const sp_mat& X, int k);
    sp_mat load(const char* filename);
    bool loadSparseGraph(const char* filename);
};

class Relation {
public:
    Relation(const char* filename, const Entity& e1, const Entity& e2) {
        assert(load(filename, e1, e2));
    }
    /*
     * @DEFINITION
     * edge := (index_in_entity1, index_in_entity2, strength)
     */
    std::vector<triplet> edges;
    /*
     * @FORMAT
     * instance_id_for_e1 instance_id_for_e2
     */
private:
    bool load(const char* filename, const Entity& e1, const Entity& e2);
};

class Result {
public:
    val mae;
    val rmse;
};

#endif


/***************************************************************************
 *
 *   Copyright (C) 2021 by fxzjshm
 *   Licensed under the GNU General Public License, version 2.0
 *
 ***************************************************************************/

#define BOOST_COMPUTE_DEBUG_KERNEL_COMPILATION
#define HD_BENCHMARK

#include <boost/compute/system.hpp>
#include <boost/program_options.hpp>
#include <clFFT.h>
#include <filesystem>
#include <fstream>
#include <iostream>

#include "benchmark.hpp"
#include "io.hpp"
#include "kernel.hpp"
#include "types.h"

Stopwatch setup_timer, generate_timer, fft_timer, normalize_timer, copy_timer, write_timer;

int main(int argc, char **argv) {
    std::ios::sync_with_stdio(false);

    // ------------
    // Parse arguments & show help
    // ------------
    start_timer(setup_timer);
    boost::program_options::options_description general_option("General Options"), fft_option("FFT Options"), all_option("Options");
    using boost::program_options::value;
    /* clang-format off */
    general_option.add_options()
        ("help,h", "Show help message")
        ("input_file,f", value<std::string>(), "Input file")
        ("output_file,o", value<std::string>(), "Output file")
        ("text", "Read input file as text")
        ("inverse", "Transform filterbank to wave")
    ;
    fft_option.add_options()
        ("nsamp_seg", value<size_t>(), "Number of points to be FFT-ed in one segment")
        ("seg_count", value<size_t>()->default_value(1), "Number of segments of points to be FFT-ed at one kernel call")
        ("sample_rate", value<float>(), "Sample rate of input time series")
        ("fmin", value<float>(), "Min of frequency of output channel")
        ("fmax", value<float>(), "Max of frequency of output channel")
    ;
    /* clang-format on */
    all_option.add(general_option).add(fft_option);
    boost::program_options::positional_options_description p;
    p.add("input_file", 1);
    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(all_option).positional(p).run(), vm);
    boost::program_options::notify(vm);

    if (vm.count("help")) {
        std::cout << general_option << std::endl;
        std::cout << fft_option << std::endl;
        return 0;
    }
    if ((!vm.count("inverse")) && !(vm.count("nsamp_seg") && vm.count("input_file") && vm.count("sample_rate"))) {
        std::cout << general_option << std::endl;
        std::cout << fft_option << std::endl;
        return 1;
    }
    // ------------

    // ------------
    // Read arguments and set up
    // ------------
    bool inverse = (vm.count("inverse") != 0);
    size_t in_nsamp_seg = vm["nsamp_seg"].as<size_t>();
    size_t seg_count = vm["seg_count"].as<size_t>();
    size_t out_nsamp_seg;
    if (!inverse) {
        out_nsamp_seg = 1 + in_nsamp_seg / 2; // Note: count of complex numbers
    } else {
        out_nsamp_seg = 2 * (in_nsamp_seg - 1); // Note: count of real numbers, as here `in_nsamp_seg` is count of complex numbers
    }
    float sample_rate = vm["sample_rate"].as<float>();
    float df = sample_rate / in_nsamp_seg;
    float fmin = 0.0f;
    if (vm.count("fmin")) {
        fmin = vm["fmin"].as<float>();
    }
    float fmax;
    if (vm.count("fmax")) {
        fmax = vm["fmax"].as<float>();
    } else {
        fmax = (in_nsamp_seg / 2) * df;
    }
    size_t fmin_id = std::max(static_cast<size_t>(std::round(fmin / df)), static_cast<size_t>(0));
    size_t fmax_id = std::min(static_cast<size_t>(std::round(fmax / df)), static_cast<size_t>(out_nsamp_seg - 1));
    if (fmin_id > fmax_id) {
        std::cerr << "fmin_id = " << fmin_id << "but max fmax_id = " << fmax_id << std::endl;
        return -1;
    }
    size_t out_part_nsamp_seg = (fmax_id - fmin_id + 1);

    std::string in_file_name = vm["input_file"].as<std::string>();
    size_t in_file_nsamps;
    std::vector<data_type> h_in;
    if (vm.count("text")) {
        std::ifstream in_file_stream(in_file_name);
        // h_in = std::vector<data_type>(std::istream_iterator<data_type>(in_file_stream), {});
        data_type tmp;
        while (in_file_stream >> tmp) {
            h_in.push_back(tmp);
        }
        in_file_nsamps = h_in.size();
    } else {
        FILE *in_file_stream;
        in_file_stream = fopen(in_file_name.c_str(), "rb");
        size_t in_file_length = std::filesystem::file_size(in_file_name);
        in_file_nsamps = in_file_length / sizeof(data_type);
        h_in.resize(in_file_nsamps);
        fread(&h_in[0], 1, in_file_length, in_file_stream);
        fclose(in_file_stream);
    }
    size_t seg_count_all = in_file_nsamps / in_nsamp_seg;
    size_t out_file_nsamps = out_nsamp_seg * seg_count_all;
    size_t out_part_file_nsamps = out_part_nsamp_seg * seg_count_all;
    std::vector<data_type> h_out_complex(2 * out_file_nsamps), h_out_real(out_file_nsamps);
    std::vector<data_type> h_out_part(out_part_file_nsamps);

    // Set up device side
    namespace bc = boost::compute;
    bc::command_queue queue = bc::system::default_queue();
    bc::context context = queue.get_context();
    bc::device device = queue.get_device();
    clfft_caller<data_type> fft_caller(queue, in_nsamp_seg, seg_count, out_nsamp_seg, inverse);
    // ------------
    stop_timer(setup_timer);
    std::cout << "setup_timer: " << setup_timer.getTime() << std::endl;

    // ------------
    // Print info
    // ------------
    /* clang-format off */
    std::cout << "in_nsamp_seg = " << in_nsamp_seg << std::endl
              << "seg_count = " << seg_count << std::endl
              << "in_file_name = " << in_file_name << std::endl
              << "in_file_nsamps = " << in_file_nsamps << std::endl
              << "fmin = " << fmin << "  " << "fmax = " << fmax << "  " << "df = " << df << std::endl
              << "fmin_id = " << fmin_id << "  " << "fmax_id = " << fmax_id << std::endl;
    /* clang-format on */

    std::cout << "Using device " << device.name() << " on platform " << device.platform().name() << std::endl;
    // ------------

    /*
    // ------------
    start_timer(generate_timer);
    fft_caller.generate();
    stop_timer(generate_timer);
    std::cout << "generate_timer: " << generate_timer.getTime() << std::endl;
    // ------------
*/

    // ------------
    // Do fft
    // ------------
    fft_caller.call_fft(h_in, h_out_complex, h_out_real);
    std::cout << "\nfft_timer (average): " << fft_timer.getAverageTime() << " ms" << std::endl;
    // ------------

    fft_caller.teardown();

    start_timer(copy_timer);
    for (size_t i = 0; i < seg_count_all; i++) {
        for (size_t j = 0; j < out_part_nsamp_seg; j++) {
            size_t h_out_part_idx = out_part_nsamp_seg * i + j;
            size_t h_out_real_idx = out_nsamp_seg * i + (fmax_id - j);
            assert(h_out_part_idx < h_out_part.size());
            assert(h_out_real_idx < h_out_real.size());
            h_out_part[h_out_part_idx] = h_out_real[h_out_real_idx];
        }
        // write_vector(h_out_part, out_part_nsamp_seg, seg_count_all, std::string("fil-test-part") + std::to_string(i) + ".txt");
    }
    stop_timer(copy_timer);

    // ------------
    start_timer(write_timer);
    write_vector(h_in, in_file_nsamps, 1, "fil-test-in-1d.txt");
    write_vector(h_in, in_nsamp_seg, seg_count_all, "fil-test-in-2d.txt");
    write_vector(h_out_complex, 2 * out_nsamp_seg, seg_count_all, "fil-test-complex.txt");
    if (!inverse) {
        write_vector(h_out_real, out_nsamp_seg, seg_count_all, "fil-test-real.txt");
    } else {
        write_vector(h_out_real, "fil-test-real.txt");
    }
    write_vector(h_out_part, out_part_nsamp_seg, seg_count_all, "fil-test-part.txt");
    stop_timer(write_timer);
    std::cout << "write_timer: " << write_timer.getTime() << std::endl;
    // ------------

    return 0;
}


#include <string>
#include <vector>

#include <boost/filesystem.hpp>
#include <gmock/gmock.h>
#include <gtest/gtest.h>

#include <irkit/bitstream.hpp>
#include <irkit/coding/prefix_codec.hpp>
#include <irkit/prefixmap.hpp>

namespace {

namespace fs = boost::filesystem;

using map_type = irk::prefix_map<int, std::vector<char>>;
using block_builder = irk::prefix_map<int, std::vector<char>>::block_builder;
using block_ptr = irk::prefix_map<int, std::vector<char>>::block_ptr;

TEST(hutucker, individual_coding)
{
    std::string terms_file("terms.txt");

    // Build
    std::ifstream in(terms_file.c_str());
    std::vector<std::size_t> frequencies(256, 0);
    std::string item;
    while (std::getline(in, item)) {
        for (const char& ch : item) {
            ++frequencies[static_cast<unsigned char>(ch)];
        }
    }
    in.close();
    irk::hutucker_codec<char> codec(frequencies);

    std::ifstream in_terms(terms_file);
    std::string term;
    while (std::getline(in_terms, term)) {
        std::ostringstream enc;
        auto encoded = codec.encode(term.begin(), term.end());
        std::vector<char> data(encoded.size() / 8 + 1, 0);
        irk::bitptr<char> bp(data.data());
        irk::bitcpy(bp, encoded);
        auto reader = bp.reader();
        codec.decode(reader, enc, term.size());
        ASSERT_THAT(term, ::testing::ElementsAreArray(enc.str())) << term;
    }
}

TEST(hutucker, prefix_coding)
{
    std::string terms_file("terms.txt");

    // Build
    std::ifstream in(terms_file.c_str());
    std::vector<std::size_t> frequencies(256, 0);
    std::string item;
    while (std::getline(in, item)) {
        for (const char& ch : item) {
            ++frequencies[static_cast<unsigned char>(ch)];
        }
    }
    in.close();
    auto codec = irk::hutucker_codec<char>(frequencies);
    irk::prefix_codec<irk::hutucker_codec<char>> pref_codec(std::move(codec));

    int count = 0;
    std::vector<std::string> terms;
    std::ostringstream out_buffer;
    irk::output_bit_stream bout(out_buffer);
    std::string term, last = "";
    std::ifstream in_terms(terms_file);
    while (std::getline(in_terms, term)) {
        pref_codec.encode(term, bout);
        terms.push_back(term);
        ++count;
        break;
    }
    bout.flush();

    std::istringstream in_buffer(out_buffer.str());
    irk::input_bit_stream bin(in_buffer);
    for (int idx = 0; idx < count; ++idx) {
        std::string term;
        pref_codec.decode(bin, term);
        ASSERT_THAT(term, ::testing::ElementsAreArray(terms[idx]))
            << terms[idx] << "(" << idx << ")";
    }
}

}  // namespace

int main(int argc, char** argv)
{
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


/**
 * @file dbscan.hpp
 * @author Ryan Curtin
 *
 * An implementation of the DBSCAN clustering method, which is flexible enough
 * to support other algorithms for finding nearest neighbors.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_DBSCAN_DBSCAN_HPP
#define MLPACK_METHODS_DBSCAN_DBSCAN_HPP

#include <mlpack/core.hpp>
#include <mlpack/methods/range_search/range_search.hpp>
#include <mlpack/methods/emst/union_find.hpp>
#include "random_point_selection.hpp"
#include <boost/dynamic_bitset.hpp>

namespace mlpack {
namespace dbscan {

/**
 * DBSCAN (Density-Based Spatial Clustering of Applications with Noise) is a
 * clustering technique described in the following paper:
 *
 * @code
 * @inproceedings{ester1996density,
 *   title={A density-based algorithm for discovering clusters in large spatial
 *       databases with noise.},
 *   author={Ester, M. and Kriegel, H.-P. and Sander, J. and Xu, X.},
 *   booktitle={Proceedings of the Second International Conference on Knowledge
 *       Discovery and Data Mining (KDD '96)},
 *   pages={226--231},
 *   year={1996}
 * }
 * @endcode
 *
 * The DBSCAN algorithm iteratively clusters points using range searches with a
 * specified radius parameter.  This implementation allows configuration of the
 * range search technique used and the point selection strategy by means of
 * template parameters.
 *
 * @tparam RangeSearchType Class to use for range searching.
 * @tparam PointSelectionPolicy Strategy for selecting next point to cluster
 *      with.
 */
template<typename RangeSearchType = range::RangeSearch<>,
         typename PointSelectionPolicy = RandomPointSelection>
class DBSCAN
{
 public:
  /**
   * Construct the DBSCAN object with the given parameters.  The batchMode
   * parameter should be set to false in the case where RAM issues will be
   * encountered (i.e. if the dataset is very large or if epsilon is large).
   * When batchMode is false, each point will be searched iteratively, which
   * could be slower but will use less memory.
   *
   * @param epsilon Size of range query.
   * @param minPoints Minimum number of points for each cluster.
   * @param batchMode If true, all points are searched in batch.
   * @param rangeSearch Optional instantiated RangeSearch object.
   * @param pointSelector OptionL instantiated PointSelectionPolicy object.
   */
  DBSCAN(const double epsilon,
         const size_t minPoints,
         const bool batchMode = true,
         RangeSearchType rangeSearch = RangeSearchType(),
         PointSelectionPolicy pointSelector = PointSelectionPolicy());

  /**
   * Performs DBSCAN clustering on the data, returning number of clusters
   * and also the centroid of each cluster.
   *
   * @tparam MatType Type of matrix (arma::mat or arma::sp_mat).
   * @param data Dataset to cluster.
   * @param centroids Matrix in which centroids are stored.
   */
  template<typename MatType>
  size_t Cluster(const MatType& data,
                 arma::mat& centroids);

  /**
   * Performs DBSCAN clustering on the data, returning number of clusters
   * and also the list of cluster assignments.  If assignments[i] == SIZE_MAX,
   * then the point is considered "noise".
   *
   * @tparam MatType Type of matrix (arma::mat or arma::sp_mat).
   * @param data Dataset to cluster.
   * @param assignments Vector to store cluster assignments.
   */
  template<typename MatType>
  size_t Cluster(const MatType& data,
                 arma::Row<size_t>& assignments);

  /**
   * Performs DBSCAN clustering on the data, returning number of clusters,
   * the centroid of each cluster and also the list of cluster assignments.
   * If assignments[i] == SIZE_MAX, then the point is considered "noise".
   *
   * @tparam MatType Type of matrix (arma::mat or arma::sp_mat).
   * @param data Dataset to cluster.
   * @param assignments Vector to store cluster assignments.
   * @param centroids Matrix in which centroids are stored.
   */
  template<typename MatType>
  size_t Cluster(const MatType& data,
                 arma::Row<size_t>& assignments,
                 arma::mat& centroids);

 private:
  //! Maximum distance between two points to be part of same cluster.
  double epsilon;

  //! Minimum number of points to be in the epsilon-neighborhood (including
  //! itself) for the point to be a core-point.
  size_t minPoints;

  //! Whether or not to perform the search in batch mode.  If false, single
  bool batchMode;

  //! Instantiated range search policy.
  RangeSearchType rangeSearch;

  //! Instantiated point selection policy.
  PointSelectionPolicy pointSelector;

  /**
   * Performs DBSCAN clustering on the data, returning the number of clusters and
   * also the list of cluster assignments.  This searches each point iteratively,
   * and can save on RAM usage.  It may be slower than the batch search with a
   * dual-tree algorithm.
   *
   * @param data Dataset to cluster.
   * @param assignments Assignments for each point.
   * @param uf UnionFind structure that will be modified.
   */
  template<typename MatType>
  void PointwiseCluster(const MatType& data,
                        emst::UnionFind& uf);

  /**
   * Performs DBSCAN clustering on the data, returning number of clusters
   * and also the list of cluster assignments.  This can perform search in batch,
   * so it is well suited for dual-tree or naive search.
   *
   * @param data Dataset to cluster.
   * @param assignments Assignments for each point.
   * @param uf UnionFind structure that will be modified.
   */
  template<typename MatType>
  void BatchCluster(const MatType& data,
                    emst::UnionFind& uf);
};

} // namespace dbscan
} // namespace mlpack

// Include implementation.
#include "dbscan_impl.hpp"

#endif