/*
    Copyright (C) 2008 Wei Dong <wdong@princeton.edu>. All Rights Reserved.

    This file is part of LSHKIT.

    LSHKIT is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    LSHKIT is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with LSHKIT.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <boost/program_options.hpp>
#include <boost/progress.hpp>
#include <boost/format.hpp>
#include <boost/timer.hpp>
#include <lshkit.h>

/**
  * \file apost-run.cpp
  * \brief Example of using A Posteriori MPLSH.
  *
  * This program is an example of using A Posteriori MPLSH index.
  *
  * You need to run this program twice: once for index building
  * and training, once for testing.
  * For index building (with --build), you need to specify the following parameters:
  *     -W -M -L -Q -K|-R -D -B --index --build
  *     -N --expand --k-sigma
  * The parameter -N specify the quantization granularity (Nz in equation 17).
  * For now, do not specify the expand parameter (just use the default).
  * k-sigma specifies the sigma in Gaussian kernel used in equation 14 & 15.
  * Here the benchmark is used to train the a posteriori model.  The index
  * will be written to the --index parameter.
  *
  * For testing (without --build), you need to specify the following:
  *     -Q -K|-R -D -B --index -T|--recall
  *
  * The benchmark provided for training and benchmarking should be different.
  * To generate the benchmark file, use the scan program with different --seed
  * parameters.
  *
  * Following is a example run on the audio dataset:
  * Training:
  *     apost-run -D audio.data -B audio.train  -L 10 -W 4.32 -M 20 -Q 1000 -K 50 --build --index audio.apost
  * Testig:
  *     apost-run -D audio.data -B audio.query  -T 20 -Q 100 -K 50 --index audio.apost
  *
\verbatim
  -h [ --help ]                         produce help message.
  -W [ -- ] arg (=1)
  -M [ -- ] arg (=1)
  -N [ -- ] arg (=2500)
  -T [ -- ] arg (=1)                    # probes
  -L [ -- ] arg (=1)                    # hash tables
  -Q [ -- ] arg (=100)                  # queries
  -K [ -- ] arg (=0)                    # nearest neighbor to retrieve
  -R [ --radius ] arg (=3.40282347e+38) R-NN distance range (L2)
  --recall arg                          desired recall
  -D [ --data ] arg                     data file
  -B [ --benchmark ] arg                benchmark file
  --index arg                           index file
  --build                               build index, using benchmark as
                                        training examples
  -H [ -- ] arg (=1017881)              hash table size, use the default value.
  --expand arg (=0)
  --k-sigmal arg (=0.2)
\endverbatim
  */

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

int main (int argc, char *argv[])
{
    string data_file;
    string benchmark;
    string index_file;

    float W, R, desired_recall = 1.0, expand, k_sigma;
    unsigned M, L, H, Nz;
    unsigned Q, K, T;
    bool do_recall = false;
    bool do_build = false;

    boost::timer timer;

    po::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "produce help message.")
        (",W", po::value<float>(&W)->default_value(1.0), "")
        (",M", po::value<unsigned>(&M)->default_value(1), "")
        (",N", po::value<unsigned>(&Nz)->default_value(2500), "")
        (",T", po::value<unsigned>(&T)->default_value(1), "# probes")
        (",L", po::value<unsigned>(&L)->default_value(1), "# hash tables")
        (",Q", po::value<unsigned>(&Q)->default_value(100), "# queries")
        (",K", po::value<unsigned>(&K)->default_value(0), "# nearest neighbor to retrieve")
        ("radius,R", po::value<float>(&R)->default_value(numeric_limits<float>::max()), "R-NN distance range (L2)")
        ("recall", po::value<float>(&desired_recall), "desired recall")
        ("data,D", po::value<string>(&data_file), "data file")
        ("benchmark,B", po::value<string>(&benchmark), "benchmark file")
        ("index", po::value<string>(&index_file), "index file")
        ("build", "build index, using benchmark as training examples")
        (",H", po::value<unsigned>(&H)->default_value(1017881), "hash table size, use the default value.")
        ("k-sigma", po::value<float>(&k_sigma)->default_value(1.0/5), "")
        ("expand", po::value<float>(&expand)->default_value(0), "")
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help") || (vm.count("data") < 1))
    {
        cout << desc;
        return 0;
    }

    if (vm.count("build") >= 1) {
        do_build = true;
    }

    if (vm.count("radius") >= 1) {
        R *= R; // we use L2sqr in the program.
    }

    if (vm.count("recall") >= 1)
    {
        do_recall = true;
    }

    if ((Q == 0) || (vm.count("benchmark") == 0)) {
        cerr << "No benchmark data." << endl;
        return -1;
    }

    if (vm.count("index") != 1) {
        cerr << "No index file specified." << endl;
        return -1;
    }

    cout << "LOADING DATA..." << endl;
    timer.restart();
    FloatMatrix data(data_file);
    cout << boost::format("LOAD TIME: %1%s.") % timer.elapsed() << endl;

    typedef APostLshIndex<unsigned> Index;

    FloatMatrix::Accessor accessor(data);
    Index index;

    if (do_build) {
        // We define a short name for the MPLSH index.
        Index::Parameter param;

        // Setup the parameters.  Note that L is not provided here.
        param.W = W;
        param.range = H; // See H in the program parameters.  You can just use the default value.
        param.repeat = M;
        param.dim = data.getDim();
        DefaultRng rng;

        index.init(param, rng, L);
        // The accessor.

        // Initialize the index structure.  Note L is passed here.
        cout << "CONSTRUCTING INDEX..." << endl;

        timer.restart();
        {
            boost::progress_display progress(data.getSize());
            for (int i = 0; i < data.getSize(); ++i)
            {
                // Insert an item to the hash table.
                // Note that only the key is passed in here.
                // MPLSH will get the feature from the accessor.
                index.insert(i, data[i]);
                ++progress;
            }
        }
        cout << boost::format("CONSTRUCTION TIME: %1%s.") % timer.elapsed() << endl;

        Benchmark<> bench;
        cout << "LOADING BENCHMARK..." << endl;
        bench.load(benchmark);
        bench.resize(Q, K);
        cout << "DONE." << endl;

        cout << "TRAINING INDEX..." << endl;
        timer.restart();

        vector<APostExample> examples(Q);
        for (unsigned i = 0; i < Q; ++i) {
            examples[i].query = data[bench.getQuery(i)];
            const Topk<unsigned> &topk = bench.getAnswer(i);
            examples[i].results.resize(topk.size());
            for (unsigned j = 0; j < topk.size(); j++) {
                examples[i].results[j] = data[topk[j].key];
            }
        }

        index.train(examples, Nz, k_sigma, expand);
        cout << boost::format("TRAINING TIME: %1%s.") % timer.elapsed() << endl;

        timer.restart();
        cout << "SAVING INDEX..." << endl;
        {
            ofstream os(index_file.c_str(), ios_base::binary);
            os.exceptions(ios_base::eofbit | ios_base::failbit | ios_base::badbit);
            index.save(os);
        }
        cout << boost::format("SAVING TIME: %1%s") % timer.elapsed() << endl;
    }
    else {

        // try loading index
        {
            ifstream is(index_file.c_str(), ios_base::binary);
            BOOST_VERIFY(is);
            is.exceptions(ios_base::eofbit | ios_base::failbit | ios_base::badbit);
            cout << "LOADING INDEX..." << endl;
            timer.restart();
            index.load(is);
            BOOST_VERIFY(is);
            cout << boost::format("LOAD TIME: %1%s.") % timer.elapsed() << endl;
        }

        Benchmark<> bench;
        cout << "LOADING BENCHMARK..." << endl;
        bench.load(benchmark);
        bench.resize(Q, K);
        cout << "DONE." << endl;

        for (unsigned i = 0; i < Q; ++i)
        {
            for (unsigned j = 0; j < K; ++j)
            {
                assert(bench.getAnswer(i)[j].key < data.getSize());
            }
        }

        cout << "RUNNING QUERIES..." << endl;

        Stat recall;
        Stat cost;
        metric::l2sqr<float> l2sqr(data.getDim());
        TopkScanner<FloatMatrix::Accessor, metric::l2sqr<float> > query(accessor, l2sqr, K, R);
        vector<Topk<unsigned> > topks(Q);

        timer.restart();
        if (do_recall)
            // Specify the required recall
            // and let MPLSH to guess how many bins to probe.
        {
            boost::progress_display progress(Q);
            for (unsigned i = 0; i < Q; ++i)
            {
                // Query for one point.
                query.reset(data[bench.getQuery(i)]);
                index.query_recall(data[bench.getQuery(i)], desired_recall, query);
                cost << double(query.cnt())/double(data.getSize());
                topks[i].swap(query.topk());
                ++progress;
            }
        }
        else
            // specify how many bins to probe.
        {
            boost::progress_display progress(Q);
            for (unsigned i = 0; i < Q; ++i)
            {
                query.reset(data[bench.getQuery(i)]);
                index.query(data[bench.getQuery(i)], T, query);
                cost << double(query.cnt())/double(data.getSize());
                topks[i].swap(query.topk());
                ++progress;
            }
        }

        for (unsigned i = 0; i < Q; ++i) {
            recall << bench.getAnswer(i).recall(topks[i]);
        }

        cout << boost::format("QUERY TIME: %1%s.") % timer.elapsed() << endl;

        cout << "[RECALL] " << recall.getAvg() << " +/- " << recall.getStd() << endl;
        cout << "[COST] " << cost.getAvg() << " +/- " << cost.getStd() << endl;

    }

    return 0;
}


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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 BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_GEOMETRY_COLLECTION_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_GEOMETRY_COLLECTION_HPP

#include <boost/geometry/algorithms/detail/visit.hpp>
#include <boost/geometry/algorithms/dispatch/envelope.hpp>
#include <boost/geometry/algorithms/is_empty.hpp>

#include <boost/geometry/core/tags.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Collection>
struct envelope<Collection, geometry_collection_tag>
{
    template <typename Geometry, typename Box, typename Strategy>
    static inline void apply(Geometry const& geometry,
                             Box& mbr,
                             Strategy const& strategy)
    {
        using strategy_t = decltype(strategy.envelope(geometry, mbr));
        using state_t = typename strategy_t::template multi_state<Box>;

        state_t state;
        detail::visit_breadth_first([&](auto const& g)
        {
            if (! geometry::is_empty(g))
            {
                Box b;
                envelope<util::remove_cref_t<decltype(g)>>::apply(g, b, strategy);
                state.apply(b);
            }
            return true;
        }, geometry);
        state.result(mbr);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_GEOMETRY_COLLECTION_HPP


#include <vector>

#include <boost/make_shared.hpp>
#include <sm/eigen/gtest.hpp>

#include <aslam/backend/ErrorTerm.hpp>
#include <aslam/backend/ProbDataAssocPolicy.hpp>

using aslam::backend::ErrorTermFs;
using aslam::backend::JacobianContainer;
using aslam::backend::ProbDataAssocPolicy;
using aslam::backend::FixedWeightMEstimator;

class DummyError : public ErrorTermFs<1> {
 private:
  double _error;

 public:
  explicit DummyError(double error) {
    _error = error;
    updateRawSquaredError();
  }

 protected:
  virtual double evaluateErrorImplementation() { return _error * _error; }

  virtual void evaluateJacobiansImplementation(JacobianContainer &) {}
};

TEST(ProbDataAssocPolicyTestSuite, callbackTest) {
  std::vector<ProbDataAssocPolicy::ErrorTermPtr> error_group_1;
  std::vector<ProbDataAssocPolicy::ErrorTermPtr> error_group_2;

  for (int i = 0; i < 3; i++) {
    boost::shared_ptr<FixedWeightMEstimator> m_estimator(
        new FixedWeightMEstimator(1));
    ProbDataAssocPolicy::ErrorTermPtr err(new DummyError(1));
    err->setMEstimatorPolicy(m_estimator);
    error_group_1.push_back(err);
  }
  for (int i = 1; i < 5; i++) {
    boost::shared_ptr<FixedWeightMEstimator> m_estimator(
        new FixedWeightMEstimator(1));
    ProbDataAssocPolicy::ErrorTermPtr err(new DummyError(i));
    err->setMEstimatorPolicy(m_estimator);
    error_group_2.push_back(err);
  }
  ProbDataAssocPolicy::ErrorTermGroups error_groups(
      new std::vector<ProbDataAssocPolicy::ErrorTermGroup>);
  error_groups->push_back(
      boost::make_shared<std::vector<ProbDataAssocPolicy::ErrorTermPtr>>(
          error_group_1));
  error_groups->push_back(
      boost::make_shared<std::vector<ProbDataAssocPolicy::ErrorTermPtr>>(
          error_group_2));
  ProbDataAssocPolicy policy(error_groups, 1);
  for (ProbDataAssocPolicy::ErrorTermGroup group : *error_groups) {
    for (ProbDataAssocPolicy::ErrorTermPtr error_term : *group) {
      // Initially all the weights should be 1
      EXPECT_EQ(error_term->getCurrentMEstimatorWeight(), 1);
    }
  }
  policy.callback();

  std::vector<double> group1_expected_w = {-1.09861228866811, -1.09861228866811,
                                           -1.09861228866811};
  std::vector<double> group2_expected_w = {
      -0.216722084482954, -1.716722084482954,
      -4.216722084482954, -7.716722084482954};
  std::vector<std::vector<double>> expected_weights;
  expected_weights.push_back(group1_expected_w);
  expected_weights.push_back(group2_expected_w);

  for (std::size_t i = 0; i < error_groups->size(); i++) {
    ProbDataAssocPolicy::ErrorTermGroup group = error_groups->at(i);
    for (std::size_t j = 0; j < group->size(); j++) {
      ProbDataAssocPolicy::ErrorTermPtr error_term = group->at(j);
      EXPECT_NEAR(expected_weights[i][j],
                  error_term->getCurrentMEstimatorWeight(), 1e-6);
    }
  }
}


#include "rfpe.h"
#include <boost/regex.hpp>

namespace Message {
	static void display(string msg) {
		cout << msg << endl;
	}
	static void alert(string msg) {
		cout << "* Error : " << msg << endl;
	}
}

void checkRFPE(string datafile, string prefix, int argc, char **argv) {
	boost::timer timer;

	Reader reader(datafile);

	Environment env;
	Payoff pd;
	vector<Player> players;
	RepeatedGame rg;
	string title;

	READSTATUS result = reader.read(title, env, pd, players);
	if (result != NOERROR) {
		Message::alert("Fail to read datafile");
		Message::display("Abort");
		return;
	}
	else {
		map<string, PreciseNumber> variables = reader.getVariables();

		boost::regex expr("([a-zA-Z]+)=(.+)");
		boost::smatch what;

		PreciseNumber value;
		for (int i = 3; i < argc; i++) {
			string exp = argv[i];
			if (boost::regex_match(exp, what, expr)) {
				value = reader.numberToRational(what[2]);
				variables[what[1]] = value;
			}
			else {
				Message::alert("Fail to parse variables in arguments");
				Message::display("Abort");
				return;
			}
		}

		if (players[0].getAutomaton() == players[1].getAutomaton()) {
			env.setSignalDistributionFromRawSignalDistribution(variables);
			env.setCorrelationDeviceFromRawCorrelationDevice(variables);

			pd.setPayoffFromRawPayoff(variables);

			if (!env.checkEnvironment()) {
				Message::alert("Signal distribution / Correlation Device error");
				Message::display("Abort");
				return;
			}

			rg.setVariables(variables);
			rg.configure(env, pd, players);
			rg.solve();

			int result = rg.checkNoEquilibrium();
			Writer resultRFSE(prefix + "-result.txt");
			cout << result << endl;
			resultRFSE.put(toString(result));
			if (result == 2) {
				vector<Belief> candidates = rg.getCandidates();
				for (int i = 0; i < candidates.size(); i++) {
					cout << candidates[i] << endl;
					resultRFSE.put(toString(candidates[i]));
				}
			}
		}
		else {
			Environment env2;
			Payoff pd2;
			vector<Player> players2;
			RepeatedGame rg2;

			env2.setEnvironmentFromEnvironment(env, true);
			pd2.setPayoffFromPayoff(pd, true);

			players2 = players;
			reverse(players2.begin(), players2.end());

			env.setSignalDistributionFromRawSignalDistribution(variables);
			pd.setPayoffFromRawPayoff(variables);

			if (!env.checkEnvironment()) {
				Message::alert("Signal distribution error");
				Message::display("Abort");
				return;
			}

			env2.setSignalDistributionFromRawSignalDistribution(variables);
			pd2.setPayoffFromRawPayoff(variables);

			if (!env2.checkEnvironment()) {
				Message::alert("Signal distribution error");
				Message::display("Abort");
				return;
			}

			rg.setVariables(variables);
			rg.configure(env, pd, players);
			rg.solve();

			rg2.setVariables(variables);
			rg2.configure(env2, pd2, players2);
			rg2.solve();

			Writer resultRFSE(prefix + "-result.txt");
			int result = rg.checkNoEquilibrium();
			cout << result << endl;
			resultRFSE.put(toString(result));
			if (result == 2) {
				vector<Belief> candidates = rg.getCandidates();
				for (int i = 0; i < candidates.size(); i++) {
					cout << candidates[i] << endl;
					resultRFSE.put(toString(candidates[i]));
				}
			}

			int result2 = rg2.checkNoEquilibrium();
			cout << result2 << endl;
			resultRFSE.put(toString(result2));
			if (result == 2) {
				vector<Belief> candidates = rg2.getCandidates();
				for (int i = 0; i < candidates.size(); i++) {
					cout << candidates[i] << endl;
					resultRFSE.put(toString(candidates[i]));
				}
			}
		}
		Message::display("End gracefully");
	}
}

int main(int argc, char **argv) {
	if (argc >= 3) {
		checkRFPE(string(argv[1]), string(argv[2]), argc, argv);
	}
	else {
		Message::display("Usage : rfpe [datafile] [output-prefix] {[variables], ...}");
		Message::display("variables :");
		Message::display("	[name]=[value]    e.g) p=0.5 q=1/6");
	}

	return EXIT_SUCCESS;
}


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*
 Copyright (C) 2008 Roland Stamm
 Copyright (C) 2009 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <qle/instruments/cdsoption.hpp>
#include <qle/pricingengines/blackcdsoptionengine.hpp>

#include <ql/exercise.hpp>
#include <ql/instruments/payoffs.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/volatility/equityfx/blackconstantvol.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {

namespace {

class ImpliedVolHelper {
public:
    ImpliedVolHelper(const CdsOption& cdsoption, const Handle<DefaultProbabilityTermStructure>& probability,
                     Real recoveryRate, const Handle<YieldTermStructure>& termStructure, Real targetValue)
        : targetValue_(targetValue) {

        vol_ = boost::shared_ptr<SimpleQuote>(new SimpleQuote(0.0));
        Handle<BlackVolTermStructure> h(
            boost::make_shared<BlackConstantVol>(0, NullCalendar(), Handle<Quote>(vol_), Actual365Fixed()));
        engine_ = boost::shared_ptr<PricingEngine>(
            new QuantExt::BlackCdsOptionEngine(probability, recoveryRate, termStructure, h));
        cdsoption.setupArguments(engine_->getArguments());

        results_ = dynamic_cast<const Instrument::results*>(engine_->getResults());
    }
    Real operator()(Volatility x) const {
        vol_->setValue(x);
        engine_->calculate();
        return results_->value - targetValue_;
    }

private:
    boost::shared_ptr<PricingEngine> engine_;
    Real targetValue_;
    boost::shared_ptr<SimpleQuote> vol_;
    const Instrument::results* results_;
};
} // namespace

CdsOption::CdsOption(const boost::shared_ptr<CreditDefaultSwap>& swap, const boost::shared_ptr<Exercise>& exercise,
                     bool knocksOut, const Real strike, const StrikeType strikeType)
    : Option(boost::shared_ptr<Payoff>(new NullPayoff), exercise), swap_(swap), knocksOut_(knocksOut),
      strike_(strike == Null<Real>() ? swap_->runningSpread() : strike), strikeType_(strikeType) {
    registerWith(swap_);
}

bool CdsOption::isExpired() const { return detail::simple_event(exercise_->dates().back()).hasOccurred(); }

void CdsOption::setupExpired() const {
    Instrument::setupExpired();
    riskyAnnuity_ = 0.0;
}

void CdsOption::setupArguments(PricingEngine::arguments* args) const {
    swap_->setupArguments(args);
    Option::setupArguments(args);

    CdsOption::arguments* arguments = dynamic_cast<CdsOption::arguments*>(args);

    QL_REQUIRE(arguments != 0, "wrong argument type");

    arguments->swap = swap_;
    arguments->knocksOut = knocksOut_;
    arguments->strike = strike_;
    arguments->strikeType = strikeType_;
}

void CdsOption::fetchResults(const PricingEngine::results* r) const {
    Option::fetchResults(r);
    const CdsOption::results* results = dynamic_cast<const CdsOption::results*>(r);
    QL_ENSURE(results != 0, "wrong results type");
    riskyAnnuity_ = results->riskyAnnuity;
}

Rate CdsOption::atmRate() const { return swap_->fairSpread(); }

Real CdsOption::riskyAnnuity() const {
    calculate();
    QL_REQUIRE(riskyAnnuity_ != Null<Real>(), "risky annuity not provided");
    return riskyAnnuity_;
}

Volatility CdsOption::impliedVolatility(Real targetValue, const Handle<YieldTermStructure>& termStructure,
                                        const Handle<DefaultProbabilityTermStructure>& probability, Real recoveryRate,
                                        Real accuracy, Size maxEvaluations, Volatility minVol,
                                        Volatility maxVol) const {
    calculate();
    QL_REQUIRE(!isExpired(), "instrument expired");

    Volatility guess = 0.10;

    ImpliedVolHelper f(*this, probability, recoveryRate, termStructure, targetValue);
    Brent solver;
    solver.setMaxEvaluations(maxEvaluations);
    return solver.solve(f, accuracy, guess, minVol, maxVol);
}

void CdsOption::arguments::validate() const {
    CreditDefaultSwap::arguments::validate();
    Option::arguments::validate();
    QL_REQUIRE(swap, "CDS not set");
    QL_REQUIRE(exercise, "exercise not set");
}

void CdsOption::results::reset() {
    Option::results::reset();
    riskyAnnuity = Null<Real>();
}
} // namespace QuantExt


// source: http://dlib.net/fhog_object_detector_ex.cpp.html
#include "fhog.h"
#include "widget.h"

#include <AnnotatorLib/Annotation.h>
#include <AnnotatorLib/Commands/NewAnnotation.h>
#include <AnnotatorLib/Frame.h>
#include <AnnotatorLib/Session.h>

#include <dlib/data_io.h>
#include <dlib/gui_widgets.h>
#include <dlib/image_processing.h>
#include <dlib/opencv/cv_image.h>
#include <dlib/svm_threaded.h>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <ctype.h>
#include <chrono>
#include <iostream>
#include <thread>

using namespace Annotator::Plugins;

Annotator::Plugins::FHOG::FHOG() {
  widget.setFHOG(this);
  initScanner();
}

FHOG::~FHOG() {}

QString FHOG::getName() { return "FHOG"; }

QWidget *FHOG::getWidget() { return &widget; }

bool FHOG::setFrame(shared_ptr<Frame> frame, cv::Mat image) {
  this->lastFrame = this->frame;
  this->frame = frame;
  this->frameImg = image;
  return lastFrame != frame;
}

// first call
void FHOG::setObject(shared_ptr<Object> object) {
  if (object != this->object) {
    this->object = object;
    widget.setObjectPixmap(getImgCrop(object->getFirstAnnotation(), 96));
  }
}

shared_ptr<Object> FHOG::getObject() const { return object; }

void FHOG::setLastAnnotation(shared_ptr<Annotation> /*annotation*/) {}

std::vector<shared_ptr<Commands::Command>> FHOG::getCommands() {
  std::vector<shared_ptr<Commands::Command>> commands;
  if (object == nullptr || frame == nullptr || lastFrame == nullptr ||
      lastFrame == frame)
    return commands;

  try {
    cv::Rect res = findObject();

    if (res.width > 0 && res.height > 0) {
      int x = res.x;
      int y = res.y;
      int w = res.width;
      int h = res.height;

      shared_ptr<Commands::NewAnnotation> nA =
          std::make_shared<Commands::NewAnnotation>(project->getSession(),
                                                    this->object, this->frame,
                                                    x, y, w, h, 0.9f);
      commands.push_back(nA);
    }
  } catch (std::exception &e) {
  }

  return commands;
}

void FHOG::train() {
  if (!object) return;
  widget.setProgress(10);
  getImagesTrain();
  widget.setProgress(20);
  // upsampleImages();
  widget.setProgress(30);

  dlib::structural_object_detection_trainer<image_scanner_type> trainer(
      scanner);
  trainer.set_num_threads(4);
  // The trainer is a kind of support vector machine and therefore has the usual
  // SVM
  // C parameter.  In general, a bigger C encourages it to fit the training data
  // better but might lead to overfitting.  You must find the best C value
  // empirically by checking how well the trained detector works on a test set
  // of
  // images you haven't trained on.  Don't just leave the value set at 1.  Try a
  // few
  // different C values and see what works best for your data.
  trainer.set_c(1);
  trainer.be_verbose();
  trainer.set_epsilon(0.01);

  dlib::image_window win;
  for (unsigned long i = 0; i < images_train.size(); ++i) {
    // Run the detector and get the face detections.
    std::vector<dlib::rectangle> dets = boxes_train[i];
    win.clear_overlay();
    win.set_image(images_train[i]);
    win.add_overlay(dets, dlib::rgb_pixel(255, 0, 0));
    std::this_thread::sleep_for(std::chrono::seconds(1));
  }

  widget.setProgress(40);
  detector = trainer.train(images_train, boxes_train);
  widget.setProgress(50);
  widget.setProgress(0);
}

void FHOG::getImagesTrain() {
  assert(object);
  this->images_train.clear();
  this->boxes_train.clear();
  this->object->getAnnotations();

  for (auto annotation : this->object->getAnnotations()) {
    std::shared_ptr<AnnotatorLib::Annotation> a = annotation.second.lock();
    if (a->isTemporary()) continue;
    cv::Mat image =
        project->getImageSet()->getImage(a->getFrame()->getFrameNumber());
    cv::Mat imageGray;
    cv::cvtColor(image, imageGray, CV_BGR2GRAY);
    dlib::array2d<unsigned char> dlibImageGray;
    dlib::assign_image(dlibImageGray, dlib::cv_image<unsigned char>(imageGray));
    this->images_train.push_back(dlibImageGray);
    std::vector<dlib::rectangle> rects;
    long x1 = std::max(0L, (long)a->getX());
    long y1 = std::max(0L, (long)a->getY());
    long x2 =
        std::min((long)image.cols - 1L, (long)a->getX() + (long)a->getWidth());
    long y2 =
        std::min((long)image.rows - 1L, (long)a->getY() + long(a->getHeight()));
    dlib::rectangle rect(x1, y1, x2, y2);
    rects.push_back(rect);
    this->boxes_train.push_back(rects);
  }
}

void FHOG::upsampleImages() {
  dlib::upsample_image_dataset<dlib::pyramid_down<2>>(images_train,
                                                      boxes_train);
}

void FHOG::loadSVM(std::string file) {
  try {
    dlib::deserialize(file) >> detector;
  } catch (...) {
  }
}

void FHOG::saveSVM(std::string file) {
  try {
    dlib::serialize(file) << detector;
  } catch (...) {
  }
}

cv::Rect FHOG::findObject() {
  cv::Mat imageGray;
  cv::cvtColor(this->frameImg, imageGray, CV_BGR2GRAY);
  dlib::array2d<unsigned char> dlibImageGray;
  dlib::assign_image(dlibImageGray, dlib::cv_image<unsigned char>(imageGray));

  std::vector<dlib::rectangle> dets = detector(dlibImageGray);
  if (dets.size() < 1) return cv::Rect();

  dlib::rectangle found = dets[0];
  return cv::Rect(found.left(), found.top(), found.width(), found.height());
}

QPixmap FHOG::getImgCrop(shared_ptr<AnnotatorLib::Annotation> annotation,
                         int size) const {
  if (annotation == nullptr) return QPixmap();

  cv::Mat cropped = getImg(annotation);

  cropped.convertTo(cropped, CV_8U);
  cv::cvtColor(cropped, cropped, CV_BGR2RGB);

  QImage img((const unsigned char *)(cropped.data), cropped.cols, cropped.rows,
             cropped.step, QImage::Format_RGB888);

  QPixmap pim = QPixmap::fromImage(img);
  pim = pim.scaledToHeight(size);
  return pim;
}

cv::Mat FHOG::getImg(shared_ptr<Annotation> annotation) const {
  cv::Mat tmp = project->getImageSet()->getImage(
      annotation->getFrame()->getFrameNumber());

  float x = std::max(annotation->getX(), 0.f);
  float y = std::max(annotation->getY(), 0.f);
  float w = std::min(annotation->getWidth(), tmp.cols - x);
  float h = std::min(annotation->getHeight(), tmp.rows - y);

  cv::Rect rect(x, y, w, h);
  cv::Mat cropped;
  try {
    tmp(rect).copyTo(cropped);
  } catch (cv::Exception &e) {
    std::cout << e.what();
  }
  return cropped;
}

void FHOG::initScanner() { scanner.set_detection_window_size(80, 80); }


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
#include"taskspaceonline3.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<geometry_msgs/Wrench.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"
#include<termios.h>
#include<gazebo_msgs/LinkStates.h>
#include<sensor_msgs/JointState.h>
#include"pidcontroller.h"

using namespace  std;
using namespace  Eigen;
Robot SURENA;
MatrixXd PoseRoot(6,1);
MatrixXd PoseRFoot(6,1);
MatrixXd PoseLFoot(6,1);
QList<LinkM> links;
bool FileExists(QString path) {
    QFileInfo check_file(path);
    // check if file exists and if yes: Is it really a file and no directory?
    if (check_file.exists() && check_file.isFile()) {
        return true;
    } else {
        return false;
    }
}

QList <QByteArray> GetContent(QByteArray content)
{
    QList <QByteArray> result;
    int index=0;
    while (index<content.length()) {

        index=content.indexOf(":",index);
        if (index<0)
        {
            break;
        }

        QByteArray temp=content.mid(index,content.length());
        QByteArray line=temp.split('\n')[0];
        QList <QByteArray> values=line.split(':');
        if (values.length()>1){
            result.append(values[1]);
        }

        index++;
    }
    return result;
}

MatrixXd ExtractionOfMatrix(QByteArray data)
{
    MatrixXd mat;
    QByteArray insideBrackets=data.split(']')[0];
    insideBrackets= insideBrackets.split('[')[1];
    QList <QByteArray> rows=insideBrackets.split(';');
    QList <QByteArray> columns=rows[0].split(',');

    mat.resize(rows.length(),columns.length());

    //////initial mat values
    for (int i = 0; i < rows.length(); i++) {
        QList <QByteArray> currentCols=rows[i].split(',');
        for (int j = 0; j < currentCols.length(); j++) {
            mat(i,j) = currentCols[j].toDouble();

        }
    }

    return mat;
}

void numplot(double num,double min,double max){
    //⬛

    QString str;
    int l=100;
    int n=int((num-min)/(max-min)*l);
    if (num<min){n=0;}
    if (num>max){n=100;}
    str+=QString::number(min);
    str+="|";
    if (n<=l/2){
        for (int i = 0; i < n; ++i) {
            str+=" ";
        }
        for (int i = 0; i < l/2-n; ++i) {
            str+="|";

        }
        str+="|";
        for (int i = 0; i < l/2; ++i) {
            str+=" ";
        }
    }
    else {
        for (int i = 0; i < l/2; ++i) {
            str+=" ";
        }
        for (int i = 0; i < n-l/2; ++i) {
            str+="|";

        }
        str+="|";
        for (int i = 0; i < l-n; ++i) {
            str+=" ";
        }

    }

    str+="|";
    str+=QString::number(max);
    str+="=>";str+=QString::number(num);
    qDebug()<<str;
    qDebug()<<"";

}

void matrix_view(MatrixXd M){

    for (int i = 0; i <M.rows() ; ++i) {
        QString str;
        for (int j = 0; j <M.cols() ; ++j) {
            str+=QString::number(M(i,j));
            str+="   ";
        }
        qDebug()<<str;
    }
    qDebug()<<"";
}

void matrix_view(VectorXd M){
    QString str;
    for (int i = 0; i <M.rows() ; ++i) {str+=QString::number(M(i));str+="   ";}
    qDebug()<<str;
    qDebug()<<"";
}

int getch()
{
    static struct termios oldt, newt;
    tcgetattr( STDIN_FILENO, &oldt);           // save old settings
    newt = oldt;
    newt.c_lflag &= ~(ICANON);                 // disable buffering
    tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings

    int c = getchar();  // read character (non-blocking)

    tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
    return c;
}

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

    QString address="/media/cast/MalekZadeh/dataout.txt";

    if (!FileExists(address))
    {
        qWarning()<<"Invalid Robot data Path:"<<address;

    }
    QFile file(address);
    file.open(QFile::ReadWrite);
    QByteArray content;
    content = file.readAll();file.close();

    QList<QByteArray> Position= GetContent(content);
    int N =Position.count();
    MatrixXd positionmat[N];
    for (int i = 0; i < N; ++i) {
        positionmat[i]=ExtractionOfMatrix(Position[i]);
        //  matrix_view(positionmat[i]);
    }

    //*******************This part of code is for initialization of joints of the robot for walking**********************************
    int count = 0;
    double time=0;

    ros::init(argc, argv, "myNode");
    ros::NodeHandle nh;

    ros::Rate loop_rate(200);

    ROS_INFO("press any key to start!");
    getch();
    ROS_INFO("started!");
QByteArray data;
    while (ros::ok())
    {

        PoseRFoot<<positionmat[count](0,0),positionmat[count](0,1),positionmat[count](0,2),0,0,0;
        PoseLFoot<<positionmat[count](0,3),positionmat[count](0,4),positionmat[count](0,5),0,0,0;
        PoseRoot<<positionmat[count](0,6),positionmat[count](0,7),positionmat[count](0,8),0,0,0;

        SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
        SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);

//qDebug()<<time;
//MatrixXd temp=PoseRFoot.transpose();
//matrix_view(temp);
//temp=PoseLFoot.transpose();
//matrix_view(temp);
//temp=PoseRoot.transpose();
//matrix_view(temp);

links = SURENA.GetLinks();
data.append(QString::number(links[3].JointAngle)+","+QString::number(links[4].JointAngle)+","+QString::number(links[5].JointAngle)+","+
        QString::number(links[9].JointAngle)+","+QString::number(links[10].JointAngle)+","+QString::number(links[11].JointAngle)+","+"\n");

        ++count;
        ros::spinOnce();
        loop_rate.sleep();
        if(count>=N){break;}
        time+=.005;

}

    QFile myfile("/media/cast/MalekZadeh/data_joints.txt");
    myfile.remove();
    myfile.open(QFile::ReadWrite);
    myfile.write(data);
    myfile.close();
    return 0;
}


// Boost.Geometry Index
//
// R-tree R*-tree insert algorithm implementation
//
// Copyright (c) 2011-2015 Adam Wulkiewicz, Lodz, Poland.
//
// This file was modified by Oracle on 2019.
// Modifications copyright (c) 2019 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_INDEX_DETAIL_RTREE_RSTAR_INSERT_HPP
#define BOOST_GEOMETRY_INDEX_DETAIL_RTREE_RSTAR_INSERT_HPP

#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/index/detail/algorithms/content.hpp>

namespace boost { namespace geometry { namespace index {

namespace detail { namespace rtree { namespace visitors {

namespace rstar {

// Utility to distinguish between default and non-default index strategy
template <typename Point1, typename Point2, typename Strategy>
struct comparable_distance_point_point
{
    typedef typename Strategy::comparable_distance_point_point_strategy_type strategy_type;
    typedef typename geometry::comparable_distance_result
        <
            Point1, Point2, strategy_type
        >::type result_type;

    static inline strategy_type get_strategy(Strategy const& strategy)
    {
        return strategy.get_comparable_distance_point_point_strategy();
    }
};

template <typename Point1, typename Point2>
struct comparable_distance_point_point<Point1, Point2, default_strategy>
{
    typedef default_strategy strategy_type;
    typedef typename geometry::default_comparable_distance_result
        <
            Point1, Point2
        >::type result_type;

    static inline strategy_type get_strategy(default_strategy const& )
    {
        return strategy_type();
    }
};

template <typename MembersHolder>
class remove_elements_to_reinsert
{
public:
    typedef typename MembersHolder::box_type box_type;
    typedef typename MembersHolder::parameters_type parameters_type;
    typedef typename MembersHolder::translator_type translator_type;
    typedef typename MembersHolder::allocators_type allocators_type;

    typedef typename MembersHolder::node node;
    typedef typename MembersHolder::internal_node internal_node;
    typedef typename MembersHolder::leaf leaf;

    //typedef typename Allocators::internal_node_pointer internal_node_pointer;
    typedef internal_node * internal_node_pointer;

    template <typename ResultElements, typename Node>
    static inline void apply(ResultElements & result_elements,
                             Node & n,
                             internal_node_pointer parent,
                             size_t current_child_index,
                             parameters_type const& parameters,
                             translator_type const& translator,
                             allocators_type & allocators)
    {
        typedef typename rtree::elements_type<Node>::type elements_type;
        typedef typename elements_type::value_type element_type;
        typedef typename geometry::point_type<box_type>::type point_type;
        typedef typename index::detail::strategy_type<parameters_type>::type strategy_type;
        // TODO: awulkiew - change second point_type to the point type of the Indexable?
        typedef rstar::comparable_distance_point_point
            <
                point_type, point_type, strategy_type
            > comparable_distance_pp;
        typedef typename comparable_distance_pp::result_type comparable_distance_type;

        elements_type & elements = rtree::elements(n);

        const size_t elements_count = parameters.get_max_elements() + 1;
        const size_t reinserted_elements_count = (::std::min)(parameters.get_reinserted_elements(), elements_count - parameters.get_min_elements());

        BOOST_GEOMETRY_INDEX_ASSERT(parent, "node shouldn't be the root node");
        BOOST_GEOMETRY_INDEX_ASSERT(elements.size() == elements_count, "unexpected elements number");
        BOOST_GEOMETRY_INDEX_ASSERT(0 < reinserted_elements_count, "wrong value of elements to reinsert");

        // calculate current node's center
        point_type node_center;
        geometry::centroid(rtree::elements(*parent)[current_child_index].first, node_center);

        // fill the container of centers' distances of children from current node's center
        typedef typename index::detail::rtree::container_from_elements_type<
            elements_type,
            std::pair<comparable_distance_type, element_type>
        >::type sorted_elements_type;

        sorted_elements_type sorted_elements;
        // If constructor is used instead of resize() MS implementation leaks here
        sorted_elements.reserve(elements_count);                                                         // MAY THROW, STRONG (V, E: alloc, copy)

        typename comparable_distance_pp::strategy_type
            cdist_strategy = comparable_distance_pp::get_strategy(index::detail::get_strategy(parameters));

        for ( typename elements_type::const_iterator it = elements.begin() ;
              it != elements.end() ; ++it )
        {
            point_type element_center;
            geometry::centroid( rtree::element_indexable(*it, translator), element_center);
            sorted_elements.push_back(std::make_pair(
                geometry::comparable_distance(node_center, element_center, cdist_strategy),
                *it));                                                                                // MAY THROW (V, E: copy)
        }

        // sort elements by distances from center
        std::partial_sort(
            sorted_elements.begin(),
            sorted_elements.begin() + reinserted_elements_count,
            sorted_elements.end(),
            distances_dsc<comparable_distance_type, element_type>);                                                // MAY THROW, BASIC (V, E: copy)

        // copy elements which will be reinserted
        result_elements.clear();
        result_elements.reserve(reinserted_elements_count);                                             // MAY THROW, STRONG (V, E: alloc, copy)
        for ( typename sorted_elements_type::const_iterator it = sorted_elements.begin() ;
              it != sorted_elements.begin() + reinserted_elements_count ; ++it )
        {
            result_elements.push_back(it->second);                                                      // MAY THROW (V, E: copy)
        }

        BOOST_TRY
        {
            // copy remaining elements to the current node
            elements.clear();
            elements.reserve(elements_count - reinserted_elements_count);                                // SHOULDN'T THROW (new_size <= old size)
            for ( typename sorted_elements_type::const_iterator it = sorted_elements.begin() + reinserted_elements_count;
                  it != sorted_elements.end() ; ++it )
            {
                elements.push_back(it->second);                                                         // MAY THROW (V, E: copy)
            }
        }
        BOOST_CATCH(...)
        {
            elements.clear();

            for ( typename sorted_elements_type::iterator it = sorted_elements.begin() ;
                  it != sorted_elements.end() ; ++it )
            {
                destroy_element<MembersHolder>::apply(it->second, allocators);
            }

            BOOST_RETHROW                                                                                // RETHROW
        }
        BOOST_CATCH_END

        ::boost::ignore_unused(parameters);
    }

private:
    template <typename Distance, typename El>
    static inline bool distances_asc(
        std::pair<Distance, El> const& d1,
        std::pair<Distance, El> const& d2)
    {
        return d1.first < d2.first;
    }

    template <typename Distance, typename El>
    static inline bool distances_dsc(
        std::pair<Distance, El> const& d1,
        std::pair<Distance, El> const& d2)
    {
        return d1.first > d2.first;
    }
};

template
<
    size_t InsertIndex,
    typename Element,
    typename MembersHolder,
    bool IsValue = boost::is_same<Element, typename MembersHolder::value_type>::value
>
struct level_insert_elements_type
{
    typedef typename rtree::elements_type<
        typename rtree::internal_node<
            typename MembersHolder::value_type,
            typename MembersHolder::parameters_type,
            typename MembersHolder::box_type,
            typename MembersHolder::allocators_type,
            typename MembersHolder::node_tag
        >::type
    >::type type;
};

template <typename Value, typename MembersHolder>
struct level_insert_elements_type<0, Value, MembersHolder, true>
{
    typedef typename rtree::elements_type<
        typename rtree::leaf<
            typename MembersHolder::value_type,
            typename MembersHolder::parameters_type,
            typename MembersHolder::box_type,
            typename MembersHolder::allocators_type,
            typename MembersHolder::node_tag
        >::type
    >::type type;
};

template <size_t InsertIndex, typename Element, typename MembersHolder>
struct level_insert_base
    : public detail::insert<Element, MembersHolder>
{
    typedef detail::insert<Element, MembersHolder> base;
    typedef typename base::node node;
    typedef typename base::internal_node internal_node;
    typedef typename base::leaf leaf;

    typedef typename level_insert_elements_type<InsertIndex, Element, MembersHolder>::type elements_type;
    typedef typename index::detail::rtree::container_from_elements_type<
        elements_type,
        typename elements_type::value_type
    >::type result_elements_type;

    typedef typename MembersHolder::box_type box_type;
    typedef typename MembersHolder::parameters_type parameters_type;
    typedef typename MembersHolder::translator_type translator_type;
    typedef typename MembersHolder::allocators_type allocators_type;

    typedef typename allocators_type::node_pointer node_pointer;
    typedef typename allocators_type::size_type size_type;

    inline level_insert_base(node_pointer & root,
                             size_type & leafs_level,
                             Element const& element,
                             parameters_type const& parameters,
                             translator_type const& translator,
                             allocators_type & allocators,
                             size_type relative_level)
        : base(root, leafs_level, element, parameters, translator, allocators, relative_level)
        , result_relative_level(0)
    {}

    template <typename Node>
    inline void handle_possible_reinsert_or_split_of_root(Node &n)
    {
        BOOST_GEOMETRY_INDEX_ASSERT(result_elements.empty(), "reinsert should be handled only once for level");

        result_relative_level = base::m_leafs_level - base::m_traverse_data.current_level;

        // overflow
        if ( base::m_parameters.get_max_elements() < rtree::elements(n).size() )
        {
            // node isn't root node
            if ( !base::m_traverse_data.current_is_root() )
            {
                // NOTE: exception-safety
                // After an exception result_elements may contain garbage, don't use it
                rstar::remove_elements_to_reinsert<MembersHolder>::apply(
                    result_elements, n,
                    base::m_traverse_data.parent, base::m_traverse_data.current_child_index,
                    base::m_parameters, base::m_translator, base::m_allocators);                            // MAY THROW, BASIC (V, E: alloc, copy)
            }
            // node is root node
            else
            {
                BOOST_GEOMETRY_INDEX_ASSERT(&n == &rtree::get<Node>(*base::m_root_node), "node should be the root node");
                base::split(n);                                                                             // MAY THROW (V, E: alloc, copy, N: alloc)
            }
        }
    }

    template <typename Node>
    inline void handle_possible_split(Node &n) const
    {
        // overflow
        if ( base::m_parameters.get_max_elements() < rtree::elements(n).size() )
        {
            base::split(n);                                                                                // MAY THROW (V, E: alloc, copy, N: alloc)
        }
    }

    template <typename Node>
    inline void recalculate_aabb_if_necessary(Node const& n) const
    {
        if ( !result_elements.empty() && !base::m_traverse_data.current_is_root() )
        {
            // calulate node's new box
            recalculate_aabb(n);
        }
    }

    template <typename Node>
    inline void recalculate_aabb(Node const& n) const
    {
        base::m_traverse_data.current_element().first =
            elements_box<box_type>(rtree::elements(n).begin(), rtree::elements(n).end(),
                                   base::m_translator,
                                   index::detail::get_strategy(base::m_parameters));
    }

    inline void recalculate_aabb(leaf const& n) const
    {
        base::m_traverse_data.current_element().first =
            values_box<box_type>(rtree::elements(n).begin(), rtree::elements(n).end(),
                                 base::m_translator,
                                 index::detail::get_strategy(base::m_parameters));
    }

    size_type result_relative_level;
    result_elements_type result_elements;
};

template
<
    size_t InsertIndex,
    typename Element,
    typename MembersHolder,
    bool IsValue = boost::is_same<Element, typename MembersHolder::value_type>::value
>
struct level_insert
    : public level_insert_base<InsertIndex, Element, MembersHolder>
{
    typedef level_insert_base<InsertIndex, Element, MembersHolder> base;
    typedef typename base::node node;
    typedef typename base::internal_node internal_node;
    typedef typename base::leaf leaf;

    typedef typename base::parameters_type parameters_type;
    typedef typename base::translator_type translator_type;
    typedef typename base::allocators_type allocators_type;

    typedef typename base::node_pointer node_pointer;
    typedef typename base::size_type size_type;

    inline level_insert(node_pointer & root,
                        size_type & leafs_level,
                        Element const& element,
                        parameters_type const& parameters,
                        translator_type const& translator,
                        allocators_type & allocators,
                        size_type relative_level)
        : base(root, leafs_level, element, parameters, translator, allocators, relative_level)
    {}

    inline void operator()(internal_node & n)
    {
        BOOST_GEOMETRY_INDEX_ASSERT(base::m_traverse_data.current_level < base::m_leafs_level, "unexpected level");

        if ( base::m_traverse_data.current_level < base::m_level )
        {
            // next traversing step
            base::traverse(*this, n);                                                                       // MAY THROW (E: alloc, copy, N: alloc)

            // further insert
            if ( 0 < InsertIndex )
            {
                BOOST_GEOMETRY_INDEX_ASSERT(0 < base::m_level, "illegal level value, level shouldn't be the root level for 0 < InsertIndex");

                if ( base::m_traverse_data.current_level == base::m_level - 1 )
                {
                    base::handle_possible_reinsert_or_split_of_root(n);                                     // MAY THROW (E: alloc, copy, N: alloc)
                }
            }
        }
        else
        {
            BOOST_GEOMETRY_INDEX_ASSERT(base::m_level == base::m_traverse_data.current_level, "unexpected level");

            BOOST_TRY
            {
                // push new child node
                rtree::elements(n).push_back(base::m_element);                                              // MAY THROW, STRONG (E: alloc, copy)
            }
            BOOST_CATCH(...)
            {
                // NOTE: exception-safety
                // if the insert fails above, the element won't be stored in the tree, so delete it

                rtree::visitors::destroy<MembersHolder>::apply(base::m_element.second, base::m_allocators);

                BOOST_RETHROW                                                                                // RETHROW
            }
            BOOST_CATCH_END

            // first insert
            if ( 0 == InsertIndex )
            {
                base::handle_possible_reinsert_or_split_of_root(n);                                         // MAY THROW (E: alloc, copy, N: alloc)
            }
            // not the first insert
            else
            {
                base::handle_possible_split(n);                                                             // MAY THROW (E: alloc, N: alloc)
            }
        }

        base::recalculate_aabb_if_necessary(n);
    }

    inline void operator()(leaf &)
    {
        BOOST_GEOMETRY_INDEX_ASSERT(false, "this visitor can't be used for a leaf");
    }
};

template <size_t InsertIndex, typename Value, typename MembersHolder>
struct level_insert<InsertIndex, Value, MembersHolder, true>
    : public level_insert_base<InsertIndex, typename MembersHolder::value_type, MembersHolder>
{
    typedef level_insert_base<InsertIndex, typename MembersHolder::value_type, MembersHolder> base;
    typedef typename base::node node;
    typedef typename base::internal_node internal_node;
    typedef typename base::leaf leaf;

    typedef typename MembersHolder::value_type value_type;
    typedef typename base::parameters_type parameters_type;
    typedef typename base::translator_type translator_type;
    typedef typename base::allocators_type allocators_type;

    typedef typename base::node_pointer node_pointer;
    typedef typename base::size_type size_type;

    inline level_insert(node_pointer & root,
                        size_type & leafs_level,
                        value_type const& v,
                        parameters_type const& parameters,
                        translator_type const& translator,
                        allocators_type & allocators,
                        size_type relative_level)
        : base(root, leafs_level, v, parameters, translator, allocators, relative_level)
    {}

    inline void operator()(internal_node & n)
    {
        BOOST_GEOMETRY_INDEX_ASSERT(base::m_traverse_data.current_level < base::m_leafs_level, "unexpected level");
        BOOST_GEOMETRY_INDEX_ASSERT(base::m_traverse_data.current_level < base::m_level, "unexpected level");

        // next traversing step
        base::traverse(*this, n);                                                                       // MAY THROW (V, E: alloc, copy, N: alloc)

        BOOST_GEOMETRY_INDEX_ASSERT(0 < base::m_level, "illegal level value, level shouldn't be the root level for 0 < InsertIndex");

        if ( base::m_traverse_data.current_level == base::m_level - 1 )
        {
            base::handle_possible_reinsert_or_split_of_root(n);                                         // MAY THROW (E: alloc, copy, N: alloc)
        }

        base::recalculate_aabb_if_necessary(n);
    }

    inline void operator()(leaf & n)
    {
        BOOST_GEOMETRY_INDEX_ASSERT(base::m_traverse_data.current_level == base::m_leafs_level,
                                    "unexpected level");
        BOOST_GEOMETRY_INDEX_ASSERT(base::m_level == base::m_traverse_data.current_level ||
                                    base::m_level == (std::numeric_limits<size_t>::max)(),
                                    "unexpected level");

        rtree::elements(n).push_back(base::m_element);                                                  // MAY THROW, STRONG (V: alloc, copy)

        base::handle_possible_split(n);                                                                 // MAY THROW (V: alloc, copy, N: alloc)
    }
};

template <typename Value, typename MembersHolder>
struct level_insert<0, Value, MembersHolder, true>
    : public level_insert_base<0, typename MembersHolder::value_type, MembersHolder>
{
    typedef level_insert_base<0, typename MembersHolder::value_type, MembersHolder> base;
    typedef typename base::node node;
    typedef typename base::internal_node internal_node;
    typedef typename base::leaf leaf;

    typedef typename MembersHolder::value_type value_type;
    typedef typename base::parameters_type parameters_type;
    typedef typename base::translator_type translator_type;
    typedef typename base::allocators_type allocators_type;

    typedef typename base::node_pointer node_pointer;
    typedef typename base::size_type size_type;

    inline level_insert(node_pointer & root,
                        size_type & leafs_level,
                        value_type const& v,
                        parameters_type const& parameters,
                        translator_type const& translator,
                        allocators_type & allocators,
                        size_type relative_level)
        : base(root, leafs_level, v, parameters, translator, allocators, relative_level)
    {}

    inline void operator()(internal_node & n)
    {
        BOOST_GEOMETRY_INDEX_ASSERT(base::m_traverse_data.current_level < base::m_leafs_level,
                                    "unexpected level");
        BOOST_GEOMETRY_INDEX_ASSERT(base::m_traverse_data.current_level < base::m_level,
                                    "unexpected level");

        // next traversing step
        base::traverse(*this, n);                                                                       // MAY THROW (V: alloc, copy, N: alloc)

        base::recalculate_aabb_if_necessary(n);
    }

    inline void operator()(leaf & n)
    {
        BOOST_GEOMETRY_INDEX_ASSERT(base::m_traverse_data.current_level == base::m_leafs_level,
                                    "unexpected level");
        BOOST_GEOMETRY_INDEX_ASSERT(base::m_level == base::m_traverse_data.current_level ||
                                    base::m_level == (std::numeric_limits<size_t>::max)(),
                                    "unexpected level");

        rtree::elements(n).push_back(base::m_element);                                                  // MAY THROW, STRONG (V: alloc, copy)

        base::handle_possible_reinsert_or_split_of_root(n);                                             // MAY THROW (V: alloc, copy, N: alloc)

        base::recalculate_aabb_if_necessary(n);
    }
};

} // namespace rstar

// R*-tree insert visitor
// After passing the Element to insert visitor the Element is managed by the tree
// I.e. one should not delete the node passed to the insert visitor after exception is thrown
// because this visitor may delete it
template <typename Element, typename MembersHolder>
class insert<Element, MembersHolder, insert_reinsert_tag>
    : public MembersHolder::visitor
{
    typedef typename MembersHolder::parameters_type parameters_type;
    typedef typename MembersHolder::translator_type translator_type;
    typedef typename MembersHolder::allocators_type allocators_type;

    typedef typename MembersHolder::node node;
    typedef typename MembersHolder::internal_node internal_node;
    typedef typename MembersHolder::leaf leaf;

    typedef typename allocators_type::node_pointer node_pointer;
    typedef typename allocators_type::size_type size_type;

public:
    inline insert(node_pointer & root,
                  size_type & leafs_level,
                  Element const& element,
                  parameters_type const& parameters,
                  translator_type const& translator,
                  allocators_type & allocators,
                  size_type relative_level = 0)
        : m_root(root), m_leafs_level(leafs_level), m_element(element)
        , m_parameters(parameters), m_translator(translator)
        , m_relative_level(relative_level), m_allocators(allocators)
    {}

    inline void operator()(internal_node & n)
    {
        boost::ignore_unused(n);
        BOOST_GEOMETRY_INDEX_ASSERT(&n == &rtree::get<internal_node>(*m_root), "current node should be the root");

        // Distinguish between situation when reinserts are required and use adequate visitor, otherwise use default one
        if ( m_parameters.get_reinserted_elements() > 0 )
        {
            rstar::level_insert<0, Element, MembersHolder> lins_v(
                m_root, m_leafs_level, m_element, m_parameters, m_translator, m_allocators, m_relative_level);

            rtree::apply_visitor(lins_v, *m_root);                                                              // MAY THROW (V, E: alloc, copy, N: alloc)

            if ( !lins_v.result_elements.empty() )
            {
                recursive_reinsert(lins_v.result_elements, lins_v.result_relative_level);                       // MAY THROW (V, E: alloc, copy, N: alloc)
            }
        }
        else
        {
            visitors::insert<Element, MembersHolder, insert_default_tag> ins_v(
                m_root, m_leafs_level, m_element, m_parameters, m_translator, m_allocators, m_relative_level);

            rtree::apply_visitor(ins_v, *m_root);
        }
    }

    inline void operator()(leaf & n)
    {
        boost::ignore_unused(n);
        BOOST_GEOMETRY_INDEX_ASSERT(&n == &rtree::get<leaf>(*m_root), "current node should be the root");

        // Distinguish between situation when reinserts are required and use adequate visitor, otherwise use default one
        if ( m_parameters.get_reinserted_elements() > 0 )
        {
            rstar::level_insert<0, Element, MembersHolder> lins_v(
                m_root, m_leafs_level, m_element, m_parameters, m_translator, m_allocators, m_relative_level);

            rtree::apply_visitor(lins_v, *m_root);                                                              // MAY THROW (V, E: alloc, copy, N: alloc)

            // we're in the root, so root should be split and there should be no elements to reinsert
            BOOST_GEOMETRY_INDEX_ASSERT(lins_v.result_elements.empty(), "unexpected state");
        }
        else
        {
            visitors::insert<Element, MembersHolder, insert_default_tag> ins_v(
                m_root, m_leafs_level, m_element, m_parameters, m_translator, m_allocators, m_relative_level);

            rtree::apply_visitor(ins_v, *m_root);
        }
    }

private:
    template <typename Elements>
    inline void recursive_reinsert(Elements & elements, size_t relative_level)
    {
        typedef typename Elements::value_type element_type;

        // reinsert children starting from the minimum distance
        typename Elements::reverse_iterator it = elements.rbegin();
        for ( ; it != elements.rend() ; ++it)
        {
            rstar::level_insert<1, element_type, MembersHolder> lins_v(
                m_root, m_leafs_level, *it, m_parameters, m_translator, m_allocators, relative_level);

            BOOST_TRY
            {
                rtree::apply_visitor(lins_v, *m_root);                                                          // MAY THROW (V, E: alloc, copy, N: alloc)
            }
            BOOST_CATCH(...)
            {
                ++it;
                for ( ; it != elements.rend() ; ++it)
                    rtree::destroy_element<MembersHolder>::apply(*it, m_allocators);
                BOOST_RETHROW                                                                                // RETHROW
            }
            BOOST_CATCH_END

            BOOST_GEOMETRY_INDEX_ASSERT(relative_level + 1 == lins_v.result_relative_level, "unexpected level");

            // non-root relative level
            if ( lins_v.result_relative_level < m_leafs_level && !lins_v.result_elements.empty())
            {
                recursive_reinsert(lins_v.result_elements, lins_v.result_relative_level);                   // MAY THROW (V, E: alloc, copy, N: alloc)
            }
        }
    }

    node_pointer & m_root;
    size_type & m_leafs_level;
    Element const& m_element;

    parameters_type const& m_parameters;
    translator_type const& m_translator;

    size_type m_relative_level;

    allocators_type & m_allocators;
};

}}} // namespace detail::rtree::visitors

}}} // namespace boost::geometry::index

#endif // BOOST_GEOMETRY_INDEX_DETAIL_RTREE_RSTAR_INSERT_HPP


#ifndef DATE_TIME_FILETIME_FUNCTIONS_HPP__
#define DATE_TIME_FILETIME_FUNCTIONS_HPP__

/* Copyright (c) 2004 CrystalClear Software, Inc.
 * Use, modification and distribution is subject to the
 * Boost Software License, Version 1.0. (See accompanying
 * file LICENSE-1.0 or http://www.boost.org/LICENSE-1.0)
 * Author: Jeff Garland, Bart Garst
 * $Date: 2004/08/04 22:06:05 $
 */

/*! @file filetime_functions.hpp
 * Function(s) for converting between a FILETIME structure and a
 * time object. This file is only available on systems that have
 * BOOST_HAS_FTIME defined.
 */

#include <boost/date_time/compiler_config.hpp>
#if defined(BOOST_HAS_FTIME) // skip this file if no FILETIME
#include <windows.h>
#include <boost/cstdint.hpp>
#include <boost/date_time/time.hpp>

namespace boost {
namespace date_time {

  //! Create a time object from an initialized FILETIME struct.
  /*! Create a time object from an initialized FILETIME struct.
   * A FILETIME struct holds 100-nanosecond units (0.0000001). When
   * built with microsecond resolution the FILETIME's sub second value
   * will be truncated. Nanosecond resolution has no truncation. */
  template<class time_type>
  inline
  time_type time_from_ftime(const FILETIME& ft){
    typedef typename time_type::date_type date_type;
    typedef typename time_type::date_duration_type date_duration_type;
    typedef typename time_type::time_duration_type time_duration_type;

    /* OFFSET is difference between 1970-Jan-01 & 1601-Jan-01
     * in 100-nanosecond intervals */
    uint64_t c1 = 27111902UL;
    uint64_t c2 = 3577643008UL; // issues warning without 'UL'
    const uint64_t OFFSET = (c1 << 32) + c2;
    const long sec_pr_day = 86400; // seconds per day

    uint64_t filetime = ft.dwHighDateTime;
    filetime <<= 32;
    filetime += ft.dwLowDateTime;
    filetime -= OFFSET; // filetime is now 100-nanos since 1970-Jan-01

    uint64_t sec = filetime / 10000000;
#if defined(BOOST_DATE_TIME_POSIX_TIME_STD_CONFIG)
    uint64_t sub_sec = (filetime % 10000000) * 100; // nanoseconds
#else
    uint64_t sub_sec = (filetime % 10000000) / 10; // truncate to microseconds
#endif

    // split sec into usable chunks: days, hours, minutes, & seconds
    long _d = sec / sec_pr_day;
    long tmp = sec % sec_pr_day;
    long _h = tmp / 3600; // sec_pr_hour
    tmp %= 3600;
    long _m = tmp / 60; // sec_pr_min
    tmp %= 60;
    long _s = tmp; // seconds

    date_duration_type dd(_d);
    date_type d = date_type(1970, Jan, 01) + dd;
    return time_type(d, time_duration_type(_h, _m, _s, sub_sec));
  }

}} // boost::date_time

#endif // BOOST_HAS_FTIME

#endif // DATE_TIME_FILETIME_FUNCTIONS_HPP__


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#define BOOST_FILESYSTEM_NO_DEPRECATED

#include "boost/filesystem.hpp"
#include "boost/program_options.hpp"

#include "Molassembler/DistanceGeometry/DistanceBoundsMatrix.h"
#include "Molassembler/DistanceGeometry/ExplicitBoundsGraph.h"
#include "Molassembler/DistanceGeometry/MetricMatrix.h"
#include "Molassembler/DistanceGeometry/EigenRefinement.h"
#include "Molassembler/DistanceGeometry/ConformerGeneration.h"

#include "Molassembler/Temple/Optimization/Lbfgs.h"

#include "Molassembler/IO.h"
#include "Molassembler/Temple/Functional.h"
#include "Molassembler/Temple/constexpr/Numeric.h"

#include <Eigen/Dense>

#include <chrono>
#include <iomanip>

using namespace Scine;
using namespace Molassembler;

constexpr std::size_t nExperiments = 100;
constexpr std::size_t refinementStepLimit = 10000;

std::ostream& nl(std::ostream& os) {
  os << '\n';
  return os;
}

struct TimingFunctor {
  virtual boost::optional<unsigned> value(
    const Eigen::MatrixXd& squaredBounds,
    const std::vector<DistanceGeometry::ChiralConstraint>& chiralConstraints,
    const std::vector<DistanceGeometry::DihedralConstraint>& dihedralConstraints,
    const Eigen::MatrixXd& positions
  ) = 0;

  virtual std::string name() = 0;

  virtual ~TimingFunctor() = default;
};

struct FunctorResults {
  unsigned count = 0;
  double timingAverage = 0;
  double timingStddev = 0;
  double iterationsAverage = 0;
  double iterationsStddev = 0;
};

template<size_t N>
std::vector<FunctorResults> timeFunctors(
  const Molecule& molecule,
  const std::vector<
    std::unique_ptr<TimingFunctor>
  >& functors
) {
  using namespace std::chrono;

  time_point<steady_clock> start;
  time_point<steady_clock> end;

  struct Counter {
    std::vector<double> timings;
    std::vector<double> iterations;
  };

  std::vector<Counter> counters(functors.size());

  for(unsigned n = 0; n < N; ++n) {
    // Prepare new data for the functors in every experiment
    DistanceGeometry::SpatialModel spatialModel {molecule, DistanceGeometry::Configuration {}};

    const auto boundsList = spatialModel.makePairwiseBounds();

    const auto chiralConstraints = spatialModel.getChiralConstraints();
    const auto dihedralConstraints = spatialModel.getDihedralConstraints();

    DistanceGeometry::ExplicitBoundsGraph explicitGraph {
      molecule.graph().inner(),
      boundsList
    };

    auto distancesMatrixResult = explicitGraph.makeDistanceMatrix(randomnessEngine());
    if(!distancesMatrixResult) {
      throw std::runtime_error(distancesMatrixResult.error().message());
    }

    auto distanceBoundsResult = explicitGraph.makeDistanceBounds();
    if(!distanceBoundsResult) {
      throw std::runtime_error("Failure in distance bounds matrix construction: " + distanceBoundsResult.error().message());
    }

    DistanceGeometry::DistanceBoundsMatrix distanceBounds {std::move(distanceBoundsResult.value())};

    Eigen::MatrixXd squaredBounds = distanceBounds.access().cwiseProduct(
      distanceBounds.access()
    );

    auto metricMatrix = DistanceGeometry::MetricMatrix(
      std::move(distancesMatrixResult.value())
    );

    auto embeddedPositions = metricMatrix.embed();

    // Run all functors on the same
    for(unsigned functorIndex = 0; functorIndex < functors.size(); ++functorIndex) {
      start = steady_clock::now();
      auto resultOption = functors.at(functorIndex)->value(
        squaredBounds,
        chiralConstraints,
        dihedralConstraints,
        embeddedPositions
      );
      end = steady_clock::now();

      if(resultOption) {
        counters.at(functorIndex).timings.push_back(duration_cast<microseconds>(end - start).count());
        counters.at(functorIndex).iterations.push_back(resultOption.value());
      }
    }
  }

  return Temple::map(
    counters,
    [](const Counter& counter) -> FunctorResults {
      FunctorResults result;

      result.count = counter.timings.size();
      if(!counter.timings.empty()) {
        result.timingAverage = Temple::average(counter.timings);
        result.timingStddev = Temple::stddev(counter.timings, result.timingAverage);
        result.iterationsAverage = Temple::average(counter.iterations);
        result.iterationsStddev = Temple::stddev(counter.iterations, result.iterationsAverage);
      }

      return result;
    }
  );
}

template<typename EigenRefinementType>
struct InversionOrIterLimitStop {
  const EigenRefinementType& refinementFunctorReference;

  InversionOrIterLimitStop(const EigenRefinementType& functor) : refinementFunctorReference(functor) {}

  template<typename StepValues>
  bool shouldContinue(unsigned iteration, const StepValues& /* step */) {
    return (
      iteration < refinementStepLimit
      && refinementFunctorReference.proportionChiralConstraintsCorrectSign < 1.0
    );
  }
};

template<typename FloatType>
struct GradientOrIterLimitStop {
  using VectorType = Eigen::Matrix<FloatType, Eigen::Dynamic, 1>;

  template<typename StepValues>
  bool shouldContinue(unsigned iteration, const StepValues& step) {
    return (
      iteration < refinementStepLimit
      && step.gradients.current.template cast<double>().norm() > gradNorm
    );
  }

  double gradNorm = 1e-5;
};

struct OptimizerParameters {
  double c1 = 1e-4;
  double c2 = 0.9;
  double stepLength = 1.0;
};

template<unsigned dimensionality, typename FloatType, bool SIMD>
boost::optional<unsigned> eigenRefine(
  const Eigen::MatrixXd& squaredBounds,
  const std::vector<DistanceGeometry::ChiralConstraint>& chiralConstraints,
  const std::vector<DistanceGeometry::DihedralConstraint>& dihedralConstraints,
  const Eigen::MatrixXd& positions,
  OptimizerParameters optimizerParameters = {}
) {
  unsigned iterationCount = 0;

  using FullRefinementType = DistanceGeometry::EigenRefinementProblem<dimensionality, FloatType, SIMD>;

  const unsigned N = positions.size() / dimensionality;
  /* Transfer positions into vector form */

  Eigen::MatrixXd copiedPositions = positions;
  typename FullRefinementType::VectorType transformedPositions = Eigen::Map<Eigen::VectorXd>(
    copiedPositions.data(),
    copiedPositions.cols() * copiedPositions.rows()
  ).template cast<FloatType>().eval();

  FullRefinementType refinementFunctor {
    squaredBounds,
    chiralConstraints,
    dihedralConstraints
  };

  double initiallyCorrectChiralConstraints = refinementFunctor.calculateProportionChiralConstraintsCorrectSign(transformedPositions);

  if(initiallyCorrectChiralConstraints < 0.5) {
    for(unsigned i = 0; i < N; ++i) {
      transformedPositions(dimensionality * i + 1) *= -1;
    }
    initiallyCorrectChiralConstraints = 1 - initiallyCorrectChiralConstraints;
  }

  Temple::Lbfgs<FloatType, 32> optimizer;
  optimizer.c1 = optimizerParameters.c1;
  optimizer.c2 = optimizerParameters.c2;
  optimizer.stepLength = optimizerParameters.stepLength;

  /* First stage: Invert all chiral constraints */
  if(initiallyCorrectChiralConstraints < 1) {
    InversionOrIterLimitStop<FullRefinementType> inversionChecker {refinementFunctor};

    unsigned iterations;
    try {
      auto result = optimizer.minimize(
        transformedPositions,
        refinementFunctor,
        inversionChecker
      );
      iterations = result.iterations;
    } catch (...) {
      return boost::none;
    }

    if(iterations >= refinementStepLimit) {
      return boost::none;
    }

    if(refinementFunctor.proportionChiralConstraintsCorrectSign < 1.0) {
      return boost::none;
    }

    iterationCount += iterations;
  }

  /* Second stage: Refine */
  refinementFunctor.compressFourthDimension = true;

  GradientOrIterLimitStop<FloatType> gradientChecker;

  unsigned iterations;
  try {
    auto result = optimizer.minimize(
      transformedPositions,
      refinementFunctor,
      gradientChecker
    );
    iterations = result.iterations;
  } catch (...) {
    return boost::none;
  }

  if(iterations >= refinementStepLimit) {
    return boost::none;
  }

  if(refinementFunctor.proportionChiralConstraintsCorrectSign < 1) {
    return boost::none;
  }

  iterationCount += iterations;

  return iterationCount;
}

template<unsigned dimensionality, typename FloatType, bool SIMD>
struct EigenFunctor final : public TimingFunctor {
  boost::optional<unsigned> value(
    const Eigen::MatrixXd& squaredBounds,
    const std::vector<DistanceGeometry::ChiralConstraint>& chiralConstraints,
    const std::vector<DistanceGeometry::DihedralConstraint>& dihedralConstraints,
    const Eigen::MatrixXd& positions
  ) final {
    return eigenRefine<dimensionality, FloatType, SIMD>(
      squaredBounds,
      chiralConstraints,
      dihedralConstraints,
      positions
    );
  }

  std::string name() final {
    std::string composite = "Eigen<" + std::to_string(dimensionality) +", ";

    if(std::is_same<FloatType, double>::value) {
      composite += "dbl, ";
    } else {
      composite += "flt, ";
    }

    if(SIMD) {
      composite += "1>";
    } else {
      composite += "0>";
    }

    return composite;
  }
};

void writeHeaders(
  std::ofstream& benchmarkFile
) {
  // Write headers
  std::vector<std::string> headers {
    "N",
    "E",
    "Eigen",
    "EigenSIMD"
  };

  for(unsigned i = 0; i < 2; ++i) {
    benchmarkFile << "\"" << headers.at(i) << "\", ";
  }

  for(unsigned i = 2; i < headers.size(); ++i) {
    benchmarkFile << "\"" << headers.at(i) << "\", \"" << headers.at(i)
      << " sigma\"";
    if(i != headers.size() - 1) {
      benchmarkFile << ", ";
    }
  }

  benchmarkFile << nl;
}

enum class Algorithm {
  All,
  EigenDouble,
  EigenFloat,
  EigenSIMDDouble,
  EigenSIMDFloat
};

void benchmark(
  const boost::filesystem::path& filePath,
  std::ofstream& benchmarkFile,
  const Algorithm algorithmChoice
) {
  using namespace Molassembler;

  Molecule molecule = IO::read(
    filePath.string()
  );

  // Skip small molecules
  if(molecule.graph().N() < 10) {
    std::cout << "Skipping " << filePath.stem().string() << " since it's very small\n";
    return;
  }

  /* Embed, generate square bounds from distance bounds matrix and extract
   * chiral and dihedral constraints from spatial model
   */

  /* Timings */
  std::cout << std::fixed << std::setprecision(0);

  std::string nCount = "N = " + std::to_string(molecule.graph().N());

  std::cout
    << std::setw(6) << nCount
    << std::setw(10) << "Name"
    << std::setw(10) << "Rel. v"
    << std::setw(14) << "Successes"
    << std::setw(25) << "Time mu(sigma) / 1e-6s"
    << std::setw(25) << "Iter mu(sigma) / 1"
    << nl;

  std::vector<
    std::unique_ptr<TimingFunctor>
  > functors;

  if(algorithmChoice == Algorithm::All || algorithmChoice == Algorithm::EigenDouble) {
    functors.emplace_back(
      std::make_unique<
        EigenFunctor<4, double, false>
      >()
    );
  }

  if(algorithmChoice == Algorithm::All || algorithmChoice == Algorithm::EigenFloat) {
    functors.emplace_back(
      std::make_unique<
        EigenFunctor<4, float, false>
      >()
    );
  }

  if(algorithmChoice == Algorithm::All || algorithmChoice == Algorithm::EigenSIMDDouble) {
    functors.emplace_back(
      std::make_unique<
        EigenFunctor<4, double, true>
      >()
    );
  }

  if(algorithmChoice == Algorithm::All || algorithmChoice == Algorithm::EigenSIMDFloat) {
    functors.emplace_back(
      std::make_unique<
        EigenFunctor<4, float, true>
      >()
    );
  }

  auto results = timeFunctors<nExperiments>(molecule, functors);

  double smallestAverage = std::min_element(
    results.begin(),
    results.end(),
    [](const FunctorResults& a, const FunctorResults& b) -> bool {
      return a.timingAverage < b.timingAverage;
    }
  )->timingAverage;

  benchmarkFile
    << std::fixed << std::setprecision(0)
    << molecule.graph().N() << ", " << molecule.graph().B() << ", "
    << std::scientific << std::setprecision(6);

  for(unsigned i = 0; i < functors.size(); ++i) {
    const FunctorResults& functorResult = results.at(i);
    const auto& functorPtr = functors.at(i);

    std::string successCount = std::to_string(functorResult.count) + "/" + std::to_string(nExperiments);

    std::string timingStr = std::to_string(static_cast<int>(functorResult.timingAverage)) + "(" + std::to_string(static_cast<int>(functorResult.timingStddev)) + ")";

    std::string iterationStr = std::to_string(static_cast<int>(functorResult.iterationsAverage)) + "(" + std::to_string(static_cast<int>(functorResult.iterationsStddev)) + ")";

    std::cout
      << std::setw(16) << functorPtr->name()
      << std::setw(10) << (smallestAverage != 0 ? functorResult.timingAverage / smallestAverage : 0)
      << std::setw(14) << successCount
      << std::setw(25) << timingStr
      << std::setw(25) << iterationStr
      << nl;

    benchmarkFile << functorResult.timingAverage << ", " << functorResult.timingStddev;

    if(i != functors.size() - 1) {
      benchmarkFile << ", ";
    } else {
      benchmarkFile << nl;
      std::cout << nl;
    }
  }
}

using namespace std::string_literals;
const std::string algorithmChoices =
  "  0 - All\n"
  "  1 - Eigen<dimensionality=4, double, SIMD=false>\n"
  "  2 - Eigen<dimensionality=4, float, SIMD=false>\n"
  "  3 - Eigen<dimensionality=4, double, SIMD=true>\n"
  "  4 - Eigen<dimensionality=4, float, SIMD=true>\n";

constexpr const char* description =
  "Benchmarks various refinement error functions and optimizer combinations\n"
  "against one another in order to figure out if there are stability issues\n"
  "with particular combinations.\n\n"
  "It is necessary to provide a path containing MOLFiles that can be\n"
  "interpreted as single molecules and then used to benchmark the refinement\n"
  "functions. It may be interesting to have molecules of a wide range of sizes\n"
  "and differing structural features to test various error function components\n";

int main(int argc, char* argv[]) {
  using namespace Molassembler;

  // Set up option parsing
  boost::program_options::options_description options_description("Recognized options");
  options_description.add_options()
    ("help", "Produce help message")
    ("c", boost::program_options::value<unsigned>(), "Specify algorithm to benchmark")
    ("m", boost::program_options::value<std::string>(), "Path to MOLFiles to benchmark")
  ;

  // Parse
  boost::program_options::variables_map options_variables_map;
  boost::program_options::store(
    boost::program_options::parse_command_line(argc, argv, options_description),
    options_variables_map
  );
  boost::program_options::notify(options_variables_map);

  // Program options
  if(options_variables_map.count("help") > 0) {
    std::cout << description << "\n" << options_description << std::endl;
    return 0;
  }

  if(options_variables_map.count("m") == 0) {
    std::cout << "You have not specified any path to MOLFiles that could be used" << nl;
    return 0;
  }

  std::string molPath = options_variables_map["m"].as<std::string>();

  Algorithm choice = Algorithm::All;
  if(options_variables_map.count("c") > 0) {
    unsigned combination = options_variables_map["c"].as<unsigned>();

    if(combination > 4) {
      std::cout << "Specified algorithm is out of bounds. Valid choices are:" << nl
        << algorithmChoices;
      return 0;
    }

    choice = static_cast<Algorithm>(combination);
  }

  // Benchmark everything
  std::ofstream benchmarkFile ("refinement_timings.csv");
  writeHeaders(benchmarkFile);

  for(
    const boost::filesystem::path& currentFilePath :
    boost::filesystem::recursive_directory_iterator(molPath)
  ) {
    benchmark(currentFilePath, benchmarkFile, choice);
  }

  return 0;
}