#include "electronthresholds.h"
#include "python/pyreader.h"

#include <algorithm>
#include <cmath>
#include <random>

#include <Eigen/Dense>
#include <unsupported/Eigen/LevenbergMarquardt>

namespace stempy {

template <typename T>
double calculateMean(std::vector<T>& values)
{
  return std::accumulate(values.begin(), values.end(), 0.0) / values.size();
}

template <typename T>
double calculateVariance(std::vector<T>& values, double mean)
{
  double v1 = 0;
  double sigma2;

  for (size_t i = 0; i < values.size(); i++) {
    v1 += pow(values[i] - mean, 2.0);
  }
  sigma2 = v1 / (values.size() - 1.0);

  return sigma2;
}

struct GaussianErrorFunctor : Eigen::DenseFunctor<double>
{

  GaussianErrorFunctor(const Eigen::VectorXd& bins,
                       const Eigen::VectorXd& histogram)
    : Eigen::DenseFunctor<double>(3, bins.rows()), m_bins(bins),
      m_histogram(histogram)
  {}

  int operator()(const InputType& x, ValueType& fvec)
  {
    auto num = -(m_bins - ValueType::Constant(values(), x[1])).array().square();
    auto variance = pow(x[2], 2);

    fvec = x[0] * (num / (2 * variance)).exp() - m_histogram.array();

    return 0;
  }

  int df(const InputType& x, JacobianType& jacobian)
  {
    auto means = ValueType::Constant(values(), x[1]);
    auto tmp = (m_bins - means).array();
    auto num = -tmp.square();
    auto variance = pow(x[2], 2);
    auto den = 2 * variance;

    auto j0 = (num / den).exp();
    auto j1 = x[0] * tmp * j0 / variance;

    jacobian.col(0) = j0;
    jacobian.col(1) = j1;
    jacobian.col(2) = tmp * j1 / x[2];

    return 0;
  }

  Eigen::VectorXd m_bins;
  Eigen::VectorXd m_histogram;
};

template <typename BlockType, typename FrameType, bool dark>
CalculateThresholdsResults<FrameType> calculateThresholds(
  std::vector<BlockType>& blocks, const float darkReference[],
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma, const float gain[])
{
  auto frameDimensions = blocks[0].header.frameDimensions;
  auto numberOfPixels = frameDimensions.first * frameDimensions.second;

  // Setup random number engine
  std::random_device randomDevice;
  std::default_random_engine randomEngine(randomDevice());

  int numberSamplePixels =
    frameDimensions.first * frameDimensions.second * numberOfSamples;
  std::vector<FrameType> samples(numberSamplePixels, 0);
  for (int i = 0; i < numberOfSamples; i++) {
    std::uniform_int_distribution<int> randomBlockDist(0, blocks.size() - 1);
    auto randomBlockIndex = randomBlockDist(randomEngine);
    auto randomBlock = blocks[randomBlockIndex];
    std::uniform_int_distribution<int> randomFrameDist(
      0, randomBlock.header.imagesInBlock - 1);
    auto randomFrameIndex = randomFrameDist(randomEngine);
    auto blockData = randomBlock.data.get();

    for (unsigned j = 0; j < numberOfPixels; j++) {
      // For now just use the index, the image number don't seem to work, in the
      // current data set. In the future we should be using the image number.

      // This will be evaluated a compile time.
      if (std::is_integral<FrameType>::value) {
        auto value = blockData[randomFrameIndex * numberOfPixels + j];
        static_if<dark>(
          [&]() { value -= static_cast<int16_t>(darkReference[j]); })();
        samples[i * numberOfPixels + j] = value;
      }
      // if not integral type then we know we have gain and need to multiple
      else {
        auto value = blockData[randomFrameIndex * numberOfPixels + j] * gain[j];
        static_if<dark>(
          [&]() { value -= static_cast<float>(darkReference[j]); })();
        samples[i * numberOfPixels + j] = value;
      }
    }
  }

  // Calculate stats
  auto mean = calculateMean(samples);
  auto variance = calculateVariance(samples, mean);
  auto stdDev = sqrt(variance);
  auto xrayThreshold = mean + xRayThresholdNSigma * stdDev;

  // Now generate a histograms
  auto minMax = std::minmax_element(samples.begin(), samples.end());
  auto minSample = *minMax.first;
  auto maxSample = static_cast<FrameType>(std::ceil(*minMax.second));
  auto maxBin = std::min(static_cast<int>(maxSample),
                         static_cast<int>(mean + xrayThreshold * stdDev));
  auto minBin = std::max(static_cast<int>(minSample),
                         static_cast<int>(mean - xrayThreshold * stdDev));

  auto numberOfBins = maxBin - minBin;
  std::vector<double> histogram(numberOfBins, 0.0);
  std::vector<double> bins(numberOfBins);

  auto binEdge = minBin;
  for (int i = 0; i < numberOfBins; i++) {
    bins[i] = binEdge++;
  }

  // Bin the values
  for (int i = 0; i < numberSamplePixels; i++) {
    auto binIndex = static_cast<int>(samples[i] - minBin);
    // Skip values outside range
    if (binIndex >= numberOfBins) {
      continue;
    }
    histogram[binIndex] += 1;
  }

  auto b = Eigen::Map<Eigen::VectorXd>(bins.data(), bins.size());
  auto h = Eigen::Map<Eigen::VectorXd>(histogram.data(), histogram.size());

  GaussianErrorFunctor gef(b, h);
  Eigen::VectorXd state(3);
  auto indexOfMaxElement =
    std::max_element(histogram.begin(), histogram.end()) - histogram.begin();
  state << static_cast<double>(histogram[indexOfMaxElement]), mean, stdDev;

  Eigen::LevenbergMarquardt<GaussianErrorFunctor> solver(gef);
  solver.minimize(state);

  if (solver.info() != Eigen::ComputationInfo::Success) {
    throw std::runtime_error("Optimization did not converge");
  }

  auto optimizedMean = state[1];
  auto optimizedStdDev = fabs(state[2]);

  auto backgroundThreshold =
    optimizedMean + optimizedStdDev * backgroundThresholdNSigma;

  CalculateThresholdsResults<FrameType> ret;
  ret.numberOfSamples = numberOfSamples;
  ret.minSample = minSample;
  ret.maxSample = maxSample;
  ret.mean = mean;
  ret.variance = variance;
  ret.stdDev = stdDev;
  ret.numberOfBins = numberOfBins;
  ret.xRayThresholdNSigma = xRayThresholdNSigma;
  ret.backgroundThresholdNSigma = backgroundThresholdNSigma;
  ret.xRayThreshold = xrayThreshold;
  ret.backgroundThreshold = backgroundThreshold;
  ret.optimizedMean = optimizedMean;
  ret.optimizedStdDev = optimizedStdDev;

  return ret;
}

// Without gain
template <typename BlockType>
CalculateThresholdsResults<uint16_t> calculateThresholds(
  std::vector<BlockType>& blocks, Image<float>& darkreference,
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma)
{
  return calculateThresholds<BlockType, uint16_t>(
    blocks, darkreference.data.get(), numberOfSamples,
    backgroundThresholdNSigma, xRayThresholdNSigma, nullptr);
}

template <typename BlockType>
CalculateThresholdsResults<uint16_t> calculateThresholds(
  std::vector<BlockType>& blocks, const float darkreference[],
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma)
{
  return calculateThresholds<BlockType, uint16_t>(
    blocks, darkreference, numberOfSamples, backgroundThresholdNSigma,
    xRayThresholdNSigma, nullptr);
}

// With gain
template <typename BlockType>
CalculateThresholdsResults<float> calculateThresholds(
  std::vector<BlockType>& blocks, Image<float>& darkreference,
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma, const float gain[])
{
  return calculateThresholds<BlockType, float>(
    blocks, darkreference.data.get(), numberOfSamples,
    backgroundThresholdNSigma, xRayThresholdNSigma, gain);
}

// Without gain, without darkreference
template <typename BlockType>
CalculateThresholdsResults<uint16_t> calculateThresholds(
  std::vector<BlockType>& blocks, int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma)
{
  return calculateThresholds<BlockType, uint16_t, false>(
    blocks, nullptr, numberOfSamples, backgroundThresholdNSigma,
    xRayThresholdNSigma, nullptr);
}

// With gain
template CalculateThresholdsResults<float> calculateThresholds<Block>(
  std::vector<Block>& blocks, Image<float>& darkReference, int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma,
  const float gain[]);
template CalculateThresholdsResults<float> calculateThresholds<PyBlock>(
  std::vector<PyBlock>& blocks, Image<float>& darkReference,
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma, const float gain[]);
template CalculateThresholdsResults<float> calculateThresholds<Block, float, true>(
  std::vector<Block>& blocks, const float darkReference[], int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma,
  const float gain[]);
template CalculateThresholdsResults<uint16_t> calculateThresholds<Block>(
  std::vector<Block>& blocks, const float darkReference[], int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma,
  const float gain[]);
template CalculateThresholdsResults<float> calculateThresholds<PyBlock, float, true>(
  std::vector<PyBlock>& blocks, const float darkReference[],
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma, const float gain[]);

// No gain
template CalculateThresholdsResults<uint16_t> calculateThresholds<Block>(
  std::vector<Block>& blocks, Image<float>& darkReference, int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma);
template CalculateThresholdsResults<uint16_t> calculateThresholds<PyBlock>(
  std::vector<PyBlock>& blocks, Image<float>& darkReference,
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma);
template CalculateThresholdsResults<uint16_t> calculateThresholds<Block>(
  std::vector<Block>& blocks, const float darkReference[], int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma);
template CalculateThresholdsResults<uint16_t> calculateThresholds<PyBlock>(
  std::vector<PyBlock>& blocks, const float darkReference[],
  int numberOfSamples, double backgroundThresholdNSigma,
  double xRayThresholdNSigma);

template CalculateThresholdsResults<uint16_t>
calculateThresholds<Block, uint16_t, false>(std::vector<Block>& blocks,
                                            const float darkReference[],
                                            int numberOfSamples,
                                            double backgroundThresholdNSigma,
                                            double xRayThresholdNSigma,
                                            const float gain[]);
template CalculateThresholdsResults<uint16_t>
calculateThresholds<PyBlock, uint16_t, false>(std::vector<PyBlock>& blocks,
                                              const float darkReference[],
                                              int numberOfSamples,
                                              double backgroundThresholdNSigma,
                                              double xRayThresholdNSigma,
                                              const float gain[]);

template CalculateThresholdsResults<float>
calculateThresholds<Block, float, false>(std::vector<Block>& blocks,
                                         const float darkReference[],
                                         int numberOfSamples,
                                         double backgroundThresholdNSigma,
                                         double xRayThresholdNSigma,
                                         const float gain[]);
template CalculateThresholdsResults<float>
calculateThresholds<PyBlock, float, false>(std::vector<PyBlock>& blocks,
                                           const float darkReference[],
                                           int numberOfSamples,
                                           double backgroundThresholdNSigma,
                                           double xRayThresholdNSigma,
                                           const float gain[]);

// No gain or dark reference
template CalculateThresholdsResults<uint16_t> calculateThresholds(
  std::vector<Block>& blocks, int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma);

template CalculateThresholdsResults<uint16_t> calculateThresholds(
  std::vector<PyBlock>& blocks, int numberOfSamples,
  double backgroundThresholdNSigma, double xRayThresholdNSigma);
}


#include <stdio.h>
#include <FileLoop.h>
#include <FileWvOut.h>
#include <Fir.h>
#include <string.h>
#include <math.h>
#include <vector>
#include <Eigen/Eigen>
#include <Eigen/Dense>

#include "kiss_fftr.h"
#include "FIRFilterCode.h"
#include "eigen_to_image.h"

#include "ChunkStats.h"
#include "Chunk.h"

using namespace stk;

using Eigen::MatrixXf;
using Eigen::MatrixXi;
using std::list;

//main test
void process_song(std::string fn_in, std::string fn_out);

//thing with fourier transform
MatrixXf TFD_extract(FileWvIn input, int nfft, int total_slices, int samples_per_slice, int channels);
void TFDI_extract(kiss_fft_cpx* fft_input, int nfft, int samples_per_slice, StkFrames output);

//filtering methods
//using stk
void make_fir_bandpass_filter(double* coefs, int taps, int freq_center, int freq_margin,  int max_freq);
stk::Fir create_fir_from_coefs(double* coefs, int len);
stk::Fir create_1d_gaussian_filter(int length, double amplitude, double center, double stddev);
//using eigen
MatrixXf create_1d_gaussian_filter_col(int length, double amplitude, double stddev);
MatrixXf create_1d_gaussian_filter_row(int length, double amplitude, double stddev);
MatrixXf one_d_convolve(MatrixXf mat, MatrixXf kern);
MatrixXf dog(MatrixXf full, MatrixXf gauss);

//chunking functions
double snaz(MatrixXf filt, int snazr);
MatrixXi chunkify(MatrixXf tfd, int vert_range, int horz_range);
//list(Chunk) cull_chunks(MatrixXi chunk_ids, ChunkStats stats)
list<Chunk> cull_chunks(ChunkStats stats);

int main() {
  std::string file_in = "./sound/tsu.wav";
  std::string file_out = "./pet.wav";

  process_song(file_in, file_out);

  return 0;
}

void process_song(std::string fn_in, std::string fn_out) {
  FileRead song;
  FileWvIn input;
  FileWvOut output;
  fn_in = "./sound/tsu.wav";
  fn_out = "pet.wav";
  song.open(fn_in, 2);
  input.openFile(fn_in);
  output.openFile(fn_out, 2, FileWrite::FILE_WAV, Stk::STK_SINT16);

  int channels = input.channelsOut();
  long samples = song.fileSize();
  long samples_length = samples / channels;
  double file_rate = input.getFileRate();
  double len_in_sec = samples_length / file_rate;

  int samples_per_slice = 1024;
  double slices_per_second = file_rate / samples_per_slice;
  int total_slices = ceil(samples_length / samples_per_slice);

  int max_freq = file_rate / 2;
  //kiss_fft mentions that the fftr only populates nfft/2 + 1 things
  //guessing that means I initialize with nfft, then only look at nfft/2 + 1 results?
  //actualyl nfft should just be length of sample. is also the resulting frequency resolution.

  bool filter_test = false;
  if (filter_test) {
    FileWvOut filteredOut;
    filteredOut.openFile("hi.wav", 2, FileWrite::FILE_WAV, Stk::STK_SINT16);

    StkFrames slice(samples_per_slice, channels);
    int taps = 60;
    double coefs[taps];
    make_fir_bandpass_filter(coefs, taps, max_freq / 2, max_freq / 64, max_freq);
    stk::Fir bp = create_fir_from_coefs(coefs, taps);
    input.reset();

    for (int slice_i = 0; slice_i < total_slices; slice_i++) {
      input.tick(slice);
      for (int chan_i = 0; chan_i < channels; chan_i++) {
	bp.tick(slice, chan_i);
      }
      filteredOut.tick( slice );
    }
    input.reset();
  }

  int nfft = samples_per_slice;

  //generate time-frequency distribution
  MatrixXf tfd = TFD_extract(input, nfft, total_slices, samples_per_slice, channels);

  write_eigen_to_file("some_spectogram.png", tfd);
  //test out convolution code
  MatrixXf guass_col = create_1d_gaussian_filter_col(16, 1, 1);
  MatrixXf guass_row = create_1d_gaussian_filter_row(16, 1, 1);
  MatrixXf sobel_x (1,3);
  MatrixXf sobel_y (3,1);
  //3x3 sobels but I only implemented 1d convolution :(
  //sobel_x << -1, 0, 1, -3, 0, 3, -1, 0, 1;
  //sobel_y << -1, -3, -1, 0, 0, 0, 1, 3, 1;
  sobel_x << -1, 0, 1;
  sobel_y << -1, 0, 1;

  MatrixXf tfd_y = one_d_convolve(tfd, sobel_y);
  MatrixXf tfd_x = one_d_convolve(tfd, sobel_x);

  write_eigen_to_file("y_sobel_spectogram.png", tfd_y);
  write_eigen_to_file("x_sobel_spectogram.png", tfd_x);

  MatrixXf sobel = one_d_convolve(tfd, sobel_y);
  sobel = one_d_convolve(sobel, sobel_x);
  write_eigen_to_file("abs_sobel_spectogram.png", sobel);

  //chunkify(sobel, 1,2);

  MatrixXf result = one_d_convolve(tfd, guass_col);
  result = one_d_convolve(result, guass_row);
  write_eigen_to_file("gauss_spectogram.png", result);
  MatrixXf dogg = dog(tfd, result);
  write_eigen_to_file("dog_spectogram.png", dogg);

  MatrixXf x_sobeled_dog = one_d_convolve(dogg, sobel_x);
  MatrixXf y_sobeled_dog = one_d_convolve(dogg, sobel_y);

  write_eigen_to_file("x_sobel_dog_spectogram.png", x_sobeled_dog * 100);
  write_eigen_to_file("y_sobel_dog_spectogram.png", y_sobeled_dog * 100);
  int vert = 1;
  int horz = 2;
  MatrixXi chunk_ids = chunkify(y_sobeled_dog, vert,horz);
  //MatrixXi stats = chunk_stats(chunk_ids);
  ChunkStats stats = ChunkStats(chunk_ids);
  list<Chunk> y_imps = cull_chunks(stats);
  chunk_ids = chunkify(x_sobeled_dog, vert,horz);
  stats = ChunkStats(chunk_ids);
  list<Chunk> x_imps = cull_chunks(stats);
  list<Chunk> all_imps;
  all_imps.insert(all_imps.begin(), x_imps.begin(), x_imps.end());
  all_imps.insert(all_imps.begin(), y_imps.begin(), y_imps.end());
  std::cout << "length of joined list is " << all_imps.size() << "\n";

    /// close files
  song.close();
  input.closeFile();
  output.closeFile();
}

//extract the time-frequency distribution of a song
//going to have it return eigen
MatrixXf TFD_extract(FileWvIn input, int nfft, int total_slices, int samples_per_slice, int channels) {
  bool is_inverse = false;
  //numbe of binds fft puts things into. apparently it's the same as nfft.
  //since nothing is placed in the upper half, divide by 2
  //though if you want to play around with inverse fft, just set to nfft I think?
  int freq_chunks = nfft / 2;
  kiss_fftr_cfg cfg = kiss_fftr_alloc( nfft ,is_inverse,0,0 );
  size_t input_size = sizeof(kiss_fft_scalar) * samples_per_slice;
  size_t output_size = sizeof(kiss_fft_cpx) * freq_chunks;
  kiss_fft_scalar* fft_input =(kiss_fft_scalar*)KISS_FFT_MALLOC(input_size);
  kiss_fft_cpx* fft_output =(kiss_fft_cpx*)KISS_FFT_MALLOC(output_size);
  bool print = false;
  StkFrames slice(samples_per_slice, channels);
  StkFrames single_chan(samples_per_slice, 1);

  MatrixXf tfd = MatrixXf(freq_chunks, total_slices);
  tfd.setZero();
  for (int slice_i = 0; slice_i < total_slices; slice_i++) {
    input.tick(slice);
    for (int i = 0; i < channels; i++) {
      slice.getChannel(i, single_chan, 0);
      for (int i = 0; i < samples_per_slice; i++) {
	fft_input[i] = single_chan[i];
      }
      kiss_fftr(cfg, fft_input, fft_output);
      for (int freq_i = 0; freq_i < freq_chunks; freq_i++) {
	kiss_fft_cpx a = fft_output[freq_i];
	double modulus =  pow(a.r, 2) + pow(a.i, 2);
	if (print) {
	  std::cout << modulus << " ";
	}
	//plus equals to get both channels
	tfd(freq_i, slice_i) += modulus;
      }
      if (print) {
	std::cout << "\n";
      }
    }
  }
  std::cout << tfd(tfd.rows() / 4, tfd.cols() - 1) << "\n";
  kiss_fftr_free(cfg);
  return tfd;
}

//do the inverse of the TFD, which I guess just returns the original input amplitudes?
//call this inside the regular TFD extract, hand in cpx, output some scalar value into some passed in stk-frame output, then in tfd, tick that to some output file
void TFDI_extract(kiss_fft_cpx* fft_input, int nfft, int samples_per_slice, StkFrames output) {
  bool print = true;
  bool is_inverse = true;
  int freq_chunks = nfft / 2 + 1;
  kiss_fftr_cfg cfg = kiss_fftr_alloc( nfft ,is_inverse,0,0 );
  //just swapped the input/output sizes
  //actually don't even need the input size since data is handed in
  //size_t input_size = sizeof(kiss_fft_cpx) * freq_chunks;
  size_t output_size = sizeof(kiss_fft_scalar) * samples_per_slice;

  kiss_fft_scalar* fft_output =(kiss_fft_scalar*)KISS_FFT_MALLOC(output_size);

  //kiss_fftri(cfg, fft_input, fft_output);
  //output is a bunch of scalars, put into some stack frame
  for (int freq_i = 0; freq_i < samples_per_slice; freq_i++) {
    kiss_fft_scalar a = fft_output[freq_i];
    if (print) {
      std::cout << a << " ";
    }
    output[freq_i] = a;
  }
  if (print) {
    std::cout << "\n";
  }
  kiss_fftr_free(cfg);
}

//creates a 1-dimensional gaussian filter, as an stk finite impulse response filter
//  int filter_size = 20;
//  Fir gaussian_filt = create_1d_gaussian_filter(20, 1, filter_size / 2, 1);
stk::Fir create_1d_gaussian_filter(int length, double amplitude, double center, double stddev) {
  std::vector<StkFloat> kernel(0);
  StkFloat val = 0;
  double exp = 0;
  for (int i = 0; i < length; i++) {
    exp = -1 * pow(i - center, 2) / stddev;
    val = amplitude * pow(M_E, exp);
    kernel.insert(kernel.begin(), val);
  }
  Fir ret (kernel);
  return ret;
}

MatrixXf create_1d_gaussian_filter_col(int length, double amplitude, double stddev) {
  MatrixXf kern (1, length);
  float val = 0;
  double exp = 0;
  double center = (double)length / 2;
  for (int i = 0; i < length; i++) {
    exp = -1 * pow(i - center, 2) / stddev;
    val = amplitude * pow(M_E, exp);
    kern(0,i) = val;
  }
  return kern;;
}

MatrixXf create_1d_gaussian_filter_row(int length, double amplitude, double stddev) {
  return create_1d_gaussian_filter_col(length, amplitude, stddev).transpose();
}

void make_fir_bandpass_filter(double* coefs, int taps, int freq_center, int freq_margin,  int max_freq) {
  double rel_center = (double)freq_center / max_freq;
  double rel_margin = (double)freq_margin / max_freq;
  TFIRPassTypes band = firBPF;
  TWindowType wt = wtSINC;
  double some_window_param = 10;
  RectWinFIR(coefs, taps, band, rel_center, rel_margin);
  FIRFilterWindow(coefs, taps, wt, some_window_param);
}

stk::Fir create_fir_from_coefs(double* coefs, int len) {
  std::vector<StkFloat> kernel(0);
  StkFloat val = 0;
  for (int i = 0; i < len; i++) {
    val = coefs[i];
    kernel.insert(kernel.begin(), val);
  }
  Fir ret (kernel);
  return ret;
}

MatrixXf one_d_convolve(MatrixXf mat, MatrixXf kern) {
  //perform valid 1d convolution on matrix
  //or cross-corelation, planning on using symetric kernels so doesn't matter
  int res_dim;
  int kern_dim = -1;
  MatrixXf temp, result;
  int d_start, d_end, d_width;

  if (kern.rows() == 1) {
    result = MatrixXf(mat.rows(), mat.cols() - (kern.cols() - 1));
    result.setZero();
    res_dim = result.cols();
    d_width = mat.rows();
    kern_dim = kern.cols();
    for (int i = 0; i < kern_dim; i++) {
      d_start = i;
      d_end = res_dim;
      temp = mat.block(0,d_start, d_width, d_end);
      result += temp * kern(0,i);
    }
  }
  else if (kern.cols() == 1) {
    result = MatrixXf(mat.rows() - (kern.rows() - 1), mat.cols());
    result.setZero();
    res_dim = result.rows();
    d_width = mat.cols();
    kern_dim = kern.rows();
    for (int i = 0; i < kern_dim; i++) {
      d_start = i;
      d_end = res_dim;
      temp = mat.block(d_start, 0, d_end, d_width);
      result += temp * kern(i, 0);
    }
  }
  else {
    //error, kernel is not 1d
  }
  if (kern_dim % 2 == 0) {
    //kernel has even dim and doesn't have a perfect center index, funky stuff may happen
  }
  return result;
}

MatrixXf dog(MatrixXf full, MatrixXf gauss) {
  int f_rows = full.rows();
  int f_cols = full.cols();
  int g_rows = gauss.rows();
  int g_cols = gauss.cols();
  if (f_rows == g_rows && f_cols == g_cols) {
    return full - gauss;
  }
  else {
    MatrixXf temp = full.block((f_rows - g_rows) / 2, (f_cols - g_cols) / 2, g_rows, g_cols);
    temp = temp - gauss;
    return temp.cwiseAbs();
  }
}

//stands for sub nonzero-average zeroing
//take average of all non-zero elements, zero anything below average
//takes matrix and rounds to apply snaz, zeros values in matrix
//returns average of penultimate round
//having snazr as anything above 1 is typically excessive, consider at most using 2
double snaz(MatrixXf filt, int snazr) {
  int freq_range = filt.rows();
  int time_range = filt.cols();
  double nz_tot = 0;
  double nz_avg = 0;
  int nz_count = 0;
  float val = 0;
  for (int i = 0; i <= snazr; i++) {
    nz_tot = 0;
    nz_count = 0;
    //don't want to include zeros in average, can't use builtin sum methods, have to traverse :(
    //combined two tasks in one, do the sub_average zeroing then compute new average
    //need to to 1 final loop after outermost for loop finishes to do final sub_average zeroing
    //accomplished by using i <= snazr and only recomputing nz_avg if i < snazr
    for (int freq_i = 0; freq_i < freq_range; freq_i++) {
      for (int time_i = 0; time_i < time_range; time_i++) {
	val = filt(freq_i, time_i);
	if (val < nz_avg) {
	  filt(freq_i, time_i) = 0.0;
	}
	else {
	  if (i < snazr) {
	    nz_tot += val;
	    nz_count++;
	  }
	}
      }
    }
    if (i < snazr) {
      nz_avg = nz_tot / nz_count;
      std::cout << "non-zero average for round " << i << " is " << nz_avg << "\n";
    }
  }
  return nz_avg;
}

MatrixXi chunkify(MatrixXf tfd, int vert_range, int horz_range) {
  //chunkify the input matrix
  //defining chunkify as assign groups of sound in tfd that are signifigant together
  //do this by locally grouping cells within proximity in matrix
  //output is a new matrix, same dimensions, values are positive intergers
  //zero indicated cell wasn't assigned a chunk
  //positive integer represents some group id to which the cell got assigned to
  int chunk_id = 0;
  int freq_range = tfd.rows();
  int time_range = tfd.cols();
  int temp_i = -1;
  int temp = 0;
  float nz_avg = 0;
  MatrixXf filt (freq_range, time_range);
  MatrixXi chunk_ids (freq_range, time_range);
  filt << tfd;
  chunk_ids.setZero( );
  //threshold values < average to zero
  //do this to not care about very minor sounds
  //sub-nonzero-average-zeroing rounds
  nz_avg = snaz(filt, 1);

  //isn't a perfect grouping algorithim, since the local grouping is kind of naive
  //hoping further culling methods down the line will get rid of bad groups
  //but loop could be improved by having a more robust/expensive joining loop(s)
  for (int time_i = 0; time_i < time_range; time_i++) {
    for (int freq_i = 0; freq_i < freq_range; freq_i++) {
      if (filt(freq_i, time_i) > nz_avg) {
	//check if you want to join it vertically
	for (int freq_off = 1; freq_off <= vert_range; freq_off++) {
	  //group into chunks within freq_off indexes below current cell
	  temp_i = freq_i - freq_off;
	  if (temp_i >= 0) {
	    temp = chunk_ids(temp_i, time_i);
	    if (temp != 0) {
	      chunk_ids(freq_i, time_i) = temp;
	      break;
	    }
	  }
	  else {
	    break;
	  }
	}
	//check if you want to join it horizontally, potentially overwriting vertical joins
	for (int time_off = 1; time_off <= horz_range; time_off++) {
	  temp_i = time_i - time_off;
	  if (temp_i >= 0) {
	    temp = chunk_ids(freq_i, temp_i);
	    if (temp != 0) {
	      chunk_ids(freq_i, time_i) = temp;
	      break;
	    }
	  }
	  else {
	    break;
	  }
	}
	//else assigning to a new group
	if (chunk_ids(freq_i, time_i) == 0) {
	  chunk_ids(freq_i, time_i) = ++chunk_id;
	}
      }
    }
  }
  //basic error checking, making sure sufficiently low values aren't being assigned chunk_ids
  for (int time_i = 0; time_i < time_range; time_i++) {
    for (int freq_i = 0; freq_i < freq_range; freq_i++) {
      if (chunk_ids(freq_i, time_i) != 00 && filt(freq_i, time_i) < nz_avg) {
	std::cout << filt(freq_i, time_i) <<  "bad! ";
      }
    }
  }
  return chunk_ids;
}

list<Chunk> cull_chunks(ChunkStats stats) {
  //wait I don't even need the chunkids anymore

  //thinking of returning a linked list of chunk objects
  //because working with matrixes will probably be annoying past this point

  list<Chunk> chunk_list;
  MatrixXi sizes = stats.get_size();
  int chunks = sizes.rows();
  //ignoring chunk_id 0
  sizes = stats.get_size().block(1,0, chunks - 1,1);
  chunks = sizes.rows();

  double average_size = (float)sizes.sum() / sizes.sum();
  MatrixXi minf = stats.get_min_freq();
  MatrixXi maxf = stats.get_max_freq();
  MatrixXi mint = stats.get_min_time();
  MatrixXi maxt = stats.get_max_time();
  int chunk_count = 0;
  for (int i = 0; i < chunks; i++) {
    if (sizes(i) > average_size) {
      //gather min, max, delta of time and frequency, construct a chunk
      //find a better name for chunks to many things have chunk in the name
      chunk_count++;
      Chunk temp = Chunk(minf(i), maxf(i), mint(i), maxt(i));
      chunk_list.insert(chunk_list.begin(), temp);
    }
  }
  std::cout << "original size was " << chunks << " culled size is " << chunk_count << "\n";
  return chunk_list;
}


// gcc -O2 -march=native -DNDEBUG -o sfact sfact.cpp -std=c++11 -lstdc++ -lm -I../.. -I/usr/include/QtGui/ ../../libs/spacegroups/spacegroup.cpp ../../libs/spacegroups/crystalsys.cpp ../../libs/globals.cpp ../../libs/formfactors/formfact.cpp ../../tlibs/log/log.cpp -DNO_QT -lboost_system -lboost_filesystem -lboost_iostreams
/**
 * generates structure factors
 * @author Tobias Weber <tobias.weber@tum.de>
 * @date nov-2015
 * @license GPLv2
 */

#include <vector>
#include <unordered_map>
#include <tuple>
#include <algorithm>
#include <sstream>
#include "tlibs/math/linalg_ops.h"
#include "tlibs/phys/atoms.h"
#include "tlibs/phys/mag.h"
#include "tlibs/phys/lattice.h"
#include "tlibs/phys/neutrons.h"
#include "tlibs/string/string.h"
#include "tlibs/file/prop.h"
#include "libs/spacegroups/spacegroup.h"
#include "libs/formfactors/formfact.h"
#include "libs/globals.h"

namespace ublas = tl::ublas;

using t_real = t_real_glob;
typedef ublas::vector<t_real> t_vec;
typedef ublas::matrix<t_real> t_mat;

const std::string g_strXmlRoot("taz/");

// --------------------------------------------------------------------------------------------
// Lattice
// --------------------------------------------------------------------------------------------
static inline tl::Lattice<t_real> enter_lattice()
{
	std::cout << "\n----------------------------------------------------------------------\n" << std::endl;
	t_real a,b,c, alpha,beta,gamma;
	std::cout << "Enter unit cell lattice constants [a b c (in A)]: ";
	std::cin >> a >> b >> c;
	std::cout << "Enter unit cell angles [alpha beta gamma (in deg)]: ";
	std::cin >> alpha >> beta >> gamma;

	alpha = tl::d2r(alpha);
	beta = tl::d2r(beta);
	gamma = tl::d2r(gamma);

	return tl::Lattice<t_real>(a,b,c, alpha,beta,gamma);
}

static tl::Lattice<t_real> load_lattice(const tl::Prop<>& file)
{
	t_real a = file.Query<t_real>((g_strXmlRoot + "sample/a").c_str(), 0.);
	t_real b = file.Query<t_real>((g_strXmlRoot + "sample/b").c_str(), 0.);
	t_real c = file.Query<t_real>((g_strXmlRoot + "sample/c").c_str(), 0.);
	t_real alpha = file.Query<t_real>((g_strXmlRoot + "sample/alpha").c_str(), 0.);
	t_real beta = file.Query<t_real>((g_strXmlRoot + "sample/beta").c_str(), 0.);
	t_real gamma = file.Query<t_real>((g_strXmlRoot + "sample/gamma").c_str(), 0.);

	alpha = tl::d2r(alpha);
	beta = tl::d2r(beta);
	gamma = tl::d2r(gamma);

	return tl::Lattice<t_real>(a,b,c, alpha,beta,gamma);
}
// --------------------------------------------------------------------------------------------

// --------------------------------------------------------------------------------------------
// Space group
// --------------------------------------------------------------------------------------------
static inline std::string enter_spacegroup()
{
	std::cout << "\n----------------------------------------------------------------------\n" << std::endl;
	std::string strSg;
	std::cout << "Enter spacegroup: ";
	std::cin.ignore();
	std::getline(std::cin, strSg);

	return strSg;
}

static std::string load_spacegroup(const tl::Prop<>& file)
{
	std::string strSG = file.Query<std::string>((g_strXmlRoot + "sample/spacegroup").c_str(), "");
	tl::trim(strSG);

	return strSG;
}
// --------------------------------------------------------------------------------------------

// --------------------------------------------------------------------------------------------
// Atom positions
// --------------------------------------------------------------------------------------------
static inline
std::tuple<std::vector<std::string>, std::vector<t_vec>, std::unordered_map<std::string, t_real>>
enter_atoms()
{
	std::cout << "\n----------------------------------------------------------------------\n" << std::endl;
	int iAtom=0;
	std::vector<std::string> vecElems;
	std::vector<t_vec> vecAtoms;
	std::unordered_map<std::string, t_real> mapMag;

	while(1)
	{
		std::cout << "Enter element name " << (++iAtom) << " name (or <Enter> to finish): ";
		std::string strElem;
		std::getline(std::cin, strElem);
		tl::trim(strElem);
		if(strElem == "")
			break;

		std::cout << "Enter atom position " << (iAtom) << " [x y z (in frac. units)]: ";
		std::string strAtom;
		std::getline(std::cin, strAtom);
		tl::trim(strAtom);
		if(strAtom == "")
			break;

		std::cout << "Enter atom " << (iAtom) << " effective magnetic moment [muB]: ";
		std::string strMag;
		std::getline(std::cin, strMag);
		tl::trim(strMag);
		if(strMag == "")
			strMag = "0";

		vecElems.push_back(strElem);

		t_vec vec(4);
		std::istringstream istrAtom(strAtom);
		istrAtom >> vec[0] >> vec[1] >> vec[2];
		vec[3] = 1.;
		vecAtoms.push_back(vec);

		t_real dMag;
		std::istringstream istrMag(strMag);
		istrMag >> dMag;
		dMag = tl::mag_scatlen_eff(dMag);
		mapMag[strElem] = dMag;
	}

	return std::make_tuple(vecElems, vecAtoms, mapMag);
}

static inline
std::tuple<std::vector<std::string>, std::vector<t_vec>, std::unordered_map<std::string, t_real>>
load_atoms(const tl::Prop<>& file)
{
	std::vector<std::string> vecElems;
	std::vector<t_vec> vecAtoms;
	std::unordered_map<std::string, t_real> mapMag;

	std::size_t iNumAtoms = file.Query<std::size_t>((g_strXmlRoot + "sample/atoms/num").c_str(), 0);
	vecElems.reserve(iNumAtoms);
	vecAtoms.reserve(iNumAtoms);

	for(std::size_t iAtom=0; iAtom<iNumAtoms; ++iAtom)
	{
		std::string strNr = tl::var_to_str(iAtom);

		std::string strAtomName =
			file.Query<std::string>((g_strXmlRoot + "sample/atoms/" + strNr + "/name").c_str(), "");

		t_vec vec(4);
		vec[0] = file.Query<t_real>((g_strXmlRoot + "sample/atoms/" + strNr + "/x").c_str(), 0.);
		vec[1] = file.Query<t_real>((g_strXmlRoot + "sample/atoms/" + strNr + "/y").c_str(), 0.);
		vec[2] = file.Query<t_real>((g_strXmlRoot + "sample/atoms/" + strNr + "/z").c_str(), 0.);
		vec[3] = 1.;

		vecElems.push_back(strAtomName);
		vecAtoms.push_back(vec);
	}

	return std::make_tuple(vecElems, vecAtoms, mapMag);
}
// --------------------------------------------------------------------------------------------

void gen_atoms_sfact(const char *pcFile = nullptr)
{
	bool bHasFile = (pcFile != nullptr);
	tl::Prop<> file;
	if(bHasFile)
	{
		bHasFile = file.Load(pcFile, tl::PropType::XML);
		if(!bHasFile)
			std::cerr << "Error: Invalid file \"" << pcFile << "\"." << std::endl;
	}

	// --------------------------------------------------------------------------------------------
	// Tables
	// --------------------------------------------------------------------------------------------
	std::shared_ptr<const xtl::ScatlenList<t_real>> lst = xtl::ScatlenList<t_real>::GetInstance();
	std::shared_ptr<const xtl::FormfactList<t_real>> lstff = xtl::FormfactList<t_real>::GetInstance();
	std::shared_ptr<const xtl::MagFormfactList<t_real>> lstmff = xtl::MagFormfactList<t_real>::GetInstance();
	std::shared_ptr<const xtl::SpaceGroups<t_real>> sgs = xtl::SpaceGroups<t_real>::GetInstance();
	// --------------------------------------------------------------------------------------------

	// --------------------------------------------------------------------------------------------
	// Lattice
	// --------------------------------------------------------------------------------------------
	tl::Lattice<t_real> lattice;
	if(bHasFile)
		lattice = load_lattice(file);
	else
		lattice = enter_lattice();

	const t_real dVol = lattice.GetVol();
	const t_mat matA = lattice.GetBaseMatrixCov();
	const t_mat matB = lattice.GetRecip().GetBaseMatrixCov();
	std::cout << "Unit cell volume: " << dVol << " A^3" << std::endl;
	std::cout << "A = " << matA << std::endl;
	std::cout << "B = " << matB << std::endl;
	// --------------------------------------------------------------------------------------------

	// --------------------------------------------------------------------------------------------
	// Space group
	// --------------------------------------------------------------------------------------------
	std::string strSg;
	if(bHasFile)
		strSg = load_spacegroup(file);
	else
		strSg = enter_spacegroup();

	const auto pSg = sgs->Find(strSg);
	if(!pSg)
	{
		std::cerr << "Error: Unknown spacegroup." << std::endl;
		return;
	}
	std::cout << "Spacegroup " << pSg->GetNr() << ": " << pSg->GetName() << "." << std::endl;

	const std::vector<t_mat>& vecTrafos = pSg->GetTrafos();
	std::cout << vecTrafos.size() << " symmetry operations in spacegroup." << std::endl;
	// --------------------------------------------------------------------------------------------

	// --------------------------------------------------------------------------------------------
	// Atom positions
	// --------------------------------------------------------------------------------------------
	int iAtom=0;
	std::vector<std::string> vecElems;
	std::vector<t_vec> vecAtoms;
	std::unordered_map<std::string, t_real> mapMag;

	if(bHasFile)
		std::tie(vecElems, vecAtoms, mapMag) = load_atoms(file);
	else
		std::tie(vecElems, vecAtoms, mapMag) = enter_atoms();
	// --------------------------------------------------------------------------------------------

	// --------------------------------------------------------------------------------------------
	// Unit cell & Scattering lengths (assuming magnetic unit cell is the same as structural one)
	// --------------------------------------------------------------------------------------------
	std::vector<unsigned int> vecNumAtoms;
	std::vector<t_vec> vecAllAtoms;
	std::vector<std::complex<t_real>> vecScatlens;
	std::vector<std::complex<t_real>> vecMagScatlens;
	std::vector<int> vecAtomIndices;

	t_real dsigCoh = 0.;
	t_real dsigInc = 0.;
	t_real dsigScat = 0.;
	t_real dsigAbs = 0.;

	t_real dSigAbs = 0.;
	t_real dSigScat = 0.;

	for(int iAtom=0; iAtom<int(vecAtoms.size()); ++iAtom)
	{
		// generate symmetry-equivalent atom positions
		const t_vec& vecAtom = vecAtoms[iAtom];
		std::vector<t_vec> vecPos = tl::generate_atoms<t_mat, t_vec, std::vector>(vecTrafos, vecAtom);
		vecNumAtoms.push_back(vecPos.size());
		std::cout << "Generated " << vecPos.size() << " " << vecElems[iAtom] << " atoms." << std::endl;
		for(const t_vec& vec : vecPos)
			std::cout << vec << std::endl;

		// get scattering lengths
		const xtl::ScatlenList<t_real>::elem_type* pElem = lst->Find(vecElems[iAtom]);
		if(pElem == nullptr)
		{
			std::cerr << "Error: cannot get scattering length for "
				<< vecElems[iAtom] << "." << std::endl;
			return;
		}
		std::complex<t_real> b = pElem->GetCoherent();

		// microscopic cross-sections
		dsigCoh += pElem->GetXSecCoherent().real()*vecNumAtoms[iAtom];
		dsigInc += pElem->GetXSecIncoherent().real()*vecNumAtoms[iAtom];
		dsigScat += pElem->GetXSecScatter().real()*vecNumAtoms[iAtom];
		dsigAbs += pElem->GetXSecAbsorption().real()*vecNumAtoms[iAtom];

		// get magnetic scattering lengths
		std::complex<t_real> p(0.);
		auto iterMag = mapMag.find(vecElems[iAtom]);
		if(iterMag == mapMag.end())
		{
			std::cerr << "Warning: cannot get effective magnetic scattering length for "
				<< vecElems[iAtom] << "." << std::endl;
		}
		else
		{
			p = iterMag->second;
		}

		// macroscopic cross-sections
		dSigScat += tl::macro_xsect(pElem->GetXSecScatter().real()*tl::get_one_femtometer<t_real>()*tl::get_one_femtometer<t_real>(),
			vecNumAtoms[iAtom],
			dVol*tl::get_one_angstrom<t_real>()*tl::get_one_angstrom<t_real>()*tl::get_one_angstrom<t_real>()) * tl::get_one_centimeter<t_real>();

		dSigAbs += tl::macro_xsect(pElem->GetXSecAbsorption().real()*tl::get_one_femtometer<t_real>()*tl::get_one_femtometer<t_real>(),
			vecNumAtoms[iAtom],
			dVol*tl::get_one_angstrom<t_real>()*tl::get_one_angstrom<t_real>()*tl::get_one_angstrom<t_real>()) * tl::get_one_centimeter<t_real>();

		// store calculations
		for(t_vec vecThisAtom : vecPos)
		{
			vecThisAtom.resize(3,1);
			vecAllAtoms.push_back(tl::mult<t_mat, t_vec>(matA, vecThisAtom));
			vecScatlens.push_back(b);
			vecMagScatlens.push_back(p);
			vecAtomIndices.push_back(iAtom);
		}
	}

	std::cout << "\nMicroscopic coherent cross-section: " << dsigCoh << " fm^2." << std::endl;
	std::cout << "Microscopic incoherent cross-section: " << dsigInc << " fm^2." << std::endl;
	std::cout << "Microscopic total scattering cross-section: " << dsigScat << " fm^2." << std::endl;
	std::cout << "Microscopic absorption cross-section: " << dsigAbs << " fm^2." << std::endl;

	const t_real dLam0 = 1.8;	// thermal
	const t_real dLam = 4.5;
	std::cout << "\nMacroscopic total scattering cross-section for lambda = 4.5 A: "
		<< dSigScat*dLam/dLam0 << " / cm." << std::endl;
	std::cout << "Macroscopic absorption cross-section for lambda = 4.5 A: "
		<< dSigAbs*dLam/dLam0 << " / cm." << std::endl;

	//for(const t_vec& vecAt : vecAllAtoms) std::cout << vecAt << std::endl;
	//for(const std::complex<t_real>& cb : vecScatlens) std::cout << cb << std::endl;
	// --------------------------------------------------------------------------------------------

	// --------------------------------------------------------------------------------------------
	// Bragg peaks
	// --------------------------------------------------------------------------------------------
	std::vector<t_real> vecFormfacts;
	std::vector<t_real> vecMagFormfacts;

	while(1)
	{
		std::cout << "\n----------------------------------------------------------------------\n" << std::endl;

		t_real h=0., k=0., l=0.;
		std::cout << "Enter h k l [rlu]: ";
		std::cin >> h >> k >> l;

		t_vec vecG = tl::mult<t_mat, t_vec>(matB, tl::make_vec({h,k,l}));
		t_real dG = ublas::norm_2(vecG);
		std::cout << "G = " << dG << " / A" << std::endl;

		vecFormfacts.clear();
		vecMagFormfacts.clear();
		for(unsigned int iAtom=0; iAtom<vecAllAtoms.size(); ++iAtom)
		{
			//const t_vec& vecAtom = vecAllAtoms[iAtom];
			const xtl::FormfactList<t_real>::elem_type* pElemff = lstff->Find(vecElems[vecAtomIndices[iAtom]]);
			const xtl::MagFormfactList<t_real>::elem_type* pElemMff = lstmff->Find(vecElems[vecAtomIndices[iAtom]]);

			if(pElemff == nullptr)
			{
				std::cerr << "Error: cannot get form factor for "
					<< vecElems[vecAtomIndices[iAtom]] << "." << std::endl;
				return;
			}
			/*if(pElemMff == nullptr)
			{
				std::cerr << "Warning: cannot get magnetic form factor for "
					<< vecElems[vecAtomIndices[iAtom]] << "." << std::endl;
			}*/

			t_real dFF = pElemff ? pElemff->GetFormfact(dG) : 0.;
			vecFormfacts.push_back(dFF);

			t_real dMFF = pElemMff ? pElemMff->GetFormfact(dG) : 0.;
			vecMagFormfacts.push_back(dMFF);
		}

		std::vector<std::complex<t_real>> vecMag(vecMagScatlens.size());
		if(vecMagScatlens.size() != vecMagFormfacts.size())
		{
			std::cerr << "Size mismatch in magnetic scattering lengths and form factors."
				<< std::endl;
		}
		else
		{
			std::transform(vecMagScatlens.begin(), vecMagScatlens.end(), vecMagFormfacts.begin(),
				vecMag.begin(), [](const std::complex<t_real>& c, t_real d) -> std::complex<t_real>
			{
				// multiply eff. mag. scattering lengths with respective form factors
				return c * d;
			});
		}

		std::complex<t_real> F = tl::structfact<t_real, std::complex<t_real>, ublas::vector<t_real>, std::vector>
			(vecAllAtoms, vecG, vecScatlens);
			std::complex<t_real> Fm = tl::structfact<t_real, std::complex<t_real>, ublas::vector<t_real>, std::vector>
			(vecAllAtoms, vecG, vecMag);
		std::complex<t_real> Fx = tl::structfact<t_real, t_real, ublas::vector<t_real>, std::vector>
			(vecAllAtoms, vecG, vecFormfacts);

		std::cout << std::endl;
		std::cout << "Neutron nuclear structure factor: " << std::endl;
		t_real dFsq = (std::conj(F)*F).real();
		std::cout << "F = " << F << " fm" << std::endl;
		std::cout << "|F| = " << std::sqrt(dFsq) << " fm" << std::endl;
		std::cout << "|F|^2 = " << dFsq << " fm^2" << std::endl;

		std::cout << std::endl;
		std::cout << "Neutron magnetic structure factor: " << std::endl;
		t_real dFmsq = (std::conj(Fm)*Fm).real();
		std::cout << "Fm = " << Fm << " fm" << std::endl;
		std::cout << "|Fm| = " << std::sqrt(dFmsq) << " fm" << std::endl;
		std::cout << "|Fm|^2 = " << dFmsq << " fm^2" << std::endl;

		std::cout << std::endl;
		std::cout << "X-ray atomic structure factor: " << std::endl;
		t_real dFxsq = (std::conj(Fx)*Fx).real();
		std::cout << "Fx = " << Fx << std::endl;
		std::cout << "|Fx| = " << std::sqrt(dFxsq) << std::endl;
		std::cout << "|Fx|^2 = " << dFxsq << std::endl;
	}
	// --------------------------------------------------------------------------------------------
}

#include "libs/version.h"
#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;

int main(int argc, char** argv)
{
#ifdef NO_TERM_CMDS
	tl::Log::SetUseTermCmds(0);
#endif

	// plain C locale
	/*std::*/setlocale(LC_ALL, "C");
	std::locale::global(std::locale::classic());

	// Header
	tl::log_info("Libcrystal structure factor calculator.");
	tl::log_info("Written by Tobias Weber <tobias.weber@tum.de>, 2014-2017.");
	tl::log_info("License: GPLv2.");

	// possible resource paths
	const char* pcProg = argv[0];
	fs::path path(pcProg);
	path.remove_filename();
	tl::log_info("Program path: ", path.string());

	add_resource_path(path.string());
	add_resource_path((path / "..").string());
	add_resource_path((path / "resources").string());
	add_resource_path((path / "Resources").string());
	add_resource_path((path / ".." / "resources").string());
	add_resource_path((path / ".." / "Resources").string());

	// load a taz file if given
	const char *pcFile = nullptr;
	if(argc >= 2)
	{
		pcFile = argv[1];
		std::cout << "Using crystal file \"" << pcFile << "\"." << std::endl;
	}

	try
	{
		gen_atoms_sfact(pcFile);
	}
	catch(const std::exception& err)
	{
		std::cerr << err.what() << std::endl;
	}

	return 0;
}


#include "singular.h"

#include <boost/random/poisson_distribution.hpp>
#include <boost/random/binomial_distribution.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/beta_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/beta.hpp>

namespace statiskit
{
    SingularDistribution::~SingularDistribution()
    {}

    double SingularDistribution::loglikelihood(const MultivariateData& data) const
    {
        double llh = 0.;
        std::unique_ptr< MultivariateData::Generator > generator = data.generator();
        while(generator->is_valid() && boost::math::isfinite(llh))
        {
            double weight = generator->weight();
            if(weight > 0.)
            { llh += weight * probability(generator->event(), true); }
            ++(*generator);
        }
        return llh;
    }

    MultinomialSingularDistribution::MultinomialSingularDistribution(const Eigen::VectorXd& pi)
    {
        _pi = Eigen::VectorXd::Zero(pi.size());
        set_pi(pi);
    }

    MultinomialSingularDistribution::MultinomialSingularDistribution(const MultinomialSingularDistribution& splitting)
    { _pi = splitting._pi; }

    MultinomialSingularDistribution::~MultinomialSingularDistribution()
    {}

    Index MultinomialSingularDistribution::get_nb_components() const
    { return _pi.size(); }

    unsigned int MultinomialSingularDistribution::get_nb_parameters() const
    { return _pi.size() - 1; }

    double MultinomialSingularDistribution::probability(const MultivariateEvent* event, const bool& logarithm) const
    {
        double p;
        if(event && event->size() == get_nb_components())
        {
            try
            {
                p = 0.;
                int sum = 0;
                for(Index component = 0, max_component = get_nb_components(); component < max_component; ++component)
                {
                    const UnivariateEvent* uevent = event->get(component);
                    if(uevent)
                    {
                        if(uevent->get_outcome() == DISCRETE && uevent->get_event() == ELEMENTARY)
                        {
                            int value = static_cast< const DiscreteElementaryEvent* >(uevent)->get_value();
                            if(!(_pi[component] <= 0. && value == 0))
                            {
                                p += value * log(_pi[component]) - boost::math::lgamma(value + 1);
                                sum += value;
                            }
                        }
                        else
                        { throw std::exception(); }
                    }
                }
                p += boost::math::lgamma(sum + 1);
            }
            catch(const std::exception& error)
            { p = log(0.); }
        }
        else
        { p = log(0.); }
        if(!logarithm)
        { p = exp(p); }
        return p;
    }

    std::unique_ptr< MultivariateEvent > MultinomialSingularDistribution::simulate(unsigned int sum) const
    {
        double pi = 0.;
        Index component = 0, max_component = get_nb_components() - 1;
        std::unique_ptr< VectorEvent > event = std::make_unique< VectorEvent >(max_component + 1);
        while(component < max_component && sum > 0)
        {
            boost::binomial_distribution<> dist(sum, _pi[component] / (1 - pi));
            boost::variate_generator<boost::mt19937&, boost::binomial_distribution<> > simulator(__impl::get_random_generator(), dist);
            int value = simulator();
            pi += _pi[component];
            event->set(component, DiscreteElementaryEvent(value));
            sum -= value;
            ++component;
        }
        for(; component < max_component; ++component)
        { event->set(component, DiscreteElementaryEvent(0)); }
        event->set(max_component, DiscreteElementaryEvent(sum));
        return std::move(event);
    }

    const Eigen::VectorXd& MultinomialSingularDistribution::get_pi() const
    { return _pi; }

    void MultinomialSingularDistribution::set_pi(const Eigen::VectorXd& pi)
    {
        if(pi.rows() == _pi.size() - 1)
        {
            Index j = 0;
            while(j < pi.rows() && pi[j] >= 0.)
            { ++j; }
            if(j < pi.rows())
            { throw parameter_error("pi", "contains negative values"); }
            double sum = pi.sum();
            if(sum < 1)
            {
                _pi.block(0, 0, _pi.size() - 1, 1) = pi / sum;
                _pi[_pi.size()-1] = 1 - sum;
            }
            else
            { throw parameter_error("pi", "last category values"); }
        }
        else if(pi.rows() == _pi.size())
        {
            Index j = 0;
            while(j < pi.rows() && pi[j] >= 0.)
            { ++j; }
            if(j < pi.rows())
            { throw parameter_error("pi", "contains negative values"); }
            _pi = pi / pi.sum();
        }
        else
        { throw parameter_error("pi", "number of parameters"); }
    }

    DirichletMultinomialSingularDistribution::DirichletMultinomialSingularDistribution(const Eigen::VectorXd& alpha)
    {
        _alpha = Eigen::VectorXd::Zero(alpha.size());
        set_alpha(alpha);
    }

    DirichletMultinomialSingularDistribution::DirichletMultinomialSingularDistribution(const DirichletMultinomialSingularDistribution& splitting)
    { _alpha = splitting._alpha; }

    DirichletMultinomialSingularDistribution::~DirichletMultinomialSingularDistribution()
    {}

    Index DirichletMultinomialSingularDistribution::get_nb_components() const
    { return _alpha.size(); }

    unsigned int DirichletMultinomialSingularDistribution::get_nb_parameters() const
    { return _alpha.size(); }

    double DirichletMultinomialSingularDistribution::probability(const MultivariateEvent* event, const bool& logarithm) const
    {
        double p;
        if(event && event->size() == get_nb_components())
        {
            try
            {
                p = 0.;
                int sum = 0;
                for(Index component = 0, max_component = get_nb_components(); component < max_component; ++component)
                {
                    const UnivariateEvent* uevent = event->get(component);
                    if(uevent)
                    {
                        if(uevent->get_outcome() == DISCRETE && uevent->get_event() == ELEMENTARY)
                        {
                            int value = static_cast< const DiscreteElementaryEvent* >(uevent)->get_value();
                            if(!(_alpha[component] <= 0. && value == 0))
                            {
                                p += boost::math::lgamma(_alpha[component] + value);
                                p -= boost::math::lgamma(_alpha[component]) + boost::math::lgamma(value + 1);
                                sum += value;
                            }
                        }
                        else
                        { throw std::exception(); }
                    }
                }
                double alpha = _alpha.sum();
                p += boost::math::lgamma(sum + 1) + boost::math::lgamma(alpha) - boost::math::lgamma(alpha + sum);
            }
            catch(const std::exception& error)
            { p = log(0.); }
        }
        else
        { p = log(0.); }
        if(!logarithm)
        { p = exp(p); }
        return p;
    }

    std::unique_ptr< MultivariateEvent > DirichletMultinomialSingularDistribution::simulate(unsigned int sum) const
    {
        Eigen::VectorXd _pi = Eigen::VectorXd::Zero(get_nb_components());
        for(Index component = 0, max_component = get_nb_components(); component < max_component; ++component)
        {
            boost::random::gamma_distribution<> dist(_alpha(component), 1.);
            boost::variate_generator<boost::mt19937&, boost::random::gamma_distribution<> > simulator(__impl::get_random_generator(), dist);
            _pi(component) = simulator();
        }
        _pi /= _pi.sum();
        double pi = 0.;
        Index component = 0, max_component = get_nb_components() - 1;
        std::unique_ptr< VectorEvent > event = std::make_unique< VectorEvent >(max_component + 1);
        while(component < max_component && sum > 0)
        {
            boost::binomial_distribution<> dist(sum, _pi[component] / (1 - pi));
            boost::variate_generator<boost::mt19937&, boost::binomial_distribution<> > simulator(__impl::get_random_generator(), dist);
            int value = simulator();
            pi += _pi[component];
            event->set(component, DiscreteElementaryEvent(value));
            sum -= value;
            ++component;
        }
        for(; component < max_component; ++component)
        { event->set(component, DiscreteElementaryEvent(0)); }
        event->set(max_component, DiscreteElementaryEvent(sum));
        return std::move(event);
    }

    const Eigen::VectorXd& DirichletMultinomialSingularDistribution::get_alpha() const
    { return _alpha; }

    void DirichletMultinomialSingularDistribution::set_alpha(const Eigen::VectorXd& alpha)
    {
        if(alpha.rows() == _alpha.size())
        {
            Index j = 0;
            while(j < alpha.rows() && alpha[j] >= 0.)
            { ++j; }
            if(j < alpha.rows())
            { throw parameter_error("alpha", "contains negative values"); }
            _alpha = alpha;
        }
        else
        { throw parameter_error("alpha", "number of parameters"); }
    }
}


#pragma once
#include <Eigen/Dense>
#include <unsupported/Eigen/KroneckerProduct>

#include <vector>

#include "../utils.hpp"

namespace yavque
{
enum class Pauli : char
{
	X = 'X',
	Y = 'Y',
	Z = 'Z'
};

namespace detail
{
	class CompressedPauliString
	{
	private:
		const std::vector<Pauli> pstring_;
		Eigen::MatrixXcd mat_;
		mutable bool diagonalized_ = false;
		mutable Eigen::MatrixXcd evecs_;
		mutable Eigen::MatrixXcd evals_;

		static std::vector<Pauli> construct_pauli(const std::string& str)
		{
			std::vector<Pauli> res;
			for(auto c : str)
			{
				res.emplace_back(Pauli(c));
			}
			return res;
		}

		static Eigen::MatrixXcd get_pauli(Pauli p)
		{
			switch(p)
			{
			case Pauli::X:
				return pauli_x().cast<cx_double>();
			case Pauli::Y:
				return pauli_y();
			case Pauli::Z:
				return pauli_z().cast<cx_double>();
			}
			__builtin_unreachable();
			return Eigen::MatrixXcd();
		}

		void diagonalize() const
		{
			if(!diagonalized_)
			{
				Eigen::SelfAdjointEigenSolver<Eigen::MatrixXcd> es(mat_);
				evecs_ = es.eigenvectors();
				evals_ = es.eigenvalues();
				diagonalized_ = true;
			}
		}

		static uint32_t change_bits(const std::vector<uint32_t>& indices,
		                            uint32_t bitstring, uint32_t bits_to_change)
		{
			for(uint32_t n = 0; n < indices.size(); ++n)
			{
				uint32_t b = (bits_to_change >> n) & 1U;
				bitstring = (bitstring & (~(1U << indices[n]))) | (b << indices[n]);
			}
			return bitstring;
		}

		static uint32_t bits(const std::vector<uint32_t>& indices, uint32_t bitstring)
		{
			uint32_t b = 0U;

			for(uint32_t k = 0; k < indices.size(); ++k)
			{
				b |= ((bitstring >> indices[k]) & 1U) << k;
			}
			return b;
		}

		void construct_matrix()
		{
			mat_ = Eigen::MatrixXcd::Ones(1, 1);
			for(auto iter = pstring_.rbegin(); iter != pstring_.rend(); ++iter)
			{
				mat_ = Eigen::kroneckerProduct(mat_, get_pauli(*iter)).eval();
			}
		}

	public:
		explicit CompressedPauliString(const std::string& str)
			: pstring_{construct_pauli(str)}
		{
			construct_matrix();
		}

		explicit CompressedPauliString(std::vector<Pauli> pvec)
			: pstring_{std::move(pvec)}
		{
			construct_matrix();
		}

		Pauli at(uint32_t idx) const { return pstring_[idx]; }

		Eigen::VectorXcd apply(const std::vector<uint32_t>& indices,
		                       const Eigen::VectorXcd& vec) const
		{
			assert(pstring_.size() == indices.size());
			uint32_t dim = 1U << pstring_.size();
			Eigen::VectorXcd res = Eigen::VectorXcd::Zero(vec.size());

			if(indices.size() == 1)
			{
				return apply_single_qubit(vec, mat_, indices[0]);
			}
			if(indices.size() == 2)
			{
				return apply_two_qubit(vec, mat_, {indices[0], indices[1]});
			}
			if(indices.size() == 3)
			{
				return apply_three_qubit(vec, mat_, {indices[0], indices[1], indices[2]});
			}

			for(uint32_t k = 0; k < vec.size(); ++k)
			{
				cx_double v = 0.0;
				uint32_t row = bits(indices, k);
				for(uint32_t col = 0; col < dim; ++col)
				{
					uint32_t l = change_bits(indices, k, col);
					v += mat_(row, col) * vec(l);
				}
				res(k) = v;
			}
			return res;
		}

		/* return exp(t*P) applied to indices */
		Eigen::VectorXcd apply_exp(cx_double t, const std::vector<uint32_t>& indices,
		                           const Eigen::VectorXcd& vec) const
		{
			assert(pstring_.size() == indices.size());
			uint32_t dim = 1U << pstring_.size();
			Eigen::VectorXcd res = Eigen::VectorXcd::Zero(vec.size());

			if(!diagonalized_)
			{
				diagonalize();
			}

			Eigen::VectorXcd p = (t * evals_.array()).exp();
			Eigen::MatrixXcd exp_mat = evecs_ * p.asDiagonal() * evecs_.adjoint();

			if(indices.size() == 1)
			{
				return apply_single_qubit(vec, exp_mat, indices[0]);
			}
			if(indices.size() == 2)
			{
				return apply_two_qubit(vec, exp_mat, {indices[0], indices[1]});
			}
			if(indices.size() == 3)
			{
				return apply_three_qubit(vec, exp_mat,
				                         {indices[0], indices[1], indices[2]});
			}

			for(uint32_t k = 0; k < vec.size(); ++k)
			{
				cx_double v = 0.0;
				uint32_t row = bits(indices, k);
				for(uint32_t col = 0; col < dim; ++col)
				{
					uint32_t l = change_bits(indices, k, col);
					v += exp_mat(row, col) * vec(l);
				}
				res(k) = v;
			}
			return res;
		}
	};
} // namespace detail

class CPSFactory
{
private:
	std::map<std::string, std::weak_ptr<detail::CompressedPauliString>> map_;

	CPSFactory() = default;

public:
	CPSFactory(const CPSFactory&) = delete;
	CPSFactory& operator=(const CPSFactory&) = delete;

	CPSFactory(CPSFactory&&) = delete;
	CPSFactory& operator=(CPSFactory&&) = delete;
	~CPSFactory() = default;

	std::shared_ptr<detail::CompressedPauliString>
	get_pauli_string_for(const std::string& pstr)
	{
		auto iter = map_.find(pstr);
		if((iter == map_.end()) || iter->second.expired())
		{
			auto p = std::make_shared<detail::CompressedPauliString>(pstr);
			map_[pstr] = p;
			return p;
		}
		return std::shared_ptr<detail::CompressedPauliString>(map_[pstr]);
	}

	static CPSFactory& get_instance()
	{
		static CPSFactory instance;
		return instance;
	}
};
} // namespace yavque


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <Eigen/Dense>

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/distributions/inverse_gamma.hpp>

#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/normal_distribution.hpp>

#include <dpMM/basemeasure.hpp>
#include <dpMM/distribution.hpp>
#include <dpMM/normalSphere.hpp>
#include <dpMM/iw.hpp>
#include <dpMM/sphere.hpp>
#include <dpMM/karcherMean.hpp>

using namespace Eigen;
using std::endl;
using std::cout;

/* Actually just places an IW prior on covariances in the tangent
 * plane.
 *
 * The point of tangentcy has to be set "manually". This makes the
 * clase usefull for models that externaly update the point of tangency
 * such as the Manhattan Frame
 */
template<typename T>
class IwTangent : public BaseMeasure<T>
{
public:

  IW<T> iw0_; // IW prior on the covariance of the normal in T_\muS^D
  Sphere<T> S_;
  NormalSphere<T> normalS_; // normal on sphere

  IwTangent(const IW<T>& iw, boost::mt19937* pRndGen);
  ~IwTangent();

  virtual BaseMeasure<T>* copy();
  virtual IwTangent<T>* copyNative();

  virtual baseMeasureType getBaseMeasureType() const {return(NIW_TANGENT); }

  /* for any point on the sphere; maps into T_muS and rotates north first */
  virtual T logLikelihood(const Matrix<T,Dynamic,1>& x) const ;
  virtual T logLikelihood(const Matrix<T,Dynamic,Dynamic>& x, uint32_t i) const
    {return logLikelihood(x.col(i));};
  // right now this does not support actual scatter!  it supports
  // weighted directional data though.  count is the weight
  // [counts,karcherMean,scatter around KarcherMean]
  virtual T logLikelihoodFromSS(const Matrix<T,Dynamic,1>& x) const;
  /* assumes x is already in T_northS */
  virtual T logLikelihoodNorth(const Matrix<T,Dynamic,1>& x) const ;

  void posterior(const Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z,
    uint32_t k);
  void posterior(const shared_ptr<ClGMMData<T> >& cldp, uint32_t k);
  /* assumes the x are already in T_northS correctly */
  void posteriorFromPtsInTpS(const Matrix<T,Dynamic,Dynamic>& x,
    const VectorXu& z, uint32_t k);
  // right now this does not support actual scatter!  it supports
  // weighted directional data though.  count is the weight
  // [counts,karcherMean,scatter around KarcherMean]
  void posteriorFromSS(const vector<Matrix<T,Dynamic,1> >&x, const
      VectorXu& z, uint32_t k);
  void posteriorFromSS(const Matrix<T,Dynamic,1> &x);

  void sample();

  T logPdfUnderPrior() const;
  T logPdfUnderPriorMarginalized() const;
//  T logPdfUnderPriorMarginalizedMerged(const shared_ptr<IwTangent<T> >& other) const;

  void print() const;
  virtual uint32_t getDim() const {return(uint32_t(normalS_.D_));};

//  virtual IwTangent<T>* merge(const IwTangent<T>& other);
//  void fromMerge(const IwTangent<T>& niwA, const IwTangent<T>& niwB);

  const Matrix<T,Dynamic,Dynamic>& scatter() const {return iw0_.scatter();};
  Matrix<T,Dynamic,Dynamic>& scatter() {return iw0_.scatter();};
  T count() const {return iw0_.count();};
  T& count() {return iw0_.count();};

  const Matrix<T,Dynamic,Dynamic>& Sigma() const {return normalS_.Sigma();};

  const Matrix<T,Dynamic,1>& getMean() const {return normalS_.getMean();};
  void setMean(const Matrix<T,Dynamic,1>& mean) {return normalS_.setMean(mean);};

private:

//  void computeMergedSS( const IwTangent<T>& niwA,
//    const IwTangent<T>& niwB, Matrix<T,Dynamic,Dynamic>& scatterM,
//    Matrix<T,Dynamic,1>& muM, T& countM) const;

};

typedef IwTangent<double> IwTangentd;
typedef IwTangent<float> IwTangentf;


//Copyright (C) 2011 Pierre Moulon
//
//This program is free software: you can redistribute it and/or modify
//it under the terms of the GNU General Public License as published by
//the Free Software Foundation, either version 3 of the License, or
//(at your option) any later version.
//
//This program is distributed in the hope that it will be useful,
//but WITHOUT ANY WARRANTY; without even the implied warranty of
//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//GNU General Public License for more details.
//
//You should have received a copy of the GNU General Public License
//along with this program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef LIBS_SVD_H_
#define LIBS_SVD_H_

#include <Eigen/Core>
#include <Eigen/SVD>
#include "extras/libNumerics/matrix.h"
#include "third_party/svd/matConversion.hpp"
#include "third_party/svd/eigenWrapper.hpp"

namespace SVDWrapper {

  typedef libNumerics::matrix<double> Mat;
  typedef libNumerics::vector<double> Vec;
  typedef Eigen::MatrixXd MatEigen;

  /// Solve the linear system Ax = 0 with ||x|| = 1.0 via SVD.
  /// Return true if the ratio of singular values SV(0)/SV(N-2) < dRatio.
  /// The return value indicates whether the solution is unique.
  static bool Nullspace(const Mat & A, Vec *nullspace, double dRatio=1e-5)
  {
    typedef Eigen::MatrixXd MatEigen;
    MatEigen AEigen, null;
    EigenWrapper::MatToEigen(A, AEigen);
    bool bOk = EigenWrapper::Nullspace(&AEigen, &null, dRatio);
    (*nullspace).read( null.data() );
    return bOk;
  }

  /// Inverse of norm-2 condition value (ratio of extreme singular values)
  inline double InvCond(const Mat& A) {
    Eigen::MatrixXd AEigen, sing;
    EigenWrapper::MatToEigen(A, AEigen);
    EigenWrapper::SingularValues(&AEigen, &sing);
    return sing(A.nrow()-1)/sing(0);
  }

  /// Make rank<=2.
  inline void EnforceRank2_3x3(const Mat& A, Mat *ARank)
  {
    Eigen::MatrixXd MEigen, MRank2Eigen;
    EigenWrapper::MatToEigen(A, MEigen);
    EigenWrapper::EnforceRank2_3x3(MEigen, &MRank2Eigen);
    EigenWrapper::EigenToMat(MRank2Eigen, *ARank);
  }

  /// Save the two last nullspace vector as 3x3 matrices.
  /// It uses Eigen to compute the SVD decomposition.
  inline void Nullspace2_Remap33(const Mat &A, Mat& f1, Mat& f2) {
      using Eigen::Map;
      using namespace EigenWrapper;

      MatEigen f1E, f2E;
      MatEigen AEigen;
      MatToEigen(A, AEigen);
      Nullspace2(&AEigen, &f1E, &f2E);

      typedef Eigen::Matrix<double, 3, 3> Mat3;
      typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> RMat3;
      // Update f1 and f2
      EigenToMat(Map<RMat3>(f1E.data()), f1);
      EigenToMat(Map<RMat3>(f2E.data()), f2);
  }

} // namespace SVDWrapper

#endif // LIBS_SVD_H_


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))

using namespace std;

using ll = long long;
using VI = vector<int>;
using VI2D = vector<vector<int>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

using Weight = int;

// 辺
struct Edge {
	size_t from;
	size_t to;
	Weight cost;
	Edge(size_t t, Weight c) : to(t), cost(c) {}
	Edge(size_t f, size_t t, Weight c) : from(f), to(t), cost(c) {}
	bool operator<(const Edge& rhs) const { return this->cost > rhs.cost; }
};

// グラフ G=(V,E)
struct Graph {
	size_t node;
	std::vector<std::vector<Edge>> edges;

	Graph(size_t n) : node(n), edges(n) {}
};

// 最短経路探索(非負閉路)
std::vector<int> dijkstra(const Graph& graph, const size_t s) {
	size_t n = graph.node;
	std::vector<int> used(n, -1);
	std::vector<Weight> distances(n, INF);

	distances[s] = 0;
	std::priority_queue<Edge> pq;
	pq.push(Edge(0, s, 0));
	while(!pq.empty()) {
		Edge edge = pq.top();
		pq.pop();
		if(used[edge.to] != -1) {
			continue;
		}
		used[edge.to] = edge.from;
		for(auto&& e : graph.edges[edge.to]) {
			Weight alt = edge.cost + e.cost;
			if(alt < distances[e.to]) {
				distances[e.to] = alt;
				pq.push(Edge(edge.to, e.to, alt));
			}
		}
	}
	return used;
}

int main() {
	int n, m;
	cin >> n >> m;
	VI a(m);
	VI b(m);
	Graph g(n);
	REP(i, m) {
		cin >> a[i] >> b[i];
		g.edges[a[i] - 1].push_back(Edge(a[i] - 1, b[i] - 1, 1));
		g.edges[b[i] - 1].push_back(Edge(b[i] - 1, a[i] - 1, 1));
	}
	vector<int> result = dijkstra(g, 0);
	bool is_enable = true;
	REP(i, n) {
		if(result[i] == -1) {
			is_enable = false;
			break;
		}
	}
	if(is_enable) {
		cout << "Yes" << endl;
		RANGE(i, 1, n) { cout << result[i] + 1 << endl; }
	} else {
		cout << "No" << endl;
	}
	return 0;
}


#ifndef _DOUGLAS_PEUCKER_H_
#define _DOUGLAS_PEUCKER_H_

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

/**
 * This class is used to simpified a curve containing numerous points,applying the
 * Ramer-Douglas-Peucker Algorithm.
 *
 * Usage : Create a instance of this class, then call the simplifiy() function
 * Input : Points of a curve, and epsilon
 * Output: Points of the simplified curve
 */
class RDPCurveSimplifier
{
public:
  RDPCurveSimplifier()
  {
  }

  void simplify(std::vector<Eigen::Vector3d> &curve, float epsilon);

private:
  void getMostDistantPoint(std::vector<Eigen::Vector3d> segment, int &point_id, float &distance);

  void breakSegment(std::vector<Eigen::Vector3d> segment, int point_id,
                    std::vector<Eigen::Vector3d> &part1, std::vector<Eigen::Vector3d> &part2);

  bool allSegmentSimplified(std::vector<std::vector<Eigen::Vector3d>> segments);

  void getDistanceToLine(Eigen::Vector3d front, Eigen::Vector3d back, Eigen::Vector3d point,
                         float &distance);
};

void RDPCurveSimplifier::simplify(std::vector<Eigen::Vector3d> &curve, float epsilon)
{
  // create a vector containing the simplified segments
  std::vector<std::vector<Eigen::Vector3d>> segments;

  // At the begining, there is only one segment
  segments.push_back(curve);

  // begin to simplify the curve, applying Ramer-Douglas-Peucker Algorithm
  while(1)
  {
    // output the segment
    std::cout << "size:" << std::endl;
    for(int i= 0; i < segments.size(); i++)
    {
      std::cout << segments[i].size() << ",";
    }
    std::cout << std::endl;

    std::vector<std::vector<Eigen::Vector3d>> updated_segments;

    // simplify each segment
    for(int i= 0; i < segments.size(); i++)
    {
      // get one segment
      std::vector<Eigen::Vector3d> sgm= segments[i];
      if(sgm.size() == 2)
      {
        updated_segments.push_back(sgm);
        continue;
      }

      // get the most distant point's id and distance in this segment
      float dist= -1.0;
      int pt_id= -1;
      getMostDistantPoint(sgm, pt_id, dist);
      // if distance larger than epsilon, break the segemnt into two parts
      if(dist > epsilon)
      {
        std::vector<Eigen::Vector3d> part1, part2;
        breakSegment(sgm, pt_id, part1, part2);
        updated_segments.push_back(part1);
        updated_segments.push_back(part2);
      }
      // distance small enough, only hold front and back point
      else
      {
        std::vector<Eigen::Vector3d> front_back;
        front_back.push_back(sgm.front());
        front_back.push_back(sgm.back());
        updated_segments.push_back(front_back);
      }
    }

    // after one iteration, reset the segments
    segments= updated_segments;
    updated_segments.clear();

    // if all segments are simplified, stop the iteration, else the iteration continue
    if(allSegmentSimplified(segments))
      break;
  }

  // finally reset points in curve
  curve.clear();
  for(int i= 0; i < segments.size(); i++)
  {
    curve.insert(curve.end(), segments[i].begin(), segments[i].end());
  }
}

void RDPCurveSimplifier::getMostDistantPoint(std::vector<Eigen::Vector3d> segment, int &point_id,
                                             float &distance)
{
  Eigen::Vector3d front= segment.front();
  Eigen::Vector3d back= segment.back();

  point_id= -1;
  distance= -10.0;
  for(int i= 1; i < segment.size() - 1; i++)
  {
    float dist= 0;
    getDistanceToLine(front, back, segment[i], dist);
    if(dist > distance)
    {
      distance= dist;
      point_id= i;
    }
  }
}

void RDPCurveSimplifier::breakSegment(std::vector<Eigen::Vector3d> segment, int point_id,
                                      std::vector<Eigen::Vector3d> &part1,
                                      std::vector<Eigen::Vector3d> &part2)
{
  part1.insert(part1.begin(), segment.begin(), segment.begin() + point_id + 1);
  part2.insert(part2.begin(), segment.begin() + point_id, segment.end());
}

bool RDPCurveSimplifier::allSegmentSimplified(std::vector<std::vector<Eigen::Vector3d>> segments)
{
  for(int i= 0; i < segments.size(); i++)
  {
    if(segments[i].size() != 2)
    {
      return false;
    }
  }

  return true;
}

void RDPCurveSimplifier::getDistanceToLine(Eigen::Vector3d front, Eigen::Vector3d back,
                                           Eigen::Vector3d point, float &distance)
{
  Eigen::Vector4d line_dir(back(0) - front(0), back(1) - front(1), back(2) - front(2), 0);
  line_dir.normalize();
  Eigen::Vector4d p2p(point(0) - front(0), point(1) - front(1), point(2) - front(2), 0);

  distance= p2p.cross3(line_dir).squaredNorm();
  distance= sqrt(distance);
}

#endif


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"
#include "graph.hh"
#include "graph_properties.hh"

#include <boost/bind.hpp>

#ifndef __clang__
#include <ext/numeric>
using __gnu_cxx::power;
#else
template <class Value>
Value power(Value value, int n)
{
    return pow(value, n);
}
#endif

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/hypot.hpp>
#include <boost/math/constants/constants.hpp>

#include <boost/graph/fruchterman_reingold.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

namespace graph_tool
{
// convert point types
template <class Val>
struct convert<vector<Val>, typename convex_topology<2>::point>
{
    vector<Val> operator()(const typename convex_topology<2>::point& p) const
    {
        vector<Val> v(2);
        for (size_t i = 0; i < 2; ++i)
            v[i] = p[i];
        return v;
    }
};

template <class Val>
struct convert<typename convex_topology<2>::point, vector<Val> >
{
    typename convex_topology<2>::point operator()(const vector<Val>& v) const
    {
        typename convex_topology<2>::point p;
        for (size_t i = 0; i < min(size_t(2), v.size()); ++i)
            p[i] = v[i];
        return p;
    }
};
} // graph_tool namespace

template<class T>
struct anneal_cooling
{
    typedef T result_type;

    anneal_cooling(T ti, T tf, std::size_t iterations)
        : _ti(ti), _tf(tf), _iter(0), _n_iter(iterations)
    {
        _beta = (log(_tf) - log(_ti)) / _n_iter;
    }

    T operator()()
    {
        T temp = _ti * exp(T(_iter) * _beta);
        ++_iter;
        if (_iter == _n_iter)
            temp = 0;
        return temp;
    }

private:
    T _ti, _tf;
    size_t _iter;
    size_t _n_iter;
    T _beta;
};

template <class Topology>
struct get_layout
{
    template <class WeightMap, class Value>
    struct attr_force
    {
        attr_force(WeightMap w, Value a): _w(w), _a(a) {}
        WeightMap _w;
        Value _a;

        template <class Graph, class Edge, class KVal, class DVal>
        Value operator()(Edge e, KVal k, DVal dist, const Graph&) const
        {
            return _a * get(_w, e) * power(dist, 2) / k;
        }
    };

    template <class Value>
    struct rep_force
    {
        rep_force(Value r): _r(r){}
        Value _r;

        template <class Graph, class Vertex, class KVal, class DVal>
        Value operator()(Vertex, Vertex, KVal k, DVal dist, const Graph&) const
        {
            return _r * power(k, 2) / dist;
        }
    };

    template <class Graph, class PosMap, class WeightMap>
    void operator()(Graph& g, PosMap pos, WeightMap weight,
                    pair<double, double> f, double scale, bool grid,
                    pair<double, double> temp, size_t n_iter) const
    {
        typedef typename property_traits<PosMap>::value_type::value_type pos_t;
        anneal_cooling<pos_t> cool(temp.first, temp.second, n_iter);
        attr_force<WeightMap, pos_t> af(weight, f.first);
        rep_force<pos_t> rf(f.second);
        Topology topology(scale);
        ConvertedPropertyMap<PosMap, typename Topology::point_type> cpos(pos);
        if (grid)
            fruchterman_reingold_force_directed_layout
                (g, cpos,
                 topology,
                 attractive_force(af).
                 repulsive_force(rf).
                 cooling(cool));
        else
            fruchterman_reingold_force_directed_layout
                (g, cpos,
                 topology,
                 attractive_force(af).
                 repulsive_force(rf).
                 cooling(cool).
                 force_pairs(all_force_pairs()));
    }
};

void fruchterman_reingold_layout(GraphInterface& g, boost::any pos,
                                 boost::any weight, double a, double r,
                                 bool square, double scale, bool grid,
                                 double ti, double tf, size_t max_iter)
{
    typedef UnityPropertyMap<int,GraphInterface::edge_t> weight_map_t;
    typedef boost::mpl::push_back<edge_scalar_properties, weight_map_t>::type
        edge_props_t;

    if(weight.empty())
        weight = weight_map_t();
    if (square)
        run_action<graph_tool::detail::never_directed>()
            (g,
             std::bind(get_layout<square_topology<> >(), std::placeholders::_1,
                       std::placeholders::_2, std::placeholders::_3, make_pair(a, r), scale,
                       grid, make_pair(ti, tf), max_iter),
             vertex_floating_vector_properties(), edge_props_t())
            (pos, weight);
    else
        run_action<graph_tool::detail::never_directed>()
            (g,
             std::bind(get_layout<circle_topology<> >(), std::placeholders::_1,
                       std::placeholders::_2, std::placeholders::_3, make_pair(a, r),
                       scale, grid, make_pair(ti, tf), max_iter),
             vertex_floating_vector_properties(), edge_props_t()) (pos, weight);
}

#include <boost/python.hpp>

void export_fruchterman_reingold()
{
    python::def("fruchterman_reingold_layout", &fruchterman_reingold_layout);
}


#include "itkImage.h"
#include "itkImageFileReader.h"
#include "vnl/vnl_matrix_ref.h"
#include "vnl/vnl_matrix.h"
#include "tkdCmdParser.h"
#include "flens.h"
#include <cmath>
#include <boost/math/special_functions/fpclassify.hpp>

// 26-12-2009
// added to Kajo's clustercoefficient: nan-check! (for binary unconnected graphs).

class ClusterCoefficient
{
public:
	typedef float PixelType;
	typedef itk::Image< PixelType, 2 > ImageType;
	typedef vnl_matrix_ref< PixelType > DataMatrixType;
	typedef float PrecisionType;
	typedef flens::GeMatrix< flens::FullStorage< PrecisionType, flens::RowMajor > > MatrixType;
	typedef vnl_vector< PrecisionType > VectorType;

	/**
	 * Stam's algorithm ...
	 */
	void RunStam( const std::string& filename )
	{
		typedef itk::ImageFileReader< ImageType > ReaderType;
		ReaderType::Pointer reader = ReaderType::New();
		reader->SetFileName( filename.c_str() );
		reader->Update();

		ImageType::Pointer image = reader->GetOutput();
		reader = 0;

		PixelType* buffer = image->GetPixelContainer()->GetBufferPointer();
		ImageType::RegionType region = image->GetLargestPossibleRegion();
		ImageType::SizeType size = region.GetSize();
		int rows = size[0];
		int cols = size[1];
		int nans = 0;

		DataMatrixType wIn( rows, cols, buffer );

		VectorType clusteringcoefficients( rows );

		PrecisionType sum = 0;
		for ( int i = 0; i < rows; ++i )
		{
			PrecisionType numerator = 0;
			PrecisionType denominator = 0;

			for ( int k = 0; k < rows; ++k )
			{
				if ( k == i )
				{
					continue;
				}

				for ( int l = 0; l < rows; ++l )
				{
					if ( l == k || l == i )
					{
						continue;
					}

					numerator += ( wIn( i, k ) * wIn( i, l ) * wIn( k, l ) );
					denominator += ( wIn( i, k ) * wIn( i, l ) );
				}
			}

			clusteringcoefficients( i ) = numerator / denominator;

			if ( !boost::math::isnan< PixelType >( clusteringcoefficients( i ) ) )
			{
				 sum += clusteringcoefficients( i );
			}
			else
			{
				std::cout << "*** WARNING ***: nan detected!" << std::endl;
				nans++;
			}
		}

		std::cout << ( sum / static_cast< PrecisionType > ( rows - nans ) ) << std::endl;
	}

	void RunWeighted( const std::string& filename, PixelType threshold )
	{
		typedef itk::ImageFileReader< ImageType > ReaderType;
		ReaderType::Pointer reader = ReaderType::New();
		reader->SetFileName( filename.c_str() );
		reader->Update();

		ImageType::Pointer image = reader->GetOutput();
		reader = 0;

		PixelType* buffer = image->GetPixelContainer()->GetBufferPointer();
		ImageType::RegionType region = image->GetLargestPossibleRegion();
		ImageType::SizeType size = region.GetSize();
		int rows = size[0];
		int cols = size[1];

		DataMatrixType wIn( rows, cols, buffer );

		MatrixType A( rows, cols );
		MatrixType S( rows, cols );
		MatrixType B( rows, cols );
		MatrixType W( rows, cols );

		for ( int i = 0; i < rows; ++i )
		{
			for ( int j = 0; j < cols; ++j )
			{
				S( i + 1, j + 1 ) = 0;

				if ( wIn( i, j ) > threshold )
				{
					A( i + 1, j + 1 ) = 1;
					B( i + 1, j + 1 ) = vcl_pow( wIn( i, j ), 1. / 3. );
					W( i + 1, j + 1 ) = wIn( i, j );
				} else
				{
					A( i + 1, j + 1 ) = 0;
					B( i + 1, j + 1 ) = 0;
					W( i + 1, j + 1 ) = 0;
				}
			}
		}

		for ( int i = 1; i <= rows; ++i )
		{
			for ( int j = 1; j <= cols; ++j )
			{
				S( i, j ) = B( i, j ) + B( j, i );
			}
		}

		MatrixType p1( rows, cols );
		MatrixType p( rows, cols );
		MatrixType a( rows, cols );
		VectorType C( rows );
		VectorType K( rows );

		flens::copy( S * S, p1 );
		flens::copy( p1 * S, p );
		flens::copy( A * A, a );

		K.fill( 0 );
		for ( int i = 0; i < rows; ++i )
		{
			for ( int j = 0; j < cols; ++j )
			{
				K( i ) += ( A( i + 1, j + 1 ) + A( j + 1, i + 1 ) );
			}

			PrecisionType cyc = p( i + 1, i + 1 ) / 2.;
			if ( cyc == 0 )
			{
				K( i) = vcl_numeric_limits< PrecisionType>::max();
			}

			C( i ) = cyc / ( K( i ) * ( K( i ) - 1 ) - 2 * a( i + 1, i + 1) );
		}

		PrecisionType sum = 0;
		for( int i = 0; i < rows; ++i )
		{
			sum += C( i );
		}

		sum /= static_cast< PrecisionType>( rows );
		std::cout << sum << std::endl;
	}

	void Run( const std::string& filename, PixelType threshold )
	{
		typedef itk::ImageFileReader< ImageType> ReaderType;
		ReaderType::Pointer reader = ReaderType::New();
		reader->SetFileName( filename.c_str() );
		reader->Update();

		ImageType::Pointer image = reader->GetOutput();
		reader = 0;

		PixelType* buffer = image->GetPixelContainer()->GetBufferPointer();
		ImageType::RegionType region = image->GetLargestPossibleRegion();
		ImageType::SizeType size = region.GetSize();
		int rows = size[ 0 ];
		int cols = size[ 1 ];

		DataMatrixType data( rows, cols, buffer );
		for( int i = 0; i < ( rows * cols ); ++i )
		{
			buffer[ i ] *= ( buffer[ i ] < 0 ? -1. : 1. );
		}

		PixelType sum = 0;
		//
						//    std::vector< PixelType > t;
						//    for( int i = 0; i < rows; ++i )
						//      {
						//      for( int j = i + 1; j < cols; ++j )
						//        {
						//        if ( data( i, j ) > 0 )
						//          {
						//          t.push_back( data( i, j ) );
						//          }
						//        }
						//      }
						//
						//    std::sort( t.begin(), t.end() );
						//    int index = static_cast< int >( 0.8 * static_cast< float >( t.size() ) );
						//    std::cout << t[ index ] << std::endl;
						//    return;

						for( int i = 0; i < rows; ++i )
						{
							// 1) determine k = number of edges to neighbors
							int k = 0;
							std::vector< int> neighbors;
							for( int j = 0; j < cols; ++j )
							{
								if ( i == j )
								{
									continue;
								}

								if ( data( i, j ) < threshold )
								{
									continue;
								}

								++k;
								neighbors.push_back( j );
							}

							// 2) determine connections among neighbors
							int connections = 0;
							std::vector< int>::const_iterator end = neighbors.end();
							for( std::vector< int>::const_iterator j = neighbors.begin(); j != end; ++j )
							{
								for( std::vector< int>::const_iterator m = j; m != end; ++m )
								{
									if ( *j == *m || i == *j || *m == i )
									{
										continue;
									}

									if ( data( *j, *m ) < threshold )
									{
										continue;
									}

									++connections;
								}
							}

							// k(k-1)*0.5 possible connections
							if ( k> 1 )
							{
								PixelType coefficient = static_cast< PixelType>( 2 * connections ) / static_cast< PixelType>( k * ( k - 1 ) );
								sum += coefficient;
								//        std::cout << i << "\t" << coefficient << std::endl;
							}
						}

						PixelType average = sum / static_cast< PixelType>( rows );

						std::cout << average << std::endl;
					}
				};

int main( int argc, char ** argv )
{
	tkd::CmdParser p( "clustercoefficient", "Calculate voxel-wise cluster coefficients" );

	std::string inputFileName;
	float threshold;
	bool weightedGraph = false;
	bool stam = false;

	p.AddArgument( inputFileName, "input" ) ->AddAlias( "i" ) ->SetInput( "filename" ) ->SetDescription( "Input 2D image: adjacency matrix" ) ->SetRequired(
			true );

	p.AddArgument( threshold, "threshold" ) ->AddAlias( "t" ) ->SetInput( "float" ) ->SetDescription( "Threshold" ) ->SetRequired( true );

	p.AddArgument( weightedGraph, "weighted" ) ->AddAlias( "w" ) ->SetDescription( "Calculated weighted cluster coefficients" );

	p.AddArgument( stam, "stam" ) ->SetDescription(
			"Calculated weighted cluster coefficients similar to Stam et al. in Brain 2009 (no threshold options!)" );

	if ( !p.Parse( argc, argv ) )
	{
		p.PrintUsage( std::cout );
		return -1;
	}

	ClusterCoefficient cc;

	if ( stam )
	{
		cc.RunStam( inputFileName );
		return 0;
	}

	if ( weightedGraph )
	{
		cc.RunWeighted( inputFileName, threshold );
	} else
	{
		cc.Run( inputFileName, threshold );
	}

	return 0;
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_EXP_MOD_NORMAL_LPDF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_EXP_MOD_NORMAL_LPDF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <stan/math/prim/scal/fun/erfc.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
namespace math {

template <bool propto, typename T_y, typename T_loc, typename T_scale,
          typename T_inv_scale>
typename return_type<T_y, T_loc, T_scale, T_inv_scale>::type
exp_mod_normal_lpdf(const T_y& y, const T_loc& mu, const T_scale& sigma,
                    const T_inv_scale& lambda) {
  static const char* function = "exp_mod_normal_lpdf";
  typedef
      typename stan::partials_return_type<T_y, T_loc, T_scale,
                                          T_inv_scale>::type T_partials_return;

  using std::log;

  if (size_zero(y, mu, sigma, lambda))
    return 0.0;

  T_partials_return logp(0.0);

  check_not_nan(function, "Random variable", y);
  check_finite(function, "Location parameter", mu);
  check_positive_finite(function, "Inv_scale parameter", lambda);
  check_positive_finite(function, "Scale parameter", sigma);
  check_consistent_sizes(function, "Random variable", y, "Location parameter",
                         mu, "Scale parameter", sigma, "Inv_scale paramter",
                         lambda);

  if (!include_summand<propto, T_y, T_loc, T_scale, T_inv_scale>::value)
    return 0.0;

  using std::exp;
  using std::log;
  using std::sqrt;

  operands_and_partials<T_y, T_loc, T_scale, T_inv_scale> ops_partials(
      y, mu, sigma, lambda);

  scalar_seq_view<T_y> y_vec(y);
  scalar_seq_view<T_loc> mu_vec(mu);
  scalar_seq_view<T_scale> sigma_vec(sigma);
  scalar_seq_view<T_inv_scale> lambda_vec(lambda);
  size_t N = max_size(y, mu, sigma, lambda);

  for (size_t n = 0; n < N; n++) {
    const T_partials_return y_dbl = value_of(y_vec[n]);
    const T_partials_return mu_dbl = value_of(mu_vec[n]);
    const T_partials_return sigma_dbl = value_of(sigma_vec[n]);
    const T_partials_return lambda_dbl = value_of(lambda_vec[n]);

    const T_partials_return pi_dbl = boost::math::constants::pi<double>();

    if (include_summand<propto>::value)
      logp -= log(2.0);
    if (include_summand<propto, T_inv_scale>::value)
      logp += log(lambda_dbl);
    if (include_summand<propto, T_y, T_loc, T_scale, T_inv_scale>::value)
      logp += lambda_dbl
                  * (mu_dbl + 0.5 * lambda_dbl * sigma_dbl * sigma_dbl - y_dbl)
              + log(erfc((mu_dbl + lambda_dbl * sigma_dbl * sigma_dbl - y_dbl)
                         / (sqrt(2.0) * sigma_dbl)));

    const T_partials_return deriv_logerfc
        = -2.0 / sqrt(pi_dbl)
          * exp(-(mu_dbl + lambda_dbl * sigma_dbl * sigma_dbl - y_dbl)
                / (std::sqrt(2.0) * sigma_dbl)
                * (mu_dbl + lambda_dbl * sigma_dbl * sigma_dbl - y_dbl)
                / (sigma_dbl * std::sqrt(2.0)))
          / erfc((mu_dbl + lambda_dbl * sigma_dbl * sigma_dbl - y_dbl)
                 / (sigma_dbl * std::sqrt(2.0)));

    if (!is_constant_struct<T_y>::value)
      ops_partials.edge1_.partials_[n]
          += -lambda_dbl + deriv_logerfc * -1.0 / (sigma_dbl * std::sqrt(2.0));
    if (!is_constant_struct<T_loc>::value)
      ops_partials.edge2_.partials_[n]
          += lambda_dbl + deriv_logerfc / (sigma_dbl * std::sqrt(2.0));
    if (!is_constant_struct<T_scale>::value)
      ops_partials.edge3_.partials_[n]
          += sigma_dbl * lambda_dbl * lambda_dbl
             + deriv_logerfc
                   * (-mu_dbl / (sigma_dbl * sigma_dbl * std::sqrt(2.0))
                      + lambda_dbl / std::sqrt(2.0)
                      + y_dbl / (sigma_dbl * sigma_dbl * std::sqrt(2.0)));
    if (!is_constant_struct<T_inv_scale>::value)
      ops_partials.edge4_.partials_[n]
          += 1 / lambda_dbl + lambda_dbl * sigma_dbl * sigma_dbl + mu_dbl
             - y_dbl + deriv_logerfc * sigma_dbl / std::sqrt(2.0);
  }
  return ops_partials.build(logp);
}

template <typename T_y, typename T_loc, typename T_scale, typename T_inv_scale>
inline typename return_type<T_y, T_loc, T_scale, T_inv_scale>::type
exp_mod_normal_lpdf(const T_y& y, const T_loc& mu, const T_scale& sigma,
                    const T_inv_scale& lambda) {
  return exp_mod_normal_lpdf<false>(y, mu, sigma, lambda);
}

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


/*
 * InnerMapTask.cpp
 *
 *  Created on: 25 Jul 2018
 *      Author: scsjd
 */

#include <fstream>
#include <sstream>
#include <iterator>
#include <chrono>
#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>
#include <NTL/vec_ZZ.h>
#include <jsoncpp/json/json.h>
#include "InnerMapTask.h"
#include "HE1Array.h"

InnerMapTask::InnerMapTask(int numPerLine, const char* parametersPath, const char* inputPath){
	this->inputPath = inputPath;
	this->numPerLine = numPerLine;
	parseParameters(parametersPath);
	totalSumTime=0;
	numberAdditions=0;
	totalProductTime=0;
	numberMultiplications=0;
	HE1Array::create_device_handle();
	HE1Array::create_device_modulus(modulus);
}

InnerMapTask::~InnerMapTask(){
	HE1Array::delete_device_modulus();
	HE1Array::delete_device_handle();
}

void InnerMapTask::parseParameters(const char* parametersPath){
	NTL::ZZ mod;
	std::ifstream ifs(parametersPath);
	if (ifs.is_open()){
		std::string json;
		getline(ifs,json);
		Json::Value root;   // will contains the root value after parsing.
		Json::Reader reader;
		bool parsingSuccessful = reader.parse(json,root);
		if (parsingSuccessful){
			modulus = NTL::conv<NTL::ZZ>(root["modulus"].asCString());
		}
	}
}

NTL::ZZ InnerMapTask::run(){
	std::ifstream ifs(inputPath);
	if (!ifs.is_open()){
		throw std::ios_base::failure("Could not open input file.");
	}
	std::string line;

	NTL::vec_ZZ v[numPerLine];

	while(getline(ifs,line)){
		std::istringstream iss(line);
		std::vector<std::string> words((std::istream_iterator<std::string>(iss)),std::istream_iterator<std::string>());
		for(int i = 0; i < words.size(); i++){
			NTL::ZZ z = NTL::conv<NTL::ZZ>(words[i].c_str());
			v[i].append(z);
		}
	}
	ifs.close();

	//Do multiplication on device
	HE1Array prod(v[0]);
	numberMultiplications = v[0].length();
	for (int i=1; i < numPerLine; i++){
		HE1Array bn(v[i]);
		auto start = std::chrono::high_resolution_clock::now();
		prod*=bn;
		auto finish = std::chrono::high_resolution_clock::now();
		totalProductTime += std::chrono::duration_cast<std::chrono::nanoseconds>(finish-start).count();
	}

	//Do prefix sum reduce on CPU because array reduction not supported by xmp
	NTL::vec_ZZ products = prod.to_ZZ_vector();
	NTL::ZZ_p::init(modulus);
	NTL::ZZ_p result;
	for (int i = 0; i < products.length(); i++){
		auto start = std::chrono::high_resolution_clock::now();
		result += NTL::conv<NTL::ZZ_p>(products.at(i));
		auto finish = std::chrono::high_resolution_clock::now();
		totalSumTime += std::chrono::duration_cast<std::chrono::nanoseconds>(finish-start).count();
		numberAdditions++;
	}

	return NTL::rep(result);
};