#pragma once

#include <boost/random/uniform_real.hpp>
#include "gen_generic.hpp"
#include "matrix.hpp"

class random_matrix_generator: public matrix_generator
{
private:
  double _nonzero_probability;

public:
  random_matrix_generator(size_t height, size_t width, double nonzero_probability, tu::log_level level) :
    matrix_generator("random", height, width, level), _nonzero_probability(nonzero_probability)
  {

  }

  virtual ~random_matrix_generator()
  {

  }

  virtual void generate()
  {
    log_generate_start();
    boost::uniform_real<double> dist;

    for (size_t row = 0; row < _height; ++row)
    {
      for (size_t column = 0; column < _width; ++column)
      {
        _matrix(row, column) = dist(_rng) > _nonzero_probability ? 0 : 1;
      }
    }
    log_generate_end();
    sign();
  }

  virtual bool do_pivot(size_t row, size_t column)
  {
    tu::matrix_ternary_pivot(_matrix, row, column);
    return true;
  }
};


/**
 * @file IrisTest.cpp
 *
 * @breif A very basic MLP to classify Iris
 *
 * @date 12/26/17
 * @author Ben Caine
 */

#include "../nn/Net.h"
#include "../nn/loss/CrossEntropy.h"
#include <fstream>
#include <boost/algorithm/string.hpp>
#include <map>
#include <iomanip>

const std::map<std::string, int> IRIS_TYPE_TO_INT {
        {"Iris-setosa", 0},
        {"Iris-versicolor", 1},
        {"Iris-virginica", 2}
};

struct IrisDataset {
    std::vector<std::array<float, 4>> data;
    std::vector<int> labels;
};

IrisDataset loadIrisDataset(const std::string &path = "../examples/data/iris_data.csv") {
    IrisDataset dataset;

    std::ifstream irisFile(path);
    std::string line;
    while (std::getline(irisFile, line, '\n')) {
        std::vector<std::string> values;

        // TODO: Replace boost or link properly in CMake. Can't figure out canoical name
        // for algorithm/string
        boost::split(values, line, [](char c) {
            return c == ',';
        });

        if (values.size() < 5) {
            std::cout << "Found line with less than five elements, skipping" << std::endl;
            continue;
        }

        float sepalLength = std::stof(values[0]);
        float sepalWidth  = std::stof(values[1]);
        float petalLength = std::stof(values[2]);
        float petalWidth  = std::stof(values[3]);
        std::string labelName = values[4];

        auto labelIter = IRIS_TYPE_TO_INT.find(labelName);

        if (labelIter == IRIS_TYPE_TO_INT.end()) {
            std::cerr << "Unknown Iris type of: " << labelName << " please check dataset." << std::endl;
            exit(-1);
        }
        int labelInt = labelIter->second;
        dataset.data.push_back({sepalLength, sepalWidth, petalLength, petalWidth});
        dataset.labels.push_back(labelInt);
    }

    return dataset;
}

int main() {
    auto dataset = loadIrisDataset();

    // TODO: Split into training and test
    int batchSize = dataset.labels.size();
    int numFeatures = dataset.data[0].size();
    int numClasses = *std::max_element(dataset.labels.begin(), dataset.labels.end()) + 1;

    Eigen::Tensor<float, 2> input(batchSize, numFeatures);
    Eigen::Tensor<float, 2> labels(batchSize, numClasses);
    input.setZero();
    labels.setZero();

    for (unsigned int ii = 0; ii < batchSize; ++ii) {
        for (unsigned int feature = 0; feature < numFeatures; ++feature) {
            input(ii, feature) = dataset.data[ii][feature];
        }

        labels(ii, dataset.labels[ii]) = 1.0;
    }

    int numHiddenNodes = 20;
    bool useBias = true;

    nn::Net<float> net;
    net.add(new nn::Dense<>(batchSize, numFeatures, numHiddenNodes, useBias));
    net.add(new nn::Relu<>());
    net.add(new nn::Dense<>(batchSize, numHiddenNodes, numHiddenNodes, useBias));
    net.add(new nn::Relu<>());
    net.add(new nn::Dense<>(batchSize, numHiddenNodes, numClasses, useBias));
    net.add(new nn::Softmax<>());

    nn::CrossEntropyLoss<float, 2> lossFunc;
    net.registerOptimizer(new nn::Adam<float>(0.01));

    int numEpoch = 250;
    for (unsigned int ii = 0; ii < numEpoch; ++ii) {
        auto result = net.forward<2, 2>(input);

        float loss = lossFunc.loss(result, labels);
        float accuracy = lossFunc.accuracy(result, labels);
        std::cout << std::setprecision(5);
        std::cout << "Epoch: " << ii << " loss: " << loss << " accuracy: " << accuracy << std::endl;

        auto lossBack = lossFunc.backward(result, labels);
        net.backward(lossBack);
        net.step();
    }

    return 0;
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/concept/value.hpp>
#include <eve/constant/nan.hpp>
#include <eve/constant/pio_2.hpp>
#include <eve/function/ellint_rc.hpp>
#include <boost/math/special_functions/ellint_rc.hpp>
#include <cmath>

//==================================================================================================
// Types tests
//==================================================================================================
EVE_TEST_TYPES( "Check return types of ellint_rc"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;

  TTS_EXPR_IS( eve::ellint_rc(T(), T())  , T);
  TTS_EXPR_IS( eve::ellint_rc(v_t(), v_t()), v_t);
  TTS_EXPR_IS( eve::ellint_rc(T(), v_t()), T);
  TTS_EXPR_IS( eve::ellint_rc(v_t(), T()), T);
};

//==================================================================================================
// ellint_rc  tests
//==================================================================================================
EVE_TEST( "Check behavior of ellint_rc on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate( eve::test::randoms(0, 100.0)
                             , eve::test::randoms(0, 100.0))
        )
<typename T>(T const& x, T const& y)
{
  using eve::detail::map;
  using v_t = eve::element_type_t<T>;

  TTS_ULP_EQUAL(eve::ellint_rc(x, y) , map([](auto e, auto f) -> v_t { return boost::math::ellint_rc(e, f); }, x, y), 11);
};


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "WindowFuncs.hpp"
#include "../util/AlgorithmUtils.hpp"
#include "../util/FFT.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/FluidTensor.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cmath>

namespace fluid {
namespace algorithm {

class STFT
{
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXXd = Eigen::ArrayXXd;
  using ArrayXcd = Eigen::ArrayXcd;
  using ArrayXXcd = Eigen::ArrayXXcd;

public:
  STFT(index windowSize, index fftSize, index hopSize, index windowType = 0)
      : mWindowSize(windowSize), mHopSize(hopSize), mFrameSize(fftSize / 2 + 1),
        mFFT(fftSize)
  {
    mWindow = ArrayXd::Zero(mWindowSize);
    auto windowTypeIndex = static_cast<WindowFuncs::WindowTypes>(windowType);
    WindowFuncs::map()[windowTypeIndex](mWindowSize, mWindow);
  }

  static void magnitude(const FluidTensorView<std::complex<double>, 2> in,
                        FluidTensorView<double, 2>                     out)
  {
    ArrayXXd mag = _impl::asEigen<Eigen::Array>(in).abs().real();
    out <<= _impl::asFluid(mag);
  }

  static void magnitude(const FluidTensorView<std::complex<double>, 1> in,
                        FluidTensorView<double, 1>                     out)
  {
    ArrayXd mag = _impl::asEigen<Eigen::Array>(in).abs().real();
    out <<= _impl::asFluid(mag);
  }

  static void phase(const FluidTensorView<std::complex<double>, 2> in,
                    FluidTensorView<double, 2>                     out)
  {
    ArrayXXd phase = _impl::asEigen<Eigen::Array>(in).arg().real();
    out <<= _impl::asFluid(phase);
  }

  static void phase(const FluidTensorView<std::complex<double>, 1> in,
                    FluidTensorView<double, 1>                     out)
  {
    phase(FluidTensorView<std::complex<double>, 2>(in),
          FluidTensorView<double, 2>(out));
  }

  void process(const RealVectorView audio, ComplexMatrixView spectrogram)
  {
    index   halfWindow = mWindowSize / 2;
    ArrayXd padded(audio.size() + mWindowSize + mHopSize);
    padded.fill(0);
    padded.segment(halfWindow, audio.size()) =
        Eigen::Map<const ArrayXd>(audio.data(), audio.size());
    index nFrames = static_cast<index>(
        std::floor((padded.size() - mWindowSize) / mHopSize));

    ArrayXXcd result(nFrames, mFrameSize);
    for (index i = 0; i < nFrames; i++)
    {
      result.row(i) =
          mFFT.process(padded.segment(i * mHopSize, mWindowSize) * mWindow);
    }
    spectrogram <<= _impl::asFluid(result);
  }

  void processFrame(const RealVectorView frame, ComplexVectorView out)
  {
    assert(frame.size() == mWindowSize);
    ArrayXcd spectrum =
        mFFT.process(_impl::asEigen<Eigen::Array>(frame) * mWindow);
    out <<= _impl::asFluid(spectrum);
  }

  void processFrame(Eigen::Ref<ArrayXd> frame, Eigen::Ref<ArrayXcd> out)
  {
    assert(frame.size() == mWindowSize);
    out = mFFT.process(frame * mWindow);
  }

  RealVectorView window()
  {
    return RealVectorView(mWindow.data(), 0, mWindowSize);
  }

private:
  index   mWindowSize;
  index   mHopSize;
  index   mFrameSize;
  ArrayXd mWindow;
  FFT     mFFT;
};

class ISTFT
{
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXcd = Eigen::ArrayXcd;
  using ArrayXXcd = Eigen::ArrayXXcd;

public:
  ISTFT(index windowSize, index fftSize, index hopSize, index windowType = 0)
      : mWindowSize(windowSize), mHopSize(hopSize), mScale(1 / double(fftSize)),
        mIFFT(fftSize), mBuffer(mWindowSize)
  {
    mWindow = ArrayXd::Zero(mWindowSize);
    auto windowTypeIndex = static_cast<WindowFuncs::WindowTypes>(windowType);
    WindowFuncs::map()[windowTypeIndex](mWindowSize, mWindow);
    mWindowSquared = mWindow * mWindow;
  }

  void process(const ComplexMatrixView spectrogram, RealVectorView audio)
  {
    const auto& epsilon = std::numeric_limits<double>::epsilon;

    index halfWindow = mWindowSize / 2;
    index nFrames = spectrogram.rows();
    index outputSize = mWindowSize + (nFrames - 1) * mHopSize;
    outputSize += mWindowSize + mHopSize;
    ArrayXXcd specData = _impl::asEigen<Eigen::Array>(spectrogram);
    ArrayXd   outputPadded = ArrayXd::Zero(outputSize);
    ArrayXd   norm = ArrayXd::Zero(outputSize);
    for (index i = 0; i < nFrames; i++)
    {
      ArrayXd frame = mIFFT.process(specData.row(i)).segment(0, mWindowSize);
      outputPadded.segment(i * mHopSize, mWindowSize) +=
          frame * mScale * mWindow;
      norm.segment(i * mHopSize, mWindowSize) += mWindow * mWindow;
    }
    outputPadded = outputPadded / norm.max(epsilon());
    ArrayXd trimmed = outputPadded.segment(halfWindow, audio.size());
    audio <<= _impl::asFluid(trimmed);
  }

  void processFrame(const ComplexVectorView frame, RealVectorView audio)
  {
    mBuffer = mIFFT.process(_impl::asEigen<Eigen::Array>(frame))
                  .segment(0, mWindowSize) *
              mWindow * mScale;
    audio <<= _impl::asFluid(mBuffer);
  }

  void processFrame(Eigen::Ref<ArrayXcd> frame, Eigen::Ref<ArrayXd> audio)
  {
    audio = mIFFT.process(frame).segment(0, mWindowSize) * mWindow * mScale;
  }

  RealVectorView window()
  {
    return RealVectorView(mWindow.data(), 0, mWindowSize);
  }

private:
  index   mWindowSize{1024};
  index   mHopSize{512};
  ArrayXd mWindow;
  ArrayXd mWindowSquared;
  double  mScale{1};
  IFFT    mIFFT;
  ArrayXd mBuffer;
};

} // namespace algorithm
} // namespace fluid


#include <stan/math/fwd/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/gamma.hpp>
#include <test/unit/math/fwd/scal/fun/nan_util.hpp>

TEST(AgradFwdGammaQ, Fvar){
  using stan::math::fvar;
  using stan::math::gamma_q;
  using boost::math::gamma_q;

  fvar<double> x(0.5);
  x.d_ = 1.0;
  fvar<double> y (1.0);
  y.d_ = 1.0;

  fvar<double> a = gamma_q(x,y);
  EXPECT_FLOAT_EQ(gamma_q(0.5,1.0), a.val_);
  EXPECT_FLOAT_EQ(0.18228334, a.d_);

  double z = 1.0;
  double w = 0.5;

  a = gamma_q(x,z);
  EXPECT_FLOAT_EQ(gamma_q(0.5,1.0), a.val_);
  EXPECT_FLOAT_EQ(0.38983709, a.d_);

  a = gamma_q(w,y);
  EXPECT_FLOAT_EQ(gamma_q(0.5,1.0), a.val_);
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5,1.0), a.d_);

  EXPECT_THROW(gamma_q(-x,y), std::domain_error);
  EXPECT_THROW(gamma_q(x,-y), std::domain_error);
}

TEST(AgradFwdGammaQ, FvarFvarDouble) {
  using stan::math::fvar;
  using boost::math::gamma_q;

  fvar<fvar<double> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<double> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<double> > a = gamma_q(x,y);

  EXPECT_FLOAT_EQ(gamma_q(0.5,1.0), a.val_.val_);
  EXPECT_FLOAT_EQ(0.38983709, a.val_.d_);
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5,1.0), a.d_.val_);
  EXPECT_FLOAT_EQ(-0.40753385, a.d_.d_);
}

struct gamma_q_fun {
  template <typename T0, typename T1>
  inline
  typename boost::math::tools::promote_args<T0,T1>::type
  operator()(const T0 arg1,
             const T1 arg2) const {
    return gamma_q(arg1,arg2);
  }
};

TEST(AgradFwdGammaQ, nan) {
  gamma_q_fun gamma_q_;
  test_nan_fwd(gamma_q_,3.0,5.0,false);
}


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

#include <vector>
#include <memory>

#include "SDOT/SemiDiscreteOT.h"
#include "SDOT/PolygonRasterize.h"
#include "SDOT/RegularGrid.h"
#include "SDOT/DiscretizedDistribution.h"
#include "SDOT/OptionUtilities.h"
#include "SDOT/Distances/Wasserstein2.h"

using namespace sdot;
using namespace sdot::distances;

void AddCircle(Eigen::MatrixXd &dens, double x, double y, double r, double dx, double dy)
{
  for(int j=0; j<dens.cols(); ++j){
    double yj = double(j)*dy;//  + 0.5*dy;

    for(int i=0; i<dens.rows(); ++i){
      double xi = double(i)*dx;// + 0.5*dx;

      if((xi-x)*(xi-x) + (yj-y)*(yj-y) < r*r)
        dens(i,j) = 1.0;
    }
  }
}

int main(int argc, char* argv[])
{
  OptionList opts;
  opts["Print Level"] = 1;
  opts["Max Steps"] = 300;
  opts["GTol Abs"] = 1e-8;
  opts["XTol Abs"] = 1e-9;

  int N = 1;

  Eigen::Matrix2Xd domain(2,4);
  domain << 0.0, 2.0, 2.0, 0.0,
            0.0, 0.0, 2.0, 2.0;

  // Construct the continuous distribution
  auto grid = std::make_shared<RegularGrid>(domain(0,0),domain(1,0), domain(0,2), domain(1,2), N, N);

  // Unnormalized density.  Will be normalized in DiscretizedDistribution constructor
  Eigen::MatrixXd density = Eigen::MatrixXd::Ones(grid->NumCells(0), grid->NumCells(1))/(N*N*grid->dx*grid->dy);

  //double radius = 0.1001;
  //AddCircle(density, 0.4,0.8, radius, grid->dx, grid->dy);
  //AddCircle(density, 1.4,0.4, radius, grid->dx, grid->dy);

  std::cout << "Density = \n" << density << std::endl;

  Eigen::MatrixXd pts(2,2);
  pts << 0.1, 1.5,
         1.0, 1.0;
  //pts << 1.26996324, 1.11996324, 1.11996324, 0.83003676, 0.68003676, 0.68003676,
  //       1.64491023, 1.73151277, 1.55830769, 0.60508977, 0.69169231, 0.51848723;
  unsigned int numPts = pts.cols();

  auto dist = std::make_shared<DiscretizedDistribution>(grid, density);

  // Evalaute the SDOT objective
  Eigen::VectorXd discrProbs = Eigen::VectorXd::Ones(pts.cols());
  discrProbs /= pts.cols();

  auto sdot = std::make_shared<SemidiscreteOT<Wasserstein2>>(dist, pts, discrProbs);

  Eigen::VectorXd optPrices;
  double optVal;
  std::tie(optPrices,optVal) = sdot->Solve(Eigen::VectorXd::Ones(numPts), opts);
  std::cout << "Optimal prices = " << optPrices.transpose() << std::endl;

  std::shared_ptr<LaguerreDiagram> lagDiag = sdot->Diagram();

  std::cout << "Laguerre cells = " << std::endl;
  for(int polyInd=0; polyInd<numPts; ++polyInd){
    std::cout << "Working on " <<  polyInd << std::endl;
    std::shared_ptr<PolygonRasterizeIter::Polygon_2> poly = lagDiag->GetCell(polyInd)->ToCGAL();

    if(poly->size()>0){
      auto vertIt = poly->vertices_begin();
      std::cout << "[[" << vertIt->x() << "," << vertIt->y() << "]";
      vertIt++;
      for(;  vertIt != poly->vertices_end(); ++vertIt){
        std::cout << ", [" << vertIt->x() << "," << vertIt->y() << "]";
      }
      std::cout << "]" << std::endl;
    }
  }

  // Check the gradient wrt the points
  Eigen::Matrix2Xd grad = sdot->PointGradient();

  // Compute a finite difference approximation
  double fdStep = 1e-5;
  Eigen::VectorXd newPrices;
  double newVal;

  for(int ptInd=0; ptInd<pts.cols(); ++ptInd){
    Eigen::MatrixXd newPts = pts;
    newPts(0,ptInd) += fdStep;

    auto newSdot = std::make_shared<SemidiscreteOT<Wasserstein2>>(dist, newPts, discrProbs);
    std::tie(newPrices,newVal) = newSdot->Solve(Eigen::VectorXd::Ones(numPts), opts);

    std::cout << "Point " << ptInd << std::endl;
    std::cout << "  FD Deriv:   " << (newVal-optVal)/fdStep << std::endl;
    std::cout << "  True Deriv: " << grad(0,ptInd) << std::endl;
  }

  return 0;
}


/*
 * Copyright 2015 C. Brett Witherspoon
 */

#define BOOST_TEST_MODULE signum_tests
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include <cmath>

#include "signum/oscillator.hpp"

BOOST_AUTO_TEST_CASE(double_oscillator_test)
{
  const double thresh = 1e-14;
  const double freq = 10;
  const double rate = 50;

  std::array<double, 50> output;

  signum::oscillator<double> osc(freq, rate);

  osc(output);

  for (auto i = 0U; i < output.size(); ++i)
    if (std::abs(output[i]) > thresh)
      BOOST_CHECK_CLOSE(output[i], sin(2*M_PI*freq/rate*(i+1)), 0.0001);
}

BOOST_AUTO_TEST_CASE(complex_oscillator_test)
{
  const double thresh = 1e-12;
  const double freq = 100;
  const double rate = 650;

  std::array<std::complex<double>, 650> output;

  signum::oscillator<std::complex<double>> osc(freq, rate);

  osc(output);

  for (auto i = 0U; i < output.size(); ++i)
  {
    if (std::abs(output[i].real()) > thresh)
      BOOST_CHECK_CLOSE(output[i].real(), cos(2*M_PI*freq/rate*i), 0.0001);
    if (std::abs(output[i].imag()) > thresh)
      BOOST_CHECK_CLOSE(output[i].imag(), sin(2*M_PI*freq/rate*i), 0.01);
  }
}


#include "stdafx.h"

#include <iostream>
#include <stdio.h>

#include <Math.h>
#include <Eigen/Geometry>

#include <opencv2/core/core.hpp>

using namespace std;
using namespace cv;

const int SMOOTHING_WINDOW_SIZE = 10;

vector<Point2d> point_history;

vector<float> weights;
float weight_sum = 0;

// create and initialize weights and history vectors
void gaze_smoothing_init(){
	for (float i = 1; i < SMOOTHING_WINDOW_SIZE; i++){
		weights.push_back(i);
		weight_sum += i;
		point_history.push_back(Point2d(0,0));
	}
}

// very simple smoothing low-pass filter
Point2d smooth_gaze(Point2d gaze_point){

	Point2d point_to_return(0,0);

	rotate(point_history.begin(), point_history.begin() + 1, point_history.end());
	point_history[point_history.size()-1] = gaze_point;

	for (int i = 0; i < weights.size(); i++)
		point_to_return += point_history[i] * (weights[i]/weight_sum);

	return point_to_return;
}


/*
expressionGenerator.cpp - This file is part of the Bayesembler (v1.1.1)

The MIT License (MIT)

Copyright (c) 2014 Lasse Maretty and Jonas Andreas Sibbesen

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include <expressionGenerator.h>
#include <boost/random/uniform_01.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/discrete_distribution.hpp>

typedef boost::random::mt19937* mt_rng_pt_t;
typedef boost::random::uniform_01<boost::random::mt19937*> uniform_01_sampler_t;
typedef boost::random::gamma_distribution<> gamma_distribution_t;
typedef boost::random::variate_generator<boost::random::mt19937*, boost::random::gamma_distribution<> > gamma_sampler_t;
typedef boost::random::uniform_int_distribution<> uniform_sampler_t;

ExpressionGenerator::ExpressionGenerator(int num_fragments_in, int num_transcripts_in, mt_rng_pt_t mt_rng_pt_in) {

	num_fragments = num_fragments_in;
	num_transcripts = num_transcripts_in;
	mt_rng_pt = mt_rng_pt_in;
}

ExpressionValueContainer ExpressionGenerator::generateEnsembleExpression(vector<int> indices, double gamma, CountValueContainer map_counts) {

	// Init container
	ExpressionValueContainer expression(num_transcripts);

	// Sample gammas for s-plus
	double norm_const_expression = 0;

	for (int i = 0; i < indices.size(); i++) {

	    int trans_id = indices[i];

	    gamma_distribution_t gamma_dist((map_counts.getCount(trans_id) + gamma),1);
        gamma_sampler_t sample_gamma(mt_rng_pt, gamma_dist);

        double gamma_sample = sample_gamma();

	    norm_const_expression += gamma_sample;
	    expression.setBinaryOn(trans_id);
	    expression.setValue(gamma_sample, trans_id);
        expression.addToPlus(trans_id);
	}

	expression.normalise(norm_const_expression);

	return expression;
}

ExpressionValueContainer ExpressionGenerator::generateExpression(int b, double gamma, CountValueContainer map_counts) {

	// Init container
	ExpressionValueContainer expression(num_transcripts);

	// Sample gammas for s-plus
	double norm_const_expression = 0;

	for (int i = 0; i < map_counts.getPlusSize(); i++) {

	    int trans_id = map_counts.getPlus(i);

	    gamma_distribution_t gamma_dist((map_counts.getCount(trans_id) + gamma),1);
        gamma_sampler_t sample_gamma(mt_rng_pt, gamma_dist);

        double gamma_sample = sample_gamma();

	    norm_const_expression += gamma_sample;
	    expression.setBinaryOn(trans_id);
	    expression.setValue(gamma_sample, trans_id);
        expression.addToPlus(trans_id);
	}

    gamma_distribution_t gamma_dist_base (gamma, 1);
    gamma_sampler_t sample_gamma_base (mt_rng_pt, gamma_dist_base);

	// Sample gammas for the expanded simplex
	for (int i = map_counts.getPlusSize(); i < b ; i++) {

	    uniform_sampler_t sample_trans(0,(map_counts.getNullSize()-1));

	    int s_null_idx = sample_trans(*mt_rng_pt);
	    int trans_id = map_counts.getNull(s_null_idx);

	    assert (map_counts.getCount(trans_id) == 0);

	    double gamma_base_sample = sample_gamma_base();
	    assert(gamma_base_sample >= double_underflow);

        // if (gamma_base_sample < almost_zero) {

        //     gamma_base_sample = almost_zero;
        // }

	    norm_const_expression += gamma_base_sample;
	    expression.setBinaryOn(trans_id);
	    expression.setValue(gamma_base_sample, trans_id);
        expression.addToPlus(trans_id);
	    map_counts.eraseNull(s_null_idx);
	}

	expression.normalise(norm_const_expression);

	return expression;
}


#include <iostream>
#include <chrono>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>

#include "utils/FileUtil.h"
#include "algorithm/BFFSolver.h"
#include "algorithm/BonSolver.h"

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

int **computeAllShortestPaths2(vector<pair<int, int>> &edges_vec, int n) {
    const int m = edges_vec.size();

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

    typedef std::pair<int, int> Edge;
    Edge edges_array[m];
    for (int i = 0; i < m; ++i)
        edges_array[i] = Edge(edges_vec[i].first, edges_vec[i].second);

    Graph G(edges_array, edges_array + m, n);

    property_map<Graph, edge_weight_t>::type w = get(edge_weight, G);
    int weights[m];
    std::fill(weights, weights + m, 1);
    int *wp = weights;

    graph_traits<Graph>::edge_iterator e, e_end;
    for (boost::tie(e, e_end) = edges(G); e != e_end; ++e)
        w[*e] = *wp++;

    int **D = new int *[n];
    for (int i = 0; i < n; ++i) {
        D[i] = new int[n];
    }
    johnson_all_pairs_shortest_paths(G, D);
    return D;
}

int **computeAllShortestPaths(vector<vector<int>> &adj, int n) {

    int **D = new int *[n];
    for (int i = 0; i < n; ++i) {
        D[i] = new int[n];
        fill(D[i], D[i] + n, n);
        D[i][i] = 0;
    }

    for (int i = 0; i < n; ++i) {
        queue<int> q;
        vector<bool> visited(n);
        q.push(i);
        visited[i] = true;
        while (!q.empty()) {
            int s = q.front();
            q.pop();
            for (auto u: adj[s]) {
                if (!visited[u]) {
                    D[i][u] = D[i][s] + 1;
                    visited[u] = true;
                    q.push(u);
                }
            }
        }
    }

    return D;
}

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

    string input_file = argv[1];
    string alg = argv[2];

    // read instance
    vector<pair<int, int>> edges_vec;
    int n;
    tie(edges_vec, n) = FileUtil::load_graph(input_file);
    vector<vector<int>> adj = AlgUtils::createAdjList(edges_vec, n);

    // time of compute all shortest paths
    auto start = high_resolution_clock::now();
    int **D = computeAllShortestPaths(adj, n);

    auto stop = high_resolution_clock::now();
    auto duration = duration_cast<milliseconds>(stop - start);
    double time_APSP = duration.count() / (double) 1000;
    // end computations of all shortest paths
    cout << "Compute all shortest paths running time: " << time_APSP << " seconds" << endl;

    // time of algorithm
    vector<int> f;
    start = high_resolution_clock::now();

    if (alg == "bon") {
        BonSolver solver(n, D);
        f = solver.run();
    } else if (alg == "bff") {
        BFFSolver solver(n, D, adj);
        f = solver.run();
    } else if (alg == "bff+") {
        BFFSolver solver(n, D, adj);
        solver.setPlus(true);
        f = solver.run();
    } else {
        cerr << "Invalid algorithm!" << endl;
    }
    stop = high_resolution_clock::now();
    duration = duration_cast<milliseconds>(stop - start);
    double time_alg = duration.count() / (double) 1000;

    // print computations times and solution
    cout << "Algorithm running time: " << time_alg << " seconds" << endl;

    cout << "[";
    for (int i = 0; i < f.size() - 1; ++i) {
        cout << f[i] << ", ";
    }
    cout << f.back() << "]" << endl;
    cout << f.size() << endl;

    // clear memory
    for (int i = 0; i < n; ++i) {
        delete[] D[i];
    }
    return 0;
}


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

#ifndef GEN_RANDOM_HPP_
#define GEN_RANDOM_HPP_

#include <boost/random/uniform_real.hpp>
#include "gen_generic.hpp"
#include "matrix.hpp"

class random_matrix_generator: public matrix_generator
{
private:
  double _nonzero_probability;

public:
  random_matrix_generator(size_t height, size_t width, double nonzero_probability, unimod::log_level level) :
    matrix_generator("random", height, width, level), _nonzero_probability(nonzero_probability)
  {

  }

  virtual ~random_matrix_generator()
  {

  }

  virtual void generate()
  {
    log_generate_start();
    boost::uniform_real<double> dist;

    for (size_t row = 0; row < _height; ++row)
    {
      for (size_t column = 0; column < _width; ++column)
      {
        _matrix(row, column) = dist(_rng) > _nonzero_probability ? 0 : 1;
      }
    }
    log_generate_end();
    sign();
  }

  virtual bool do_pivot(size_t row, size_t column)
  {
    unimod::matrix_ternary_pivot(_matrix, row, column);
    return true;
  }
};

#endif


#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>
#include <unordered_map>

using namespace std;
using boost::multiprecision::cpp_int;

// Generate the n'th term for the factorial sequence. Uses a cache to speed up results.
cpp_int factorial(int n) {
	static unordered_map<int, cpp_int> cache;
	if(auto it = cache.find(n); it != cache.end()) {
		return (*it).second;
	}
	if(n > 1) {
		cpp_int res = n * factorial(n - 1);
		cache.insert({n, res});
		return res;
	} else {
		return 1;
	}
}

// Gets the combination.
cpp_int nCr(int n, int r) {
	return (factorial(n) / (factorial(r) * factorial(n - r)));
}

int main(int argc, char *argv[]) {
	int over_million_count = 0;
	for(int n = 1; n < 101; n++) {
		for(int r = 1; r <= n; r++) {
			if(nCr(n, r) > 1'000'000) {
				over_million_count++;
			}
		}
	}
	cout << over_million_count << endl;
	return 0;
}


#include <geometrycentral/direction_fields.h>

#include "geometrycentral/linear_solvers.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <Eigen/SparseQR>

using std::cout;
using std::endl;

namespace geometrycentral {

// Anonymous namespace for helper functions
namespace {

VertexData<Complex> computeSmoothestVertexDirectionField_noBoundary(Geometry<Euclidean>* geometry, int nSym,
                                                                    bool alignCurvature) {
  HalfedgeMesh* mesh = geometry->getMesh();
  size_t N = mesh->nVertices();

  GeometryCache<Euclidean>& gc = geometry->cache;
  gc.requireVertexTransportCoefs();
  gc.requireEdgeCotanWeights();
  gc.requireVertexIndices();
  gc.requireVertexDualAreas();

  // Energy matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> energyMatrix(
      N, N); // have to use ColMajor because LU solver below demands it

  // Supposedly reserving space in the matrix makes construction real zippy
  // below
  Eigen::VectorXi nEntries(N);
  for (VertexPtr v : mesh->vertices()) {
    nEntries[gc.vertexIndices[v]] = v.degree() + 1;
  }
  energyMatrix.reserve(nEntries);

  // Mass matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> massMatrix(N, N);
  massMatrix.reserve(1);

  // === Build matrices

  // Build the mass matrix
  for (VertexPtr v : mesh->vertices()) {
    size_t i = gc.vertexIndices[v];
    massMatrix.insert(i, i) = gc.vertexDualAreas[v];
  }

  // Build the energy matrix
  for (VertexPtr v : mesh->vertices()) {
    size_t i = gc.vertexIndices[v];

    std::complex<double> weightISum = 0;
    for (HalfedgePtr he : v.incomingHalfedges()) {
      size_t j = gc.vertexIndices[he.vertex()];
      std::complex<double> rBar = std::pow(gc.vertexTransportCoefs[he], nSym);
      double weight = gc.edgeCotanWeights[he.edge()];
      energyMatrix.insert(i, j) = -weight * rBar;
      weightISum += weight;
    }

    energyMatrix.insert(i, i) = weightISum;
  }

  // Shift to avoid singularity
  Eigen::SparseMatrix<Complex> eye(N, N);
  eye.setIdentity();
  energyMatrix += 1e-4 * eye;

  // Store the solution here
  Eigen::VectorXcd solution;

  // If requested, align to principal curvatures
  if (alignCurvature) {

    gc.requirePrincipalDirections();

    Eigen::VectorXcd dirVec(N);
    if (nSym == 2) {
      for (VertexPtr v : mesh->vertices()) {
        dirVec[gc.vertexIndices[v]] = gc.principalDirections[v];
      }
    } else if (nSym == 4) {
      for (VertexPtr v : mesh->vertices()) {
        dirVec[gc.vertexIndices[v]] = std::pow(gc.principalDirections[v], 2);
      }
    }

    // Normalize the alignment field
    double scale = std::sqrt(std::abs((dirVec.adjoint() * massMatrix * dirVec)[0]));
    dirVec /= scale;

    double lambdaT = 0.0; // this is something of a magical constant, see
                          // "Globally Optimal Direction Fields", eqn 16

    // Eigen::VectorXcd RHS = massMatrix * dirVec;
    Eigen::VectorXcd RHS = dirVec;
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix - lambdaT * massMatrix;
    solution = solveSquare(LHS, RHS);
  }
  // Otherwise find the smallest eigenvector
  else {
    std::cout << "Solving smoothest field eigenvalue problem..." << std::endl;
    solution = smallestEigenvectorPositiveDefinite(energyMatrix, massMatrix);
  }

  // Copy the result to a VertexData vector
  VertexData<Complex> toReturn(mesh);
  for (VertexPtr v : mesh->vertices()) {
    toReturn[v] = solution[gc.vertexIndices[v]] / std::abs(solution[gc.vertexIndices[v]]);
  }

  return toReturn;
}

VertexData<Complex> computeSmoothestVertexDirectionField_boundary(Geometry<Euclidean>* geometry, int nSym,
                                                                  bool alignCurvature) {
  HalfedgeMesh* mesh = geometry->getMesh();
  size_t nInterior = mesh->nInteriorVertices();

  GeometryCache<Euclidean>& gc = geometry->cache;
  gc.requireVertexTransportCoefs();
  gc.requireEdgeCotanWeights();
  gc.requireVertexBases();
  gc.requireInteriorVertexIndices();
  gc.requireVertexDualAreas();

  // Compute the boundary values
  VertexData<std::complex<double>> boundaryValues(mesh);
  for (VertexPtr v : mesh->vertices()) {
    if (v.isBoundary()) {
      Vector3 b = geometry->boundaryNormal(v);
      Complex bC(dot(gc.vertexBases[v][0], b), dot(gc.vertexBases[v][1], b)); // TODO can do better
      bC = unit(bC);
      boundaryValues[v] = std::pow(bC, nSym);
    } else {
      boundaryValues[v] = 0;
    }
  }

  VertexData<size_t> vertInd = mesh->getInteriorVertexIndices();

  // Energy matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> energyMatrix(nInterior, nInterior);

  Eigen::VectorXi nEntries(nInterior);
  for (VertexPtr v : mesh->vertices()) {
    if (v.isBoundary()) {
      continue;
    }
    nEntries[gc.interiorVertexIndices[v]] = v.degree() + 1;
  }
  energyMatrix.reserve(nEntries);

  // Mass matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> massMatrix(nInterior, nInterior);
  massMatrix.reserve(1);

  // RHS
  Eigen::VectorXcd b(nInterior);

  // === Build matrices

  // Build the mass matrix and zero b
  for (VertexPtr v : mesh->vertices()) {
    if (v.isBoundary()) {
      continue;
    }
    size_t i = gc.interiorVertexIndices[v];
    b(i) = 0.0;
    massMatrix.insert(i, i) = gc.vertexDualAreas[v];
  }

  // Build the energy matrix
  for (VertexPtr v : mesh->vertices()) {
    if (v.isBoundary()) {
      continue;
    }
    size_t i = gc.interiorVertexIndices[v];

    std::complex<double> weightISum = 0;
    for (HalfedgePtr he : v.incomingHalfedges()) {
      std::complex<double> rBar = std::pow(gc.vertexTransportCoefs[he], nSym);
      double w = gc.edgeCotanWeights[he.edge()];

      // Interior-boundary term
      if (he.vertex().isBoundary()) {
        std::complex<double> bVal = boundaryValues[he.vertex()];
        b(i) += w * rBar * bVal;
      } else { // Interior-interior term
        size_t j = gc.interiorVertexIndices[he.vertex()];
        energyMatrix.insert(i, j) = -w * rBar;
      }
      weightISum += w;
    }

    energyMatrix.insert(i, i) = weightISum;
  }

  // Shift to avoid singularities
  Eigen::SparseMatrix<Complex> eye(nInterior, nInterior);
  eye.setIdentity();
  energyMatrix += 1e-4 * eye;

  // Compute the actual solution
  std::cout << "Solving linear problem..." << std::endl;

  // Store the solution here
  Eigen::VectorXcd solution;

  // If requested, align to principal curvatures
  if (alignCurvature) {

    gc.requirePrincipalDirections();

    Eigen::VectorXcd dirVec(nInterior);
    for (VertexPtr v : mesh->vertices()) {
      if (v.isBoundary()) {
        continue;
      }

      Complex directionVal = gc.principalDirections[v];
      if (nSym == 4) {
        directionVal = std::pow(directionVal, 2);
      }

      // Normalize the curvature vectors. By doing so, we lose the property of adjusting the strength of the alignment
      // based on the strength of the curvature, but resolve any scaling issues between the magnitude of the normals and
      // the magnitude of the desired field.  Be careful when interpreting this as opposed to the usual direction field
      // optimization.
      dirVec[gc.interiorVertexIndices[v]] = directionVal / std::abs(directionVal);
    }

    double t = 0.01; // this is something of a magical constant, see "Globally
                     // Optimal Direction Fields", eqn 9

    Eigen::VectorXcd RHS = massMatrix * (t * dirVec + b);
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    solution = solveSquare(LHS, RHS);
  }
  // Otherwise find the general closest solution
  else {
    std::cout << "Solving smoothest field dirichlet problem..." << std::endl;
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    Eigen::VectorXcd RHS = massMatrix * b;
    solution = solveSquare(LHS, RHS);
  }

  // Copy the result to a VertexData vector for both the boudary and interior
  VertexData<Complex> toReturn(mesh);
  for (VertexPtr v : mesh->vertices()) {
    if (v.isBoundary()) {
      toReturn[v] = boundaryValues[v];
    } else {
      toReturn[v] = unit(solution[gc.interiorVertexIndices[v]]);
    }
  }

  return toReturn;
}
}; // namespace

VertexData<Complex> computeSmoothestVertexDirectionField(Geometry<Euclidean>* geometry, int nSym, bool alignCurvature) {
  std::cout << "Computing globally optimal direction field" << std::endl;

  if (alignCurvature && !(nSym == 2 || nSym == 4)) {
    throw std::logic_error("ERROR: It only makes sense to align with curvature when nSym = 2 or "
                           "4");
  }

  // Dispatch to either the boundary of no boundary variant depending on the
  // mesh type
  bool hasBoundary = false;
  for (VertexPtr v : geometry->getMesh()->vertices()) {
    hasBoundary |= v.isBoundary();
  }

  if (hasBoundary) {
    std::cout << "Mesh has boundary, computing dirichlet boundary condition solution" << std::endl;
    return computeSmoothestVertexDirectionField_boundary(geometry, nSym, alignCurvature);
  } else {
    std::cout << "Mesh has no boundary, computing unit-norm solution" << std::endl;
    return computeSmoothestVertexDirectionField_noBoundary(geometry, nSym, alignCurvature);
  }
}

// Helpers for computing face-based direction fields
namespace {

FaceData<Complex> computeSmoothestFaceDirectionField_noBoundary(Geometry<Euclidean>* geometry, int nSym,
                                                                bool alignCurvature) {

  HalfedgeMesh* mesh = geometry->getMesh();
  unsigned int N = mesh->nFaces();

  GeometryCache<Euclidean>& gc = geometry->cache;
  gc.requireFaceTransportCoefs();
  gc.requireFaceNormals();
  gc.requireFaceAreas();
  gc.requireDihedralAngles();
  gc.requireFaceIndices();

  // === Allocate matrices
  // Energy matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> energyMatrix(N, N);
  energyMatrix.reserve(Eigen::VectorXi::Constant(N, 4));

  // Mass matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> massMatrix(N, N);
  massMatrix.reserve(Eigen::VectorXi::Constant(N, 1));

  // === Build matrices

  // Build the mass matrix
  for (FacePtr f : mesh->faces()) {
    size_t i = gc.faceIndices[f];
    massMatrix.insert(i, i) = gc.faceAreas[f];
  }

  // Build the energy matrix
  for (FacePtr f : mesh->faces()) {
    size_t i = gc.faceIndices[f];

    std::complex<double> weightISum = 0;
    for (HalfedgePtr he : f.adjacentHalfedges()) {

      if (!he.twin().isReal()) {
        continue;
      }

      FacePtr neighFace = he.twin().face();
      unsigned int j = gc.faceIndices[neighFace];

      // LC connection between the faces
      Complex rBar = std::pow(gc.faceTransportCoefs[he.twin()], nSym);

      double weight = 1; // FIXME TODO figure out weights
      energyMatrix.insert(i, j) = -weight * rBar;
      weightISum += weight;
    }

    energyMatrix.insert(i, i) = weightISum;
  }

  // Shift to avoid singularity
  Eigen::SparseMatrix<Complex> eye(N, N);
  eye.setIdentity();
  energyMatrix += 1e-4 * eye;

  // Store the solution here
  Eigen::VectorXcd solution;

  // If requested, align to principal curvatures
  if (alignCurvature) {

    Eigen::VectorXcd dirVec(N);
    for (FacePtr f : mesh->faces()) {

      // Compute something like the principal directions
      double weightSum = 0;
      Complex sum = 0;

      for (HalfedgePtr he : f.adjacentHalfedges()) {

        double dihedralAngle = std::abs(gc.dihedralAngles[he.edge()]);
        double weight = norm(geometry->vector(he));
        weightSum += weight;
        double angleCoord = angleInPlane(geometry->vector(f.halfedge()), geometry->vector(he), gc.faceNormals[f]);
        Complex coord = std::exp(angleCoord * IM_I *
                                 (double)nSym); // nsym should be 2 or 4, checked in the funciton which calls this

        sum += coord * weight * dihedralAngle;
      }

      sum /= weightSum;

      dirVec[gc.faceIndices[f]] = sum;
    }

    // Normalize the alignment field
    double scale = std::sqrt(std::abs((dirVec.adjoint() * massMatrix * dirVec)[0]));
    dirVec /= scale;

    double lambdaT = 0.0; // this is something of a magical constant, see "Globally Optimal Direction Fields", eqn 16

    // Eigen::VectorXcd RHS = massMatrix * dirVec;
    Eigen::VectorXcd RHS = dirVec;
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix - lambdaT * massMatrix;
    solution = solveSquare(LHS, RHS);

  }
  // Otherwise find the smallest eigenvector
  else {
    std::cout << "Solving smoothest field eigenvalue problem..." << std::endl;
    solution = smallestEigenvectorPositiveDefinite(energyMatrix, massMatrix);
  }

  // Copy the result to a FaceData object
  FaceData<Complex> field(mesh);
  for (FacePtr f : mesh->faces()) {
    field[f] = solution[gc.faceIndices[f]] / std::abs(solution[gc.faceIndices[f]]);
  }

  return field;
}

FaceData<Complex> computeSmoothestFaceDirectionField_boundary(Geometry<Euclidean>* geometry, int nSym,
                                                              bool alignCurvature) {

  HalfedgeMesh* mesh = geometry->getMesh();

  GeometryCache<Euclidean>& gc = geometry->cache;
  gc.requireFaceTransportCoefs();
  gc.requireFaceNormals();
  gc.requireFaceAreas();
  gc.requireDihedralAngles();

  // Index interior faces
  size_t nInteriorFace = 0;
  FaceData<size_t> interiorFaceInd(mesh, -77);
  FaceData<char> isInterior(mesh);
  for (FacePtr f : mesh->faces()) {
    bool isBoundary = false;
    for (EdgePtr e : f.adjacentEdges()) {
      isBoundary |= e.isBoundary();
    }
    isInterior[f] = !isBoundary;
    if (!isBoundary) {
      interiorFaceInd[f] = nInteriorFace++;
    }
  }

  // Compute boundary values
  FaceData<Complex> boundaryValues(mesh);
  for (FacePtr f : mesh->faces()) {
    if (isInterior[f]) {
      boundaryValues[f] = 0;
    } else {
      Vector3 bVec = Vector3::zero();
      for (HalfedgePtr he : f.adjacentHalfedges()) {
        if (he.edge().isBoundary()) {
          bVec += geometry->vector(he).rotate_around(gc.faceNormals[f], -PI / 2.0);
        }
      }
      Complex bC(dot(gc.faceBases[f][0], bVec), dot(gc.faceBases[f][1], bVec));
      bC = unit(bC);
      boundaryValues[f] = std::pow(bC, nSym);
    }
  }

  // === Allocate matrices
  // Energy matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> energyMatrix(nInteriorFace, nInteriorFace);
  energyMatrix.reserve(Eigen::VectorXi::Constant(nInteriorFace, 4));

  // Mass matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> massMatrix(nInteriorFace, nInteriorFace);
  massMatrix.reserve(Eigen::VectorXi::Constant(nInteriorFace, 1));

  // RHS
  Eigen::VectorXcd b(nInteriorFace);

  // === Build matrices

  // Build the mass matrix
  for (FacePtr f : mesh->faces()) {
    if (isInterior[f]) {
      size_t i = interiorFaceInd[f];
      massMatrix.insert(i, i) = gc.faceAreas[f];
    }
  }

  // Build the energy matrix
  for (FacePtr f : mesh->faces()) {
    if (isInterior[f]) {
      size_t i = interiorFaceInd[f];

      std::complex<double> weightISum = 0;
      for (HalfedgePtr he : f.adjacentHalfedges()) {

        FacePtr neighFace = he.twin().face();
        double weight = 1; // FIXME TODO figure out weights
        Complex rBar = std::pow(gc.faceTransportCoefs[he.twin()], nSym);

        if (isInterior[neighFace]) {
          size_t j = interiorFaceInd[neighFace];
          energyMatrix.insert(i, j) = -weight * rBar;
        } else {
          std::complex<double> bVal = boundaryValues[neighFace];
          b(i) += weight * rBar * bVal;
        }

        weightISum += weight;
      }

      energyMatrix.insert(i, i) = weightISum;
    }
  }

  // Shift to avoid singularity
  Eigen::SparseMatrix<Complex> eye(nInteriorFace, nInteriorFace);
  eye.setIdentity();
  energyMatrix += 1e-4 * eye;

  // Store the solution here
  Eigen::VectorXcd solution;

  // If requested, align to principal curvatures
  if (alignCurvature) {

    Eigen::VectorXcd dirVec(nInteriorFace);
    for (FacePtr f : mesh->faces()) {
      if (isInterior[f]) {

        // Compute something like the principal directions
        double weightSum = 0;
        Complex sum = 0;

        for (HalfedgePtr he : f.adjacentHalfedges()) {

          double dihedralAngle = std::abs(gc.dihedralAngles[he.edge()]);
          double weight = norm(geometry->vector(he));
          weightSum += weight;
          double angleCoord = angleInPlane(geometry->vector(f.halfedge()), geometry->vector(he), gc.faceNormals[f]);
          Complex coord = std::exp(angleCoord * IM_I *
                                   (double)nSym); // nsym should be 2 or 4, checked in the funciton which calls this

          sum += coord * weight * dihedralAngle;
        }

        sum /= weightSum;

        // Normalize the curvature vectors. By doing so, we lose the property of adjusting the strength of the alignment
        // based on the strength of the curvature, but resolve any scaling issues between the magnitude of the normals
        // and the magnitude of the desired field.  Be careful when interpreting this as opposed to the usual direction
        // field optimization.
        dirVec[interiorFaceInd[f]] = unit(sum);
      }
    }

    double t = 0.1;  // this is something of a magical constant, see "Globally
                     // Optimal Direction Fields", eqn 9
                     // NOTE: This value is different from the one used for vertex fields; seems to work better?

    std::cout << "Solving smoothest field dirichlet problem with curvature term..." << std::endl;
    Eigen::VectorXcd RHS = massMatrix * (t * dirVec + b);
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    solution = solveSquare(LHS, RHS);

  }
  // Otherwise find the general closest solution
  else {
    std::cout << "Solving smoothest field dirichlet problem..." << std::endl;
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    Eigen::VectorXcd RHS = massMatrix * b;
    solution = solveSquare(LHS, RHS);
  }

  // Copy the result to a FaceData object
  FaceData<Complex> field(mesh);
  for (FacePtr f : mesh->faces()) {
    if (isInterior[f]) {
      field[f] = unit(solution[interiorFaceInd[f]]);
    } else {
      field[f] = unit(boundaryValues[f]);
    }
  }

  return field;
}

} // namespace

FaceData<Complex> computeSmoothestFaceDirectionField(Geometry<Euclidean>* geometry, int nSym, bool alignCurvature) {

  std::cout << "Computing globally optimal direction field in faces" << std::endl;

  if (alignCurvature && !(nSym == 2 || nSym == 4)) {
    throw std::logic_error("ERROR: It only makes sense to align with curvature when nSym = 2 or "
                           "4");
  }

  // Dispatch to either the boundary of no boundary variant depending on the mesh type
  bool hasBoundary = false;
  for (VertexPtr v : geometry->getMesh()->vertices()) {
    hasBoundary |= v.isBoundary();
  }

  if (hasBoundary) {
    std::cout << "Mesh has boundary, computing dirichlet boundary condition solution" << std::endl;
    return computeSmoothestFaceDirectionField_boundary(geometry, nSym, alignCurvature);
  } else {
    std::cout << "Mesh has no boundary, computing unit-norm solution" << std::endl;
    return computeSmoothestFaceDirectionField_noBoundary(geometry, nSym, alignCurvature);
  }
}

FaceData<int> computeFaceIndex(Geometry<Euclidean>* geometry, VertexData<Complex> directionField, int nSym) {
  HalfedgeMesh* mesh = geometry->getMesh();

  GeometryCache<Euclidean>& gc = geometry->cache;
  gc.requireFaceTransportCoefs();

  // Store the result here
  FaceData<int> indices(mesh);

  // TODO haven't tested that this correctly reports the index when it is larger
  // than +-1

  for (FacePtr f : mesh->faces()) {
    // Trace the direction field around the face and see how many times it
    // spins!
    double totalRot = 0;

    for (HalfedgePtr he : f.adjacentHalfedges()) {
      // Compute the rotation along the halfedge implied by the field
      Complex x0 = directionField[he.vertex()];
      Complex x1 = directionField[he.twin().vertex()];
      Complex transport = std::pow(gc.vertexTransportCoefs[he], nSym);

      // Find the difference in angle
      double theta0 = std::arg(transport * x0);
      double theta1 = std::arg(x1);
      double deltaTheta = regularizeAngle(theta1 - theta0 + PI) - PI; // regularize to [-PI,PI]

      totalRot += deltaTheta; // accumulate
    }

    // Compute the net rotation and corresponding index
    int index = static_cast<int>(std::round(totalRot / (2 * PI))); // should be very close to a multiple of 2PI
    indices[f] = index;
  }

  return indices;
}

VertexData<int> computeVertexIndex(Geometry<Euclidean>* geometry, FaceData<Complex> directionField, int nSym) {

  HalfedgeMesh* mesh = geometry->getMesh();
  GeometryCache<Euclidean>& gc = geometry->cache;
  gc.requireFaceTransportCoefs();

  // Store the result here
  VertexData<int> indices(mesh);

  // TODO haven't tested that this correctly reports the index when it is larger
  // than +-1

  for (VertexPtr v : mesh->vertices()) {

    // Trace the direction field around the face and see how many times it
    // spins!
    double totalRot = 0;

    for (HalfedgePtr he : v.incomingHalfedges()) {
      // Compute the rotation along the halfedge implied by the field
      Complex x0 = directionField[he.face()];
      Complex x1 = directionField[he.twin().face()];
      Complex transport = std::pow(gc.faceTransportCoefs[he], nSym);

      // Find the difference in angle
      double theta0 = std::arg(transport * x0);
      double theta1 = std::arg(x1);
      double deltaTheta = std::arg(x1 / (transport * x0));

      totalRot += deltaTheta;
    }

    double angleDefect = geometry->angleDefect(v);
    totalRot += angleDefect * nSym;

    // Compute the net rotation and corresponding index
    int index = static_cast<int>(std::round(totalRot / (2 * PI))); // should be very close to a multiple of 2PI
    indices[v] = index;
  }

  return indices;
}

} // namespace geometrycentral


#include <Eigen/Eigen>
#include <Eigen/Eigenvalues>

#include "py4dgeo/compute.hpp"
#include "py4dgeo/kdtree.hpp"
#include "py4dgeo/openmp.hpp"
#include "py4dgeo/py4dgeo.hpp"

#include <algorithm>
#include <complex>
#include <vector>

namespace py4dgeo {

void
compute_multiscale_directions(const Epoch& epoch,
                              EigenPointCloudConstRef corepoints,
                              const std::vector<double>& scales,
                              EigenNormalSetConstRef orientation,
                              EigenNormalSetRef result)
{
  // Instantiate a container for the first thrown exception in
  // the following parallel region.
  CallbackExceptionVault vault;
#ifdef PY4DGEO_WITH_OPENMP
#pragma omp parallel for schedule(dynamic, 1)
#endif
  for (IndexType i = 0; i < corepoints.rows(); ++i) {
    vault.run([&]() {
      double highest_planarity = 0.0;
      for (auto scale : scales) {
        // Find the working set on this scale
        KDTree::RadiusSearchResult points;
        auto qp = corepoints.row(i).eval();
        epoch.kdtree.radius_search(&(qp(0, 0)), scale, points);
        auto subset = epoch.cloud(points, Eigen::all).cast<double>();

        // Calculate covariance matrix
        auto centered = subset.rowwise() - subset.colwise().mean();
        auto cov = (centered.adjoint() * centered) / double(subset.rows() - 1);
        auto coveval = cov.eval();

        // Calculate Eigen vectors
        Eigen::SelfAdjointEigenSolver<decltype(coveval)> solver(coveval);
        const auto& evalues = solver.eigenvalues();

        // Calculate planarity
        double planarity = (evalues[1] - evalues[0]) / evalues[2];
        if (planarity > highest_planarity) {
          highest_planarity = planarity;

          double prod =
            (solver.eigenvectors().col(0).dot(orientation.row(0).transpose()));
          double sign = (prod < 0.0) ? -1.0 : 1.0;
          result.row(i) = sign * solver.eigenvectors().col(0);
        }
      }
    });
  }

  // Potentially rethrow an exception that occurred in above parallel region
  vault.rethrow();
}

}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// With contributions from Cornelius Steinhardt

#ifndef MTL_MATRIX_SVD_INCLUDE
#define MTL_MATRIX_SVD_INCLUDE

#include <cmath>
#include <limits>
#include <algorithm>
#include <boost/numeric/mtl/matrix/strict_upper.hpp>
#include <boost/numeric/mtl/operation/diagonal.hpp>
#include <boost/numeric/mtl/operation/one_norm.hpp>
#include <boost/numeric/mtl/operation/sub_matrix.hpp>
#include <boost/numeric/mtl/operation/trans.hpp>
#include <boost/numeric/mtl/operation/two_norm.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace mat {

/// Returns A=S*V*D' for matrix A as references
template <typename Matrix>
inline void svd(const Matrix& A, Matrix& S, Matrix& V, Matrix& D, double tol= 10e-10)
{
	vampir_trace<3037> tracer;
    typedef typename Collection<Matrix>::value_type   value_type;
    typedef typename Collection<Matrix>::size_type    size_type;
    size_type        ncols= num_cols(A), nrows= num_rows(A), loops, col= ncols, row= nrows;
    value_type       ref, zero= math::zero(ref), one= math::one(ref);
    double 	     err(std::numeric_limits<double>::max()), e, f;

    if (nrows != ncols) // important for right dimension
	std::swap(row, col);

     //init
    Matrix Q(row,row),  R(row,col),  VT(row,col), E(row,col),
 	   QT(col,col), RT(col,row);

    loops= 100 * std::max(nrows,ncols);
    S= one; D= one; E= zero;
    for (size_type i= 0; err > tol && i < loops; ++i) {
	boost::tie(QT, RT)= qr(V);
 	S*= QT;
	VT= trans(RT);
	boost::tie(Q, R)= qr(VT);
	D*= Q;
	E= triu(R,1);
	V= trans(R);

	//ready for exit when upper(R)=0
	f= two_norm(diagonal(R));
	e= one_norm(E);
	if ( f== zero ) f= 1;
	err= e/f;
    } //end for

    {
	V= 0;
	mtl::mat::inserter<Matrix>  ins_V(V);
	mtl::mat::inserter<Matrix,  mtl::operations::update_times<value_type> > ins_S(S);

	for (size_type i= 0, end= std::min(nrows, ncols); i < end; i++) {
	    ins_V[i][i] << std::abs(R[i][i]);
	    if (R[i][i] < zero)
		for (size_type j= 0; j < nrows; j++)
		    ins_S[j][i] << -1;  //carefull changing: multiplication with minus one
	}
    }
}

/// Returns A=S*V*D' for matrix A as triplet
template <typename Matrix>
boost::tuple<Matrix, Matrix, Matrix >
inline svd(const Matrix& A, double tol= 10e-10)
{
	vampir_trace<3038> tracer;
    typedef typename Collection<Matrix>::size_type    size_type;
    size_type    ncols= num_cols(A), nrows= num_rows(A), col= ncols, row= nrows;
    if (nrows != ncols) // important for right dimension
	std::swap(row, col);

    Matrix       ST(col,col), V(A), D(row,row);
    svd(A, ST, V, D, tol);
    return boost::make_tuple(ST, V, D);
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_SVD_INCLUDE


#include <cmath>
#include <array>
#include <stdexcept>
#ifdef HAS_EIGEN3
#include <Eigen/Eigenvalues>
#endif

#include <Kernels/precision.hpp>
#include <Physics/InitialField.h>
#include <Model/Setup.h>
#include <Solver/Interoperability.h>

extern seissol::Interoperability e_interoperability;

seissol::physics::Planarwave::Planarwave(real phase)
  : m_setVar(27),
    m_kVec{3.14159265358979323846, 3.14159265358979323846, 3.14159265358979323846},
    m_phase(phase)
{
#ifdef HAS_EIGEN3
  const double rho = 1.0;
  const double mu = 1.0;
  const double lambda = 2.0;
  const double Qp = 20.0;
  const double Qs = 10.0;

  seissol::model::Material material;
  e_interoperability.fitAttenuation(rho, mu, lambda, Qp, Qs, material);

  std::complex<real> planeWaveOperator[NUMBER_OF_QUANTITIES*NUMBER_OF_QUANTITIES];
  model::getPlaneWaveOperator(material, m_kVec.data(), planeWaveOperator);

  using Matrix = Eigen::Matrix<std::complex<real>, NUMBER_OF_QUANTITIES, NUMBER_OF_QUANTITIES, Eigen::ColMajor>;
  using Vector = Eigen::Matrix<std::complex<real>, NUMBER_OF_QUANTITIES, 1, Eigen::ColMajor>;
  Matrix A(planeWaveOperator);
  Eigen::ComplexEigenSolver<Matrix> ces;
  ces.compute(A);

  auto eigenvalues = ces.eigenvalues();
  for (size_t i = 0; i < NUMBER_OF_QUANTITIES; ++i) {
    m_lambdaA[i] = eigenvalues(i,0);
  }

  Vector ic;
  for (size_t j = 0; j < 9; ++j) {
    ic(j) = 1.0;
  }
  for (size_t j = 9; j < NUMBER_OF_QUANTITIES; ++j) {
    ic(j) = 0.0;
  }

  auto eigenvectors = ces.eigenvectors();
  Vector amp = eigenvectors.colPivHouseholderQr().solve(ic);
  for (int j = 0; j < m_setVar; ++j) {
    m_varField.push_back(j);
    m_ampField.push_back(amp(j));
  }

  auto R = yateto::DenseTensorView<2,std::complex<real>>(m_eigenvectors, {NUMBER_OF_QUANTITIES, NUMBER_OF_QUANTITIES});
  for (size_t j = 0; j < NUMBER_OF_QUANTITIES; ++j) {
    for (size_t i = 0; i < NUMBER_OF_QUANTITIES; ++i) {
      R(i,j) = eigenvectors(i,j);
    }
  }
#else
  throw std::runtime_error("Eigen3 required for anelastic planarwave.");
#endif
}

void seissol::physics::Planarwave::evaluate(  double time,
                                              std::vector<std::array<double, 3>> const& points,
                                              yateto::DenseTensorView<2,real,unsigned>& dofsQP ) const
{
  dofsQP.setZero();

  auto R = yateto::DenseTensorView<2,std::complex<real>>(
             const_cast<std::complex<real>*>(m_eigenvectors),
             {NUMBER_OF_QUANTITIES, NUMBER_OF_QUANTITIES}
           );
  for (int v = 0; v < m_setVar; ++v) {
    const auto omega =  m_lambdaA[m_varField[v]];
    for (unsigned j = 0; j < dofsQP.shape(1); ++j) {
      for (size_t i = 0; i < points.size(); ++i) {
        dofsQP(i,j) += (R(j,m_varField[v]) * m_ampField[v] *
                        std::exp(std::complex<real>(0.0, 1.0) * (
                          omega * time - m_kVec[0]*points[i][0] - m_kVec[1]*points[i][1] - m_kVec[2]*points[i][2] + m_phase
                        ))).real();
      }
    }
  }
}


#include <utility>
#include <random>
#include <vector>
#include <random>
#include <algorithm>
#include <iostream>
#include <fstream>

#include <Eigen/Dense>

typedef std::pair<double, double> CoordinatePair;
typedef struct {
  double value;
  CoordinatePair location;
} Target;

const std::string ln = "\r\n";
const std::string del = " ";

const char* DATA_FILE_ARG = "data";
const char* NUM_NODES_ARG = "nodes";
const char* SENSING_RANGE_ARG = "sense_range";
const char* COMMUNICATION_RANGE_ARG = "comm_range";
const char* FIELD_X_ARG = "x";
const char* FIELD_Y_ARG = "y";
const char* FIELD_CELLS_ARG = "cells";

const double C_W_CONSTANT = 0.01;

const int SEED = 5;
const double NO_READING = -9999.9999;

const double ERROR_THRESHOLD = 0.0001;

class Configurations{
 public:
  std::string data_file_name;
  int number_of_sensor_nodes;
  double sensing_range;
  double communication_range;
  double field_x_size;
  double field_y_size;
  int field_cells;

  Configurations() {
    data_file_name = "data_files_should_have_names";
    number_of_sensor_nodes = 10;
    sensing_range = 1.6;
    communication_range = 1.5;
    field_x_size = 4.0;
    field_y_size = 4.0;
    field_cells = 25;
  };
};

Configurations ProcessCommandLineArguments(int pArgc, char** pArguments);

CoordinatePair GenerateNewCoordinatePair(
  std::default_random_engine& random_generator,
  const Configurations& configurations);

double GenerateNoisyReading(
  Target target,
  double constant,
  CoordinatePair sensor_node_location,
  CoordinatePair average_sensor_location,
  double sensing_range,
  std::default_random_engine generator);

double ComputeDistance(
  CoordinatePair node_location,
  CoordinatePair reckoning_point);

double ComputeNoiseCovariance(
  const CoordinatePair& node_coordinates,
  const CoordinatePair& reckoning_point,
  double p_weight_constant,
  double p_node_sensing_range);

int CountNeighbors(
  double communication_range,
  CoordinatePair source,
  std::vector<CoordinatePair> all_nodes);

int CountNeighborsWhoSenseTarget(
  double communication_range,
  CoordinatePair source,
  std::vector<CoordinatePair> all_nodes,
  Eigen::VectorXd estimates);

double ComputeAverageEstimate(
  Eigen::VectorXd estimates,
  Eigen::MatrixXd weights,
  std::function<double(Eigen::VectorXd, Eigen::MatrixXd)> averaging_method);

double ComputeMaxDegreeWeight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights);

double ComputeMetropolisWeight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights);

bool EmptyEstimatePresent(Eigen::VectorXd estimates);

bool SomeNodeNotConverged(
  Eigen::VectorXd estimates,
  double average_estimate,
  double error_threshold,
  bool method);

void DumpResultsToFile(
  Configurations configurations,
  std::string filename,
  std::vector<CoordinatePair> nodes,
  std::vector<Eigen::VectorXd> estimates);

std::vector<Eigen::VectorXd> MaxDegreeAnalysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates);

std::vector<Eigen::VectorXd> MetropolisAnalysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates);

std::vector<Eigen::VectorXd> WeightDesign1Analysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates);

std::vector<Eigen::VectorXd> WeightDesign2Analysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates);

double ComputeWeightDesign1Weight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  double sensing_range,
  CoordinatePair target_location,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights);

double ComputeWeightDesign2Weight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  double sensing_range,
  CoordinatePair target_location,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights,
  Eigen::VectorXd estimates);

std::vector<Target> ReadInData(Configurations configurations, std::string filename);

void DumpFieldDataToFile(
  Configurations configurations,
  std::string filename,
  std::vector<CoordinatePair> nodes,
  std::vector<double> estimates);

int
main(int arg_count, char** arg_values) {
  Configurations configurations = ProcessCommandLineArguments(
    arg_count,
    arg_values
  );

  // setup (RNGs 'n' stuff)
  std::default_random_engine random_generator(1);

  // generate targets
  std::vector<Target> targets;
  targets.push_back({50.0, {0.0, 0.0}});

  // generate node coordinates
  std::vector<CoordinatePair> sensor_nodes;
  CoordinatePair average_node_location = {0.0, 0.0};
  for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
    sensor_nodes.push_back(
      GenerateNewCoordinatePair(random_generator, configurations)
    );

    std::cout << "x: " << sensor_nodes[i].first << " y: " << sensor_nodes[i].second << std::endl;

    average_node_location.first += sensor_nodes[i].first;
      average_node_location.second += sensor_nodes[i].second;
  }
  average_node_location.first /= (double) configurations.number_of_sensor_nodes;
  average_node_location.second /=
    (double) configurations.number_of_sensor_nodes;

  std::cout << "x: " << average_node_location.first << " y: " << average_node_location.second << std::endl;

  // for each target
  for (Target target : targets) {

    Eigen::VectorXd estimates(configurations.number_of_sensor_nodes);
    for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
      estimates(i) = GenerateNoisyReading(
        target,
        0.01,
        sensor_nodes[i],
        average_node_location,
        configurations.sensing_range,
        random_generator
      );

      std::cout << "estimate: " << estimates(i) << std::endl;
    }

    std::vector<Eigen::VectorXd> results = MaxDegreeAnalysis(
      configurations,
      target,
      sensor_nodes,
      average_node_location,
      random_generator,
      estimates
    );
    DumpResultsToFile(configurations, "MaxDegreeResults.txt", sensor_nodes, results);

    results = MetropolisAnalysis(
      configurations,
      target,
      sensor_nodes,
      average_node_location,
      random_generator,
      estimates
    );
    DumpResultsToFile(configurations, "MetropolisResults.txt", sensor_nodes, results);

    results = WeightDesign1Analysis(
      configurations,
      target,
      sensor_nodes,
      average_node_location,
      random_generator,
      estimates
    );
    DumpResultsToFile(configurations, "WeightDesign1Results.txt", sensor_nodes, results);

    results = WeightDesign2Analysis(
      configurations,
      target,
      sensor_nodes,
      average_node_location,
      random_generator,
      estimates
    );
    DumpResultsToFile(configurations, "WeightDesign2Results.txt", sensor_nodes, results);
  }

  // part 2/////////////////////////////////////////////////////////////////////////////////
  configurations.field_x_size = 12.0;
  configurations.field_y_size = 12.0;
  configurations.field_cells = 25;
  configurations.number_of_sensor_nodes = 30;
  configurations.sensing_range = 5.0;
  configurations.communication_range = 4.5;

  sensor_nodes.clear();
  average_node_location = {0.0, 0.0};
  for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
    sensor_nodes.push_back(
      {0.0, 0.0}
    );

    std::cout << "x: " << sensor_nodes[i].first << " y: " << sensor_nodes[i].second << std::endl;

    average_node_location.first += sensor_nodes[i].first;
      average_node_location.second += sensor_nodes[i].second;
  }
  average_node_location.first /= (double) configurations.number_of_sensor_nodes;
  average_node_location.second /=
    (double) configurations.number_of_sensor_nodes;

  std::cout << "x: " << average_node_location.first << " y: " << average_node_location.second << std::endl;

  std::vector<double> final_cell_estimates_1;
  std::vector<double> final_cell_estimates_2;

  std::vector<Target> new_targets = ReadInData(configurations, "field1.txt");

  // for each target
  for (Target target : new_targets) {

    Eigen::VectorXd estimates(configurations.number_of_sensor_nodes);
    for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
      estimates(i) = GenerateNoisyReading(
        target,
        0.01,
        sensor_nodes[i],
        average_node_location,
        configurations.sensing_range,
        random_generator
      );

      std::cout << "estimate: " << estimates(i) << std::endl;
    }

    std::vector<Eigen::VectorXd> wd1_results = WeightDesign1Analysis(
      configurations,
      target,
      sensor_nodes,
      average_node_location,
      random_generator,
      estimates
    );
    std::vector<Eigen::VectorXd> wd2_results = WeightDesign2Analysis(
      configurations,
      target,
      sensor_nodes,
      average_node_location,
      random_generator,
      estimates
    );

    final_cell_estimates_1.push_back(wd1_results.back()(0));
    final_cell_estimates_2.push_back(wd2_results.back()(0));
  }

  DumpFieldDataToFile(configurations, "WeightDesign1_field.txt", sensor_nodes, final_cell_estimates_1);
  DumpFieldDataToFile(configurations, "WeightDesign2_field.txt", sensor_nodes, final_cell_estimates_2);

  return 0;
}

Configurations
ProcessCommandLineArguments(int pArgc, char** pArguments) {
  Configurations configurations;

  for (int i = 1; i < pArgc; ++i) {
    char* arg_value_pair = pArguments[i];
    std::string argument = strtok(arg_value_pair, "-=");
    char* value = strtok(NULL, "=");

    if (argument == DATA_FILE_ARG) {
      configurations.data_file_name = value;
      printf(
        "Data File: %s\n",
        configurations.data_file_name.c_str()
      );
    } else if (argument == NUM_NODES_ARG) {
      configurations.number_of_sensor_nodes = atoi(value);
      printf(
        "Number of senor nodes: %i\n",
        configurations.number_of_sensor_nodes
      );
    } else if (argument == SENSING_RANGE_ARG) {
      configurations.sensing_range = strtod(value,NULL);
      printf(
        "Node sensing range: %f\n",
        configurations.sensing_range
      );
    } else if (argument == COMMUNICATION_RANGE_ARG) {
      configurations.communication_range = strtod(value, NULL);
      printf(
        "Node communication range: %f\n",
        configurations.communication_range
      );
    } else if (argument == FIELD_X_ARG) {
      configurations.field_x_size = strtod(value, NULL);
      printf(
        "Field X size: %f\n",
        configurations.field_x_size
      );
    } else if (argument == FIELD_Y_ARG) {
      configurations.field_y_size = strtod(value, NULL);
      printf(
        "Field Y size: %f\n",
        configurations.field_y_size
      );
    } else if (argument == FIELD_CELLS_ARG) {
      configurations.field_cells = atoi(value);
      printf(
        "Cells per side of the field: %i\n",
        configurations.field_cells
      );
    }else {
      printf(
        "%s is an unrecognized argument. Program terminating.\n",
        argument.c_str()
      );
      throw std::exception();
    }
  }

  return configurations;
}

CoordinatePair
GenerateNewCoordinatePair(
  std::default_random_engine& random_generator,
  const Configurations& configurations) {

  CoordinatePair new_coordinates;

  new_coordinates.first =
    random_generator() % (int) (configurations.field_x_size * 5.0);
  new_coordinates.first /= 10.0;
  if (random_generator() % 2 == 0) {
    new_coordinates.first *= -1.0;
  }

  new_coordinates.second =
    random_generator() % (int) (configurations.field_y_size * 5.0);
  new_coordinates.second /= 10.0;
  if (random_generator() % 2 == 0) {
    new_coordinates.second *= -1.0;
  }

  return new_coordinates;
}

double GenerateNoisyReading(
  Target target,
  double constant,
  CoordinatePair sensor_node_location,
  CoordinatePair average_sensor_location,
  double sensing_range,
  std::default_random_engine generator) {

  double distance = ComputeDistance(sensor_node_location, target.location);

  if (distance <= sensing_range) {
    std::normal_distribution<double> noise_distribution(
      target.value,
      ComputeNoiseCovariance(
        sensor_node_location,
        average_sensor_location,
        constant,
        sensing_range
      )
    );

    return noise_distribution(generator);
  } else {
    return NO_READING;
  }
}

double ComputeDistance(
  CoordinatePair node_location,
  CoordinatePair reckoning_point) {

  // Euclidean Distance = sqrt((x_0 - x_1)^2 + (y_0 - y_1)^2)

  double x_diff = node_location.first - reckoning_point.first;
  double y_diff = node_location.second - reckoning_point.second;

  return sqrt((x_diff * x_diff) + (y_diff * y_diff));
}

double ComputeNoiseCovariance(
  const CoordinatePair& node_coordinates,
  const CoordinatePair& reckoning_point,
  double constant,
  double sensing_range) {

  double distance = ComputeDistance(
    node_coordinates,
    reckoning_point
  );

  double numerator = (distance * distance) + constant;

  return numerator / (sensing_range * sensing_range);
}

double ComputeAverageEstimate(
  Eigen::VectorXd estimates,
  Eigen::MatrixXd weights,
  std::function<double(Eigen::VectorXd, Eigen::MatrixXd)> averaging_method) {

  double sum = 0.0;
  int n = 0;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      sum += estimates(i);
      n++;
    }
  }

  return sum / (double) n;
}

double ComputeMaxDegreeWeight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights) {

  if (source_node_index != neighbor_index) {
    if (source_node_measurement == NO_READING) {
      return 0.0;
    }

    int num_neighbors = 0;
    for (int i = 0; i < sensor_nodes.size(); ++i) {
      if (i != source_node_index) {
        double separation = ComputeDistance(
          sensor_nodes[source_node_index],
          sensor_nodes[neighbor_index]
        );

        if (separation <= communication_range) {
          num_neighbors++;
        }
      }
    }

    if (neighbor_measurement != NO_READING) {
      double separation = ComputeDistance(
        sensor_nodes[source_node_index],
        sensor_nodes[neighbor_index]
      );

      if (separation <= communication_range) {
        return 1.0 / (double) sensor_nodes.size();
      }
    }

    return 0.0;
  } else {
    if (source_node_measurement == NO_READING) {
      return 1.0;
    } else {
      double neighbor_weights = 0.0;

      for (int j = 0; j < sensor_nodes.size(); ++j) {
        if (j != source_node_index) {
          neighbor_weights += weights(source_node_index, j);
        }
      }

      return 1.0 - neighbor_weights;
    }
  }
}

bool EmptyEstimatePresent(Eigen::VectorXd estimates) {
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) == NO_READING) {
      return true;
    }
  }

  return false;
}

bool SomeNodeNotConverged(
  Eigen::VectorXd estimates,
  double average_estimate,
  double error_threshold,
  bool method) {

  if (method) {
    for (int i = 0; i < estimates.size(); ++i) {
      if (estimates(i) != NO_READING &&
          fabs(estimates(i) - average_estimate) >= error_threshold) {
        return true;
      }
    }
  } else {
    double estimate = 0.0;
    for (int i = 0; i < estimates.size(); ++i) {
      if (estimate == 0.0 && estimates(i) != NO_READING) {
        estimate = estimates(i);
        break;
      }
    }

    for (int i = 0; i < estimates.size(); ++i) {
      if (estimates(i) != NO_READING &&
          (estimates(i) < (estimate - error_threshold) || (estimate + error_threshold) < estimates(i))) {
        return true;
      }
    }
  }

  return false;
}

void DumpResultsToFile(
  Configurations configurations,
  std::string filename,
  std::vector<CoordinatePair> nodes,
  std::vector<Eigen::VectorXd> estimates) {

  std::ofstream fout;
  fout.open(filename.c_str());

  for (CoordinatePair source : nodes) {
    int num_neighbors = 0;
    for (CoordinatePair neighbor : nodes) {
      if (configurations.communication_range <
          ComputeDistance(source, neighbor)) {
        num_neighbors++;
      }
    }

    fout << source.first << del << source.second << del << num_neighbors
         << ln;
  }

  for (Eigen::VectorXd snapshot : estimates) {
    fout << snapshot.transpose() << ln;
  }

  fout.close();
}

std::vector<Eigen::VectorXd> MaxDegreeAnalysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates) {

  // take initial measurements
  Eigen::VectorXd estimates = initial_estimates;

  // determine weight matrix
  Eigen::MatrixXd weights(
    configurations.number_of_sensor_nodes,
    configurations.number_of_sensor_nodes
  );
  weights.setZero();
  for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
    for (int j = 0; j < configurations.number_of_sensor_nodes; ++j) {
      if (i != j) {
        weights(i, j) = ComputeMaxDegreeWeight(
          i,
          estimates(i),
          j,
          estimates(j),
          configurations.communication_range,
          sensor_nodes,
          weights
        );
      }
    }

    weights(i, i) = ComputeMaxDegreeWeight(i, estimates(i), i, estimates(i), configurations.communication_range, sensor_nodes, weights);
  }

  std::cout << "Max-Degree weights" << std::endl << weights << std::endl;

  std::function<double(Eigen::VectorXd, Eigen::MatrixXd)> averaging_method;
  double average_estimate = 0;
  average_estimate = ComputeAverageEstimate(
    estimates,
    weights,
    averaging_method
  );

  std::cout << "average: " << average_estimate << std::endl;

  // iterate til consensus
  std::vector<Eigen::VectorXd> estimate_history;
  estimate_history.push_back(estimates);
  int l = 0;
  while (SomeNodeNotConverged(estimates, average_estimate, ERROR_THRESHOLD, true)) {
    estimates = weights * estimates;
    l++;
    estimate_history.push_back(estimates);
  }

  // update nodes that didn't see the target
  double consensus;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      consensus = estimates(i);
      break;
    }
  }
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) == NO_READING) {
      estimates(i) = consensus;
    }
  }
  estimate_history.push_back(estimates);

  return estimate_history;
}

std::vector<Eigen::VectorXd> MetropolisAnalysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates) {

  // take initial measurements
  Eigen::VectorXd estimates = initial_estimates;

  // determine weight matrix
  Eigen::MatrixXd weights(
    configurations.number_of_sensor_nodes,
    configurations.number_of_sensor_nodes
  );
  weights.setZero();
  for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
    for (int j = 0; j < configurations.number_of_sensor_nodes; ++j) {
      if (i != j) {
        weights(i, j) = ComputeMetropolisWeight(
          i,
          estimates(i),
          j,
          estimates(j),
          configurations.communication_range,
          sensor_nodes,
          weights
        );
      }
    }

    weights(i, i) = ComputeMetropolisWeight(i, estimates(i), i, estimates(i), configurations.communication_range, sensor_nodes, weights);
  }

  std::cout << "Metropolis Weights: " << std::endl << weights << std::endl;

  double average_estimate = 0.0;
  int num_in_average = 0;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      average_estimate += estimates(i);
      num_in_average++;
    }
  }
  average_estimate /= (double) num_in_average;

  std::cout << "average: " << average_estimate << std::endl;

  // iterate til consensus
  std::vector<Eigen::VectorXd> estimate_history;
  estimate_history.push_back(estimates);
  int l = 0;
  while (SomeNodeNotConverged(estimates, average_estimate, ERROR_THRESHOLD, true)) {
    estimates = weights * estimates;
    l++;
    estimate_history.push_back(estimates);
  }

  // update nodes that didn't see the target
  double consensus;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      consensus = estimates(i);
      break;
    }
  }
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) == NO_READING) {
      estimates(i) = consensus;
    }
  }
  estimate_history.push_back(estimates);

  return estimate_history;
}

int CountNeighbors(
  double communication_range,
  CoordinatePair source,
  std::vector<CoordinatePair> all_nodes) {

  int num_neighbors = 0;

  for (CoordinatePair node : all_nodes) {
    if (node != source &&
        ComputeDistance(source, node) <= communication_range) {

      num_neighbors++;
    }
  }

  return num_neighbors;
}

double ComputeMetropolisWeight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights) {

  if (source_node_index != neighbor_index) {
    if (source_node_measurement != NO_READING &&
        neighbor_measurement != NO_READING &&
        ComputeDistance(sensor_nodes[source_node_index], sensor_nodes[neighbor_index]) <= communication_range) {

      return 1.0 / std::max(
        CountNeighbors(
          communication_range,
          sensor_nodes[source_node_index],
          sensor_nodes
        ),
        CountNeighbors(
          communication_range,
          sensor_nodes[neighbor_index],
          sensor_nodes
        )
      );
    }

    return 0.0;
  } else {
    if (source_node_measurement == NO_READING) {
      return 1.0;
    } else {
      double neighbor_weights = 0.0;

      for (int j = 0; j < sensor_nodes.size(); ++j) {
        if (j != source_node_index) {
          neighbor_weights += weights(source_node_index, j);
        }
      }

      return 1.0 - neighbor_weights;
    }
  }
}

std::vector<Eigen::VectorXd> WeightDesign1Analysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates) {

  // take initial measurements
  Eigen::VectorXd estimates = initial_estimates;

  // determine weight matrix
  Eigen::MatrixXd weights(
    configurations.number_of_sensor_nodes,
    configurations.number_of_sensor_nodes
  );
  weights.setZero();
  for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
    for (int j = 0; j < configurations.number_of_sensor_nodes; ++j) {
      if (i != j) {
        weights(i, j) = ComputeWeightDesign1Weight(
          i,
          estimates(i),
          j,
          estimates(j),
          configurations.communication_range,
          configurations.sensing_range,
          target.location,
          sensor_nodes,
          weights
        );
      }
    }

    weights(i, i) = ComputeWeightDesign1Weight(i, estimates(i), i, estimates(i),
      configurations.communication_range,
      configurations.sensing_range,
      target.location,
      sensor_nodes,
      weights
    );
  }

  std::cout << "Weight Design 1 Weights:" << std::endl << weights << std::endl;

  double average_estimate = 0.0;
  double total_weight = 0.0;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      average_estimate += estimates(i) * weights(i, i);
      total_weight += weights(i, i);
    }
  }
  average_estimate /= total_weight;

  std::cout << "weighted average: " << average_estimate << std::endl;

  // iterate til consensus
  std::vector<Eigen::VectorXd> estimate_history;
  estimate_history.push_back(estimates);
  int l = 0;
  while (SomeNodeNotConverged(estimates, average_estimate, ERROR_THRESHOLD, false)) {
    estimates = weights * estimates;
    l++;
    estimate_history.push_back(estimates);

// char y;
// std::cin >> y;
// std::cout << estimates.transpose() << std::endl;
  }

  // update nodes that didn't see the target
  double consensus;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      consensus = estimates(i);
      break;
    }
  }
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) == NO_READING) {
      estimates(i) = consensus;
    }
  }
  estimate_history.push_back(estimates);

  return estimate_history;
}

double ComputeWeightDesign1Weight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  double sensing_range,
  CoordinatePair target_location,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights) {

  if (source_node_index != neighbor_index) {
    if (source_node_measurement != NO_READING &&
        neighbor_measurement != NO_READING &&
        ComputeDistance(sensor_nodes[source_node_index], sensor_nodes[neighbor_index]) <= communication_range) {

      return C_W_CONSTANT /
        (ComputeNoiseCovariance(
          sensor_nodes[source_node_index],
          target_location,
          C_W_CONSTANT,
          sensing_range
        ) +
        ComputeNoiseCovariance(
          sensor_nodes[source_node_index],
          target_location,
          C_W_CONSTANT,
          sensing_range
        ))
      ;
    }

    return 0.0;
  } else {
    if (source_node_measurement == NO_READING) {
      return 1.0;
    } else {
      double neighbor_weights = 0.0;

      for (int j = 0; j < sensor_nodes.size(); ++j) {
        if (j != source_node_index) {
          neighbor_weights += weights(source_node_index, j);
        }
      }

      return 1.0 - neighbor_weights;
    }
  }
}

std::vector<Eigen::VectorXd> WeightDesign2Analysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates) {

  // take initial measurements
  Eigen::VectorXd estimates = initial_estimates;

  // determine weight matrix
  Eigen::MatrixXd weights(
    configurations.number_of_sensor_nodes,
    configurations.number_of_sensor_nodes
  );
  weights.setZero();
  for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
    weights(i, i) = ComputeWeightDesign2Weight(i, estimates(i), i, estimates(i),
      configurations.communication_range,
      configurations.sensing_range,
      target.location,
      sensor_nodes,
      weights,
      estimates
    );

    for (int j = 0; j < configurations.number_of_sensor_nodes; ++j) {
      if (i != j) {
        weights(i, j) = ComputeWeightDesign2Weight(
          i,
          estimates(i),
          j,
          estimates(j),
          configurations.communication_range,
          configurations.sensing_range,
          target.location,
          sensor_nodes,
          weights,
          estimates
        );
      }
    }
  }

  std::cout << "Weight Design 2 Weights:" << std::endl << weights << std::endl;

  double average_estimate = 0.0;
  double total_weight = 0.0;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      average_estimate += estimates(i) * weights(i, i);
      total_weight += weights(i, i);
    }
  }
  average_estimate /= total_weight;

  std::cout << "weighted average: " << average_estimate << std::endl;

  // iterate til consensus
  std::vector<Eigen::VectorXd> estimate_history;
  estimate_history.push_back(estimates);
  int l = 0;
  while (SomeNodeNotConverged(estimates, average_estimate, ERROR_THRESHOLD, false)) {
    estimates = weights * estimates;
    l++;
    estimate_history.push_back(estimates);

// char y;
// std::cin >> y;
// std::cout << estimates.transpose() << std::endl;
  }

  // update nodes that didn't see the target
  double consensus;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      consensus = estimates(i);
      break;
    }
  }
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) == NO_READING) {
      estimates(i) = consensus;
    }
  }
  estimate_history.push_back(estimates);

  return estimate_history;
}

double ComputeWeightDesign2Weight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  double sensing_range,
  CoordinatePair target_location,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights,
  Eigen::VectorXd estimates) {

  if (source_node_index != neighbor_index) {
    if (source_node_measurement != NO_READING &&
        neighbor_measurement != NO_READING &&
        ComputeDistance(sensor_nodes[source_node_index], sensor_nodes[neighbor_index]) <= communication_range) {

        return (1.0 - weights(source_node_index, source_node_index)) /
          (double) CountNeighborsWhoSenseTarget(communication_range, sensor_nodes[source_node_index], sensor_nodes, estimates);
    }

    return 0.0;
  } else {
    if (source_node_measurement == NO_READING) {
      return 1.0;
    } else {
      return C_W_CONSTANT / ComputeNoiseCovariance(sensor_nodes[source_node_index], target_location, C_W_CONSTANT, sensing_range);
    }
  }
}

int CountNeighborsWhoSenseTarget(
  double communication_range,
  CoordinatePair source,
  std::vector<CoordinatePair> all_nodes,
  Eigen::VectorXd estimates) {

  int num_neighbors = 0;

  for (int i = 0; i < all_nodes.size(); ++i) {
    CoordinatePair node = all_nodes[i];

    if (node != source &&
        ComputeDistance(source, node) <= communication_range &&
        estimates(i) != NO_READING) {

      num_neighbors++;
    }
  }

  return num_neighbors;
}

std::vector<Target> ReadInData(Configurations configurations, std::string filename) {

  std::ifstream fin;
  fin.open(filename.c_str());

  std::vector<Target> targets;

  char dummy;

  double x;
  double y;

  double increment = 0.5;
  double startval = -6.0;

  y = startval;
  for (int i = 0; i < configurations.field_cells; ++i) {
    x = startval;
    for (int j = 0; j < configurations.field_cells; ++j) {
      Target target;
      fin >> target.value >> dummy;
      target.value *= 1;
      target.location = {x, y};
      targets.push_back(target);

std::cout << target.value << ", ";

      x += increment;
    }

std::cout << std::endl;

    y += increment;
  }

char c;
std::cin >> c;

  fin.close();

  return targets;
}

void DumpFieldDataToFile(
  Configurations configurations,
  std::string filename,
  std::vector<CoordinatePair> nodes,
  std::vector<double> estimates) {

  std::ofstream fout;
  fout.open(filename.c_str());

  for (CoordinatePair source : nodes) {
    int num_neighbors = 0;
    for (CoordinatePair neighbor : nodes) {
      if (configurations.communication_range <
          ComputeDistance(source, neighbor)) {
        num_neighbors++;
      }
    }

    fout << source.first << ", " << source.second << ", " << num_neighbors
         << ln;
  }

  for (int i = 0; i < configurations.field_cells; ++i) {
    for (int j = 0; j < configurations.field_cells; ++j) {
      fout << estimates[(i * configurations.field_cells) + j] << ", ";
    }
    fout << ln;
  }

  fout.close();
}


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

#include "math_unit_test.hpp"
#include <numeric>
#include <utility>
#include <boost/math/interpolators/cardinal_quadratic_b_spline.hpp>
using boost::math::interpolators::cardinal_quadratic_b_spline;

template<class Real>
void test_constant()
{
    Real c = 7.2;
    Real t0 = 0;
    Real h = Real(1)/Real(16);
    size_t n = 512;
    std::vector<Real> v(n, c);
    auto qbs = cardinal_quadratic_b_spline<Real>(v.data(), v.size(), t0, h);

    size_t i = 0;
    while (i < n) {
      Real t = t0 + i*h;
      CHECK_ULP_CLOSE(c, qbs(t), 2);
      CHECK_MOLLIFIED_CLOSE(0, qbs.prime(t), 100*std::numeric_limits<Real>::epsilon());
      ++i;
    }

    i = 0;
    while (i < n) {
      Real t = t0 + i*h + h/2;
      CHECK_ULP_CLOSE(c, qbs(t), 2);
      CHECK_MOLLIFIED_CLOSE(0, qbs.prime(t), 300*std::numeric_limits<Real>::epsilon());
      t = t0 + i*h + h/4;
      CHECK_ULP_CLOSE(c, qbs(t), 2);
      CHECK_MOLLIFIED_CLOSE(0, qbs.prime(t), 150*std::numeric_limits<Real>::epsilon());
      ++i;
    }
}

template<class Real>
void test_linear()
{
    Real m = 8.3;
    Real b = 7.2;
    Real t0 = 0;
    Real h = Real(1)/Real(16);
    size_t n = 512;
    std::vector<Real> y(n);
    for (size_t i = 0; i < n; ++i) {
      Real t = i*h;
      y[i] = m*t + b;
    }
    auto qbs = cardinal_quadratic_b_spline<Real>(y.data(), y.size(), t0, h);

    size_t i = 0;
    while (i < n) {
      Real t = t0 + i*h;
      CHECK_ULP_CLOSE(m*t+b, qbs(t), 2);
      CHECK_ULP_CLOSE(m, qbs.prime(t), 820);
      ++i;
    }

    i = 0;
    while (i < n) {
      Real t = t0 + i*h + h/2;
      CHECK_ULP_CLOSE(m*t+b, qbs(t), 2);
      CHECK_MOLLIFIED_CLOSE(m, qbs.prime(t), 1500*std::numeric_limits<Real>::epsilon());
      t = t0 + i*h + h/4;
      CHECK_ULP_CLOSE(m*t+b, qbs(t), 3);
      CHECK_MOLLIFIED_CLOSE(m, qbs.prime(t), 1500*std::numeric_limits<Real>::epsilon());
      ++i;
    }
}

template<class Real>
void test_quadratic()
{
    Real a = 8.2;
    Real b = 7.2;
    Real c = -9.2;
    Real t0 = 0;
    Real h = Real(1)/Real(16);
    size_t n = 513;
    std::vector<Real> y(n);
    for (size_t i = 0; i < n; ++i) {
      Real t = i*h;
      y[i] = a*t*t + b*t + c;
    }
    Real t_max = t0 + (n-1)*h;
    auto qbs = cardinal_quadratic_b_spline<Real>(y, t0, h, b, 2*a*t_max + b);

    size_t i = 0;
    while (i < n) {
      Real t = t0 + i*h;
      CHECK_ULP_CLOSE(a*t*t + b*t + c, qbs(t), 2);
      ++i;
    }

    i = 0;
    while (i < n) {
      Real t = t0 + i*h + h/2;
      CHECK_ULP_CLOSE(a*t*t + b*t + c, qbs(t), 47);

      t = t0 + i*h + h/4;
      if (!CHECK_ULP_CLOSE(a*t*t + b*t + c, qbs(t), 104)) {
          std::cerr << "  Problem abscissa t = " << t << "\n";
      }
      ++i;
    }
}

int main()
{
    test_constant<float>();
    test_constant<double>();
    test_constant<long double>();

    test_linear<float>();
    test_linear<double>();
    test_linear<long double>();

    test_quadratic<double>();
    test_quadratic<long double>();

    return boost::math::test::report_errors();
}