/*
 * circuit.cpp
 * Creates up to generate_count transactions as follows:
 * 1.  INN transactions create addr_count coins for every address
 * 2.  Each peer address sends 1 coin to every other address
 * 3.  Each peer address returns addr_count coins to the INN
 *
 *  For perfect circuits,
 *    make sure that generate_count is one more than a perfect square.
 *
 *  Created on: May 24, 2018
 *      Author: Nick Williams
 */

#include <algorithm>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <memory>

#include <boost/program_options.hpp>

#include "common/devcash_context.h"
#include "common/logger.h"

#include "modules/BlockchainModule.h"

using namespace Devcash;

struct circuit_options {
  eAppMode mode  = eAppMode::T1;
  unsigned int node_index = 0;
  unsigned int shard_index = 0;
  std::string write_file;
  std::string inn_keys;
  std::string node_keys;
  std::string wallet_keys;
  std::string key_pass;
  unsigned int generate_count;
  uint64_t tx_amount;
  eDebugMode debug_mode;
  bool clear_state;
};

std::unique_ptr<struct circuit_options> ParseCircuitOptions(int argc, char** argv);

int main(int argc, char* argv[]) {
  init_log();

  CASH_TRY {
    std::unique_ptr<circuit_options> options = ParseCircuitOptions(argc, argv);

    if (!options) {
      exit(-1);
    }

    DevcashContext this_context(options->node_index, options->shard_index, options->mode, options->inn_keys,
                                options->node_keys, options->key_pass);

    KeyRing keys(this_context);
    keys.LoadWallets(options->wallet_keys, options->key_pass);

    std::vector<byte> out;
    EVP_MD_CTX* ctx;
    if (!(ctx = EVP_MD_CTX_create())) {
      std::string err("Could not create signature context!");
      throw std::runtime_error(err);
    }

    Address inn_addr = keys.getInnAddr();

    if (options->generate_count < 2) {
      LOG_FATAL << "Must generate at least 2 transactions for a complete circuit.";
      CASH_THROW("Invalid number of transactions to generate.");
    }
    // Need sqrt(N-1) addresses (x) to create N circuits: 1+x(x-1)+2x=N
    double need_addrs = std::sqrt(options->generate_count - 1);
    // if sqrt(N-1) is not an int, circuits will be incomplete
    if (std::floor(need_addrs) != need_addrs) {
      LOG_WARNING << "For complete circuits generate a perfect square + 1 transactions (ie 2,5,10,17...)";
    }

    size_t addr_count = std::min(keys.CountWallets(), static_cast<size_t>(need_addrs));

    size_t counter = 0;

    std::vector<Transfer> xfers;
    Transfer inn_transfer(inn_addr, 0, -1l * addr_count * (addr_count - 1) * options->tx_amount, 0);
    xfers.push_back(inn_transfer);
    for (size_t i = 0; i < addr_count; ++i) {
      Transfer transfer(keys.getWalletAddr(i), 0, options->tx_amount * (addr_count - 1), 0);
      xfers.push_back(transfer);
    }
    uint64_t nonce = GetMillisecondsSinceEpoch() + (1000000
                     * (options->node_index + 1) * (options->tx_amount + 1));
	std::vector<byte> nonce_bin;
    Uint64ToBin(nonce, nonce_bin);
    Tier2Transaction inn_tx(eOpType::Create, xfers, nonce_bin,
                            keys.getKey(inn_addr), keys);
    std::vector<byte> inn_canon(inn_tx.getCanonical());
    out.insert(out.end(), inn_canon.begin(), inn_canon.end());
    LOG_DEBUG << "Circuit test generated inn_tx with sig: " << inn_tx.getSignature().getJSON();
    counter++;

    while (counter < options->generate_count) {
      for (size_t i = 0; i < addr_count; ++i) {
        for (size_t j = 0; j < addr_count; ++j) {
          if (i == j) { continue; }
          std::vector<Transfer> peer_xfers;
          Transfer sender(keys.getWalletAddr(i), 0, -1l * options->tx_amount, 0);
          peer_xfers.push_back(sender);
          Transfer receiver(keys.getWalletAddr(j), 0, options->tx_amount, 0);
          peer_xfers.push_back(receiver);
          nonce_bin.clear();
          nonce = GetMillisecondsSinceEpoch() + (1000000
                     * (options->node_index + 1) * (i + 1) * (j + 1));
          Uint64ToBin(nonce, nonce_bin);
          Tier2Transaction peer_tx(
              eOpType::Exchange, peer_xfers, nonce_bin,
              keys.getWalletKey(i), keys);
          std::vector<byte> peer_canon(peer_tx.getCanonical());
          out.insert(out.end(), peer_canon.begin(), peer_canon.end());
          LOG_TRACE << "Circuit test generated tx with sig: " << peer_tx.getSignature().getJSON();
          counter++;
          if (counter >= options->generate_count) { break; }
        }  // end inner for
        if (counter >= options->generate_count) { break; }
      }  // end outer for
      if (counter >= options->generate_count) { break; }
      if (options->clear_state) {
        for (size_t i = 0; i < addr_count; ++i) {
          std::vector<Transfer> peer_xfers;
          Transfer sender(keys.getWalletAddr(i), 0, -1l * (addr_count - 1) * options->tx_amount, 0);
          peer_xfers.push_back(sender);
          Transfer receiver(inn_addr, 0, (addr_count - 1) * options->tx_amount, 0);
          peer_xfers.push_back(receiver);
          nonce = GetMillisecondsSinceEpoch() + (1000000
                       * (options->node_index + 1) * (i + 1) * (addr_count + 2));
          Uint64ToBin(nonce, nonce_bin);
          Tier2Transaction peer_tx(
              eOpType::Exchange, peer_xfers, nonce_bin,
              keys.getWalletKey(i), keys);
          std::vector<byte> peer_canon(peer_tx.getCanonical());
          out.insert(out.end(), peer_canon.begin(), peer_canon.end());
          LOG_TRACE << "GenerateTransactions(): generated tx with sig: " << peer_tx.getSignature().getJSON();
          counter++;
          if (counter >= options->generate_count) { break; }
        }  // end outer for
      }
      if (counter >= options->generate_count) { break; }
    }  // end counter while

    LOG_INFO << "Generated " << counter << " transactions.";

    if (!options->write_file.empty()) {
      std::ofstream out_file(options->write_file, std::ios::out | std::ios::binary);
      if (out_file.is_open()) {
        out_file.write((const char*) out.data(), out.size());
        out_file.close();
      } else {
        LOG_FATAL << "Failed to open output file '" << options->write_file << "'.";
        return (false);
      }
    }

    return (true);
  }
  CASH_CATCH(...) {
    std::exception_ptr p = std::current_exception();
    std::string err("");
    err += (p ? p.__cxa_exception_type()->name() : "null");
    LOG_FATAL << "Error: " + err << std::endl;
    std::cerr << err << std::endl;
    return (false);
  }
}

std::unique_ptr<struct circuit_options> ParseCircuitOptions(int argc, char** argv) {

  namespace po = boost::program_options;

  std::unique_ptr<circuit_options> options(new circuit_options());

  try {
    po::options_description desc("\n\
" + std::string(argv[0]) + " [OPTIONS] \n\
\n\
Creates up to generate_count transactions as follows:\n\
 1.  INN transactions create addr_count coins for every address\n\
 2.  Each peer address sends 1 coin to every other address\n\
 3.  Each peer address returns addr_count coins to the INN\n\
\n\
For perfect circuits, make sure that generate_count is one more \n\
than a perfect square.\n\
\n\
Required parameters");
    desc.add_options()
        ("mode", po::value<std::string>(), "Devcash mode (T1|T2|scan)")
        ("node-index", po::value<unsigned int>(), "Index of this node")
        ("shard-index", po::value<unsigned int>(), "Index of this shard")
        ("output", po::value<std::string>(), "Output path in binary JSON or CBOR")
        ("inn-keys", po::value<std::string>(), "Path to INN key file")
        ("node-keys", po::value<std::string>(), "Path to Node key file")
        ("wallet-keys", po::value<std::string>(), "Path to Wallet key file")
        ("key-pass", po::value<std::string>(), "Password for private keys")
        ("generate-tx", po::value<unsigned int>(), "Generate at least this many Transactions")
        ("tx-amount", po::value<uint64_t>(), "Number of coins to transfer in transaction")
        ;

    po::options_description d2("Optional parameters");
    d2.add_options()
        ("help", "produce help message")
        ("clear-state", po::value<bool>(), "Return coins to INN address?")
        ("debug-mode", po::value<std::string>(), "Debug mode (on|off|perf) for testing")
        ;
    desc.add(d2);

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

    if (vm.count("help")) {
      std::cout << desc << "\n";
      return nullptr;
    }

    if (vm.count("mode")) {
      std::string mode = vm["mode"].as<std::string>();
      if (mode == "SCAN") {
        options->mode = scan;
      } else if (mode == "T1") {
        options->mode = T1;
      } else if (mode == "T2") {
        options->mode = T2;
      } else {
        LOG_WARNING << "unknown mode: " << mode;
      }
      LOG_INFO << "mode: " << options->mode;
    } else {
      LOG_INFO << "mode was not set.";
    }

    if (vm.count("debug-mode")) {
      std::string debug_mode = vm["debug-mode"].as<std::string>();
      if (debug_mode == "on") {
        options->debug_mode = on;
      } else if (debug_mode == "toy") {
        options->debug_mode = toy;
      } else if (debug_mode == "perf") {
        options->debug_mode = perf;
      } else {
        options->debug_mode = off;
      }
      LOG_INFO << "debug_mode: " << options->debug_mode;
    } else {
      LOG_INFO << "debug_mode was not set.";
    }

    if (vm.count("node-index")) {
      options->node_index = vm["node-index"].as<unsigned int>();
      LOG_INFO << "Node index: " << options->node_index;
    } else {
      LOG_INFO << "Node index was not set.";
    }

    if (vm.count("shard-index")) {
      options->shard_index = vm["shard-index"].as<unsigned int>();
      LOG_INFO << "Shard index: " << options->shard_index;
    } else {
      LOG_INFO << "Shard index was not set.";
    }

    if (vm.count("output")) {
      options->write_file = vm["output"].as<std::string>();
      LOG_INFO << "Output file: " << options->write_file;
    } else {
      LOG_INFO << "Output file was not set.";
    }

    if (vm.count("inn-keys")) {
      options->inn_keys = vm["inn-keys"].as<std::string>();
      LOG_INFO << "INN keys file: " << options->inn_keys;
    } else {
      LOG_INFO << "INN keys file was not set.";
    }

    if (vm.count("node-keys")) {
      options->node_keys = vm["node-keys"].as<std::string>();
      LOG_INFO << "Node keys file: " << options->node_keys;
    } else {
      LOG_INFO << "Node keys file was not set.";
    }

    if (vm.count("wallet-keys")) {
      options->wallet_keys = vm["wallet-keys"].as<std::string>();
      LOG_INFO << "Wallet keys file: " << options->wallet_keys;
    } else {
      LOG_INFO << "Wallet keys file was not set.";
    }

    if (vm.count("key-pass")) {
      options->key_pass = vm["key-pass"].as<std::string>();
      LOG_INFO << "Key pass: " << options->key_pass;
    } else {
      LOG_INFO << "Key pass was not set.";
    }

    if (vm.count("generate-tx")) {
      options->generate_count = vm["generate-tx"].as<unsigned int>();
      LOG_INFO << "Generate Transactions: " << options->generate_count;
    } else {
      LOG_INFO << "Generate Transactions was not set, defaulting to 0";
      options->generate_count = 0;
    }

    if (vm.count("tx-amount")) {
      options->tx_amount = vm["tx-amount"].as<uint64_t>();
      LOG_INFO << "Transaction amount: " << options->tx_amount;
    } else {
      options->tx_amount = 3210123;
      LOG_INFO << "Transaction amount was not set, defaulting to " << options->tx_amount;
    }

    if (vm.count("clear-state")) {
      options->clear_state = vm["clear-state"].as<bool>();
      LOG_INFO << "Clear state: " << options->clear_state;
    } else {
      options->clear_state = true;
      LOG_INFO << "Clear state was not set, defaulting to " << options->clear_state;
    }
  }
  catch (std::exception& e) {
    LOG_ERROR << "error: " << e.what();
    return nullptr;
  }

  return options;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_test_common.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>

#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/algorithms/within.hpp>

#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

#include "test_within.hpp"

template <typename P>
void test_all()
{
    typedef bg::model::multi_polygon<bg::model::polygon<P> > mp;

    // test multi-with-one-polygon (trivial case)
    test_geometry<P, mp>("POINT(1 1)", "MULTIPOLYGON(((0 0,0 2,2 2,2 0,0 0)))", true);
    test_geometry<P, mp>("POINT(3 3)", "MULTIPOLYGON(((0 0,0 2,2 2,2 0,0 0)))", false);
    test_geometry<P, mp>("POINT(0 1)", "MULTIPOLYGON(((0 0,0 2,2 2,2 0,0 0)))", false);
    test_geometry<P, mp>("POINT(4 4)", "MULTIPOLYGON(((0 0,0 2,2 2,2 0,0 0)))", false);

    // test if it is in one of them
    std::string multi("MULTIPOLYGON("
        "((0 0,0 2,2 2,2 0,0 0))"
        "((3 3,3 6,6 6,6 3,3 3))"
        ")");
    test_geometry<P, mp>("POINT(4 4)", multi, true);
    test_geometry<P, mp>("POINT(1 1)", multi, true);
    test_geometry<P, mp>("POINT(0 1)", multi, false);
}

int test_main( int , char* [] )
{
    //test_all<bg::model::d2::point_xy<int> >();
    test_all<bg::model::d2::point_xy<double> >();

#if defined(HAVE_TTMATH)
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

    return 0;
}


#define BOOST_TEST_MODULE "IntegratorTest"
#include <boost/test/included/unit_test.hpp>
#include "lib/Integrator.hpp"
#include "lib/ValueBundle.hpp"

class SpinParser
{
public:
	SpinParser(FrequencyDiscretization *f)
	{
		FrgCommon::_frequency = f;
	}

	~SpinParser()
	{
		delete FrgCommon::_frequency;
	}
};

struct IntegratorFixture
{
	IntegratorFixture()
	{
		std::vector<float> values({ 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0 });
		FrequencyDiscretization *f = new FrequencyDiscretization(values);
		spinParser = new SpinParser(f);
	}

	~IntegratorFixture()
	{
		delete spinParser;
	}

	SpinParser *spinParser;
};

BOOST_FIXTURE_TEST_SUITE(IntegratorTest, IntegratorFixture);

BOOST_AUTO_TEST_CASE(IntegratorIntegrate, *boost::unit_test::tolerance(1.0e-6))
{
	std::function<float(float)> integrandLinear = [](float x)->float { return x; };
	std::function<float(float)> integrandQuadratic = [](float x)->float { return x*x; };
	auto min = FrgCommon::frequency().begin();
	auto max = FrgCommon::frequency().last();

	BOOST_CHECK_EQUAL(Integrator::integrate(min, max, integrandLinear), 49.5f);
	BOOST_CHECK_EQUAL(Integrator::integrate(min, max, integrandQuadratic), 334.5f);
}

BOOST_AUTO_TEST_CASE(IntegratorIntegrateWithObscureLeftBoundary, *boost::unit_test::tolerance(1.0e-6))
{
	std::function<float(float)> integrandLinear = [](float x)->float { return x; };
	std::function<float(float)> integrandQuadratic = [](float x)->float { return x * x; };
	auto min = 1.5f;
	auto max = FrgCommon::frequency().last();

	BOOST_CHECK_EQUAL(Integrator::integrateWithObscureLeftBoundary(min, max, integrandLinear), 48.875f);
	BOOST_CHECK_EQUAL(Integrator::integrateWithObscureLeftBoundary(min, max, integrandQuadratic), 333.5625f);
}

BOOST_AUTO_TEST_CASE(IntegratorIntegrateWithObscureRightBoundary, *boost::unit_test::tolerance(1.0e-6))
{
	std::function<float(float)> integrandLinear = [](float x)->float { return x; };
	std::function<float(float)> integrandQuadratic = [](float x)->float { return x * x; };
	auto min = FrgCommon::frequency().begin();
	auto max = 9.5f;

	BOOST_CHECK_EQUAL(Integrator::integrateWithObscureRightBoundary(min, max, integrandLinear), 44.625f);
	BOOST_CHECK_EQUAL(Integrator::integrateWithObscureRightBoundary(min, max, integrandQuadratic), 286.8125f);
}

BOOST_AUTO_TEST_CASE(IntegratorIntegrateWithObscureBoundaries, *boost::unit_test::tolerance(1.0e-6))
{
	std::function<float(float)> integrandLinear = [](float x)->float { return x; };
	std::function<float(float)> integrandQuadratic = [](float x)->float { return x * x; };
	auto min = 1.5f;
	auto max = 9.5f;

	BOOST_CHECK_EQUAL(Integrator::integrateWithObscureBoundaries(min, max, integrandLinear), 44.0f);
	BOOST_CHECK_EQUAL(Integrator::integrateWithObscureBoundaries(min, max, integrandQuadratic), 285.875f);
}

BOOST_AUTO_TEST_CASE(ImplicitIntegratorIntegrateWithObscureLeftBoundary, *boost::unit_test::tolerance(1.0e-6))
{
	std::function<void(float, ValueSuperbundle<float, 1> &)> integrand = [](float x, ValueSuperbundle<float, 1> &out)->void { out.bundle(0)[0] = x; out.bundle(0)[1] = x * x; };
	auto min = 1.5f;
	auto max = FrgCommon::frequency().last();

	ValueSuperbundle<float, 1> buffer = ValueSuperbundle<float, 1>(2);
	ValueSuperbundle<float, 1> result = ValueSuperbundle<float, 1>(2);
	ImplicitIntegrator::integrateWithObscureLeftBoundary<ValueSuperbundle<float, 1> >(min, max, integrand, buffer, result);
	BOOST_CHECK_EQUAL(result.bundle(0)[0], 48.875f);
	BOOST_CHECK_EQUAL(result.bundle(0)[1], 333.5625f);
}

BOOST_AUTO_TEST_CASE(ImplicitIntegratorIntegrateWithObscureRightBoundary, *boost::unit_test::tolerance(1.0e-6))
{
	std::function<void(float, ValueSuperbundle<float, 1> &)> integrand = [](float x, ValueSuperbundle<float, 1> &out)->void { out.bundle(0)[0] = x; out.bundle(0)[1] = x * x; };
	auto min = FrgCommon::frequency().begin();
	auto max = 9.5f;

	ValueSuperbundle<float, 1> buffer = ValueSuperbundle<float, 1>(2);
	ValueSuperbundle<float, 1> result = ValueSuperbundle<float, 1>(2);
	ImplicitIntegrator::integrateWithObscureRightBoundary<ValueSuperbundle<float, 1> >(min, max, integrand, buffer, result);
	BOOST_CHECK_EQUAL(result.bundle(0)[0], 44.625f);
	BOOST_CHECK_EQUAL(result.bundle(0)[1], 286.8125f);
}

BOOST_AUTO_TEST_CASE(ImplicitIntegratorIntegrateWithObscureBoundaries, *boost::unit_test::tolerance(1.0e-6))
{
	std::function<void(float, ValueSuperbundle<float, 1> &)> integrand = [](float x, ValueSuperbundle<float, 1> &out)->void { out.bundle(0)[0] = x; out.bundle(0)[1] = x * x; };
	auto min = 1.5f;
	auto max = 9.5f;

	ValueSuperbundle<float, 1> buffer = ValueSuperbundle<float, 1>(2);
	ValueSuperbundle<float, 1> result = ValueSuperbundle<float, 1>(2);
	ImplicitIntegrator::integrateWithObscureBoundaries<ValueSuperbundle<float, 1> >(min, max, integrand, buffer, result);
	BOOST_CHECK_EQUAL(result.bundle(0)[0], 44.0f);
	BOOST_CHECK_EQUAL(result.bundle(0)[1], 285.875f);
}

BOOST_AUTO_TEST_SUITE_END();


/*
 * Copyright 2019 Daniel Harvey
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#define BOOST_TEST_MODULE test_profiler

#include "profile/performance_counter.h"
#include "profile/do_not_optimize.h"

#include <boost/test/included/unit_test.hpp>

#include <numeric>
#include <iostream>

using namespace profile;

namespace
{
  template<typename T>
  void do_not_optimize (const T &t)
  {
    asm volatile ("" : : "r,m"(t) : "memory");
  }

  double count_random (int n)
  {
    auto x = 0.;

    for (auto i = 0; i < n; ++i)
    {
      x += rand ();
    }

    return x;
  }

  template<typename T>
  std::vector<std::vector<T>>
  transpose (const std::vector<std::vector<T>> &input)
  {
    if (input.empty ())
    {
      return {};
    }

    const auto nrows = input.size ();
    const auto ncols = input.front ().size ();

    std::vector<std::vector<T>> output (
        ncols, std::vector<T> (nrows, T {}));

    for (auto i = 0u; i < nrows; ++i)
    {
      for (auto j = 0u; j < ncols; ++j)
      {
        output[j][i] = input[i][j];
      }
    }

    return output;
  }

  struct MeanStdev
  {
    double mean = {};
    double stdev = {};
  };

  template<typename T>
  MeanStdev
  mean_stdev (const std::vector<T> &ts)
  {
    const auto n = static_cast<double> (ts.size ());

    if (!n)
    {
      return {{}, {}};
    }

    const auto mean
      = std::accumulate (ts.begin (), ts.end (), 0.) / n;

    const auto variance
      = std::accumulate (ts.begin (), ts.end (), 0.,
          [mean] (auto &lhs, auto &rhs)
          {
            return lhs + (rhs - mean) * (rhs - mean);
          }) / n;

    return {mean, std::sqrt (variance)};
  }

  std::vector<MeanStdev>
  mean_stdev (const std::vector<std::vector<long>> &input)
  {
    const auto transposed = transpose (input);

    std::vector<MeanStdev> output (transposed.size ());

    std::transform (
        transposed.begin (),
        transposed.end (),
        output.begin (),
        &mean_stdev<long>
    );

    return output;
  }

  struct CountRandomFixture
  {
    std::vector<PerformanceCounter> counters;

    std::vector<long> profile (int n)
    {
      std::for_each (counters.begin (), counters.end (),
                     std::mem_fn (&PerformanceCounter::reset));

      std::for_each (counters.begin (), counters.end (),
                     std::mem_fn (&PerformanceCounter::start));

      do_not_optimize (count_random (n));

      std::for_each (counters.begin (), counters.end (),
                     std::mem_fn (&PerformanceCounter::stop));

      std::vector<long> result (counters.size ());

      std::transform (counters.begin (), counters.end (),
                      result.begin (),
                      std::mem_fn (&PerformanceCounter::get));
      return result;
    }

    std::vector<MeanStdev>
    run_benchmark (int n)
    {
      static const int nruns = 100;

      std::for_each (counters.begin (), counters.end (),
                     std::mem_fn (&PerformanceCounter::reset));

      std::vector<std::vector<long>> counts;
      counts.reserve (nruns);

      for (auto i = 0; i < nruns; ++i)
      {
        counts.emplace_back (profile (n));
      }

      return mean_stdev (counts);
    }
  };
}

/*
 * Test we get consistent profiling results
 */
BOOST_FIXTURE_TEST_CASE (test_profiler_basic_consistency,
                         CountRandomFixture)
{
  counters.emplace_back (
      PerformanceCounter (
        Event::from_string ("perf_count_hw_instructions"), 0));

  /*
   * Check that the variance of the measured instructions is low
   */
  BOOST_REQUIRE_EQUAL (run_benchmark (1 << 10).size (), 1ul);
  BOOST_CHECK_LE (run_benchmark (1 << 10).front ().stdev, 1.0);
  BOOST_CHECK_LE (run_benchmark (1 << 11).front ().stdev, 1.0);

  /*
   * Check that the relationship between the means
   * is as expected for simple counting functions.
   */
  BOOST_CHECK_CLOSE (run_benchmark (1 << 11).front ().mean,
                     run_benchmark (1 << 10).front ().mean * 2.0,
                     10 /*pct*/);

  /*
   * Check two counters counting the same event give consistent
   * results.
   */
  counters.emplace_back (
      PerformanceCounter (
        Event::from_string ("perf_count_hw_instructions"), 0));

  const auto results = run_benchmark (1 << 11);

  BOOST_REQUIRE_EQUAL (results.size (), 2ul);
  BOOST_CHECK_CLOSE (results.at (0).mean, results.at (1).mean, 1.0);
}


#include <vector>
#include <deque>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <chrono>

#include <unistd.h>
#include <sys/wait.h>

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

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

#include "../../../core/image.h"
#include "../../../core/helper.h"
#include "../../../stereo_odometry/base/visual_odometry_base.h"
#include "../../../stereo_odometry/base/visual_odometry_ransac.h"
#include "../../../tracker/stereo/experiment_factory.h"
#include "../../../tracker/stereo/stereo_tracker_base.h"
#include "../../../tracker/refiner/feature_refiner_base.cc"
#include "../../../tracker/refiner/feature_refiner_klt.h"
#include "../../../tracker/detector/feature_detector_base.h"
#include "../../../tracker/stereo/tracker_helper.h"
#include "../../../optimization/bundle_adjustment/bundle_adjuster_base.h"
#include "../../../optimization/bundle_adjustment/bundle_adjuster_multiframe.h"
#include "../../math_helper.h"
#include "../../../tracker/base/eval_helper.h"
#include "../../../core/format_helper.h"
#include "../../helper_libviso.h"
#include "../../cv_plotter.h"

using namespace core;
using namespace vo;
using namespace track;
using namespace std;

void run_visual_odometry(const std::string& source_folder,
                         const std::string& imagelistfn,
                         const std::string& experiment_config,
                         const std::string& cparams_file,
                         const std::string& gt_filepath)
{
  std::deque<cv::Mat> extr_params; // pose mat
  std::deque<cv::Mat> Rt_params;

  double cam_params[5];
  FormatHelper::readCameraParams(cparams_file, cam_params);

  std::vector<std::string> imagelist;
  bool ok = FormatHelper::readStringList(imagelistfn, imagelist);
  if(!ok || imagelist.empty())
    throw "can not open " + imagelistfn + " or the string list is empty\n";

  int start_frame = 0 * 2;
  // 01
  //int start_frame = 431 * 2;
  //int start_frame = 482 * 2;
  int end_frame = imagelist.size();
  //int nframes = (imagelist.size() - start_frame) / 2;
  bool debug = false;
  //debug = true;

  cv::Mat img_left_prev, img_right_prev;
  std::cout << source_folder + imagelist[start_frame] << "\n";
  cv::Mat img_left = cv::imread(source_folder + imagelist[start_frame], CV_LOAD_IMAGE_GRAYSCALE);
  cv::Mat img_right = cv::imread(source_folder + imagelist[start_frame+1], CV_LOAD_IMAGE_GRAYSCALE);
  if(img_left.empty()) {
    std::cout << "Error: no images!\n";
    throw 1;
  }

  std::string output_folder;
  FeatureDetectorBase* detector = nullptr;
  TrackerBase* mono_tracker = nullptr;
  StereoTrackerBase* stereo_tracker = nullptr;
  visodom::VisualOdometryBase* viso = nullptr;
  //optim::FeatureHelperSBA* bundle_adjuster = nullptr;
  optim::BundleAdjusterBase* bundle_adjuster = nullptr;
  bool use_ba; // use bundle adjustment
  //track::ExperimentFactory::create_experiment(experiment_config, "/home/kivan/Dropbox/experiment_data/04_deformation_field_matrix.yml", cam_params,
  //                                            img_left.rows, img_left.cols, output_folder, &detector,
  //                                            &mono_tracker, &stereo_tracker, &viso, use_ba, &bundle_adjuster);
  track::ExperimentFactory::create_experiment(experiment_config, "", cam_params,
                                              0, 0, output_folder, &detector,
                                              &mono_tracker, &stereo_tracker, &viso, use_ba, &bundle_adjuster);

  //libviso::Matrix pose = libviso::Matrix::eye(4);
  cv::Mat pose = cv::Mat::eye(4, 4, CV_64F);
  //Matrix point_rt = Matrix::eye(4);
  //cv::Mat pose_mat = cv::Mat::eye(4, 4, CV_64F);
  cv::Mat Rt_inv;
  cv::Mat mat_I = cv::Mat::eye(4, 4, CV_64F);
  mat_I.copyTo(Rt_inv);
  cv::Vec<double,7> trans_vec;
  extr_params.push_back(mat_I.clone());
  cv::Mat Rt(4, 4, CV_64F);
  cv::Mat Rt_gt(4, 4, CV_64F);
  cv::Mat Rt_gt_prev = cv::Mat::eye(4, 4, CV_64F);
  cv::Mat Rt_gt_curr = cv::Mat::eye(4, 4, CV_64F);
  core::MathHelper::invTrans(Rt_inv, Rt);
  Rt_params.push_back(Rt.clone());

  cv::Mat prev_location_viso = cv::Mat::zeros(4, 1, CV_64F);

  // init the tracker
  stereo_tracker->init(img_left, img_right);

  // matrix transforms points in current camera coordinate system to world coord system
  std::vector<cv::Mat> libviso_Rt_all;
  cv::Mat pose_libviso = cv::Mat::eye(4, 4, CV_64F);
  libviso_Rt_all.push_back(pose_libviso.clone());

  std::string track_name;
  bool have_gt = false;
  std::ifstream gt_file;
  if(!gt_filepath.empty()) {
    have_gt = true;
    gt_file.open(gt_filepath);
  }
  else throw 1;
  // no need - Tsukuba baseline is in cm
  bool using_kitti = false;
  double error_thr = 0.0;
  if(experiment_config.find("tsukuba") != std::string::npos) {
    printf("---------- Using Tsukuba dataset! ------------\n");
    track_name = "00";
    error_thr = 2.0;
  }
  else if(experiment_config.find("bb2") != std::string::npos) {
    printf("---------- Using Bumblebee dataset! ------------\n");
    track_name = "bb";
  }
  // else it is KITTI
  else {
    printf("---------- Using KITTI dataset! ------------\n");
    track_name = imagelistfn.substr(imagelistfn.size()-10,2);
    using_kitti = true;
    error_thr = 8.0;
  }

  int num_of_motions = ((imagelist.size()/2) - 1);
  std::vector<cv::Mat> gt_world_motion, gt_camera_motion;
  core::FormatHelper::Read2FrameMotionFromAccCameraMotion(gt_filepath, num_of_motions,
                                                          gt_world_motion, gt_camera_motion);

  //std::string bad_folder = "/home/kivan/Projects/datasets/KITTI/tracks/bad_patches/";
  //std::string good_folder = "/home/kivan/Projects/datasets/KITTI/tracks/good_patches/";
  //core::Helper::CleanFolder(bad_folder);
  //core::Helper::CleanFolder(good_folder);

  std::vector<size_t> track_index, track_cnt;
  track_index.assign(stereo_tracker->countFeatures(), 0);
  track_cnt.assign(stereo_tracker->countFeatures(), 0);
  size_t all_tracks_cnt = 0;

  // KITTI
  int h_bins = 15;
  int v_bins = 5;
  //int h_bins = 20;
  //int v_bins = 10;
  // tsukuba
  //int h_bins = 15;
  //int v_bins = 11;
  //int h_bins = 9;
  //int v_bins = 7;

  std::vector<std::vector<double>> left_reproj_errors, right_reproj_errors;
  std::vector<std::vector<Eigen::Vector2d>> left_error_vectors, right_error_vectors;
  left_reproj_errors.resize(h_bins * v_bins);
  right_reproj_errors.resize(h_bins * v_bins);
  left_error_vectors.resize(h_bins * v_bins);
  right_error_vectors.resize(h_bins * v_bins);
  //double scale_factor = 1.2;
  double scale_factor = 1.8;
  cv::Mat error_img = cv::Mat::zeros(scale_factor*img_left.rows, scale_factor*img_left.cols, CV_8UC3);
  //int gt_better_cnt = 0;
  int frame_num = 0;
  auto start = std::chrono::system_clock::now();
  //for(unsigned i = start_frame + 2; i < start_frame+60; i+=2) {
  for(unsigned i = start_frame + 2; i < end_frame; i+=2) {
    int motion_idx = i / 2 - 1;
    frame_num++;
    std::cout << "Frame num: " << frame_num << "\n";
    std::cout << "Motion idx: " << motion_idx << " / " << (end_frame/2-2) << "\n";
    cout << source_folder + imagelist[i] << endl;
    cv::swap(img_left_prev, img_left);
    cv::swap(img_right_prev, img_right);
    img_left = cv::imread(source_folder + imagelist[i], CV_LOAD_IMAGE_GRAYSCALE);
    img_right = cv::imread(source_folder + imagelist[i+1], CV_LOAD_IMAGE_GRAYSCALE);

    stereo_tracker->track(img_left, img_right);

    // default prior
    double max_z_dist = 300.0; // for tsukuba in cm
    double min_disp = 0.1;
    double max_disp_diff = 50.0;
    // for KITTI
    if(using_kitti) {
      max_disp_diff = 30.0;
      max_z_dist = 3.0;         // 3m
    }
    // filter tracks with prior knowledge
    FeatureHelper::FilterTracksWithPriorOld(*stereo_tracker, max_disp_diff, min_disp);

    // filter outliers with GT
    bool gt_filtering_passed = false;
    if(have_gt) {
      std::vector<std::vector<double>> left_tmp, right_tmp;
      gt_filtering_passed = track::EvalHelper::FilterOutliersWithGroundtruth(*stereo_tracker,
                             cam_params, gt_world_motion[motion_idx], error_thr); // the best thresh - 4.0
    }

    // draw tracks before ransac
    //if(!gt_filtering_passed || debug)
    //  EvalHelper::DrawStereoTracks(*stereo_tracker, img_left_prev, img_right_prev,
    //                               "all_left", "all_right");

    // run odometry optimization
    visodom::VisualOdometryRansac* viso_ransac = dynamic_cast<visodom::VisualOdometryRansac*>(viso);
    viso_ransac->setLeftPrevImage(img_left_prev);
    // Rt is the motion of world points with respect to the camera
    Rt = viso->getMotion(*stereo_tracker);
    // TODO - add diff check to test for estimation chrashes on KITTI test
    // CheckMotionDiff(Rt, Rt_prev);
    //if(viso->process(libviso_tracks)) {
    if(!Rt.empty()) {
      // is it better to kill or not to kill outliers in tracker?
      std::vector<int> inliers = viso->getTrackerInliers();
      std::vector<int> outliers = viso->getTrackerOutliers();
      std::cout << "Tracks before RANSAC: " << stereo_tracker->countActiveTracks() << "\n";
      //FeatureHelper::FilterOutlierTracks(*stereo_tracker, active_tracks, inliers, 30.0, 0.1);
      //FeatureHelper::FilterRansacOutliers(*stereo_tracker, active_tracks, inliers);
      for(size_t j = 0; j < outliers.size(); j++)
        stereo_tracker->removeTrack(outliers[j]);
      std::cout << "Tracks after RANSAC: " << stereo_tracker->countActiveTracks() << "\n";
      //// draw tracks
      if(debug)
        EvalHelper::DrawStereoTracks(*stereo_tracker, img_left_prev, img_right_prev,
                                     "ransac_left", "ransac_right");

      //////FeatureHelper::drawStereoRefinerTracks(tracker_basic, tracker, disp_camera_left, disp_camera_right);
      //waitKey(0);

      //// run GT filter after RANSAC
      //bool filter_bad = false;
      ////if(using_kitti)
      ////  filter_bad = true;
      ////double max_reproj_error = 0.6; // for BA experiment
      //double max_reproj_error = 1.0;
      //double max_remove_ratio = 0.5;  // remove max 50%
      //int bad_tracks = track::EvalHelper::CountFilterStoreBadTracks(*stereo_tracker, cam_params, Rt_gt,
      //    good_folder, bad_folder, track_index, track_cnt, all_tracks_cnt, filter_bad, max_reproj_error,
      //    max_remove_ratio);
      //std::cout << "Num of bad tracks = " << bad_tracks << '\n';

      //track::DebugHelper::DebugStereoRefiner(img_left_prev, img_right_prev, img_left, img_right,
      //    *dynamic_cast<StereoTrackerRefiner*>(stereo_tracker), Rt_gt, cam_params);

      //track::EvalHelper::DrawTracksAndErrors(*stereo_tracker, cam_params, gt_motion[i/2], img_left_prev,
      //    img_right_prev, "left_prev_ransac", true, left_reproj_errors, right_reproj_errors,
      //    h_bins, v_bins, false);
      if(gt_filtering_passed)
        track::EvalHelper::CalculateReprojectionErrors(*stereo_tracker, cam_params, gt_world_motion[motion_idx],
                                                      img_left.rows, img_left.cols,
                                                      left_reproj_errors, right_reproj_errors,
                                                      left_error_vectors, right_error_vectors, h_bins, v_bins);
      //if(frame_num > 20) {
      if(i == (end_frame-2)) {
        std::vector<double> left_means, right_means, left_variances, right_variances;
        std::vector<Eigen::Vector2d> left_vec_means, left_vec_variances;
        //track::EvalHelper::DrawErrorDistribution(h_bins, v_bins, left_reproj_errors, error_img, true, false);
        track::EvalHelper::CalculateErrorStatistics(left_reproj_errors, left_error_vectors, left_means,
                                                    left_variances, left_vec_means, left_vec_variances);
        track::EvalHelper::DrawErrorStatistics(v_bins, h_bins, left_means, left_variances,
                                               left_vec_means, left_vec_variances, left_reproj_errors,
                                               true, error_img);

        //cv::imshow("error_distribution", error_img);
        //cv::waitKey(100);
        track::EvalHelper::SaveErrorStatistics(v_bins, h_bins, left_reproj_errors, left_means,
                                               left_variances, left_vec_means, left_vec_variances,
                                               track_name + "_statistics_matrix.yml");
        cv::Mat eq_img;
        //cv::cvtColor(error_img, eq_img, cv::COLOR_RGB2GRAY);
        //cv::equalizeHist(eq_img, eq_img);
        //cv::imwrite("heat_map_with_stats.png", eq_img);
        cv::imwrite(track_name + "_heat_map_with_stats.png", error_img);
        track::EvalHelper::DrawErrorStatistics(v_bins, h_bins, left_means, left_variances,
                                               left_vec_means, left_vec_variances, left_reproj_errors,
                                               false, error_img);
        cv::cvtColor(error_img, eq_img, cv::COLOR_RGB2GRAY);
        cv::equalizeHist(eq_img, eq_img);
        cv::imwrite(track_name + "_heat_map_orig.png", error_img);
        cv::imwrite(track_name + "_heat_map.png", eq_img);
      }

      if(have_gt)
        std::cout << "groundtruth:\n" << gt_world_motion[motion_idx] << "\n";
      std::cout << "two-frame odometry:\n" << Rt << "\n";
      //if(!gt_filtering_passed || debug)
      //  while(cv::waitKey(0) != 27);

      //track::EvalHelper::DrawTracksAndErrors(*stereo_tracker, cam_params, Rt, img_left_prev, img_right_prev);
      // TODO:
      //Eigen::Vector3d t, r;
      //core::MathHelper::GetEulerAngles(Rt, r);
      //core::MathHelper::GetTranslation(Rt, t);

      // if the motion is to small skip it
      // tracker.undo_state();
      core::MathHelper::invTrans(Rt, Rt_inv);    // better
      extr_params.push_back(Rt_inv.clone());
      // on success, update current pose
      pose = pose * Rt_inv;
      libviso_Rt_all.push_back(pose.clone());

      //double num_matches = viso->getNumberOfMatches();
      //double num_inliers = viso->getNumberOfInliers();
      //cout << "[Libviso] Matches: " << num_matches << ", Inliers: " << num_inliers << " -> "
      //     << 100.0*num_inliers/num_matches << " %" << endl;
      std::cout << "-----------------------------------------------------------------------\n\n";
      cv::Mat location_viso(pose_libviso, cv::Range(0,4), cv::Range(3,4)); // extract 4-th column
      location_viso.copyTo(prev_location_viso);
    } else {
      std::cout << "libviso ... failed!" << endl;
      return;
      //throw "Error\n";
      //extr_params.push_back(Rt_inv.clone());
      //exit(1);
    }
  }
  auto end = std::chrono::system_clock::now();
  //auto elapsed = end - start;
  std::chrono::duration<double> elapsed = end - start;
  std::cout << "Elapsed time = " << elapsed.count() << " secs\n";

  std::cout << "Writing track " << track_name << " to files\n";
  std::ofstream libviso_ofile(output_folder + '/' + track_name + ".txt");

  for(size_t i = 0; i < libviso_Rt_all.size(); i++) {
    FormatHelper::WriteMatRt(libviso_Rt_all[i], libviso_ofile);
  }

  delete detector;
  delete mono_tracker;
  delete stereo_tracker;
  delete viso;

  //// run evaluation
  //std::string cmd = "/home/kivan/Projects/cv-stereo/stereo_odometry/evaluation/evaluate_odometry_dense 7 7 " +
  //    std::to_string(start_frame/2) + " " + std::to_string(nframes) + " ./results/ eval_stats";
  //std::cout << cmd << "\n";
  //system(cmd.c_str());
}

int main(int argc, char** argv)
{
  std::string config_file;
  std::string experiment_config;
  std::string imagelistfn;
  std::string cam_params_file;
  std::string source_folder;
  std::string gt_filepath;

  if(argc != 5){
    std::cout << "Usage:\n" << argv[0] << " -c dataset_config -e experiment_config\n";
    return 0;
  }

  try {
    po::options_description generic("Generic options");
    generic.add_options()
      ("help, h", "produce help message")
      ("config,c", po::value<string>(&config_file), "config filename")
      ("experiment_config,e", po::value<string>(&experiment_config), "experiment config filename");
    po::options_description cmdline_options;
    cmdline_options.add(generic);

    po::variables_map vm;
    //po::store(po::command_line_parser(argc, argv).options(cmdline_options).run(), vm);
    po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
    notify(vm);
    if(vm.count("help")) {
      cout << generic;
      return 0;
    }

    po::options_description config("Config file options");
    config.add_options()
      ("camera_params,p", po::value<std::string>(&cam_params_file)->default_value("camera_params.txt"), "camera params file")
      ("source_folder,s", po::value<std::string>(&source_folder), "folder with source")
      ("imglist,l", po::value<std::string>(&imagelistfn), "file with image list")
      ("groundtruth,g", po::value<std::string>(&gt_filepath), "file with motion GT");

    std::ifstream ifs(config_file);
    if(!ifs)
      throw "can not open config file: " + config_file + "\n";
    else {
      po::store(parse_config_file(ifs, config, true), vm);
      notify(vm);
    }
  }
  catch(std::exception& e) {
    cout << e.what() << "\n";
    return -1;
  }

  std::cout << "Using track config = " << config_file << '\n';
  run_visual_odometry(source_folder, imagelistfn, experiment_config, cam_params_file, gt_filepath);

  return 0;
}


//   Copyright 2015 Patrick Putnam
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
#ifndef CLOTHO_RANDOM_SAMPLE_GENERATOR_HPP_
#define CLOTHO_RANDOM_SAMPLE_GENERATOR_HPP_

#include <vector>
#include <boost/dynamic_bitset.hpp>
#include <boost/random/uniform_int_distribution.hpp>

namespace clotho {
namespace genetics {

/*
template < class RNG, class GeneticSpaceType >
class random_sample_generator {
public:
    typedef RNG                                 random_engine_type;
    typedef GeneticSpaceType                    genetic_space_type;
    typedef std::vector< size_t >                  sample_set_type;

    typedef typename sample_set_type::iterator sample_iterator;

    random_sample_generator( random_engine_type * rng, genetic_space_type * gs, size_t samp_size, bool with_replacement = false ) :
        m_rand(rng)
        , m_pop( gs )
    {
        generate( gs->sequence_count(), samp_size, with_replacement );
    }

    sample_iterator begin() {
        return m_sample.begin();
    }

    sample_iterator end() {
        return m_sample.end();
    }

    genetic_space_type * getPopulation() {
        return m_pop;
    }

    size_t getSampleAt( size_t idx ) {
        assert( 0 <= idx && idx < m_sample.size() );

        return m_sample[idx];
    }

    virtual ~random_sample_generator() {}
protected:
    void generate(size_t pop_size, size_t target_size, bool with_replacement ) {
        boost::random::uniform_int_distribution< size_t > dist( 0, pop_size - 1 );
        m_sample.reserve( target_size );

        if( with_replacement ) {
            while( m_sample.size() < target_size ) {
                size_t t = dist( *m_rand );
                m_sample.push_back( t );
            }
        } else if( target_size >= pop_size ) { // all samples should be unique
            size_t i = 0;
            while( i < target_size ) {
                m_sample.push_back( i++ );
            }
        } else {
            //std::vector< bool > indices( m_pop->sequence_count(), false );
            boost::dynamic_bitset< >    indices( m_pop->sequence_count(), false );

            while( m_sample.size() < target_size ) {
                size_t t = dist( *m_rand );
                while( indices[ t ] ) {
                    t = dist( *m_rand );
                }

                indices[ t ] = true;
                m_sample.push_back( t );
            }
        }
    }

    random_engine_type  * m_rand;
    genetic_space_type  * m_pop;
    sample_set_type     m_sample;
};
*/

template < class RNG >
class random_sample_generator {
public:
    typedef RNG                                 random_engine_type;
    typedef std::vector< size_t >                  sample_set_type;

    typedef typename sample_set_type::iterator iterator;
    typedef typename sample_set_type::const_iterator const_iterator;

    random_sample_generator( random_engine_type * rng, size_t space_size , size_t samp_size, bool with_replacement = false ) {
        generate( rng, space_size , samp_size, with_replacement );
    }

    iterator begin() {
        return m_sample.begin();
    }

    iterator end() {
        return m_sample.end();
    }

    const_iterator begin() const {
        return m_sample.begin();
    }

    const_iterator end() const {
        return m_sample.end();
    }

    size_t getSampleAt( size_t idx ) {
        assert( 0 <= idx && idx < m_sample.size() );

        return m_sample[idx];
    }

    virtual ~random_sample_generator() {}
protected:
    void generate( random_engine_type * rng, size_t pop_size, size_t target_size, bool with_replacement ) {
        boost::random::uniform_int_distribution< size_t > dist( 0, pop_size - 1 );
        m_sample.reserve( target_size );

        if( with_replacement ) {
            while( m_sample.size() < target_size ) {
                size_t t = dist( *rng );
                m_sample.push_back( t );
            }
        } else if( target_size >= pop_size ) { // all samples should be unique
            size_t i = 0;
            while( i < target_size ) {
                m_sample.push_back( i++ );
            }
        } else {
            boost::dynamic_bitset< unsigned int >    indices( pop_size, false );

            while( m_sample.size() < target_size ) {
                size_t t = dist( *rng );
                while( indices[ t ] ) {
                    t = dist( *rng );
                }

                indices[ t ] = true;
                m_sample.push_back( t );
            }
        }
    }

    sample_set_type     m_sample;
};
}   // namespace genetics
}   // namespace clotho

#endif  // CLOTHO_RAtarget_sizeDOM_SAMPLE_GEtarget_sizeERATOR_HPP_


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2013 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef DIRECT_HH
#define DIRECT_HH

#include <cmath>
#include <memory>
#include <type_traits>

#include <boost/timer/timer.hpp>

#include "dune/istl/operators.hh"
#include <dune/istl/solvers.hh>

#include "fem/istlinterface.hh"
#include "linalg/factorization.hh"

namespace Kaskade
{
  /// \internal
  namespace DirectSolver_Detail
  {
    class NoScalar{};
    class NoFieldType{};

    template <class T>
    typename T::Scalar hasScalar(typename T::Scalar*);
    template <class T>
    NoScalar hasScalar(...);

    template <class T>
    typename T::field_type hasFieldType(typename T::field_type*);
    template <class T>
    NoFieldType hasFieldType(...);

    template <class T>
    struct HasScalar
    {
      typedef decltype(hasScalar<T>(nullptr)) type;
      static constexpr bool value = !std::is_same<type,NoScalar>::value;
    };

    template <class T>
    struct HasFieldType
    {
      typedef decltype(hasFieldType<T>(nullptr)) type;
      static constexpr bool value = !std::is_same<type,NoFieldType>::value;
    };

    template <class T>
    struct ScalarType
    {
      typedef typename std::conditional< HasScalar<T>::value, typename HasScalar<T>::type,
                                           typename std::conditional< HasFieldType<T>::value, typename HasFieldType<T>::type, void>::type >::type type;

    };
  }
  /// \endinternal

  /**
   * \ingroup direct
   * \brief Dune::InverseOperator and Dune::Preconditioner interface for direct solvers.
   *
   * This keeps a factorization during the lifetime of the object, however, due to
   * shared data, efficient copying is possible.
   */
  template <class Domain_, class Range_>
  class DirectSolver: public Dune::InverseOperator<Domain_,Range_>,
		      public Dune::Preconditioner<Domain_,Range_>
  {
  public:
    typedef typename DirectSolver_Detail::ScalarType<Domain_>::type Scalar;
    typedef Domain_ Domain;
    typedef Range_ Range;

    /**
     * \brief Default constructor.
     *
     * A default constructed DirectSolver implements a (pretty useless) zero operator.
     */
    DirectSolver() {}

    /**
     * \brief Constructs a direct solver from an assembled linear operator.
     *
     * A copy of the linear operator is held internally, thus the linear
     * operator object is better not too large.
     */
    template <class AssembledGOP>
    explicit DirectSolver(AssembledGOP const& A,
                          DirectType directType=DirectType::UMFPACK, MatrixProperties properties=MatrixProperties::GENERAL):
        solver(getFactorization(directType,properties,A.template get<MatrixAsTriplet<typename AssembledGOP::Scalar> >()))
    {
      assert(solver.get()); // make sure the factorization has been successful - otherwise it should have raised an exception
    }

    /**
     * \brief Constructs a direct solver from a triplet matrix.
     */
    template <class FieldType>
    explicit DirectSolver(MatrixAsTriplet<FieldType> const& A,
                          DirectType directType=DirectType::UMFPACK, MatrixProperties properties=MatrixProperties::GENERAL):
        solver(getFactorization(directType,properties,A))
    {
      assert(solver.get()); // make sure the factorization has been successful - otherwise it should have raised an exception
    }

    /**
     * \brief Constructs a direct solver from a BCRS matrix.
     * A copy of the linear operator is held internally, thus the linear
     * operator object is better not too large.
     */
    template <class FieldType>
    explicit DirectSolver(Dune::BCRSMatrix<Dune::FieldMatrix<FieldType,1,1>> const& A,
                          DirectType directType=DirectType::UMFPACK, MatrixProperties properties=MatrixProperties::GENERAL):
        solver(getFactorization<FieldType,int>(directType,properties,A))
    {
      assert(solver.get()); // make sure the factorization has been successful - otherwise it should have raised an exception
    }

    /**
     * \brief Constructs a direct solver from a NumaBCRS matrix.
     * A copy of the linear operator is held internally, thus the linear
     * operator object is better not too large.
     */
    template <class FieldType, int n, int m, class Index>
    explicit DirectSolver(NumaBCRSMatrix<Dune::FieldMatrix<FieldType,n,m>,Index> const& A,
                          DirectType directType=DirectType::UMFPACK, MatrixProperties properties=MatrixProperties::GENERAL):
        solver(getFactorization<FieldType,n,m,Index,int>(directType,properties,A))
    {
      assert(solver.get()); // make sure the factorization has been successful - otherwise it should have raised an exception
    }

    /**
     * \brief Solves the system for the given right hand side \arg b.
     *
     * The provided right hand side \arg b is not modified (except possibly via aliasing
     * \arg x).
     */
    virtual void apply(Domain& x, Range& b, Dune::InverseOperatorResult& res) { apply(x,b,1e-10,res); }

    /**
     * \brief Solves the system for the given right hand side \arg b.
     *
     * As an extension to the Dune::InverseOperator interface, we provide const versions of
     * the apply methods.
     */
    void apply(Domain& x, Range& b, Dune::InverseOperatorResult& res) const { apply(x,b,1e-10,res); }

    /**
     * Solves the system for the given right hand side \arg b, which is
     * guaranteed not to be overwritten (except possibly via aliasing
     * \arg x).
     */
    virtual void apply(Domain& x, Range& b, double reduction, Dune::InverseOperatorResult& res)
    {
      const_cast<DirectSolver<Domain,Range> const*>(this)->apply(x,b,res);
    }

    /**
     * \brief Solves the system for the given right hand side \arg b.
     *
     * As an addition to the Dune::InverseOperator interface, we provide const versions of
     * the apply methods, since the factorization is not modified during the solution phase.
     */
    void apply(Domain& x, Range const& b, double reduction, Dune::InverseOperatorResult& res) const
    {
      boost::timer::cpu_timer timer;

      std::vector<Scalar> rhs(b.dim());
      vectorToSequence(b,begin(rhs));
      apply(rhs,reduction,res);
      vectorFromSequence(x,begin(rhs));
    }

    /**
     * \brief The input vector v contains the rhs. It will be overwritten by the solution.
     *
     * Factorization is not touched by solving. As an addition to the
     * Dune::InverseOperator interface, we provide const versions of
     * the apply methods.
     */
    void apply(std::vector<Scalar> &v, double reduction, Dune::InverseOperatorResult& res) const
    {
      boost::timer::cpu_timer timer;

      // Check for solver availability. Otherwise return zero solution.
      if (solver)
      {
        #ifndef NDEBUG
        size_t rhsNan = 0, solNan = 0;
        size_t rhsInf = 0;
        for (auto const& x: v)
        {
          if (std::isnan(x)) ++rhsNan;
          if (std::isinf(x)) ++rhsInf;
        }
        if (rhsNan>0) std::cerr << __FILE__ << ':' << __LINE__ << ": " << rhsNan << " rhs entries are nan\n";
        if (rhsInf>0) std::cerr << __FILE__ << ':' << __LINE__ << ": " << rhsInf << " rhs entries are inf\n";
        #endif
        solver->solve(v);
        #ifndef NDEBUG
        for (int i=0; i<v.size(); ++i)
          if (std::isnan(v[i])) ++solNan;
          if (solNan>0) std::cerr << __FILE__ << ':' << __LINE__ << ": " << solNan << " solution entries are nan\n";
          #endif
      } else
        std::fill(v.begin(),v.end(),0.0);

      // Write solver statistics. Currently dummy.
      res.clear();
      res.iterations = 1;
      res.reduction = 1e-10; // dummy!
      res.converged = true;
      res.conv_rate = 1e-10;
      res.elapsed = (double)(timer.elapsed().user)/1e9;
    }

    virtual void  apply (std::vector<Scalar> &v)
    {
      Dune::InverseOperatorResult dummy_res;
      apply(v,1e-10,dummy_res);
    }

    virtual void pre (Domain &x, Range &b) {}

    virtual void apply (Domain &v, const Range &d)
    {
      Dune::InverseOperatorResult dummy_res;
      apply(v,d, 1e-10,dummy_res);
    }

    virtual void post (Domain &x) {}

  private:
    std::shared_ptr<Factorization<Scalar>> solver;
  };

  //---------------------------------------------------------------------

  // partial specialization for tiny fixed-size matrices
  template <class S, int n>
  class DirectSolver<Dune::FieldVector<S,n>,Dune::FieldVector<S,n>>: public Dune::InverseOperator<Dune::FieldVector<S,n>,Dune::FieldVector<S,n>>,
                                                                     public Dune::Preconditioner<Dune::FieldVector<S,n>,Dune::FieldVector<S,n>>
  {
  public:
    typedef S Scalar;
    typedef Dune::FieldVector<S,n> Domain;
    typedef Dune::FieldVector<S,n> Range;

    /**
     * \brief Default constructor.
     *
     * A default constructed DirectSolver implements a (pretty useless) zero operator.
     */
    DirectSolver() {}

    /**
     * \brief Constructs a direct solver from a FieldMatrix matrix.
     */
    template <class FieldType>
    explicit DirectSolver(Dune::FieldMatrix<FieldType,n,n> const& A):
        inverse(A)
    {
      inverse.invert();
    }

    /**
     * \brief Solves the system for the given right hand side \arg b.
     *
     * The provided right hand side \arg b is not modified (except possibly via aliasing
     * \arg x).
     */
    virtual void apply(Domain& x, Range& b, Dune::InverseOperatorResult& res) { apply(x,b,1e-10,res); }

    /**
     * \brief Solves the system for the given right hand side \arg b.
     *
     * As an extension to the Dune::InverseOperator interface, we provide const versions of
     * the apply methods.
     */
    void apply(Domain& x, Range& b, Dune::InverseOperatorResult& res) const { apply(x,b,1e-10,res); }

    /**
     * Solves the system for the given right hand side \arg b, which is
     * guaranteed not to be overwritten (except possibly via aliasing
     * \arg x).
     */
    virtual void apply(Domain& x, Range& b, double reduction, Dune::InverseOperatorResult& res)
    {
      const_cast<DirectSolver<Domain,Range> const*>(this)->apply(x,b,res);
    }

    /**
     * \brief Solves the system for the given right hand side \arg b.
     *
     * As an addition to the Dune::InverseOperator interface, we provide const versions of
     * the apply methods, since the factorization is not modified during the solution phase.
     */
    void apply(Domain& x, Range const& b, double reduction, Dune::InverseOperatorResult& res) const
    {
      x = inverse*b;

      // Write solver statistics. Currently dummy.
      res.clear();
      res.iterations = 1;
      res.reduction = 1e-10; // dummy!
      res.converged = true;
      res.conv_rate = 1e-10;
      res.elapsed = 0;
    }

    virtual void        pre (Domain &x, Range &b) {}

    virtual void        apply (Domain &v, const Range &d)
    {
      v = inverse*d;
    }

    virtual void        post (Domain &x) {}

  private:
    Dune::FieldMatrix<Scalar,n,n> inverse;
  };

  //---------------------------------------------------------------------

  /**
   * \ingroup linalgsolution
   * \brief Dune::LinearOperator interface for inverse operators.
   */
  template <class InverseOperator>
  class InverseLinearOperator: public Dune::LinearOperator<typename InverseOperator::Range, typename InverseOperator::Domain>
  {
  public:
    typedef typename InverseOperator::Domain Domain;
    typedef typename InverseOperator::Range Range;
    typedef typename InverseOperator::Scalar Scalar;

    InverseLinearOperator() = default;

    InverseLinearOperator(InverseOperator const& op_) : op(op_)
    {}

    virtual void apply(Domain const& x, Range& y) const
    {
      Dune::InverseOperatorResult result;
      Domain rhs(x);
      op.apply(y,rhs,result);
    }

    virtual void applyscaleadd(Scalar alpha, Domain const& x, Range& y) const
    {
      Range ynew(y);
      apply(x,ynew);
      y.axpy(alpha,ynew);
    }

  private:
    InverseOperator op;
  };

  //---------------------------------------------------------------------

  /**
   * \ingroup direct
   * \brief convenience function for constructing a DirectInverseOperator
   */
  template <class GOP, int firstRow, int lastRow, int firstCol, int lastCol>
  InverseLinearOperator<DirectSolver<typename AssembledGalerkinOperator<GOP,firstRow,lastRow,firstCol,lastCol>::Domain,
                                     typename AssembledGalerkinOperator<GOP,firstRow,lastRow,firstCol,lastCol>::Range> >
  directInverseOperator(AssembledGalerkinOperator<GOP,firstRow,lastRow,firstCol,lastCol> const& A,
                        DirectType directType=DirectType::UMFPACK, MatrixProperties properties=MatrixProperties::GENERAL)
  {
    typedef typename AssembledGalerkinOperator<GOP,firstRow,lastRow,firstCol,lastCol>::Domain Domain;
    typedef typename AssembledGalerkinOperator<GOP,firstRow,lastRow,firstCol,lastCol>::Range Range;
    return InverseLinearOperator<DirectSolver<Domain,Range> >(DirectSolver<Domain,Range>(A,directType,properties));
  }

  template <class Matrix, class Domain, class Range>
  InverseLinearOperator<DirectSolver<Domain,Range> >
  directInverseOperator(MatrixRepresentedOperator<Matrix,Domain,Range> const& A,
                        DirectType directType=DirectType::UMFPACK, MatrixProperties properties=MatrixProperties::GENERAL)
  {
    return InverseLinearOperator<DirectSolver<Domain,Range> >(DirectSolver<Domain,Range>(A,directType,properties));
  }

} // namespace Kaskade

//---------------------------------------------------------------------

#endif


#ifndef CMAESAG_HPP
#define CMAESAG_HPP

#include <vector>
#include <string>
#include <type_traits>
#include <boost/serialization/list.hpp>
#include <boost/serialization/set.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/functional/hash.hpp>

#include "arch/ARLAgent.hpp"
#include "bib/Seed.hpp"
#include "bib/Utils.hpp"
#include <bib/MetropolisHasting.hpp>
#include <bib/XMLEngine.hpp>
#include <bib/Combinaison.hpp>
#include "nn/MLP.hpp"
#include "nn/DevMLP.hpp"
#include "nn/DODevMLP.hpp"
#include "cmaes_interface.h"

template<typename NN = MLP>
class CMAESAg : public arch::ARLAgent<arch::AgentProgOptions> {
 public:
  CMAESAg(unsigned int _nb_motors, unsigned int _nb_sensors)
    : arch::ARLAgent<arch::AgentProgOptions>(_nb_motors, _nb_sensors), nb_sensors(_nb_sensors) {

  }

  virtual ~CMAESAg() {
    cmaes_exit(evo);
    delete evo;
    delete ann;
    delete hidden_unit_a;
  }

  const std::vector<double>& _run(double, const std::vector<double>& sensors,
                                  bool, bool, bool) override {

    vector<double>* next_action = ann->computeOut(sensors);

//  CMA-ES already implement exploration in parameter space
    last_action.reset(next_action);

    return *next_action;
  }

  void _unique_invoke(boost::property_tree::ptree* pt, boost::program_options::variables_map* vm) override {
    hidden_unit_a               = bib::to_array<uint>(pt->get<std::string>("agent.hidden_unit_a"));
    actor_hidden_layer_type     = pt->get<uint>("agent.actor_hidden_layer_type");
    actor_output_layer_type     = pt->get<uint>("agent.actor_output_layer_type");
    batch_norm                  = pt->get<uint>("agent.batch_norm");
    population                  = pt->get<uint>("agent.population");
    initial_deviation           = pt->get<double>("agent.initial_deviation");

    check_feasible = true;
    ignore_null_lr = true;
    racing = false;
    error_count = 0;
    xbestever_score = std::numeric_limits<double>::max();

    try {
      check_feasible = pt->get<bool>("agent.check_feasible");
    } catch(boost::exception const& ) {
    }

    try {
      ignore_null_lr = pt->get<bool>("agent.ignore_null_lr");
    } catch(boost::exception const& ) {
    }

    try {
      racing = pt->get<bool>("agent.racing");
    } catch(boost::exception const& ) {
    }

    episode = 0;

    ann = new NN(nb_sensors, *hidden_unit_a, nb_motors, 0.1, 1, actor_hidden_layer_type, actor_output_layer_type,
                 batch_norm);
    if(std::is_same<NN, DevMLP>::value)
      ann->exploit(pt, static_cast<CMAESAg *>(old_ag)->ann);
    else if(std::is_same<NN, DODevMLP>::value)
      ann->exploit(pt, nullptr);

//     const uint dimension = (nb_sensors+1)*hidden_unit_a->at(0) + (hidden_unit_a->at(0)+1)*nb_motors;
    const uint dimension = ann->number_of_parameters(ignore_null_lr);
    double* startx  = new double[dimension];
    double* deviation  = new double[dimension];
    for(uint j=0; j< dimension; j++) {
      deviation[j] = initial_deviation;
    }
    ann->copyWeightsTo(startx, ignore_null_lr);

    evo = new cmaes_t;
    arFunvals = cmaes_init(evo, dimension, startx, deviation, 0, population, NULL/*"config.cmaes.ini"*/);
    delete[] startx;
    delete[] deviation;
//     evo->sp.stopTolFun = 1e-150;
//     evo->sp.stopTolFunHist = 1e-150;
//     evo->sp.stopTolUpXFactor = 1e50;

    printf("%s\n", cmaes_SayHello(evo));
    new_population();
    LOG_DEBUG(cmaes_Get(evo, "lambda") << " " << dimension << " " << population << " " << cmaes_Get(evo, "N"));
    if (population < 2)
      LOG_DEBUG("population too small, changed to : " << (4+(int)(3*log((double)dimension))));

    if(vm->count("continue") > 0) {
      uint continue_save_each          = DEFAULT_AGENT_SAVE_EACH_CONTINUE;
      try {
        continue_save_each            = pt->get<uint>("simulation.continue_save_each");
      } catch(boost::exception const& ) {
      }

      if(continue_save_each % (int) cmaes_Get(evo, "lambda") != 0) {
        LOG_ERROR("continue_save_each must be a multiple of the population size !");
        exit(1);
      }
    }

    if(std::is_same<NN, DODevMLP>::value){
      try {
        if(pt->get<double>("devnn.ewc") >= 0.){
          LOG_ERROR("CMA-ES doesn't rely on gradient (difficult to compute Fisher Matrix)");
          exit(1);
        }
      } catch(boost::exception const& ) {
      }
    }
  }

  bool is_feasible(const double* parameters) {
    for(uint i=0; i < (uint) cmaes_Get(evo, "dim"); i++)
      if(fabs(parameters[i]) >= 500.f) {
        return false;
      }

    return true;
  }

  void new_population() {
    const char * terminate =  cmaes_TestForTermination(evo);
    if(terminate) {
      LOG_INFO("mismatch "<< terminate);
      error_count++;

      if(error_count > 20 && racing) {
        LOG_FILE(DEFAULT_END_FILE, "-1");
        exit(0);
      }
    }
    //ASSERT(!cmaes_TestForTermination(evo), "mismatch "<< cmaes_TestForTermination(evo));

    current_individual = 0;
    pop = cmaes_SamplePopulation(evo);

    if(check_feasible) {
      //check that the population is feasible
      bool allfeasible = true;
      for (int i = 0; i < cmaes_Get(evo, "popsize"); ++i)
        while (!is_feasible(pop[i])) {
          cmaes_ReSampleSingle(evo, i);
          allfeasible = false;
        }

      if(!allfeasible)
        LOG_INFO("non feasible solution produced");
    }
  }

  void start_instance(bool learning) override {
    last_action = nullptr;
    scores.clear();

    if(std::is_same<NN, DODevMLP>::value && learning) {
      auto dodevmlp = static_cast<DODevMLP *>(ann);
      bool reset_operator, changed;
      std::tie(reset_operator, changed) = dodevmlp->inform(episode, last_sum_weighted_reward);
      if(reset_operator && changed) {
        LOG_INFO("reset learning catched");

        if(!dodevmlp->ewc_enabled()){
          const double* parameters = nullptr;
          parameters = getBestSolution();
          loadPolicyParameters(parameters);
        }

        cmaes_exit(evo);
        delete evo;

        const uint dimension = ann->number_of_parameters(ignore_null_lr);
        double* startx  = new double[dimension];
        double* deviation  = new double[dimension];
        for(uint j=0; j< dimension; j++) {
          deviation[j] = initial_deviation;
        }
        ann->copyWeightsTo(startx, ignore_null_lr);

        xbestever_score = std::numeric_limits<double>::max();
        cmaes_UpdateDistribution_done_once = false;
        evo = new cmaes_t;
        arFunvals = cmaes_init(evo, dimension, startx, deviation, 0, population, NULL/*"config.cmaes.ini"*/);
        delete[] startx;
        delete[] deviation;
        new_population();
      }
    }

    if(!justLoaded) {
      //put individual into NN
      const double* parameters = nullptr;
      if(learning || !cmaes_UpdateDistribution_done_once)
        parameters = pop[current_individual];
      else
        parameters = getBestSolution();

      loadPolicyParameters(parameters);
    }

    if(learning)
      episode++;
    //LOG_FILE("policy_exploration", ann->hash());
  }

  void restoreBest() override {
    const double* parameters = getBestSolution();
    loadPolicyParameters(parameters);
  }

  void end_episode(bool) override {
    scores.push_back(-sum_weighted_reward + ann->ewc_cost());
  }

  void end_instance(bool learning) override {
    if(learning) {
      arFunvals[current_individual] = std::accumulate(scores.begin(), scores.end(), 0.f) / scores.size();

      //TODO with instance
      ann->update_best_param_previous_task(sum_weighted_reward);

      current_individual++;
      if(current_individual >= cmaes_Get(evo, "lambda")) {
        cmaes_UpdateDistribution(evo, arFunvals);
        cmaes_UpdateDistribution_done_once=true;
        new_population();
      }
    }

    justLoaded = false;
  }

  void save(const std::string& path, bool save_best, bool) override {
    if(!save_best || !cmaes_UpdateDistribution_done_once) {
      ann->save(path+".actor");
    } else if(save_best && -sum_weighted_reward < xbestever_score ) {
      xbestever_score = -sum_weighted_reward;
      ann->save(path+".actor");
    }
//     else {
//       //TODO : check this part, apparently it modify the learning
//       LOG_WARNING("be careful it might be a problem here");
//       NN* to_be_restaured = new NN(*ann, false);
//       const double* parameters = getBestSolution();
//       loadPolicyParameters(parameters);
//       ann->save(path+".actor");
//       delete ann;
//       ann = to_be_restaured;
//     }
  }

  void load(const std::string& path) override {
    justLoaded = true;
    ann->load(path+".actor");
  }

  void save_run() override {
    if(current_individual == 1) {
      const double* xbptr_ = getBestSolution();
      std::vector<double> xbestever_((int)cmaes_Get(evo, "N"));
      xbestever_.assign(xbptr_, xbptr_ + (int)cmaes_Get(evo, "N"));
      double bs = getBestScore();

      struct algo_state st = {scores, justLoaded, cmaes_UpdateDistribution_done_once,
               current_individual, episode, error_count, bs, xbestever_
      };
      bib::XMLEngine::save(st, "algo_state", "continue.algo_state.data");
      cmaes_WriteToFile(evo, "resume", "continue.cmaes.data");
    }
  }

  void load_previous_run() override {
    auto algo_state_ = bib::XMLEngine::load<struct algo_state>("algo_state", "continue.algo_state.data");
    scores = algo_state_->scores;
    justLoaded = algo_state_->justLoaded;
    cmaes_UpdateDistribution_done_once = algo_state_->cmaes_UpdateDistribution_done_once;
    current_individual = algo_state_->current_individual;
    episode = algo_state_->episode;
    error_count = algo_state_->error_count;
    xbestever_score = algo_state_->xbestever_score;
    xbestever_ptr = algo_state_->xbestever_ptr;
    delete algo_state_;
    char file_[] = "continue.cmaes.data";
    cmaes_resume_distribution(evo, file_);
    new_population();
  }

  MLP* getNN() {
    return ann;
  }

 protected:
  void _display(std::ostream& out) const override {
    out << std::setw(8) << std::fixed << std::setprecision(5) << sum_weighted_reward << " " << ann->ewc_cost();
  }

  void _dump(std::ostream& out) const override {
    out << std::setw(8) << std::fixed << std::setprecision(5) << sum_weighted_reward;
  }

 private:
  void loadPolicyParameters(const double* parameters) {
    ann->copyWeightsFrom(parameters, ignore_null_lr);
  }

  const double* getBestSolution() {
    //TODO:
//     LOG_DEBUG(cmaes_Get(evo, "fbestever")  << " " << xbestever_score);
    if(cmaes_Get(evo, "fbestever") < xbestever_score)
      return cmaes_GetPtr(evo, "xbestever");
    else
      return xbestever_ptr.data();
  }

  double getBestScore() {
    return std::min(cmaes_Get(evo, "fbestever"), xbestever_score);
  }

 private:
  //initilized by constructor
  uint nb_sensors;

  //initialized by invoke
  std::vector<uint>* hidden_unit_a;
  uint population, actor_hidden_layer_type, actor_output_layer_type, batch_norm;
  double initial_deviation;
  bool check_feasible;
  bool ignore_null_lr;
  bool racing;
  MLP* ann;
  cmaes_t* evo;
  double *arFunvals;

  //internal mecanisms
  std::shared_ptr<std::vector<double>> last_action;
  std::list<double> scores;
  double *const *pop;
  bool justLoaded = false;
  bool cmaes_UpdateDistribution_done_once = false;
  uint current_individual;
  uint episode;
  uint error_count;
  double xbestever_score;
  std::vector<double> xbestever_ptr;

  struct algo_state {
    std::list<double> scores;
    bool justLoaded;
    bool cmaes_UpdateDistribution_done_once;
    uint current_individual;
    uint episode;
    uint error_count;
    double xbestever_score;
    std::vector<double> xbestever_ptr;

    friend class boost::serialization::access;
    template <typename Archive>
    void serialize(Archive& ar, const unsigned int) {
      ar& BOOST_SERIALIZATION_NVP(scores);
      ar& BOOST_SERIALIZATION_NVP(justLoaded);
      ar& BOOST_SERIALIZATION_NVP(cmaes_UpdateDistribution_done_once);
      ar& BOOST_SERIALIZATION_NVP(current_individual);
      ar& BOOST_SERIALIZATION_NVP(episode);
      ar& BOOST_SERIALIZATION_NVP(error_count);
      ar& BOOST_SERIALIZATION_NVP(xbestever_score);
      ar& BOOST_SERIALIZATION_NVP(xbestever_ptr);
    }
  };
};

#endif


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

#include <boost/simd/arithmetic/functions/correct_fma.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/split.hpp>
#include <boost/simd/include/functions/simd/group.hpp>
#include <boost/simd/include/functions/simd/two_prod.hpp>
#include <boost/simd/include/functions/simd/two_add.hpp>
#include <boost/simd/sdk/meta/is_upgradable.hpp>
#include <boost/dispatch/meta/upgrade.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/attributes.hpp>

#ifndef BOOST_SIMD_DONT_CARE_FMA_OVERFLOW
#include <boost/simd/include/functions/simd/ldexp.hpp>
#include <boost/simd/include/functions/simd/exponent.hpp>
#include <boost/simd/include/functions/simd/maxmag.hpp>
#include <boost/simd/include/functions/simd/minmag.hpp>
#include <boost/simd/include/functions/simd/shift_right.hpp>
#include <boost/simd/include/functions/simd/unary_minus.hpp>
#endif

namespace boost { namespace simd { namespace ext
{

  BOOST_DISPATCH_IMPLEMENT_IF          ( correct_fma_, tag::cpu_
                                       , (A0)(X)
                                       , ( simd::meta::is_upgradable_on_ext<A0,X> )
                                       , ((simd_<single_<A0>,X>))
                                         ((simd_<single_<A0>,X>))
                                         ((simd_<single_<A0>,X>))
                                       )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(3)
    {
      typedef typename dispatch::meta::upgrade<A0>::type ivtype;
      ivtype a0l, a0h, a1l, a1h, a2l, a2h;
      split(a0, a0l, a0h);
      split(a1, a1l, a1h);
      split(a2, a2l, a2h);
      return group(a0l*a1l+a2l, a0h*a1h+a2h);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( correct_fma_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<floating_<A0>,X>))
                                      ((simd_<floating_<A0>,X>))
                                      ((simd_<floating_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(3)
    {
      result_type p, rp, s, rs;
#ifndef BOOST_SIMD_DONT_CARE_FMA_OVERFLOW
        typedef typename boost::dispatch::meta::as_integer<A0>::type iA0;
        result_type amax =  maxmag(a0, a1);
        result_type amin =  minmag(a0, a1);
        iA0 e0 = -shift_right(exponent(amax), 1);
        amax = ldexp(amax, e0);
        result_type a02 = ldexp(a2, e0);
        two_prod(amax, amin, p, rp);
        two_add(p, a02, s, rs);
        return ldexp(s+(rp+rs), -e0);
#else
        two_prod(a0, a1, p, rp);
        two_add(p, a2, s, rs);
        return s+(rp+rs);
#endif

    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( correct_fma_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_< int_<A0>,X >))
                                      ((simd_< int_<A0>,X >))
                                      ((simd_< int_<A0>,X >))
                                    )
  {
    typedef A0 result_type;
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type utype;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(3)
    {
      // correct fma has to ensure "no intermediate overflow".
      // This is done in the case of signed integers by transtyping to unsigned type
      // to perform the computations in a guaranteed 2-complement environment
      // since signed integer oveflow in C++ produces "undefined results"
      return bitwise_cast<A0>(correct_fma(bitwise_cast<utype>(a0), bitwise_cast<utype>(a1), bitwise_cast<utype>(a2)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( correct_fma_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_< uint_<A0>,X >))
                                      ((simd_< uint_<A0>,X >))
                                      ((simd_< uint_<A0>,X >))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(3)
    {
      return a0*a1+a2;
    }
  };
} } }

#endif


//
//  Simulator.hpp
//  proteintools
//
//  Created by Salik Syed on 10/5/17.
//  Copyright © 2017 N/A. All rights reserved.
//

#ifndef Simulator_hpp
#define Simulator_hpp

#include <stdio.h>
#include "Forcefield.hpp"
#include "Chain.hpp"
#include "Conformation.hpp"
#include "EnergyEvaluator.hpp"
#include <vector>
#include <Eigen/Core>
#include <assert.h>
using namespace std;

struct AtomInfo {
    AtomType atom;
    ResidueType residue;
    bool geometryValid;
    bool isBackboneParent;
    bool isBackboneChild;
    Eigen::Vector4f position;
};

class Simulator {
public:
    Simulator(Chain& chain, ForceField& forcefield);
    ~Simulator();
    float getEnergy() const;
    void setConformation(Conformation& conformation, bool forceUpdate=false);
    Conformation getConformation() const;
    void getAtoms(vector<AtomInfo> & atoms) const;
    void getBonds(vector<pair<int, int>> & bonds) const;
protected:
    Eigen::Matrix4Xf* _atoms;
    Eigen::Matrix4Xf* _atomsTransformed;
    Eigen::Matrix4Xf* _atomParams;
    size_t _numAtoms;
    EnergyEvaluator* _evaluator;
    vector<const Residue*> _residues;
    vector<Eigen::Matrix4f> _matrixStack;
    vector<Eigen::Matrix4f> _translationTransforms;
    Conformation* _conformation;

};

#endif /* Simulator_hpp */


//////////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::location_scale::description.hpp             //
//                                                                                //
//  (C) Copyright 2009 Erwann Rogard                                                //
//  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)                //
//////////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_LOCATION_SCALE_DESCRIPTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_LOCATION_SCALE_DESCRIPTION_HPP_ER_2009
#include <string>
#include <boost/format.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/location_scale.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace toolkit{

    template<typename Z>
    std::string
    description(
        const location_scale_distribution<Z>& d
    )
    {
        const char* msg = "location_scale_distribution( %1% + %2% %3%)";
        format f(msg); f % d.mu() % d.sigma() % description(d.z());
        return f.str();
    }

}// distribution
}// toolkit

}// detail
}// statistics
}// boost

#endif


// Copyright (C) 2006 Douglas Gregor <doug.gregor@gmail.com>

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

// An example using Boost.MPI's gather(): [main]

#include <boost/mpi.hpp>
#include <boost/mpi/collectives.hpp>
#include <iostream>
#include <cstdlib>
#include <vector>

namespace mpi = boost::mpi;

int main(int argc, char* argv[])
{
  mpi::environment env(argc, argv);
  mpi::communicator world;

  std::srand(time(0) + world.rank());
  std::vector<int> all;
  int mine = -1;
  if (world.rank() == 0) {
    all.resize(world.size());
    std::generate(all.begin(), all.end(), std::rand);
  }
  mpi::scatter(world, all, mine, 0);
  for (int r = 0; r < world.size(); ++r) {
    world.barrier();
    if (r == world.rank()) {
      std::cout << "Rank " << r << " got " << mine << '\n';
    }
  }
  return 0;
}


// Copyright 2014, Max Planck Society.
// Distributed under the BSD 3-Clause license.
// (See accompanying file LICENSE.txt or copy at
// http://opensource.org/licenses/BSD-3-Clause)

#ifndef GRASSMANN_AVERAGES_PCA_TEST_MAIN_HPP__
#define GRASSMANN_AVERAGES_PCA_TEST_MAIN_HPP__

#include <boost/random/mersenne_twister.hpp>
#include <boost/numeric/ublas/matrix.hpp>
// generating data randomly
#include <boost/random/uniform_real_distribution.hpp>

#include <fstream>

#include <include/private/utilities.hpp>

// random number generator
extern boost::random::mt19937 rng;

//#define FLUSH_MATRIX_TO_FILE
const int DATA_DIMENSION=5;

// Fixture for the tests
struct fixture_simple_matrix_creation
{
  typedef boost::numeric::ublas::matrix<double> matrix_t;

  static const int nb_elements;// = 1000;
  static const int dimensions;// = 5;
  matrix_t mat_data;

  boost::random::uniform_real_distribution<double> dist;

  fixture_simple_matrix_creation() : dist(-1000, 1000)
  {
    // creating some data, 1000 lines of a vector of length 5
    mat_data.resize(nb_elements, dimensions);

    // default seed for reproductible sequences
    rng.seed();

    //std::cout << "current seed : " << ;
#ifdef FLUSH_MATRIX_TO_FILE
    const std::string filename = "./toto.txt";
    std::ofstream ff(filename.c_str());

    BOOST_REQUIRE(ff.is_open());
#endif

    for(int i = 0; i < nb_elements; i++)
    {
      for(int j = 0; j < dimensions; j++)
      {
        mat_data(i, j) = dist(rng);
#ifdef FLUSH_MATRIX_TO_FILE
        ff << mat_data(i, j) << " ";
#endif
      }
#ifdef FLUSH_MATRIX_TO_FILE
      ff << std::endl;
#endif
    }

  }
};

template <class data_t>
struct test_mean_observer : grassmann_averages_pca::grassmann_trivial_callback<data_t>
{
  struct s_signal_exception {};

  data_t mean;
  void signal_mean(const data_t& mean_)
  {
    mean = mean_;
    throw s_signal_exception();
  }

};

#endif /* GRASSMANN_AVERAGES_PCA_TEST_MAIN_HPP__ */


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

// Currently needs -DMTL_DEEP_COPY_CONSTRUCTOR !!!

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

template <typename T>
struct as {};

template <>
struct as<int>
{
	typedef mtl::ashape::scal  type;
};

template <typename V>
struct as<mtl::compressed2D<V> >
{
	typedef mtl::ashape::mat<typename as<V>::type>   type;
};

template <typename V>
struct as<mtl::dense2D<V> >
{
	typedef mtl::ashape::mat<typename as<V>::type>   type;
};

int main(int, char**)
{
    using namespace std;

#if 0
    cout << typeid(as<int>::type).name() << endl;
    cout << typeid(as<mtl::compressed2D<int> >::type).name() << endl;
    cout << typeid(as<mtl::compressed2D<mtl::dense2D<int> > >::type).name() << endl;

    cout << typeid(mtl::ashape::ashape<int>::type).name() << endl;
    cout << typeid(mtl::ashape::ashape<mtl::compressed2D<int> >::type).name() << endl;
    cout << typeid(mtl::ashape::ashape<typename mtl::compressed2D<mtl::dense2D<int> >::value_type >::type).name() << endl;
#endif

    // Define a 6x5 sparse matrix in a 3x3 block-sparse
    typedef mtl::dense2D<double>    m_t;
    typedef mtl::compressed2D<m_t>  matrix_t;
    matrix_t                        A(3, 3);
    {
	mtl::mat::inserter<matrix_t> ins(A);

	// First block
	m_t  b1(1, 1);
	b1(0, 0)= 1.0;
	ins(0, 2) << b1;

	// Second block
	m_t  b2(2, 3);
	b2=       0.0;
	b2[0][1]= 2.0;
	b2[1][2]= 3.0;
	ins(1, 0) << b2;

	m_t b3(3, 1);
	b3= 0.0;
	b3[1][0]= 4.0;
	ins(2, 1) << b3;
    }
    // cout << "A is " << A << endl; // doesn't works and before it was completely unreadable anyway

    /* Should be something like this:

       [                   [ 1]] // b1
       [[  0   2   0]          ] // b2
       [[  0   0   3]          ]
       [             [  0]     ] // b3
       [             [  4]     ]
       [             [  0]     ]

    */

    // Access blocks (they are read-only) for sparse matrices
    cout << "The block A(1, 0) is \n" << A(1, 0) << endl;
    cout << "The block A[1][0] is \n" << A[1][0] << endl;

    // Access elements in blocks
    cout << "In block A(1, 0), the element (0, 1) is " << A(1, 0)(0, 1) << endl;
    cout << "In block A(1, 0), the element [0][1] is " << A(1, 0)[0][1] << endl;
    cout << "In block A[1][0], the element [0][1] is " << A[1][0][0][1] << endl << endl;

    typedef mtl::dense_vector<double> v_t;
    typedef mtl::dense_vector<v_t>    vector_t;
    vector_t                          x(3), y(3);

    // x= [[0, 5, 3], [1], [8]]^T
    x[0]= v_t(3, 0.0); x[0][1]= 5.0; x[0][2]= 3.0; // first block of x = [0, 5, 3]^T
    x[1]= v_t(1, 1.0);                             // second block of x
    x[2]= v_t(1, 8.0);                             // third block

    cout << "x is " << x << endl;

    // For y we would only need the vector sizes [[?], [?, ?], [?, ?, ?]]^T
    // To avoid valgrind complains we set to 0
    y[0]= v_t(1, 0.0); y[1]= v_t(2, 0.0); y[2]= v_t(3, 0.0);

    cout << "y is " << y << endl;

    // Block-sparse matrix * blocked vector !!!
    y= A*x;

    cout << "y after multiplication is " << y << endl
	 << "Should be [[8], [10, 9], [0, 4, 0]]^T." << endl;

    return 0;
}


/**
 * base_function.hpp
 *
 * Abstract class that defines the basic structure of a function in SISL.
 * Basically, all functions are mappings from R^n to R, and only need to
 * provide a means of reconstructing values from that function space.
 *
 * @author Joshua Horacsek
 **/

#ifndef _SISL_FUN_CTION_H_
#define _SISL_FUN_CTION_H_

#include <Eigen/Dense>
#include <sisl/primitives.hpp>

namespace sisl {
    /*! \brief Abstract base class for a function.
     * Defines a function that is valid over all R^n, it's up
     * to the underlying implementation to decide how those values map to
     * real values.
     */
    class function {
    public:
        function() { }
        virtual ~function() { }

        /*! \brief Evaluate the function at a point.
         * Evaluate the function at a point.
         */
        virtual const double operator()(double d0, ...) const = 0;

        /*! \brief Evaluate the function at a point.
         */
        virtual const double operator()(const vector &p) const = 0;

        /*! \brief Evaluate the function at a point.
         */
        virtual const double eval(double d0, ...) {
        	va_list args;
        	int n = this->dim();
        	vector p(n);

        	p[0] = d0;
			va_start(args, d0);
        	for(int i = 1; i < n; i++) {
        		p[i] = va_arg (args, double);
        	}
        	va_end (args);
        	return this->operator()(p);
        }

        /*! \brief Evaluate the function at a point.
         */
        virtual const double eval(const vector &p) {
        	return this->operator()(p);
        }

        /*! \brief Evaluate the derivative of a function at a point.
         */
        virtual const double d(int component, double d0, ...) const = 0;

        /*! \brief Evaluate the derivative of a function at a point.
         */
        virtual const double d(int component, const vector &p) const = 0;

        /*! \brief Evaluate the gradient of the function at a point
         */
        virtual const vector grad(double d0, ...) const = 0;

        /*! \brief Evaluate the gradient of the function at a point
         */
        virtual const vector grad(const vector &p) const = 0;

        /*! \brief Evaluate the n'th partial derivative at a point
         */
        virtual const double n_d(const int_tuple &order, double d0, ...) const = 0;

        /*! \brief Evaluate the n'th partial derivative at a point
         */
        virtual const double n_d(const int_tuple &order, vector &p) const = 0;

        /*! \brief Returns the input dimension of this function
        */
        virtual const int dim() const = 0;
    };
};

#endif // _SISL_FUN_CTION_H_


#ifndef KEYPOINTS_HPP
#define KEYPOINTS_HPP

#include <iostream>
#include <cstdlib>
#include <time.h>
#include <algorithm>
#include <numeric>
#include <thread>
#include <chrono>
#include <random>

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/kdtree.h>

#include <pcl/keypoints/iss_3d.h>
#include <pcl/keypoints/harris_3d.h>
#include <pcl/keypoints/sift_keypoint.h>

#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/features/shot.h>
#include <pcl/features/shot_omp.h>

#include <Eigen/Dense>

template <typename T>
typename pcl::PointCloud<T>::Ptr eigen2p_pcl(const Eigen::MatrixXf points){
    typename pcl::PointCloud<T>::Ptr p_pc(new typename pcl::PointCloud<T>());
    // Fill in the model cloud
    p_pc->width    = points.rows();
    p_pc->height   = 1;
    p_pc->is_dense = true;
    p_pc->points.resize (p_pc->width * p_pc->height);

    for (size_t i = 0; i < p_pc->points.size(); ++i){
        Eigen::Vector3f pt = points.row(i);
        p_pc->points[i].x = pt[0];
        p_pc->points[i].y = pt[1];
        p_pc->points[i].z = pt[2];
    }

    return p_pc;
}

template <typename T>
typename pcl::PointCloud<T>::Ptr eigen2p_pclNormal(const Eigen::MatrixXf points){
    typename pcl::PointCloud<T>::Ptr p_pc(new typename pcl::PointCloud<T>());
    // Fill in the model cloud
    p_pc->width    = points.rows();
    p_pc->height   = 1;
    p_pc->is_dense = true;
    p_pc->points.resize (p_pc->width * p_pc->height);

    for (size_t i = 0; i < p_pc->points.size(); ++i){
        Eigen::Vector3f pt = points.row(i);
        p_pc->points[i].normal_x = pt[0];
        p_pc->points[i].normal_y = pt[1];
        p_pc->points[i].normal_z = pt[2];
    }

    return p_pc;
}

template <typename T>
Eigen::MatrixXf p_pcl2eigen(const typename pcl::PointCloud<T>::Ptr p_pcl){
    Eigen::MatrixXf point_eigen(p_pcl->size(), 3);
    for(size_t i=0;i<p_pcl->size();++i){
        point_eigen(i, 0) = p_pcl->points[i].x;
        point_eigen(i, 1) = p_pcl->points[i].y;
        point_eigen(i, 2) = p_pcl->points[i].z;
    }
    return point_eigen;
}

template <typename T>
Eigen::MatrixXf p_pclNormal2eigen(const typename pcl::PointCloud<T>::Ptr p_pcl){
    Eigen::MatrixXf point_eigen(p_pcl->size(), 3);
    for(size_t i=0;i<p_pcl->size();++i){
        point_eigen(i, 0) = p_pcl->points[i].normal_x;
        point_eigen(i, 1) = p_pcl->points[i].normal_y;
        point_eigen(i, 2) = p_pcl->points[i].normal_z;
    }
    return point_eigen;
}

template <typename T>
Eigen::MatrixXf p_histogramFeature2eigen(const typename pcl::PointCloud<T>::Ptr p_feature){
    Eigen::MatrixXf feature_eigen(p_feature->size(), p_feature->points[0].descriptorSize());
    for(size_t i=0;i<feature_eigen.rows();++i){
        for(size_t j=0;j<feature_eigen.cols();++j){
            feature_eigen(i, j) = p_feature->points[i].histogram[j];
        }
    }

    return feature_eigen;
}

template <typename T>
Eigen::MatrixXf p_descriptorFeature2eigen(const typename pcl::PointCloud<T>::Ptr p_feature){
    Eigen::MatrixXf feature_eigen(p_feature->size(), p_feature->points[0].descriptorSize());
    for(size_t i=0;i<feature_eigen.rows();++i){
        for(size_t j=0;j<feature_eigen.cols();++j){
            feature_eigen(i, j) = p_feature->points[i].descriptor[j];
        }
    }

    return feature_eigen;
}

Eigen::MatrixXf keypointIss(const Eigen::MatrixXf points,
                            const float iss_salient_radius=3.0,
                            const float iss_non_max_radius=2.0,
                            const float iss_gamma_21=0.975,
                            const float iss_gamma_32=0.975,
                            const int iss_min_neighbors=5,
                            const int threads=0);

Eigen::MatrixXf keypointHarris(const Eigen::MatrixXf points,
                               const float radius=0.5,
                               const float nms_threshold=1e-3,
                               const int threads=0);

namespace pcl
{
template<>
struct SIFTKeypointFieldSelector<PointXYZ>
{
    inline float
    operator () (const PointXYZ &p) const
    {
        return p.y;
    }
};
}

Eigen::MatrixXf keypointSift(const Eigen::MatrixXf points,
                             const float min_scale=0.1,
                             const int n_octaves=6,
                             const int n_scales_per_octave=10,
                             const float min_contrast=0.05);

Eigen::MatrixXf featureFPFH33(const Eigen::MatrixXf pointcloud,
                              const Eigen::MatrixXf keypoints,
                              const int compute_normal_k,
                              const float feature_radius);

Eigen::MatrixXf featureFPFH33WithNormal(const Eigen::MatrixXf pointcloud,
                              const Eigen::MatrixXf normals,
                              const Eigen::MatrixXf keypoints,
                              const float feature_radius);

Eigen::MatrixXf featureSHOT352(const Eigen::MatrixXf pointcloud,
                               const Eigen::MatrixXf keypoints,
                               const int compute_normal_k,
                               const float feature_radius);

Eigen::MatrixXf featureSHOT352WithNormal(const Eigen::MatrixXf pointcloud,
                               const Eigen::MatrixXf normals,
                               const Eigen::MatrixXf keypoints,
                               const float feature_radius);

#endif // KEYPOINTS_HPP