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

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

#include "graph_augment.hh"

#include <boost/mpl/if.hpp>
#include <boost/mpl/or.hpp>
namespace std
{
using namespace boost;

// we need a min() function with arguments of different types

template <class T1, class T2>
typename boost::mpl::if_<
    typename boost::mpl::or_<typename std::is_floating_point<T1>::type,
                             typename std::is_floating_point<T2>::type>::type,
    double, int>::type
min(const T1& v1, const T2& v2)
{
    if (v1 <= T1(v2))
        return v1;
    else
        return v2;
}
}

#include <boost/graph/push_relabel_max_flow.hpp>

#include <boost/bind.hpp>

using namespace graph_tool;
using namespace boost;

struct get_push_relabel_max_flow
{
    template <class Graph, class VertexIndex, class EdgeIndex, class CapacityMap,
              class ResidualMap>
    void operator()(Graph& g, VertexIndex vertex_index, EdgeIndex edge_index,
                    size_t max_e, size_t src, size_t sink, CapacityMap cm,
                    ResidualMap res) const
    {
        typedef typename graph_traits<Graph>::edge_descriptor edge_t;
        checked_vector_property_map<bool,EdgeIndex>
            augmented(edge_index);
        unchecked_vector_property_map<edge_t,EdgeIndex>
            reverse_map(edge_index, max_e);

        typedef typename std::remove_const<Graph>::type GT;
        GT& u = const_cast<GT&>(g);

        augment_graph(u, augmented, cm,
                      reverse_map.get_checked(), res);

        boost::push_relabel_max_flow(g, vertex(src, g), vertex(sink, g),
                                     get_unchecked(cm),
                                     res.get_unchecked(),
                                     reverse_map, vertex_index);
        deaugment_graph(u, augmented);
    }
};

void push_relabel_max_flow(GraphInterface& gi, size_t src, size_t sink,
                           boost::any capacity, boost::any res)
{
    run_action<graph_tool::detail::always_directed, boost::mpl::true_>()
        (gi, std::bind(get_push_relabel_max_flow(),
                       placeholders::_1, gi.GetVertexIndex(), gi.GetEdgeIndex(),
                       gi.GetMaxEdgeIndex(),
                       src, sink,  placeholders::_2,  placeholders::_3),
         writable_edge_scalar_properties(), writable_edge_scalar_properties())
        (capacity,res);
}


// Copyright David Stone 2020.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#pragma once

#include <tm/clients/pokemon_online/conversion.hpp>

#include <tm/stat/calculate_ivs_and_evs.hpp>
#include <tm/stat/ev.hpp>
#include <tm/stat/ingame_id_to_nature.hpp>

#include <tm/string_conversions/species.hpp>

#include <tm/team.hpp>

#include <containers/integer_range.hpp>
#include <containers/size.hpp>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>

#include <filesystem>

namespace technicalmachine {

namespace po {

inline void write_blank_stats(Generation const generation, boost::property_tree::ptree & pt) {
	constexpr auto stat_names = containers::enum_range<SplitSpecialPermanentStat>();
	for ([[maybe_unused]] auto const stat : stat_names) {
		auto const dv = generation <= Generation::two ? 15 : 31;
		pt.add("DV", dv);
	}
	for ([[maybe_unused]] auto const stat : stat_names) {
		pt.add("EV", 0);
	}
}

template<Generation generation>
void write_stats(Pokemon<generation> const & pokemon, boost::property_tree::ptree & pt) {
	auto const stats = calculate_ivs_and_evs(pokemon);
	constexpr auto stat_names = containers::enum_range<SplitSpecialPermanentStat>();
	// In older generations, Spc is written into the SpA slot. The SpD slot is
	// written as a 15 but unused in the official client, but here we instead
	// just write Spc into both.
	for (auto const stat : stat_names) {
		pt.add("DV", stats.dvs_or_ivs[stat].value());
	}
	for (auto const stat : stat_names) {
		pt.add("EV", stats.evs[stat].value());
	}
}

inline void write_blank_move(boost::property_tree::ptree & pt) {
	pt.add("Move", 0);
}

inline void write_move(Move const move, boost::property_tree::ptree & pt) {
	pt.add("Move", move_to_id(move.name()));
}

inline auto write_blank_pokemon(Generation const generation, boost::property_tree::ptree & pt) -> void {
	auto & member = pt.add("Pokemon", "");
	member.put("<xmlattr>.Item", 0);
	member.put("<xmlattr>.Ability", 0);
	member.put("<xmlattr>.Num", 0);
	member.put("<xmlattr>.Nature", 0);
	member.put("<xmlattr>.Shiny", 0);
	member.put("<xmlattr>.Nickname", "Missingno");
	member.put("<xmlattr>.Gen", 4);
	member.put("<xmlattr>.Forme", 0);
	member.put("<xmlattr>.Happiness", 0);
	member.put("<xmlattr>.Lvl", 100);
	member.put("<xmlattr>.Gender", 0);

	for (unsigned n = 0; n != 4; ++n) {
		write_blank_move(member);
	}

	write_blank_stats(generation, member);
}

template<Generation generation>
void write_pokemon(Pokemon<generation> const & pokemon, boost::property_tree::ptree & pt) {
	auto & member = pt.add("Pokemon", "");
	member.put("<xmlattr>.Item", item_to_id(pokemon.item(false, false)));
	member.put("<xmlattr>.Ability", ability_to_id (pokemon.initial_ability()));
	auto const species = pokemon.species();
	auto const po_species = species_to_id(species);
	member.put("<xmlattr>.Num", po_species.id);
	member.put("<xmlattr>.Nature", ingame_nature_to_id(pokemon.nature()));
	member.put("<xmlattr>.Shiny", 0);
	member.put("<xmlattr>.Nickname", to_string(species));
	member.put("<xmlattr>.Gen", 4);
	member.put("<xmlattr>.Forme", po_species.forme);
	member.put("<xmlattr>.Happiness", pokemon.happiness());
	member.put("<xmlattr>.Lvl", pokemon.level()());
	member.put("<xmlattr>.Gender", gender_to_id (pokemon.gender()));

	auto const & moves = pokemon.regular_moves();
	for (auto const & move : moves) {
		write_move(move, member);
	}
	for (auto const n [[maybe_unused]] : containers::integer_range(containers::size(moves), max_moves_per_pokemon)) {
		write_blank_move(member);
	}

	write_stats(pokemon, member);
}

template<Generation generation>
void write_team(Team<generation> const & team, std::filesystem::path const & file_name) {
	auto pt = boost::property_tree::ptree();
	auto settings = boost::property_tree::xml_writer_settings<boost::property_tree::ptree::key_type>('\t', 1);
	auto & t = pt.add("Team", "");
	t.put("<xmlattr>.version", 1);
	t.put("<xmlattr>.gen", static_cast<int>(generation));
	t.put("<xmlattr>.defaultTier", "");
	auto & trainer = t.add("Trainer", "TM");
	trainer.put("<xmlattr>.loseMsg", "");
	trainer.put("<xmlattr>.avatar", 1);
	trainer.put("<xmlattr>.winMsg", "");
	trainer.put("<xmlattr>.infoMsg", "");
	for (auto const & pokemon : team.all_pokemon()) {
		write_pokemon(pokemon, t);
	}
	for (auto const unused [[maybe_unused]] : containers::integer_range(containers::size(team.all_pokemon()), max_pokemon_per_team)) {
		write_blank_pokemon(generation, t);
	}
	write_xml(file_name.string(), pt, std::locale(), settings);
}

} // namespace po
} // namespace technicalmachine


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

// Copyright (c) 2014, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DISTANCE_COMPARABLE_TO_REGULAR_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DISTANCE_COMPARABLE_TO_REGULAR_HPP

#include <cmath>
#include <boost/geometry/strategies/distance_comparable_to_regular.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template
<
    typename ComparableStrategy,
    typename Strategy,
    typename Geometry1,
    typename Geometry2
>
struct comparable_to_regular
    <
        ComparableStrategy, Strategy,
        Geometry1, Geometry2,
        cartesian_tag, cartesian_tag
    >
{
    typedef typename return_type
        <
            Strategy,
            typename point_type<Geometry1>::type,
            typename point_type<Geometry2>::type
        >::type calculation_type;

    typedef typename return_type
        <
            ComparableStrategy,
            typename point_type<Geometry1>::type,
            typename point_type<Geometry2>::type
        >::type comparable_calculation_type;

    static inline calculation_type apply(comparable_calculation_type const& cd)
    {
        return math::sqrt( boost::numeric_cast<calculation_type>(cd) );
    }
};

template <typename ComparableStrategy, typename Geometry1, typename Geometry2>
struct comparable_to_regular
    <
        ComparableStrategy,
        ComparableStrategy,
        Geometry1,
        Geometry2,
        cartesian_tag,
        cartesian_tag
    >
{
    typedef typename return_type
        <
            ComparableStrategy,
            typename point_type<Geometry1>::type,
            typename point_type<Geometry2>::type
        >::type comparable_calculation_type;

    static inline comparable_calculation_type
    apply(comparable_calculation_type const& cd)
    {
        return cd;
    }
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DISTANCE_COMPARABLE_TO_REGULAR_HPP


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

#include <boost/make_shared.hpp>
#include <ored/portfolio/enginefactory.hpp>
#include <ored/portfolio/equityfuturesoption.hpp>
#include <ored/utilities/indexparser.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/parsers.hpp>
#include <ored/utilities/to_string.hpp>
#include <ql/errors.hpp>

using namespace QuantLib;

namespace ore {
namespace data {

EquityFutureOption::EquityFutureOption(Envelope& env, OptionData option, const string& currency, Real quantity,
                                       const boost::shared_ptr<ore::data::Underlying>& underlying, Real strike,
                                       QuantLib::Date forwardDate, const boost::shared_ptr<QuantLib::Index>& index,
                                       const std::string& indexName)
    : VanillaOptionTrade(env, AssetClass::EQ, option, underlying->name(), currency, strike, quantity, index, indexName,
                         forwardDate),
      underlying_(underlying) {
    tradeType_ = "EquityFutureOption";
}

void EquityFutureOption::build(const boost::shared_ptr<EngineFactory>& engineFactory) {
    QL_REQUIRE(quantity_ > 0, "Commodity option requires a positive quatity");
    assetName_ = name();
    // FIXME: use index once implemented
    // const boost::shared_ptr<Market>& market = engineFactory->market();
    // Handle<PriceTermStructure> priceCurve =
    //    market->equityPriceCurve(underlying_->name(), engineFactory->configuration(MarketContext::pricing));
    // index_ = boost::make_shared<EquityFuturesIndex>(underlying_->name(), forwardDate, NullCalendar(), priceCurve);

    // FIXME: we set the automatic exercise to false until the Equity Futures Index is implemented

    QuantLib::Exercise::Type exerciseType = parseExerciseType(option_.style());
    QL_REQUIRE(exerciseType == Exercise::European, "only european option currently supported");
    option_.setAutomaticExercise(false);
    VanillaOptionTrade::build(engineFactory);
}

void EquityFutureOption::fromXML(XMLNode* node) {
    Trade::fromXML(node);
    XMLNode* eqNode = XMLUtils::getChildNode(node, "EquityFutureOptionData");
    QL_REQUIRE(eqNode, "No EquityFutureOptionData Node");
    option_.fromXML(XMLUtils::getChildNode(eqNode, "OptionData"));
    currency_ = XMLUtils::getChildValue(eqNode, "Currency", true);
    quantity_ = XMLUtils::getChildValueAsDouble(eqNode, "Quantity", true);

    XMLNode* tmp = XMLUtils::getChildNode(eqNode, "Underlying");
    if (!tmp)
        tmp = XMLUtils::getChildNode(eqNode, "Name");
    UnderlyingBuilder underlyingBuilder("Underlying", "Name");
    underlyingBuilder.fromXML(tmp);
    underlying_ = underlyingBuilder.underlying();

    strike_ = XMLUtils::getChildValueAsDouble(eqNode, "Strike", true);

    forwardDate_ = parseDate(XMLUtils::getChildValue(eqNode, "FutureExpiryDate", true));
}

XMLNode* EquityFutureOption::toXML(XMLDocument& doc) {
    XMLNode* node = Trade::toXML(doc);
    XMLNode* eqNode = doc.allocNode("EquityFutureOptionData");
    XMLUtils::appendNode(node, eqNode);
    XMLUtils::appendNode(eqNode, option_.toXML(doc));

    XMLUtils::addChild(doc, eqNode, "Currency", currency_);
    XMLUtils::addChild(doc, eqNode, "Quantity", quantity_);

    XMLUtils::appendNode(eqNode, underlying_->toXML(doc));

    XMLUtils::addChild(doc, eqNode, "Strike", strike_);
    XMLUtils::addChild(doc, eqNode, "FutureExpiryDate", to_string(forwardDate_));

    return node;
}

std::map<AssetClass, std::set<std::string>>
EquityFutureOption::underlyingIndices(const boost::shared_ptr<ReferenceDataManager>& referenceDataManager) const {
    return {{AssetClass::EQ, std::set<std::string>({name()})}};
}

} // namespace data
} // namespace ore


/**
 * @file random_forest_test.cpp
 * @author Manish Kumar
 *
 * Test mlpackMain() of random_forest_main.cpp.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#define BINDING_TYPE BINDING_TYPE_TEST

#include <mlpack/core.hpp>
static const std::string testName = "RandomForest";

#include <mlpack/core/util/mlpack_main.hpp>
#include <mlpack/methods/random_forest/random_forest_main.cpp>
#include "test_helper.hpp"

#include <boost/test/unit_test.hpp>
#include "../test_tools.hpp"

using namespace mlpack;

struct RandomForestTestFixture
{
 public:
  RandomForestTestFixture()
  {
    // Cache in the options for this program.
    CLI::RestoreSettings(testName);
  }

  ~RandomForestTestFixture()
  {
    // Clear the settings.
    bindings::tests::CleanMemory();
    CLI::ClearSettings();
  }
};

BOOST_FIXTURE_TEST_SUITE(RandomForestMainTest, RandomForestTestFixture);

/**
 * Check that number of output points and number of input
 * points are equal and have appropriate number of classes.
 */
BOOST_AUTO_TEST_CASE(RandomForestOutputDimensionTest)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  arma::mat testData;
  if (!data::Load("vc2_test.csv", testData))
    BOOST_FAIL("Cannot load test dataset vc2.csv!");

  size_t testSize = testData.n_cols;

  // Input training data.
  SetInputParam("training", std::move(inputData));
  SetInputParam("labels", std::move(labels));

  // Input test data.
  SetInputParam("test", std::move(testData));

  mlpackMain();

  // Check that number of output points are equal to number of input points.
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::Row<size_t>>("predictions").n_cols,
                      testSize);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("probabilities").n_cols,
                      testSize);

  // Check number of output rows equals number of classes in case of
  // probabilities and 1 for predictions.
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::Row<size_t>>("predictions").n_rows,
                      1);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("probabilities").n_rows, 3);
}

/**
 * Ensure that saved model can be used again.
 */
BOOST_AUTO_TEST_CASE(RandomForestModelReuseTest)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  arma::mat testData;
  if (!data::Load("vc2_test.csv", testData))
    BOOST_FAIL("Cannot load test dataset vc2.csv!");

  size_t testSize = testData.n_cols;

  // Input training data.
  SetInputParam("training", std::move(inputData));
  SetInputParam("labels", std::move(labels));

  // Input test data.
  SetInputParam("test", testData);

  mlpackMain();

  arma::Row<size_t> predictions;
  arma::mat probabilities;
  predictions = std::move(CLI::GetParam<arma::Row<size_t>>("predictions"));
  probabilities = std::move(CLI::GetParam<arma::mat>("probabilities"));

  // Reset passed parameters.
  CLI::GetSingleton().Parameters()["training"].wasPassed = false;
  CLI::GetSingleton().Parameters()["labels"].wasPassed = false;
  CLI::GetSingleton().Parameters()["test"].wasPassed = false;

  // Input trained model.
  SetInputParam("test", std::move(testData));
  SetInputParam("input_model",
                CLI::GetParam<RandomForestModel*>("output_model"));

  mlpackMain();

  // Check that number of output points are equal to number of input points.
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::Row<size_t>>("predictions").n_cols,
                      testSize);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("probabilities").n_cols,
                      testSize);

  // Check number of output rows equals number of classes in case of
  // probabilities and 1 for predicitions.
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::Row<size_t>>("predictions").n_rows,
                      1);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("probabilities").n_rows, 3);

  // Check that initial predictions and predictions using saved model are same.
  CheckMatrices(predictions, CLI::GetParam<arma::Row<size_t>>("predictions"));
  CheckMatrices(probabilities, CLI::GetParam<arma::mat>("probabilities"));
}

/**
 * Make sure number of trees specified is always a positive number.
 */
BOOST_AUTO_TEST_CASE(RandomForestNumOfTreesTest)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  SetInputParam("num_trees", (int) 0); // Invalid.

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Make sure minimum leaf size specified is always a positive number.
 */
BOOST_AUTO_TEST_CASE(RandomForestMinimumLeafSizeTest)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  SetInputParam("minimum_leaf_size", (int) 0); // Invalid.

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Make sure only one of training data or pre-trained model is passed.
 */
BOOST_AUTO_TEST_CASE(RandomForestTrainingVerTest)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  // Input training data.
  SetInputParam("training", std::move(inputData));
  SetInputParam("labels", std::move(labels));

  mlpackMain();

  // Input pre-trained model.
  SetInputParam("input_model",
                CLI::GetParam<RandomForestModel*>("output_model"));

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Ensure that training accuracy goes up as minimum_leaf_size decreases.
 */
BOOST_AUTO_TEST_CASE(RandomForestDiffMinLeafSizeTest)
{
  // Train for minimum leaf size 20.
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  // Input training data.
  SetInputParam("training", inputData);
  SetInputParam("labels", labels);
  SetInputParam("minimum_leaf_size", (int) 20);

  mlpackMain();

  // Calculate training accuracy.
  arma::Row<size_t> predictions;
  CLI::GetParam<RandomForestModel*>("output_model")->rf.Classify(inputData,
       predictions);

  size_t correct = arma::accu(predictions == labels);
  double accuracy20 = (double(correct) / double(labels.n_elem) * 100);

  bindings::tests::CleanMemory();

  // Train for minimum leaf size 10.

  // Input training data.
  SetInputParam("training", inputData);
  SetInputParam("labels", labels);
  SetInputParam("minimum_leaf_size", (int) 10);

  mlpackMain();

  // Calculate training accuracy.
  CLI::GetParam<RandomForestModel*>("output_model")->rf.Classify(inputData,
       predictions);

  correct = arma::accu(predictions == labels);
  double accuracy10 = (double(correct) / double(labels.n_elem) * 100);

  bindings::tests::CleanMemory();

  // Train for minimum leaf size 1.

  // Input training data.
  SetInputParam("training", inputData);
  SetInputParam("labels", labels);
  SetInputParam("minimum_leaf_size", (int) 1);

  mlpackMain();

  // Calculate training accuracy.
  CLI::GetParam<RandomForestModel*>("output_model")->rf.Classify(inputData,
       predictions);

  correct = arma::accu(predictions == labels);
  double accuracy1 = (double(correct) / double(labels.n_elem) * 100);

  BOOST_REQUIRE(accuracy1 > accuracy10 && accuracy10 > accuracy20);
}

/**
 * Ensure that test accuracy goes up as num_trees increases.
 */
BOOST_AUTO_TEST_CASE(RandomForestDiffNumTreeTest)
{
  // Train for num_trees 1.
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  arma::mat testData;
  if (!data::Load("vc2_test.csv", testData))
    BOOST_FAIL("Cannot load test dataset vc2_test.csv!");

  arma::Row<size_t> testLabels;
  if (!data::Load("vc2_test_labels.txt", testLabels))
    BOOST_FAIL("Cannot load labels for vc2__test_labels.txt");

  // Input training data.
  SetInputParam("training", inputData);
  SetInputParam("labels", labels);
  SetInputParam("num_trees", (int) 1);

  mlpackMain();

  // Calculate training accuracy.
  arma::Row<size_t> predictions;
  CLI::GetParam<RandomForestModel*>("output_model")->rf.Classify(testData,
       predictions);
  bindings::tests::CleanMemory();

  size_t correct = arma::accu(predictions == testLabels);
  double accuracy1 = (double(correct) / double(testLabels.n_elem) * 100);

  // Train for num_trees 5.

  // Input training data.
  SetInputParam("training", inputData);
  SetInputParam("labels", labels);
  SetInputParam("num_trees", (int) 5);

  mlpackMain();

  // Calculate training accuracy.
  CLI::GetParam<RandomForestModel*>("output_model")->rf.Classify(testData,
       predictions);
  bindings::tests::CleanMemory();

  correct = arma::accu(predictions == testLabels);
  double accuracy5 = (double(correct) / double(testLabels.n_elem) * 100);

  // Train for num_trees 10.

  // Input training data.
  SetInputParam("training", std::move(inputData));
  SetInputParam("labels", std::move(labels));
  SetInputParam("num_trees", (int) 10);

  mlpackMain();

  // Calculate training accuracy.
  CLI::GetParam<RandomForestModel*>("output_model")->rf.Classify(testData,
       predictions);

  correct = arma::accu(predictions == testLabels);
  double accuracy10 = (double(correct) / double(testLabels.n_elem) * 100);

  BOOST_REQUIRE(accuracy10 >= accuracy5 && accuracy5 >= accuracy1);
}

BOOST_AUTO_TEST_SUITE_END();


#include "FilteringArmors.hpp"
#include "../Modules/ImageDebugUtility.hpp"
#include "../Components/PONElement.hpp"
#include <boost/algorithm/string.hpp>

namespace Icarus
{
    std::vector<double> CastStringToDoubleVector(const std::string& text)
    {
        std::vector<std::string> element_texts;
        boost::split(element_texts, text, boost::is_any_of(",;"), boost::token_compress_on);
        std::vector<double> element_values;
        element_values.reserve(element_texts.size());
        for (const auto& value_text : element_texts)
        {
            element_values.push_back(std::stod(value_text));
        }
        return element_values;
    }

    void FilteringArmors::LoadConfigurations()
    {
        FarChecker.LoadConfiguration();
        MediumChecker.LoadConfiguration();
        NearChecker.LoadConfiguration();
    }

    void FilteringArmors::OnInitialize()
    {
        InitializeFacilities();

        ArmorsWriter = std::make_unique<SharedPicture::PictureWriter>("icarus.detector.armors", 1920 * 1080 * 3);

        MainPicture = GetBlackboard()->GetPointer<cv::Mat>("MainPicture");
        Contours = GetBlackboard()->GetPointer<
                tbb::concurrent_vector<ContourElement::Pointer>>("Contours");
        LightBarFarLayer = GetBlackboard()->GetPointer<Tags::Index>(
                "LightBarFarLayer");
        LightBarMediumLayer = GetBlackboard()->GetPointer<Tags::Index>(
                "LightBarMediumLayer");
        LightBarNearLayer = GetBlackboard()->GetPointer<Tags::Index>(
                "LightBarNearLayer");
        FoundTarget = GetBlackboard()->GetPointer<std::optional<cv::RotatedRect>>("FoundTarget", std::nullopt);
        HitPoint = GetBlackboard()->GetPointer<cv::Point2i>("HitPoint");
        HitCommand = GetBlackboard()->GetPointer<int>("HitCommand");
        HitDistance = GetBlackboard()->GetPointer<double>("HitDistance", 0);
        MotionStatus = GetBlackboard()->GetPointer<int>("MotionStatus", 0);
        InterestedAreaUsed = GetBlackboard()->GetPointer<bool>("InterestedAreaUsed", false);
        FarChecker.Initialize(GetConfigurator());
        MediumChecker.Initialize(GetConfigurator());
        NearChecker.Initialize(GetConfigurator());

        auto camera_matrix_string = GetConfigurator()->Get("CameraMatrix");
        auto distortion_coefficient_string = GetConfigurator()->Get("DistortionCoefficient");
        ArmorSizeRatioSeparator = GetConfigurator()->Get<double>("ArmorSizeRatioSeparator").value_or(8.0);
        if (camera_matrix_string.has_value() && distortion_coefficient_string.has_value())
        {
            auto camera_matrix_vector = CastStringToDoubleVector(camera_matrix_string.value());
            auto distortion_coefficient_vector = CastStringToDoubleVector(
                    distortion_coefficient_string.value());

            if (camera_matrix_vector.size() == 9 && distortion_coefficient_vector.size() == 4)
            {
                auto camera_matrix = cv::Mat(camera_matrix_vector, true);
                camera_matrix = camera_matrix.reshape(0, 3);

                auto distortion_coefficient = cv::Mat(distortion_coefficient_vector, true);
                distortion_coefficient = distortion_coefficient.reshape(0, 1);

                static constexpr double BigArmorHalfLength = 23.0 / 2.0;
                static constexpr double SmallArmorHalfLength = 13.0 / 2.0;
                static constexpr double ArmorHalfHeight = 5.5 / 2.0;
                BigArmorDistanceEstimator = std::make_unique<Modules::PnPDistanceEstimator>();
                BigArmorDistanceEstimator->CameraMatrix = camera_matrix;
                BigArmorDistanceEstimator->DistortionCoefficient = distortion_coefficient;
                BigArmorDistanceEstimator->WorldPoints = {
                        cv::Point3d(-BigArmorHalfLength, -ArmorHalfHeight, 0),
                        cv::Point3d(-BigArmorHalfLength, ArmorHalfHeight, 0),
                        cv::Point3d(BigArmorHalfLength, ArmorHalfHeight, 0),
                        cv::Point3d(BigArmorHalfLength, -ArmorHalfHeight, 0)};
                SmallArmorDistanceEstimator = std::make_unique<Modules::PnPDistanceEstimator>();
                SmallArmorDistanceEstimator->CameraMatrix = camera_matrix;
                SmallArmorDistanceEstimator->DistortionCoefficient = distortion_coefficient;
                SmallArmorDistanceEstimator->WorldPoints = {
                        cv::Point3d(-SmallArmorHalfLength, -ArmorHalfHeight, 0),
                        cv::Point3d(-SmallArmorHalfLength, ArmorHalfHeight, 0),
                        cv::Point3d(SmallArmorHalfLength, ArmorHalfHeight, 0),
                        cv::Point3d(SmallArmorHalfLength, -ArmorHalfHeight, 0)};
            }
        }

        LoadConfigurations();
    }

    BehaviorTree::Result FilteringArmors::OnExecute()
    {
        DEBUG_BEGIN
            CheckReloadConfiguration();
        DEBUG_END

        *HitCommand = 0;
        if (!*InterestedAreaUsed)
        {
            *HitDistance = 0;
            HitPoint->x = 0;
            HitPoint->y = 0;
        }
        *MotionStatus = 0;
        *FoundTarget = std::nullopt;

        std::vector<tbb::concurrent_vector<ContourElement*>> light_bars;
        light_bars.resize(3);
        auto layers = std::vector{LightBarFarLayer, LightBarMediumLayer, LightBarNearLayer};
        tbb::parallel_for(0ul, layers.size(),
                          [&light_bars, &layers](unsigned long layer_index){
            tbb::parallel_for_each(layers[layer_index]->GetHolders(), [&light_bars, &layer_index]
            (Tags::Holder* holder){
                light_bars[layer_index].push_back(dynamic_cast<ContourElement*>(holder));
            });
        });

        std::vector<tbb::concurrent_vector<PONElement::Pointer>> possible_armors;
        possible_armors.resize(3);
        tbb::parallel_for(0ul, layers.size(),
                          [this, &light_bars, &possible_armors](unsigned long layer_index){
            auto& layer = light_bars[layer_index];
            tbb::parallel_for(0ul, layer.size(),
                              [this, layer_index, &layer, &possible_armors]
            (unsigned long main_contour_index)
            {
                auto* main_contour = layer[main_contour_index];
                for (auto vice_contour_index = main_contour_index + 1; vice_contour_index < layer.size();
                    ++vice_contour_index)
                {
                    auto* vice_contour = layer[vice_contour_index];

                    auto PON_element = std::make_shared<PONElement>();
                    PON_element->ContourA = main_contour;
                    PON_element->ContourB = vice_contour;
                    std::vector<cv::Point> points;
                    points.reserve(main_contour->Points.size() + vice_contour->Points.size());
                    points.insert(points.end(), main_contour->Points.begin(), main_contour->Points.end());
                    points.insert(points.end(), vice_contour->Points.begin(), vice_contour->Points.end());
                    PON_element->Rectangle = cv::minAreaRect(points);
                    PON_element->Feature = Modules::GeometryFeature::Standardize(PON_element->Rectangle);

                    switch (layer_index)
                    {
                        case 0:
                            if (this->FarChecker.Check(PON_element.get()))
                            {
                                possible_armors[layer_index].push_back(PON_element);
                            }
                            break;
                        case 1:
                            if (this->MediumChecker.Check(PON_element.get()))
                            {
                                possible_armors[layer_index].push_back(PON_element);
                            }
                            break;
                        case 2:
                            if (this->NearChecker.Check(PON_element.get()))
                            {
                                possible_armors[layer_index].push_back(PON_element);
                            }
                            break;
                        default:
                            break;
                    }
                }
            });
        });

        std::optional<double> best_score = std::nullopt;
        std::optional<std::shared_ptr<PONElement>> best_armor = std::nullopt;

        // The 'center' position to lock.
        cv::Point2f locking_center;
        locking_center.x = static_cast<float>(MainPicture->cols) * 0.5f;
        locking_center.y = static_cast<float>(MainPicture->rows) * 0.5f;
        for (const auto& armor_layer : possible_armors)
        {
            for (const auto& armor : armor_layer)
            {
                auto delta_position = armor->Rectangle.center - locking_center;
                auto current_score = delta_position.ddot(delta_position);
                if (!best_score || current_score < *best_score)
                {
                    best_score = current_score;
                    best_armor = armor;
                }
            }
        }
        if (best_armor.has_value())
        {
            *FoundTarget = best_armor.value()->Rectangle;
        }
        else
        {
            *FoundTarget = std::nullopt;
        }

        if (*FoundTarget)
        {
            *HitCommand = 1;
            int raw_hit_point_x = (static_cast<int>(best_armor.value()->ContourA->Rectangle.center.x) +
                    static_cast<int>(best_armor.value()->ContourB->Rectangle.center.x)) / 2;
            int raw_hit_point_y = (static_cast<int>(best_armor.value()->ContourA->Rectangle.center.y) +
                    static_cast<int>(best_armor.value()->ContourB->Rectangle.center.y)) / 2;

            HitPoint->x = raw_hit_point_x - static_cast<int>(MainPicture->cols / 2.0);
            HitPoint->y = static_cast<int>(MainPicture->rows / 2.0) - raw_hit_point_y;

            if (BigArmorDistanceEstimator && SmallArmorDistanceEstimator)
            {
                auto& best_armor_found = best_armor.value();
                std::vector<cv::Point2d> camera_points;

                Modules::GeometryFeature* left_contour_feature = nullptr;
                Modules::GeometryFeature* right_contour_feature = nullptr;
                if (best_armor_found->ContourA->Rectangle.center.x < best_armor_found->ContourB->Rectangle.center.y)
                {
                    left_contour_feature = &best_armor_found->ContourA->Feature;
                    right_contour_feature = &best_armor_found->ContourB->Feature;
                }
                else
                {
                    left_contour_feature = &best_armor_found->ContourB->Feature;
                    right_contour_feature = &best_armor_found->ContourA->Feature;
                }

                camera_points.emplace_back(static_cast<cv::Vec2d>(
                        static_cast<cv::Point_<double>>(right_contour_feature->Center)) +
                        static_cast<cv::Vec2d>(right_contour_feature->Vectors.Direction / 2.0));
                camera_points.emplace_back(static_cast<cv::Vec2d>(
                        static_cast<cv::Point_<double>>(right_contour_feature->Center)) -
                        static_cast<cv::Vec2d>(right_contour_feature->Vectors.Direction / 2.0));
                camera_points.emplace_back(static_cast<cv::Vec2d>(
                        static_cast<cv::Point_<double>>(left_contour_feature->Center)) -
                        static_cast<cv::Vec2d>(left_contour_feature->Vectors.Direction / 2.0));
                camera_points.emplace_back(static_cast<cv::Vec2d>(
                        static_cast<cv::Point_<double>>(left_contour_feature->Center)) +
                        static_cast<cv::Vec2d>(left_contour_feature->Vectors.Direction / 2.0));
                if (best_armor_found->Feature.Length / best_armor_found->Feature.Width > ArmorSizeRatioSeparator)
                {
                    *HitDistance = BigArmorDistanceEstimator->GetDistance(camera_points);
                }
                else
                {
                    *HitDistance = SmallArmorDistanceEstimator->GetDistance(camera_points);
                }
            }
        }

        DEBUG_BEGIN

            cv::Mat armors_picture = MainPicture->clone();
            for (const auto& armor : possible_armors[0])
            {
                Modules::ImageDebugUtility::DrawRotatedRectangle(armors_picture, armor->Rectangle,
                                                                 cv::Scalar(0, 0, 255), 2);
            }
            for (const auto& armor : possible_armors[1])
            {
                Modules::ImageDebugUtility::DrawRotatedRectangle(armors_picture, armor->Rectangle,
                                                                 cv::Scalar(0, 255, 0), 2);
            }
            for (const auto& armor : possible_armors[2])
            {
                Modules::ImageDebugUtility::DrawRotatedRectangle(armors_picture, armor->Rectangle,
                                                                 cv::Scalar(255, 0, 0), 2);
            }
            cv::resize(armors_picture, armors_picture, armors_picture.size() / 2);
            ArmorsWriter->Write(armors_picture);
        DEBUG_END

        return BehaviorTree::Result::Success;
    }
}


/*******************************************************
 * Copyright (c) 2014, ArrayFire
 * All rights reserved.
 *
 * This file is distributed under 3-clause BSD license.
 * The complete license agreement can be obtained at:
 * http://arrayfire.com/licenses/BSD-3-Clause
 ********************************************************/

#pragma once

#include <boost/functional/hash.hpp>
#include <common/util.hpp>

#include <unordered_map>

namespace forge {
namespace common {

using CellIndex     = std::tuple<int, int, int>;
using MatrixHashMap = std::unordered_map<CellIndex, glm::mat4>;

constexpr float PI        = 3.14159f;
constexpr float BLACK[]   = {0.0f, 0.0f, 0.0f, 1.0f};
constexpr float GRAY[]    = {0.75f, 0.75f, 0.75f, 1.0f};
constexpr float WHITE[]   = {1.0f, 1.0f, 1.0f, 1.0f};
constexpr float AF_BLUE[] = {0.0588f, 0.1137f, 0.2745f, 1.0f};
static const glm::mat4 IDENTITY(1.0f);

#if defined(OS_WIN)
#define __PRETTY_FUNCTION__ __FUNCSIG__
#define __FG_FILENAME__ (forge::common::clipPath(__FILE__, "src\\").c_str())
#else
#define __FG_FILENAME__ (forge::common::clipPath(__FILE__, "src/").c_str())
#endif

}  // namespace common
}  // namespace forge

namespace std {

template<>
struct hash<forge::common::CellIndex> {
    std::size_t operator()(const forge::common::CellIndex& key) const {
        size_t seed = 0;
        boost::hash_combine(seed, std::get<0>(key));
        boost::hash_combine(seed, std::get<1>(key));
        boost::hash_combine(seed, std::get<2>(key));
        return seed;
    }
};

}  // namespace std


// Copyright (c) 2018, The Graft Project
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this list of
//    conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice, this list
//    of conditions and the following disclaimer in the documentation and/or other
//    materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors may be
//    used to endorse or promote products derived from this software without specific
//    prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
// THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
// THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <cryptonote_core/cryptonote_core.h>
#include <crypto/hash.h>
#include "common/int-util.h"
#include <boost/scoped_ptr.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <string>

#include <iostream>
#include <functional>

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

using namespace cryptonote;

/*
# Tom Harold (Degnr8) WT
# Modified by Zawy to be a weighted-Weighted Harmonic Mean (WWHM)
# No limits in rise or fall rate should be employed.
# MTP should not be used.
k = (N+1)/2  * T

# original algorithm
d=0, t=0, j=0
for i = height - N+1 to height  # (N most recent blocks)
    # TS = timestamp
    solvetime = TS[i] - TS[i-1]
    solvetime = 10*T if solvetime > 10*T
    solvetime = -9*T if solvetime < -9*T
    j++
    t +=  solvetime * j
    d +=D[i] # sum the difficulties
next i
t=T*N/2 if t < T*N/2  # in case of startup weirdness, keep t reasonable
next_D = d * k / t
*/

#define DIFFICULTY_WINDOW_V3                            60
#define DIFFICULTY_BLOCKS_COUNT_V3                      DIFFICULTY_WINDOW_V3

namespace {
#if defined(__x86_64__)
  static inline void mul(uint64_t a, uint64_t b, uint64_t &low, uint64_t &high) {
    low = mul128(a, b, &high);
  }
#endif
}

difficulty_type next_difficulty_masari(std::vector<std::uint64_t> timestamps, std::vector<difficulty_type> cumulative_difficulties, size_t target_seconds) {

  if (timestamps.size() > DIFFICULTY_BLOCKS_COUNT_V3)
  {
    timestamps.resize(DIFFICULTY_BLOCKS_COUNT_V3);
    cumulative_difficulties.resize(DIFFICULTY_BLOCKS_COUNT_V3);
  }

  size_t length = timestamps.size();
  assert(length == cumulative_difficulties.size());
  if (length <= 1) {
    return 1;
  }

  uint64_t weighted_timespans = 0;
  uint64_t target;

  if (true) {
    uint64_t previous_max = timestamps[0];
    for (size_t i = 1; i < length; i++) {
      uint64_t timespan;
      uint64_t max_timestamp;

      if (timestamps[i] > previous_max) {
        max_timestamp = timestamps[i];
      } else {
        max_timestamp = previous_max;
      }

      timespan = max_timestamp - previous_max;
      if (timespan == 0) {
        timespan = 1;
      } else if (timespan > 10 * target_seconds) {
        timespan = 10 * target_seconds;
      }

      weighted_timespans += i * timespan;
      previous_max = max_timestamp;
    }
    // adjust = 0.99 for N=60, leaving the + 1 for now as it's not affecting N
    target = 99 * (((length + 1) / 2) * target_seconds) / 100;
  }

  uint64_t minimum_timespan = target_seconds * length / 2;
  if (weighted_timespans < minimum_timespan) {
    weighted_timespans = minimum_timespan;
  }

  difficulty_type total_work = cumulative_difficulties.back() - cumulative_difficulties.front();
  assert(total_work > 0);

  uint64_t low, high;
  mul(total_work, target, low, high);
  if (high != 0) {
    return 0;
  }
  return low / weighted_timespans;
}

class DAA_Tester
{
public:
    typedef std::function<cryptonote::difficulty_type (std::vector<uint64_t>,
                                                       std::vector<cryptonote::difficulty_type>, size_t target_seconds)> DifficultyFunc;

    cryptonote::difficulty_type next_difficulty(uint64_t timestamp, cryptonote::difficulty_type difficulty, size_t target_seconds)
    {
        size_t begin, end;
        if (m_counter < DIFFICULTY_WINDOW + DIFFICULTY_LAG) {
            begin = 0;
            end = min(m_counter, (uint64_t) DIFFICULTY_WINDOW);
        } else {
            end = m_counter - DIFFICULTY_LAG;
            begin = end - DIFFICULTY_WINDOW;
        }

        cryptonote::difficulty_type result = m_difficulty_func(
            vector<uint64_t>(m_timestamps.begin() + begin, m_timestamps.begin() + end),
            vector<uint64_t>(m_cumulative_difficulties.begin() + begin,
                             m_cumulative_difficulties.begin() + end), DIFFICULTY_TARGET_V2);

        m_timestamps.push_back(timestamp);
        m_cumulative_difficulties.push_back(m_cumulative_difficulty += difficulty);
        ++m_counter;
        return result;
    }

    void reset()
    {
        m_counter = 0;
        m_cumulative_difficulty = 0;
        m_timestamps.clear();
        m_cumulative_difficulties.clear();
    }

    void setDifficultyFunc(DifficultyFunc df)
    {
        m_difficulty_func = df;
    }

private:
    std::vector<uint64_t> m_timestamps;
    std::vector<cryptonote::difficulty_type> m_cumulative_difficulties;
    DifficultyFunc m_difficulty_func;
    uint64_t m_counter = 0;
    uint64_t m_cumulative_difficulty = 0;
};

int main(int argc, char **argv)
{
    string input_file;
    string output_file;

    int log_level;

    DAA_Tester daa_tester;
    daa_tester.setDifficultyFunc(cryptonote::next_difficulty);

    try {
        po::options_description desc("Allowed options");
        desc.add_options()
                ("help", "produce help message")
                ("input-file",      po::value<string>()->required(), "input file")
                ("output-file",     po::value<string>()->required(), "output file")
                ("algorithm",       po::value<string>(), "algorithm")
                ("log-level",       po::value<int>(&log_level)->default_value(1), "log-level");

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

        po::notify(vm);

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

        if (vm.count("log-level")) {
            log_level = vm["log-level"].as<int>();
        }

        mlog_configure("", true);
        mlog_set_log_level(log_level);

        if (vm.count("algorithm")) {
            string algo_s = vm["algorithm"].as<string>();
            if (algo_s == "masari") {
                daa_tester.setDifficultyFunc(next_difficulty_masari);
            }

        }

        input_file  = vm["input-file"].as<string>();

        LOG_PRINT_L0("5");

        output_file = vm["output-file"].as<string>();
        //output_file = "out2.csv";
        LOG_PRINT_L0("reading from " << input_file);
        ifstream in(input_file);

        if (!in.is_open()) {
            LOG_ERROR("Error opening file " << input_file);
            return false;
        }

        ofstream output(output_file);

        string line;

        while (getline(in, line)) {

            LOG_PRINT_L0("processing line: " << line);

            if (line.find(";") == 0)
                continue;

            typedef boost::tokenizer<boost::char_separator<char>> tokenizer;
            boost::char_separator<char> sep{","};
            vector<string> tokens;
            tokenizer tok{line, sep};
            for (const auto &t : tok) {

                tokens.push_back(boost::trim_copy(t));
            }

            if (tokens.size() != 5) {
                LOG_ERROR("Error parsing row: " << line << ", tokens size: " << tokens.size());
                return -1;
            }

            uint64_t block_num = boost::lexical_cast<uint64_t>(tokens[0]);
            uint64_t timestamp = boost::lexical_cast<uint64_t>(tokens[1]);
            uint64_t difficulty = boost::lexical_cast<uint64_t>(tokens[3]);
            uint64_t next_difficulty = daa_tester.next_difficulty(timestamp, difficulty, DIFFICULTY_TARGET_V2);
            output << block_num << ", " << next_difficulty << std::endl;
        }

    } catch (const std::exception &e) {
        // close() syncs db which throwns an exception as db opened in read-only mode
        cerr << "exception thrown: " << e.what() << endl;
    } catch (...) {

    }

    return 0;
}


#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include <tuple>

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

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

#include <pcl/common/common_headers.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
#include <pcl/sample_consensus/sac_model_normal_plane.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/surface/gp3.h>
#include <pcl/surface/mls.h>
#include <pcl/io/pcd_io.h>

#include <pcl/filters/passthrough.h>
#include <vtkPlaneSource.h>
#include <vtkImplicitPlaneWidget.h>

#include "../../../core/format_helper.h"
#include "../../../core/math_helper.h"

void createPolyFromPlane(const pcl::ModelCoefficients::Ptr coefficients,
                         pcl::PointCloud<pcl::PointXYZ>::Ptr poly_cloud)
{
  double a = coefficients->values[0];
  double b = coefficients->values[1];
  double c = coefficients->values[2];
  double d = coefficients->values[3];

  double x[4] = {-20, 20, 20, -20};
  double z[4] = {  0,  0, 100, 100};
  for(int i = 0; i < 4; i++) {
    double y = (a*x[i] + c*z[i] + d) / (-b);
    pcl::PointXYZ pt(x[i], y, z[i]);
    poly_cloud->points.push_back(pt);
  }
}

void triangulate(const double (&cam_params)[5], double x, double y, double disp, cv::Mat& pt3d) {
  double f = cam_params[0];
  double cx = cam_params[2];
  double cy = cam_params[3];
  double b = cam_params[4];

  // add - in front for x and y just to set the positive y to skies
  pt3d.at<double>(0) = -(x - cx) * b / disp;
  pt3d.at<double>(1) = -(y - cy) * b / disp;
  pt3d.at<double>(2) = f * b / disp;
  pt3d.at<double>(3) = 1.0;
}

void renderPointCloud(const std::vector<std::string>& imagelist, const std::vector<std::string>& disp_imglist,
                      const std::string& gt_file, const std::string& cparams_file,
                      const std::string& source_folder, const std::string& disp_folder, const std::string& output_folder)
{
  double cam_params[5];
  core::FormatHelper::readCameraParams(cparams_file, cam_params);

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
  std::vector<std::tuple<int,int>> points_img;

  ifstream gt_fp(gt_file);
  cv::Mat Rt = cv::Mat::eye(4, 4, CV_64F);
  cv::Mat pt3d = cv::Mat::zeros(4, 1, CV_64F);
  cv::Mat img_left;
  cv::Mat mat_disp;
  double min_disp = 30.0;
  int img_step = 1;
  //size_t start_frame = 100;
  //size_t start_frame = 315;
  //size_t start_frame = 27;
  // meadow on right
  size_t start_frame = 349;
  // car in front
  //size_t start_frame = 995;
  // traffic sign
  //size_t start_frame = 442;
  //size_t start_frame = 620;
  //for(size_t i = 0; i < end_frame; i++) {
  //for(size_t i = start_frame; i < end_frame; i++) {
  for(size_t i = start_frame; i < start_frame+1; i++) {
    //core::FormatHelper::readNextMatrixKitti(gt_fp, Rt);
    if(!(i % img_step == 0) || i < start_frame) continue;
    std::cout << "----------------\n" << source_folder + imagelist[2*i] << "\n";
    img_left = cv::imread(source_folder + imagelist[2*i], CV_LOAD_IMAGE_GRAYSCALE);
    //string fname = output_folder + imagelist[i].substr(9, imagelist[i].size()-13) + "_disp.png";
    //std::string fname = output_folder + disp_imglist[i];
    //img_disp = imread(disp_folder + disp_imglist[i], CV_LOAD_IMAGE_GRAYSCALE);
    std::string mat_fname = disp_folder + "mat/" + disp_imglist[i].substr(0,11) + ".yml";
    std::cout << "reading: " << mat_fname << "\n";
    cv::FileStorage mat_file(mat_fname, cv::FileStorage::READ);
    mat_file["disparity_matrix"] >> mat_disp;
    for(int y = 0; y < mat_disp.rows; y++) {
      for(int x = 0; x < mat_disp.cols; x++) {
        float disp = mat_disp.at<float>(y,x);
        if(disp > min_disp) {
          triangulate(cam_params, x, y, disp, pt3d);
          //pt3d = Rt * pt3d;
          //std::cout << pt3d << "\n\n";
          pcl::PointXYZ pt;
          pt.x = pt3d.at<double>(0);
          pt.y = pt3d.at<double>(1);
          pt.z = pt3d.at<double>(2);
          point_cloud->points.push_back(pt);
          pcl::PointXYZRGB point;
          point.x = pt3d.at<double>(0);
          point.y = pt3d.at<double>(1);
          point.z = pt3d.at<double>(2);
          point.r = img_left.at<uint8_t>(y,x);
          point.g = img_left.at<uint8_t>(y,x);
          point.b = img_left.at<uint8_t>(y,x);
          point_cloud_rgb->points.push_back(point);
          points_img.push_back(std::make_tuple(y,x));
        }
      }
    }
    //imshow("disparity", img_disp);
    //waitKey(0);
  }
  std::cout << "Point cloud size: " << point_cloud->points.size() << "\n";
  point_cloud->width = point_cloud->points.size();
  point_cloud->height = 1;

  // PCL MLS
  // Create a KD-Tree
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  // Output has the PointNormal type in order to store the normals calculated by MLS
  pcl::PointCloud<pcl::PointNormal> mls_points;

  // Init object (second point type is for the normals, even if unused)
  pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls;

  mls.setComputeNormals (true);
  // Set parameters
  mls.setInputCloud (point_cloud);
  mls.setPolynomialFit (true);
  mls.setSearchMethod (tree);
  //mls.setSearchRadius (0.03);
  mls.setSearchRadius (0.1);

  // Reconstruct
  mls.process (mls_points);
  pcl::io::savePCDFile ("pc-mls.pcd", mls_points);
}

int main(int argc, char** argv)
{
  std::string config_file;
  std::string imagelistfn, disp_imagelistfn;
  std::string cam_params_file;
  std::string source_folder;
  std::string disp_folder, gt_file;
  std::string output_folder;

  try {
    po::options_description generic("Generic options");
    generic.add_options()
      ("help", "produce help message")
      ("config,c", po::value<std::string>(&config_file)->default_value("config.txt"), "config filename")
      ;
    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")
      ("output_folder,o", po::value<std::string>(&output_folder), "folder for output")
      ("disp_folder,d", po::value<std::string>(&disp_folder), "folder for output")
      ("groundtruth,g", po::value<std::string>(&gt_file), "groundtruth file")
      ("imglist,l", po::value<std::string>(&imagelistfn), "file with image list")
      ("disp_imglist,dl", po::value<std::string>(&disp_imagelistfn), "file with image list");

    po::options_description cmdline_options;
    cmdline_options.add(generic).add(config);

    po::options_description config_file_options;
    config_file_options.add(config);
    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")) {
      std::cout << generic;
      std::cout << config;
      return 0;
    }

    ifstream ifs(config_file.c_str());
    if (!ifs) {
      std::cout << "can not open config file: " << config_file << "\n";
      std::cout << generic;
      std::cout << config;
      return 0;
    }
    else {
      po::store(parse_config_file(ifs, config_file_options), vm);
      notify(vm);
    }
    std::cout << "Configuring done, using:" << endl;

    if(vm.count("camera_params")) {
      std::cout << "Camera params: ";
      std::cout << cam_params_file << endl;
    }
    if(vm.count("source_folder")) {
      std::cout << "Source folder: ";
      std::cout << source_folder << endl;
    }
    if(vm.count("output_folder")) {
      std::cout << "Output folder: ";
      std::cout << output_folder << endl;
    }
    if(vm.count("imglist")) {
      std::cout << "Image list file: ";
      std::cout << imagelistfn << endl;
    }
  }
  catch(std::exception& e) {
    std::cout << e.what() << "\n";
    return 1;
  }

  if(imagelistfn == "")
  {
    std::cout << "error: no xml image list given." << endl;
    return -1;
  }
  if(source_folder == "")
  {
    std::cout << "error: no output or source folder given." << endl;
    return -1;
  }

  std::vector<std::string> imagelist, disp_imagelist;
  bool ok = core::FormatHelper::readStringList(imagelistfn, imagelist);
  if(!ok || imagelist.empty())
  {
    std::cout << "can not open " << imagelistfn << " or the string list is empty" << endl;
    return -1;
  }
  ok = core::FormatHelper::readStringList(disp_imagelistfn, disp_imagelist);

  renderPointCloud(imagelist, disp_imagelist, gt_file, cam_params_file, source_folder, disp_folder, output_folder);

  return 0;
}


#include <vector>
#include <string>
#include <iostream>
#include <fstream>
using namespace std;

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

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace cv;

#include <pcl/common/common_headers.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <vtkPlaneSource.h>
#include <vtkImplicitPlaneWidget.h>

#include "../../../core/format_helper.h"
#include "../../../core/math_helper.h"

vtkSmartPointer<vtkPolyData> createPlane(const pcl::ModelCoefficients& coefficients)
{
  vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
  //plane->SetCenter(0,0,0);
  //plane->SetPoint1(10, 0, 0);
  //plane->SetPoint2(0, 0, 10);

  plane->SetNormal (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
  double norm_sqr = coefficients.values[0] * coefficients.values[0]
                  + coefficients.values[1] * coefficients.values[1]
                  + coefficients.values[2] * coefficients.values[2];

  plane->Push(-coefficients.values[3] / sqrt(norm_sqr));
  plane->SetResolution(200, 200);
  plane->Update();

  double pt1[3], pt2[3], orig[3];
  plane->GetPoint1(pt1);
  plane->GetPoint2(pt2);
  plane->GetOrigin(orig);
  // TODO: buged
  double scale = 10.0;
  for(int i = 0; i < 3; i++) {
    pt1[i] = scale * (pt1[i] - orig[i]);
    pt2[i] = scale * (pt2[i] - orig[i]);
  }
  plane->SetPoint1(pt1);
  plane->SetPoint2(pt2);
  plane->Update();
  return (plane->GetOutput());

  //vtkSmartPointer<vtkImplicitPlaneWidget> inf_plane = vtkSmartPointer<vtkImplicitPlaneWidget>::New ();
  //inf_plane->SetNormal(coefficients.values[0], coefficients.values[1], coefficients.values[2]);
  //inf_plane->SetOrigin(0, 0, 0);
  //vtkSmartPointer<vtkPolyData> pplane = vtkSmartPointer<vtkPolyData>::New();
  //inf_plane->SetEnabled(1);
  //inf_plane->GetPolyData(pplane);
  //return (pplane);
}

void createPolyFromPlane(const pcl::ModelCoefficients::Ptr coefficients,
                         pcl::PointCloud<pcl::PointXYZ>::Ptr poly_cloud)
{
  double a = coefficients->values[0];
  double b = coefficients->values[1];
  double c = coefficients->values[2];
  double d = coefficients->values[3];

  double x[4] = {-20, 20, 20, -20};
  double z[4] = {  0,  0, 100, 100};
  for(int i = 0; i < 4; i++) {
    double y = (a*x[i] + c*z[i] + d) / (-b);
    pcl::PointXYZ pt(x[i], y, z[i]);
    poly_cloud->points.push_back(pt);
  }
}

void triangulate(const double (&cam_params)[5], double x, double y, double disp, cv::Mat& pt3d) {
  double f = cam_params[0];
  double cx = cam_params[2];
  double cy = cam_params[3];
  double b = cam_params[4];

  pt3d.at<double>(0) = (x - cx) * b / disp;
  pt3d.at<double>(1) = (y - cy) * b / disp;
  pt3d.at<double>(2) = f * b / disp;
  pt3d.at<double>(3) = 1.0;
}

void renderPointCloud(const vector<string>& imagelist, const vector<string>& disp_imglist,
                      const string& gt_file, const string& cparams_file,
                      const string& source_folder, const string& disp_folder, const string& output_folder)
{
  double cam_params[5];
  core::FormatHelper::readCameraParams(cparams_file, cam_params);

  //pcl::PointCloud<pcl::PointXYZI> point_cloud;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);

  ifstream gt_fp(gt_file);
  Mat Rt = Mat::eye(4, 4, CV_64F);
  Mat pt3d = Mat::zeros(4, 1, CV_64F);
  Mat img_left;
  Mat img_disp;
  //for(size_t i = 0; i < disp_imglist.size(); i++) {
  //for(size_t i = 0; i < 30; i++) {
  double min_disp = 40.0;
  int img_step = 1;
  //size_t start_frame = 27;
  //size_t end_frame = 28;
  size_t start_frame = 100;
  size_t end_frame = 120;
  for(size_t i = 0; i < end_frame; i++) {
    core::FormatHelper::readNextMatrixKitti(gt_fp, Rt);
    if(!(i % img_step == 0) || i < start_frame) continue;
    cout << source_folder + imagelist[2*i] << "\n";
    img_left = imread(source_folder + imagelist[2*i], CV_LOAD_IMAGE_GRAYSCALE);

    //string fname = output_folder + imagelist[i].substr(9, imagelist[i].size()-13) + "_disp.png";
    string fname = output_folder + disp_imglist[i];
    img_disp = imread(disp_folder + disp_imglist[i], CV_LOAD_IMAGE_GRAYSCALE);
    for(int y = 0; y < img_disp.rows; y++) {
      for(int x = 0; x < img_disp.cols; x++) {
        double disp = img_disp.at<uint8_t>(y,x);
        if(disp > min_disp) {
          triangulate(cam_params, x, y, disp, pt3d);
          //cout << Rt << "\n";
          //cout << pt3d << "\n";
          //cout << Rt * pt3d << "\n";
          pt3d = Rt * pt3d;
          //cout << pt3d << "\n\n";
          pcl::PointXYZRGB point;
          //pcl::PointXYZI point;
          point.x = pt3d.at<double>(0);
          point.y = pt3d.at<double>(1);
          point.z = pt3d.at<double>(2);
          //point.intensity = img_left.at<uint8_t>(x,y);
          point.r = img_left.at<uint8_t>(y,x);
          point.g = img_left.at<uint8_t>(y,x);
          point.b = img_left.at<uint8_t>(y,x);
          point_cloud->points.push_back(point);
        }
      }
    }
    //imshow("disparity", img_disp);
    //waitKey(0);
  }
  cout << "Point cloud size: " << point_cloud->points.size() << "\n";
  point_cloud->width = point_cloud->points.size();
  point_cloud->height = 1;

  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
  // Optional
  seg.setOptimizeCoefficients(true);
  // Mandatory
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setMaxIterations(100000);
  seg.setDistanceThreshold(0.1);

  //// Create the filtering object: downsample the dataset using a leaf size of 1cm
  //pcl::VoxelGrid<pcl::PointXYZRGB> sor;
  //sor.setInputCloud(point_cloud);
  //sor.setLeafSize(0.02f, 0.02f, 0.02f);
  //sor.filter(*pc_filtered);
  //std::cerr << "PointCloud after filtering: " << pc_filtered->width * pc_filtered->height << " data points." << std::endl;
  //pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_planar(new pcl::PointCloud<pcl::PointXYZRGB>);
  //pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
  // Create the filtering object
  //pcl::ExtractIndices<pcl::PointXYZRGB> extract;
  //int i = 0, nr_points = (int)pc_filtered->points.size ();
  //// While 30% of the original cloud is still there
  //while(pc_filtered->points.size () >= 1.0 * nr_points)
  //{
  //  // Segment the largest planar component from the remaining cloud
  //  seg.setInputCloud(pc_filtered);
  //  seg.segment(*inliers, *coefficients);
  //  if(inliers->indices.size() == 0)
  //  {
  //    std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
  //    break;
  //  }

  //  std::cout << "Filtered size: " << pc_filtered->points.size() << "\n";
  //  // Extract the inliers
  //  extract.setInputCloud(pc_filtered);
  //  extract.setIndices(inliers);
  //  extract.setNegative(false);
  //  extract.filter(*cloud_planar);
  //  std::cerr << "PointCloud representing the planar component: " << cloud_planar->width * cloud_planar->height
  //            << " data points." << std::endl;

  //  // Create the filtering object
  //  extract.setNegative(true);
  //  extract.filter(*cloud_tmp);
  //  pc_filtered.swap(cloud_tmp);
  //  i++;
  //}

  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
  seg.setInputCloud(point_cloud->makeShared());
  seg.segment(*inliers, *coefficients);
  // paint the inlier points
  //for(size_t j = 0; j < inliers->indices.size(); j++) {
  //  //cout << j << "\n";
  //  int idx = inliers->indices[j];
  //  point_cloud->points[idx].r = 255;
  //  point_cloud->points[idx].g = 0;
  //  point_cloud->points[idx].b = 0;
  //}

  pcl::visualization::PCLVisualizer viewer("3D Viewer");
  viewer.setBackgroundColor(0, 0, 0);
  //pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr point_cloud_ptr(&point_cloud);
  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> hcolor(point_cloud);
  viewer.addPointCloud<pcl::PointXYZRGB>(point_cloud, hcolor, "reconstruction");

  //vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New();
  //vtkSmartPointer<vtkPolyData> plane = createPlane(*coefficients);
  //viewer.addModelFromPolyData(plane, "plane");
  //viewer.addPlane(*coefficients, "def_plane");

  //pcl::PointCloud<pcl::PointXYZ>::Ptr poly_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  //createPolyFromPlane(coefficients, poly_cloud);
  //viewer.addPolygon<pcl::PointXYZ>(poly_cloud, 0, 0, 180, "poly1");
  ////viewer.addPolygon<pcl::PointXYZ>(poly_cloud, 100, 100, 100, "poly1");
  //viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
  //                                   pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, "poly1");

  //viewer.addPolygon<pcl::PointXYZ>(poly_cloud, 0.0, 255.0, 0.0, "poly1");

  //vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
  //viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "reconstruction");
  viewer.addCoordinateSystem(1.0);
  viewer.initCameraParameters();

  // TODO
  //plot the point distances from fitted plane
  //try ti fit another plane with inliers only

  //Main loop
  while (!viewer.wasStopped()) {
    //viewer.spinOnce(100);        // 100
    viewer.spin();        // 100
    //boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    //boost::this_thread::sleep(boost::posix_time::microseconds(10000));
  }
}

int main(int argc, char** argv)
{
  string config_file;
  string imagelistfn, disp_imagelistfn;
  string cam_params_file;
  string source_folder;
  string disp_folder, gt_file;
  string output_folder;

  try {
    po::options_description generic("Generic options");
    generic.add_options()
      ("help", "produce help message")
      ("config,c", po::value<string>(&config_file)->default_value("config.txt"), "config filename")
      ;
    po::options_description config("Config file options");
    config.add_options()
      ("camera_params,p", po::value<string>(&cam_params_file)->default_value("camera_params.txt"),
       "camera params file")
      ("source_folder,s", po::value<string>(&source_folder), "folder with source")
      ("output_folder,o", po::value<string>(&output_folder), "folder for output")
      ("disp_folder,d", po::value<string>(&disp_folder), "folder for output")
      ("groundtruth,g", po::value<string>(&gt_file), "groundtruth file")
      ("imglist,l", po::value<string>(&imagelistfn), "file with image list")
      ("disp_imglist,dl", po::value<string>(&disp_imagelistfn), "file with image list")
      ;

    po::options_description cmdline_options;
    cmdline_options.add(generic).add(config);

    po::options_description config_file_options;
    config_file_options.add(config);
    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;
      cout << config;
      return 0;
    }

    ifstream ifs(config_file.c_str());
    if (!ifs) {
      cout << "can not open config file: " << config_file << "\n";
      cout << generic;
      cout << config;
      return 0;
    }
    else {
      po::store(parse_config_file(ifs, config_file_options), vm);
      notify(vm);
    }
    cout << "Configuring done, using:" << endl;

    if(vm.count("camera_params")) {
      cout << "Camera params: ";
      cout << cam_params_file << endl;
    }
    if(vm.count("source_folder")) {
      cout << "Source folder: ";
      cout << source_folder << endl;
    }
    if(vm.count("output_folder")) {
      cout << "Output folder: ";
      cout << output_folder << endl;
    }
    if(vm.count("imglist")) {
      cout << "Image list file: ";
      cout << imagelistfn << endl;
    }
  }
  catch(std::exception& e) {
    cout << e.what() << "\n";
    return 1;
  }

  if(imagelistfn == "")
  {
    cout << "error: no xml image list given." << endl;
    return -1;
  }
  if(source_folder == "")
  {
    cout << "error: no output or source folder given." << endl;
    return -1;
  }

  vector<string> imagelist, disp_imagelist;
  bool ok = core::FormatHelper::readStringList(imagelistfn, imagelist);
  if(!ok || imagelist.empty())
  {
    cout << "can not open " << imagelistfn << " or the string list is empty" << endl;
    return -1;
  }
  ok = core::FormatHelper::readStringList(disp_imagelistfn, disp_imagelist);

  renderPointCloud(imagelist, disp_imagelist, gt_file, cam_params_file, source_folder, disp_folder, output_folder);

  return 0;
}


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

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

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

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

/*! \file model/utilities.hpp
    \brief Shared utilities for model building and calibration
    \ingroup models
*/

#pragma once

#include <ored/marketdata/strike.hpp>

#include <qle/models/eqbsparametrization.hpp>
#include <qle/models/fxbsparametrization.hpp>
#include <qle/models/infdkparametrization.hpp>
#include <qle/models/infjyparameterization.hpp>
#include <qle/models/irlgm1fparametrization.hpp>
#include <qle/models/lgmcalibrationinfo.hpp>

#include <ql/models/calibrationhelper.hpp>

#include <boost/variant.hpp>

#include <vector>

namespace ore {
namespace data {
using namespace QuantExt;
using namespace QuantLib;

template <typename Helper> Real getCalibrationError(const std::vector<boost::shared_ptr<Helper>>& basket) {
    Real rmse = 0.0;
    for (auto const& h : basket) {
        Real tmp = h->calibrationError();
        rmse += tmp * tmp;
    }
    return std::sqrt(rmse / static_cast<Real>(basket.size()));
}

std::string getCalibrationDetails(
    LgmCalibrationInfo& info, const std::vector<boost::shared_ptr<BlackCalibrationHelper>>& basket,
    const boost::shared_ptr<IrLgm1fParametrization>& parametrization = boost::shared_ptr<IrLgm1fParametrization>());

std::string getCalibrationDetails(
    const std::vector<boost::shared_ptr<BlackCalibrationHelper>>& basket,
    const boost::shared_ptr<FxBsParametrization>& parametrization = boost::shared_ptr<FxBsParametrization>(),
    const boost::shared_ptr<IrLgm1fParametrization>& domesticLgm = boost::shared_ptr<IrLgm1fParametrization>());

std::string getCalibrationDetails(
    const std::vector<boost::shared_ptr<BlackCalibrationHelper>>& basket,
    const boost::shared_ptr<EqBsParametrization>& parametrization = boost::shared_ptr<EqBsParametrization>(),
    const boost::shared_ptr<IrLgm1fParametrization>& domesticLgm = boost::shared_ptr<IrLgm1fParametrization>());

std::string getCalibrationDetails(
    const std::vector<boost::shared_ptr<BlackCalibrationHelper>>& basket,
    const boost::shared_ptr<InfDkParametrization>& parametrization = boost::shared_ptr<InfDkParametrization>());

std::string getCalibrationDetails(const std::vector<boost::shared_ptr<CalibrationHelper>>& realRateBasket,
                                  const std::vector<boost::shared_ptr<CalibrationHelper>>& indexBasket,
                                  const boost::shared_ptr<InfJyParameterization>& parameterization,
                                  bool calibrateRealRateVol = false);

std::string getCalibrationDetails(const boost::shared_ptr<IrLgm1fParametrization>& parametrization);

//! Return an option's maturity date, given an explicit date or a period.
QuantLib::Date optionMaturity(const boost::variant<QuantLib::Date, QuantLib::Period>& maturity,
                              const QuantLib::Calendar& calendar,
                              const QuantLib::Date& referenceDate = Settings::instance().evaluationDate());

//! Return a cpi cap/floor strike value, the input strike can be of type absolute or atm forward
Real cpiCapFloorStrikeValue(const boost::shared_ptr<BaseStrike>& strike,
                            const boost::shared_ptr<ZeroInflationTermStructure>& curve,
                            const QuantLib::Date& optionMaturityDate);

//! Return a yoy cap/floor strike value, the input strike can be of type absolute or atm forward
Real yoyCapFloorStrikeValue(const boost::shared_ptr<BaseStrike>& strike,
                            const boost::shared_ptr<YoYInflationTermStructure>& curve,
                            const QuantLib::Date& optionMaturityDate);

} // namespace data
} // namespace ore


//
//  mesh_Generated_multi_faults.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/13/18.
//
//

#ifndef mesh_Generated_multi_faults_hpp
#define mesh_Generated_multi_faults_hpp

#include <stdio.h>
#include <iostream>
#include <Eigen/Eigen>
#include "Meshrectangular.hpp"
#include "update_mesh_topology.hpp"
#include "share_header.hpp"
using namespace Eigen;

void mesh_Generated_multi_faults(int x_min, int x_max ,int y_min, int y_max, double dx , double dy, int nx, int ny, MatrixXd &Nodes ,MatrixXi_rm &Element, ArrayXi &BIE_top_surf_nodes, ArrayXi &BIE_bot_surf_nodes, MatrixXd &fault_pos, std::vector<std::vector<int>> &fault_nodes);

#endif /* mesh_Generated_multi_faults_hpp */


#include <Eigen/Geometry>
#include <boost/format.hpp>

#include <trajopt_sco/expr_ops.hpp>
#include <trajopt_sco/modeling_utils.hpp>

#include "trajopt_interface/kinematic_terms.h"

using namespace std;
using namespace sco;
using namespace Eigen;

namespace trajopt_interface
{
VectorXd CartPoseErrCalculator::operator()(const VectorXd& dof_vals) const
{
  // TODO: create the actual error function from information in planning scene
  VectorXd err;
  return err;
}

VectorXd JointVelErrCalculator::operator()(const VectorXd& var_vals) const
{
  assert(var_vals.rows() % 2 == 0);
  // var_vals = (theta_t1, theta_t2, theta_t3 ... 1/dt_1, 1/dt_2, 1/dt_3 ...)
  int half = static_cast<int>(var_vals.rows() / 2);
  int num_vels = half - 1;
  // (x1-x0)*(1/dt)
  VectorXd vel = (var_vals.segment(1, num_vels) - var_vals.segment(0, num_vels)).array() *
                 var_vals.segment(half + 1, num_vels).array();

  // Note that for equality terms tols are 0, so error is effectively doubled
  VectorXd result(vel.rows() * 2);
  result.topRows(vel.rows()) = -(upper_tol_ - (vel.array() - target_));
  result.bottomRows(vel.rows()) = lower_tol_ - (vel.array() - target_);
  return result;
}

MatrixXd JointVelJacobianCalculator::operator()(const VectorXd& var_vals) const
{
  // var_vals = (theta_t1, theta_t2, theta_t3 ... 1/dt_1, 1/dt_2, 1/dt_3 ...)
  int num_vals = static_cast<int>(var_vals.rows());
  int half = num_vals / 2;
  int num_vels = half - 1;
  MatrixXd jac = MatrixXd::Zero(num_vels * 2, num_vals);

  for (int i = 0; i < num_vels; i++)
  {
    // v = (j_i+1 - j_i)*(1/dt)
    // We calculate v with the dt from the second pt
    int time_index = i + half + 1;
    jac(i, i) = -1.0 * var_vals(time_index);
    jac(i, i + 1) = 1.0 * var_vals(time_index);
    jac(i, time_index) = var_vals(i + 1) - var_vals(i);
    // All others are 0
  }

  // bottom half is negative velocities
  jac.bottomRows(num_vels) = -jac.topRows(num_vels);

  return jac;
}

}  // namespace trajopt_interface


/*
 * Description: Selection of time related utilities
 * Author: Michael Fletcher | EOS42
 * Date: 11/09/2018
 */
#ifndef EOSIO_ICO_SETTINGS_HPP
#define EOSIO_ICO_SETTINGS_HPP

#include <eosiolib/eosio.hpp>
#include <eosiolib/symbol.hpp>
#include <boost/assign/list_of.hpp>
#include <vector>

#include "declarations.hpp"

namespace eosio {

    struct section {
        uint32_t SPLIT_SECTION_PERCENTILE; /* size of each token payout section */
        uint32_t SPLIT_SECTION_PAYOUT_RATIO; /* tokens paid out per eos */
    };

    class settings {
        ///SETTINGS///

    public:

        /* account name of token contract */
        account_name TOKEN_CONTRACT = N(eosio.token);

        /* token name(Precision, name) */
        const symbol_type TOKEN_SYMBOL = S(4,DEF);

        /* cut-off time in seconds after launch, set to 0 to disable */
        const uint64_t CUT_OFF_TIME_SECONDS = ONE_WEEK * 4;

        /* the memo sent after a purchase */
        const char *PURCHASE_MEMO = "Thank you for purchasing DEF Token";

    /* set to empty for token to always be 1:1 */
        const std::vector<section> sections = boost::assign::list_of
            (section{10, 10}) /* first 10% will receive 10 tokens */
            (section{20, 5}) /* next 20% will receive 5 */
            (section{20, 3})
            (section{50, 1}); /* bottom 50% will receive 1 tokens per eos */

        ///END OF SETTINGS///
    };
}

#endif //EOSIO_ICO_SETTINGS_HPP


/*
 * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
 *
 * 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.
 */

#include "mav_trajectory_generation_ros/feasibility_base.h"

#include <cmath>
#include <limits>

#include <Eigen/Geometry>

#include <mav_msgs/default_values.h>

namespace mav_trajectory_generation {

std::string getInputFeasibilityResultName(InputFeasibilityResult fr) {
  switch (fr) {
    case InputFeasibilityResult::kInputFeasible:
      return "Feasible";
    case InputFeasibilityResult::kInputIndeterminable:
      return "Indeterminable";
    case InputFeasibilityResult::kInputInfeasibleThrustHigh:
      return "InfeasibleThrustHigh";
    case InputFeasibilityResult::kInputInfeasibleThrustLow:
      return "InfeasibleThrustLow";
    case InputFeasibilityResult::kInputInfeasibleVelocity:
      return "InfeasibleVelocity";
    case InputFeasibilityResult::kInputInfeasibleRollPitchRates:
      return "InfeasibleRollPitchRates";
    case InputFeasibilityResult::kInputInfeasibleYawRates:
      return "InfeasibleYawRates";
    case InputFeasibilityResult::kInputInfeasibleYawAcc:
      return "InfeasibleYawAcc";
  }
  return "Unknown!";
}

HalfPlane::HalfPlane(const Eigen::Vector3d& point,
                     const Eigen::Vector3d& normal)
    : point(point), normal(normal) {
  CHECK_GT(normal.norm(), 0.0) << "Invalid normal.";
  this->normal.normalize();
}

HalfPlane::HalfPlane(const Eigen::Vector3d& a, const Eigen::Vector3d& b,
                     const Eigen::Vector3d& c) {
  CHECK_NE(a, b);
  CHECK_NE(a, c);
  point = a;
  normal = (b - a).cross(c - a).normalized();
}

HalfPlane::Vector HalfPlane::createBoundingBox(
    const Eigen::Vector3d& point, const Eigen::Vector3d& bounding_box_size) {
  HalfPlane::Vector bounding_box;
  bounding_box.reserve(6);

  Eigen::Vector3d bbx_min = point - bounding_box_size / 2.0;
  Eigen::Vector3d bbx_max = point + bounding_box_size / 2.0;

  for (size_t axis = 0; axis < 3; axis++) {
    Eigen::Vector3d normal_min(Eigen::Vector3d::Zero());
    Eigen::Vector3d normal_max(Eigen::Vector3d::Zero());
    normal_min(axis) = 1.0;
    normal_max(axis) = -1.0;
    bounding_box.emplace_back(bbx_min, normal_min);
    bounding_box.emplace_back(bbx_max, normal_max);
  }
  return bounding_box;
}

FeasibilityBase::FeasibilityBase()
    : gravity_((Eigen::Vector3d() << 0.0, 0.0, mav_msgs::kGravity).finished()) {
}

FeasibilityBase::FeasibilityBase(const InputConstraints& input_constraints)
    : input_constraints_(input_constraints),
      gravity_((Eigen::Vector3d() << 0.0, 0.0, mav_msgs::kGravity).finished()) {
}

InputFeasibilityResult FeasibilityBase::checkInputFeasibilityTrajectory(
    const Trajectory& trajectory) const {
  InputFeasibilityResult result = InputFeasibilityResult::kInputIndeterminable;
  for (const Segment segment : trajectory.segments()) {
    result = checkInputFeasibility(segment);
    if (result != InputFeasibilityResult::kInputFeasible) {
      return result;
    }
  }
  return result;
}

bool FeasibilityBase::checkHalfPlaneFeasibility(
    const Trajectory& trajectory) const {
  for (const Segment segment : trajectory.segments()) {
    if (!checkHalfPlaneFeasibility(segment)) {
      return false;
    }
  }
  return true;
}

bool FeasibilityBase::checkHalfPlaneFeasibility(const Segment& segment) const {
  // Check user input.
  if (!(segment.D() == 3 || segment.D() == 4)) {
    LOG(WARNING) << "Feasibility check only implemented for segment dimensions "
                    "3 and 4. Got dimension "
                 << segment.D() << ".";
    return false;
  }

  for (const HalfPlane& half_plane : half_plane_constraints_) {
    // Project the segment position polynomial onto the half plane direction.
    // This polynomial represents the distance of the segment from the origin in
    // half plane direction.
    Polynomial projection(segment.N());
    for (size_t dim = 0; dim < 3; dim++) {
      projection += segment[dim] * half_plane.normal(dim);
    }
    // Find critical times.
    // These are the points in the projected polynomial with zero velocity. The
    // MAV changes directions towards the boundary normal.
    std::vector<double> extrema_candidates;
    projection.computeMinMaxCandidates(0.0, segment.getTime(),
                                       derivative_order::POSITION,
                                       &extrema_candidates);
    // Evaluate position at these critical points.
    for (double t : extrema_candidates) {
      if ((segment.evaluate(t).head(3) - half_plane.point).dot(half_plane.normal) <=
          0.0) {
        // The vector connecting the critical position and the point on the half
        // plane face different / perpendicular direction.
        return false;
      }
    }
  }
  return true;
}

}  // namespace mav_trajectory_generation


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SIMD_ASECH_INCLUDED
#define BOOST_SIMD_FUNCTION_SIMD_ASECH_INCLUDED

#include <boost/simd/function/scalar/asech.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/generic/function/asech.hpp>

#endif