/*
Copyright (c) 2014 Mohamed Elsabagh <melsabag@gmu.edu>

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

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

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

#ifndef SAXQUANTIZER_HPP
#define SAXQUANTIZER_HPP

#include <deque>
#include <vector>
#include <cassert>
#include <boost/math/distributions/normal.hpp>

using std::deque;
using std::vector;

namespace SaxQuantizer {

  /**
  * Helper routine to generate the SAX codebook (list of cutpoints) of a given size
  * @param <alphabet_size>: number of desired codewords
  * @param <cutpoints> (out): resulting codebook
  */
  inline void fill_cutpoints(size_t alphabet_size, vector<double> *cutpoints) {
    assert(alphabet_size > 0);
    static boost::math::normal dist(0.0, 1.0);
    std::cout << "alphabet: " << alphabet_size << std::endl;
    cutpoints->reserve(alphabet_size);
    cutpoints->push_back(-DBL_MAX);
    for (size_t i = 1; i < alphabet_size; ++i) {
      double cdf = ((double) i) / alphabet_size;
      cutpoints->push_back(quantile(dist, cdf));
    }
  }

  /**
   * Symbolic Aggregate Approximation with fractional sliding window, numerosity reduction, and scaling
   */
  class Sax {
  private:
    size_t m_window_size;
    size_t m_string_size;
    size_t m_alphabet_size;

    double m_baseline_mean;
    double m_baseline_stdev;
    vector<double> m_cutpoints;

    bool m_trained;

    /**
     * SAX with fractional sliding window and automatic scaling.
     * @param <it>: start iterator
     * @param <end>: end iterator
     * @param <syms> (out): quantized range
     */
    template<class Iter>
    void saxify(Iter it, const Iter end, vector<int> *syms) {
      // perform PAA using a fractional sliding window
      double paa[m_string_size];
      double paa_window = ((double) m_window_size) / m_string_size;

      double p = 0; // p for progress
      double w = 1; // w for weight
      size_t available = 0;

      for (size_t i = 0; i < m_string_size && it != end; ++i, ++available) {
        // normalize around baseline
        double normalized = (*it - m_baseline_mean) / m_baseline_stdev;

        paa[i] = 0;
        double j = 0;
        while (j < paa_window && it != end) {
          paa[i] += w * normalized; // sum of (partial) elements inside the window
          j += w;
          p += w;

          // window full
          if (paa_window == p) {
            if (fabs(w - 1.0) <= 0.01) {   // if last element fully consumed,
              ++it;                        // then just move next.
            } else {                       // o.w.,
              w = 1.0 - w;                 // set remaining portion.
            }

            p = 0;                         // reset progress

          // window not full, but next must be split
          } else if (paa_window - p < 1.0) {
            w = paa_window - p;            // set needed portion
            ++it;                          // move to next

          // window not full, next can be fully consumed
          } else {
            ++it;                          // move to next
          }
        }

        paa[i] /= j; // averaging
      }

      // map to symbols. 0-based.
      for (size_t i = 0; i < available; ++i) {
        int cnt = -1;
        for (const auto & cp : m_cutpoints) {
          if (paa[i] >= cp) ++cnt;
        }
        syms->push_back(cnt);
      }
    }

  public:
    /**
    * Constructs a SAX quantizer of a given window size, string size and alphabet size.
    * @param <window_size>: sliding window size
    * @param <string_size>: output string size for each sliding window (can be greater than window_size)
    * @param <alphabet_size>: number of codewords
    */
    Sax(size_t window_size, size_t string_size, size_t alphabet_size)
     : m_window_size(window_size), m_string_size(string_size), m_alphabet_size(alphabet_size),
       m_baseline_mean(0), m_baseline_stdev(1), m_trained(false) {

      assert(window_size > 0);
      assert(string_size > 0);
      assert(alphabet_size > 0);

      fill_cutpoints(alphabet_size, &m_cutpoints);
    }

    virtual ~Sax() {
      m_cutpoints.clear();
    }

    /**
     * Trains the quantizer from a given sample. This sets the baseline mean and stdevs, which are used in
     * normalizing the input.
     *
     * @param <samples>: list of training values
     */
    template<typename Container>
    void train(const Container & samples) {
      double mean = 0;
      double stdev = DBL_MIN;

      assert(!samples.empty());

      if (samples.size() < 2) {
        mean = samples[0];
        stdev = DBL_MIN;

      } else {
        size_t n = 0;
        double M2 = 0;
        for (const auto & val : samples) {
          ++n;
          double delta = val - mean;
          mean += delta / n;
          M2 += delta * (val - mean);
        }
        stdev = sqrt(M2 / (n-1));
      }

      if (stdev == 0) stdev = DBL_MIN;

      m_baseline_mean = mean;
      m_baseline_stdev = stdev;

      m_trained = true;
    }

    /**
     * Quantizes the given input sequence into a discrete alphabet using SAX.
     * Calling this method will also train the quantizer using the input sequence, if
     * not already trained.
     *
     * @param <seq>: the input sequence to be quantized
     * @param <qseq> (out): quantized output sequence
     * @param reduce (default true): if true, applies run-length numerosity reduction
     *
     * Returns the number of consumed symbols from the input sequence.
     */
    template<typename Container>
    size_t quantize(const Container & seq, vector<int> *qseq, bool reduce=true) {
      if (!m_trained) train(seq);

      vector<int> buf1, buf2;
      auto *syms_buf = &buf1;
      auto *old_syms_buf = &buf2;

      size_t consumed = 0;
      for (consumed = 0; consumed < seq.size(); ++consumed) {

        if (reduce) { // run-length numerosity reduction
          syms_buf->clear();
          saxify(seq.begin() +consumed, seq.end(), syms_buf);

          // skip window if same as previous
          if (*syms_buf != *old_syms_buf) {
            qseq->insert(qseq->end(), syms_buf->begin(), syms_buf->end());
            std::swap(syms_buf, old_syms_buf);
          }

        } else { // no reduction
          saxify(seq.begin() +consumed, seq.end(), qseq);
        }

        // ignore excess elements, if sequence size isn't a multiple of window size
        //if (seq.size() - consumed <= m_window_size) break;
      }

      return consumed;
    }

    /**
     * Returns the order of the quantizer (here, the window size)
     */
    inline size_t order() const {
      return m_window_size;
    }

    /**
     * Returns the compression ratio of the quantizer (input size / output size)
     */
    inline double ratio() const {
      return ((double) m_window_size) / m_string_size;
    }
  };

}

#endif


#ifndef BOOST_METAPARSE_GETTING_STARTED_11_HPP
#define BOOST_METAPARSE_GETTING_STARTED_11_HPP

// Automatically generated header file

// Definitions before section 10
#include "10.hpp"

// Definitions of section 10
using lparen_token = token<lit_c<'('>>;

using rparen_token = token<lit_c<')'>>;

using plus_exp1 =
 foldl_start_with_parser<
   sequence<one_of<plus_token, minus_token>, mult_exp4>,
   mult_exp4,
   boost::mpl::quote2<binary_op>
 >;

using paren_exp1 = sequence<lparen_token, plus_exp1, rparen_token>;

#include <boost/metaparse/middle_of.hpp>

using paren_exp2 = middle_of<lparen_token, plus_exp1, rparen_token>;

using primary_exp1 = one_of<int_token, paren_exp2>;

struct plus_exp2;

using paren_exp3 = middle_of<lparen_token, plus_exp2, rparen_token>;

using primary_exp2 = one_of<int_token, paren_exp2>;

using unary_exp2 =
 foldr_start_with_parser<
   minus_token,
   primary_exp2,
   boost::mpl::lambda<boost::mpl::negate<boost::mpl::_1>>::type
 >;

using mult_exp5 =
 foldl_start_with_parser<
   sequence<one_of<times_token, divides_token>, unary_exp2>,
   unary_exp2,
   boost::mpl::quote2<binary_op>
 >;

struct plus_exp2 :
 foldl_start_with_parser<
   sequence<one_of<plus_token, minus_token>, mult_exp5>,
   mult_exp5,
   boost::mpl::quote2<binary_op>
 > {};

using exp_parser19 = build_parser<plus_exp2>;

// query:
//    exp_parser19::apply<BOOST_METAPARSE_STRING("(1 + 2) * 3")>::type

#endif


#include <iostream>
#include <iomanip>
#include <boost/lexical_cast.hpp>

#include "../src/matrixutils.h"

void test_rotate270() {
	std::cout << "test matrixutils tools" << std::endl;
	Resolution vr = {.width = 9, .height = 16};

	std::string res[vr.height][vr.width];

	std::cout << "原始图像栅格" << std::endl;
	for (int x = 0; x < vr.width; ++x) {
		for (int y = 0; y < vr.height; ++y) {
			MES in = {.x = x, .y = y};
			MES out = rotateMatrix270(in, vr);
			std::cout << x << "," << y << " ";
			res[out.x][out.y] = boost::lexical_cast<std::string>(x) + "," + boost::lexical_cast<std::string>(y);
		}

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

	std::cout << std::endl;

	std::cout << "转换270度后图像栅格" << std::endl;
	for (int x = 0; x < vr.height; ++x) {
		for (int y = 0; y < vr.width; ++y) {
			std::cout << std::setw(4) << std::setfill(' ') << res[x][y] << " ";
		}

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

void test_rotate90() {
	std::cout << "test matrixutils tools" << std::endl;
	Resolution vr = {.width = 9, .height = 16};

	std::string res[vr.height][vr.width];

	std::cout << "原始图像栅格" << std::endl;
	for (int x = 0; x < vr.width; ++x) {
		for (int y = 0; y < vr.height; ++y) {
			MES in = {.x = x, .y = y};
			MES out = rotateMatrix90(in, vr);
			std::cout << x << "," << y << " ";
			res[out.x][out.y] = boost::lexical_cast<std::string>(x) + "," + boost::lexical_cast<std::string>(y);
		}

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

	std::cout << std::endl;

	std::cout << "转换90度后图像栅格" << std::endl;
	for (int x = 0; x < vr.height; ++x) {
		for (int y = 0; y < vr.width; ++y) {
			std::cout << std::setw(4) << std::setfill(' ') << res[x][y] << " ";
		}

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

int main(int argc, const char *argv[]) {
	test_rotate270();
	test_rotate90();
	return 0;
}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 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_SCALAR_REM_2PI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_REM_2PI_HPP_INCLUDED

#include <boost/simd/function/definition/rem_2pi.hpp>
#include <boost/simd/arch/common/generic/function/rem_2pi.hpp>

#endif


#include <Eigen/Dense>

int main(int argc, char *argv[])
{
  Eigen::Matrix3d mat3d;
  return 0;
}


// Copyright (c) 2015 The Bitcoin Core developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "main.h"
#include "pow.h"
#include "util.h"
#include "test/test_bitcoin.h"

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

using namespace std;

BOOST_FIXTURE_TEST_SUITE(pow_tests, BasicTestingSetup)

/*
 * Test that GetNextWorkRequired() is correct for block, and start populating next block
 */
void TestNextWorkRequired(CBlockIndex* const block, CBlockIndex& nextBlock, uint32_t requiredWorkExpected,
        const Consensus::Params& params) {

    uint32_t nextWorkRequired = GetNextWorkRequired(block, nextBlock.nTime, params);
    BOOST_CHECK_EQUAL(nextWorkRequired, requiredWorkExpected);
    nextBlock.pprev = block;
    nextBlock.nBits = nextWorkRequired;
    nextBlock.nHeight = (block == nullptr ? 1 : block->nHeight + 1);

    return;
}

// Build a main chain from genesis and check WTEMA target along the way
BOOST_AUTO_TEST_CASE(GetNextWorkRequired_test)
{
    SelectParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = Params().GetConsensus();

    const arith_uint256 bnPowLimit = UintToArith256(params.powLimit);
    const uint32_t nPowLimit = bnPowLimit.GetCompact();

    uint32_t requiredWorkExpected;
    CBlockIndex b0, b1, b2, b3, b4;

    // Genesis block
    b0.nTime = 0;
    requiredWorkExpected = nPowLimit;
    TestNextWorkRequired(nullptr, b0, requiredWorkExpected, params);

    // Bump against minimum difficulty (max target)
    b1.nTime = b0.nTime + 573;
    requiredWorkExpected = nPowLimit;
    TestNextWorkRequired(&b0, b1, requiredWorkExpected, params);

    // Slight target decrease
    b2.nTime = b1.nTime + 572;
    requiredWorkExpected = 0x1d00ff00;
    TestNextWorkRequired(&b1, b2, requiredWorkExpected, params);

    // Exactly 10 minutes - target increase under RTT
    b3.nTime = b2.nTime + params.nPowTargetSpacing;
    requiredWorkExpected = nPowLimit;
    TestNextWorkRequired(&b2, b3, requiredWorkExpected, params);

    // 1 second - massive target decrease
    b4.nTime = b3.nTime + 1;
    requiredWorkExpected = 0x16020c9e;
    TestNextWorkRequired(&b3, b4, requiredWorkExpected, params);
}

BOOST_AUTO_TEST_CASE(GetBlockProofEquivalentTime_test)
{
    SelectParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = Params().GetConsensus();

    std::vector<CBlockIndex> blocks(10000);
    for (int i = 0; i < 10000; i++) {
        blocks[i].pprev = i ? &blocks[i - 1] : NULL;
        blocks[i].nHeight = i;
        blocks[i].nTime = 1269211443 + i * params.nPowTargetSpacing;
        blocks[i].nBits = 0x207fffff; /* target 0x7fffff000... */
        blocks[i].nChainWork = (i ? blocks[i - 1].nChainWork : arith_uint256(0))
                               + GetBlockProof(blocks[i]);
    }

    for (int j = 0; j < 1000; j++) {
        CBlockIndex *p1 = &blocks[GetRand(10000)];
        CBlockIndex *p2 = &blocks[GetRand(10000)];
        CBlockIndex *p3 = &blocks[GetRand(10000)];

        int64_t tdiff = GetBlockProofEquivalentTime(*p1, *p2, *p3, params);
        BOOST_CHECK_EQUAL(tdiff, p1->GetBlockTime() - p2->GetBlockTime());
    }
}

BOOST_AUTO_TEST_CASE(CheckProofOfWork_test)
{
    SelectParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = Params().GetConsensus();
    const int64_t T = params.nPowTargetSpacing * RTT_RETARGET;
    const arith_uint256 bnPowLimit = UintToArith256(params.powLimit);
    const uint32_t nPowLimit = bnPowLimit.GetCompact();

    // Consensus minimum difficulty
    BOOST_CHECK_EQUAL(nPowLimit, 0x1d00ffff);
    // Real-time targeting constant
    BOOST_CHECK(RTT_CONSTANT == UintToArith256(uint256S("a099408f761")));

    // Bump against minimum difficulty
    arith_uint256 overLimit = bnPowLimit * 11 / 10;
    BOOST_CHECK(!CheckProofOfWork(uint256(), overLimit.GetCompact() , T, params));

    // Parameter checks

    // blocksecond = 0
    BOOST_CHECK(!CheckProofOfWork(uint256(), nPowLimit, 0, params));
    // Negative nBits
    BOOST_CHECK(!CheckProofOfWork(uint256(), 0x1d800000, T, params));
    // nBits overflow
    BOOST_CHECK(!CheckProofOfWork(uint256(), 0x22000001, T, params));
    // 0 target
    BOOST_CHECK(!CheckProofOfWork(uint256(), 0x1d000000, T, params));

    // Valid hash (zero)
    BOOST_CHECK(CheckProofOfWork(uint256(), nPowLimit, T, params));
    // This should not pass on mainnet (which we are on)
    BOOST_CHECK(!CheckProofOfWork(MAX_HASH, nPowLimit, T, params));

    // Subtarget checks

    auto CheckSubTargetBasic = [](arith_uint256 bnTarget, uint32_t t) {
        arith_uint256 expected, result;
        result = GetSubTarget(bnTarget, t);
        expected = bnTarget / RTT_CONSTANT * t * t * t * t * t;
        BOOST_CHECK(result == expected);
    };
    // Minimum difficulty
    CheckSubTargetBasic(bnPowLimit, T);
    // Minimum difficulty, blocksecond=1
    CheckSubTargetBasic(bnPowLimit, 1);
    // Minimum difficulty, max blocksecond
    CheckSubTargetBasic(0x04000000, MAX_BLOCKSECOND - 1);
    // Minimum difficulty, ridiculous blocksecond
    BOOST_CHECK(GetSubTarget(bnPowLimit, MAX_BLOCKSECOND * 10000) ==
                GetSubTarget(bnPowLimit, MAX_BLOCKSECOND));
    // (At regtest min difficulty, target needs to be capped at MAX_HASH)
    arith_uint256 bnRegtestLimit;
    bnRegtestLimit.SetCompact(0x207FFFFF);
    BOOST_CHECK(GetSubTarget(bnRegtestLimit, MAX_BLOCKSECOND) == UintToArith256(MAX_HASH));
    BOOST_CHECK(GetSubTarget(bnRegtestLimit, 467) == UintToArith256(MAX_HASH));
    BOOST_CHECK(GetSubTarget(bnRegtestLimit, 466) == bnRegtestLimit / RTT_CONSTANT * 466 * 466 * 466 * 466 * 466);

}

BOOST_AUTO_TEST_SUITE_END()


///////////////////////////////////////////////////////////////////////////////
// tree_view::example::tree.cpp                                              //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#include <boost/format.hpp>
#include <boost/tree_view/stage.hpp>
#include <boost/tree_view/node.hpp>
#include <boost/tree_view/dynamic_stage.hpp>
#include <libs/tree_view/example/tree.h>

void example_tree(std::ostream& out){
    out << "-> example_tree " << std::endl;

    using namespace boost;

    const unsigned n_branches = 3;
    typedef tree_view::dynamic_stage<n_branches>     stage_;
    typedef tree_view::node<n_branches>              node_;

    using namespace boost;
    const unsigned n_stages = 4;
    const node_ root_node;

    out << (format("{[i(j),i(j+1)) : j=0,...,%1%}:")%(n_stages-1)).str();
    for(unsigned j = 0; j<n_stages; j++){
        out
            << '('
            << stage_::position_first(j)
            << ','
            << stage_::position_last(j)
            << ')';
    }
    out << std::endl;
    node_ the_last = last( root_node, n_stages-1);

    {   // Breadth-first visitation
        out << node_::header << std::endl;
        node_ node = root_node;
        unsigned old_stage = node.stage;
        do{
            if(node.stage != old_stage){
                out << std::endl;
                old_stage = node.stage;
            }
            out << node;
        }while(
            ++node< the_last
        );
    }

    out << std::endl;
    {   // Breadth-first visitation, in reverse
        out << node_::header;
        node_ node = the_last;

        unsigned old_stage = node.stage;
        while(node.stage > 0){
            --node;
            if(node.stage != old_stage){
                old_stage = node.stage;
                out << std::endl;
            }
            out << node;
        }
    }

    out << " <- ";
}


/**
 * @file helper.hpp
 * @author francois.hamonic@gmail.com
 * @brief
 * @version 0.1
 * @date 2021-07-19
 */
#ifndef HELPER_HPP
#define HELPER_HPP

#include <math.h>
#include <filesystem>
#include <iterator>

#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>

#include "landscape/decored_landscape.hpp"
#include "landscape/mutable_landscape.hpp"

#include "Eigen/Dense"
#include "algorithms/multiplicative_dijkstra.hpp"
#include "lemon/dijkstra.h"

#include "fast-cpp-csv-parser/csv.h"
#include "solvers/concept/solver.hpp"

#include "lemon/dim2.h"
#include "lemon/graph_to_eps.h"

#include "indices/eca.hpp"

namespace Helper {
/**
 * @brief Compute the distance matrix of a graph.
 *
 * Generic implementation of computing the distance matrix of a graph by running
 * dijkstra for each node. The length map values must support numeric
 * operations. Complexities are $O((m+n) \cdot \log n)$ time and $O(n^2)$ space,
 * where $n$ is the number of nodes and $m$ the number of arcs. Diagonal entries
 * are $0$ and unreachable nodes entries are
 * $std::numeric_limits<Value>::max()$.
 *
 * @tparam GR : type of graph
 * @tparam DM : type of length map
 * @param g : graph
 * @param l : length map
 * @return Eigen::Matrix<typename DM::Value, Eigen::Dynamic, Eigen::Dynamic>*
 */
template <typename GR, typename DM>
static Eigen::Matrix<typename DM::Value, Eigen::Dynamic, Eigen::Dynamic> *
distanceMatrix(const GR & g, DM & l) {
    using Value = typename DM::Value;

    const int n = lemon::countNodes(g);
    Eigen::Matrix<Value, Eigen::Dynamic, Eigen::Dynamic> * distances =
        new Eigen::Matrix<Value, Eigen::Dynamic, Eigen::Dynamic>(n, n);
    distances->fill(std::numeric_limits<Value>::max());

    lemon::SimplerDijkstra<GR, DM> dijkstra(g, l);

    for(typename GR::NodeIt s(g); s != lemon::INVALID; ++s) {
        const int id_s = g.id(s);
        dijkstra.run(s);

        for(typename GR::NodeIt t(g); t != lemon::INVALID; ++t) {
            const int id_t = g.id(t);
            Value & d_st = (*distances)(id_s, id_t);
            if(!dijkstra.reached(t)) {
                d_st = std::numeric_limits<Value>::max();
                continue;
            }
            d_st = dijkstra.dist(t);
        }
    }
    return distances;
}

/**
 * @brief Compute the distance matrix of a graph.
 *
 * Generic implementation of computing the distance matrix of a graph by running
 * dijkstra for each node. The probability map values must support numeric
 * operations. Complexities are $O((m+n) \cdot \log n)$ time and $O(n^2)$ space,
 * where $n$ is the number of nodes and $m$ the number of arcs. Diagonal entries
 * are $1$ and unreachable nodes entries are $0$.
 *
 * @tparam GR : type of graph
 * @tparam DM : type of length map
 * @param g : graph
 * @param p : probability map
 * @return Eigen::Matrix<typename DM::Value, Eigen::Dynamic, Eigen::Dynamic>*
 */
template <typename GR, typename DM>
static Eigen::Matrix<typename DM::Value, Eigen::Dynamic, Eigen::Dynamic> *
multDistanceMatrix(const GR & g, DM & p) {
    using Value = typename DM::Value;

    const int n = lemon::countNodes(g);
    Eigen::Matrix<Value, Eigen::Dynamic, Eigen::Dynamic> * distances =
        new Eigen::Matrix<Value, Eigen::Dynamic, Eigen::Dynamic>(n, n);
    distances->fill(0);

    lemon::MultiplicativeSimplerDijkstra<GR, DM> dijkstra(g, p);

    for(typename GR::NodeIt s(g); s != lemon::INVALID; ++s) {
        const int id_s = g.id(s);
        dijkstra.run(s);

        for(typename GR::NodeIt t(g); t != lemon::INVALID; ++t) {
            const int id_t = g.id(t);
            Value & d_st = (*distances)(id_s, id_t);
            if(!dijkstra.reached(t)) {
                d_st = 0;
                continue;
            }
            d_st = dijkstra.dist(t);
        }
    }
    return distances;
}

template <typename LS>
double minNonZeroQuality(const LS & landscape) {
    using Graph = typename LS::Graph;
    const Graph & graph = landscape.getNetwork();
    double min = std::numeric_limits<double>::max();
    for(typename Graph::NodeIt v(graph); v != lemon::INVALID; ++v) {
        if(landscape.getQuality(v) == 0) continue;
        min = std::min(min, landscape.getQuality(v));
    }
    return min;
}

/**
 * @brief Compute the centrality of each arc, i.e the number of shortest paths
 * that contain it
 * @tparam GR
 * @tparam LM
 * @param graph
 * @param lengthMap
 * @return GR::ArcMap<int>*
 */
template <typename Graph, typename LengthMap>
std::unique_ptr<typename Graph::template ArcMap<int>> arcCentralityMap(
    const Graph & graph, const LengthMap & lengthMap) {
    using PredMap = typename Graph::template NodeMap<typename Graph::Arc>;

    std::unique_ptr<typename Graph::template ArcMap<int>> centralityMap =
        std::make_unique<typename Graph::template ArcMap<int>>(graph, 0);

    lemon::Dijkstra<Graph, LengthMap> dijkstra(graph, lengthMap);

    for(typename Graph::NodeIt s(graph); s != lemon::INVALID; ++s) {
        dijkstra.run(s);
        const PredMap & predMap = dijkstra.predMap();
        for(typename Graph::NodeIt t(graph); t != lemon::INVALID; ++t) {
            if(!dijkstra.reached(t)) continue;
            typename Graph::Node u = t;
            while(u != s) {
                (*centralityMap)[predMap[u]] += 1;
                u = graph.source(predMap[u]);
            }
        }
    }
    return centralityMap;
}

/**
 * @brief Compute the centrality of each arc, i.e the number of shortest paths
 * that contain it
 * @tparam GR
 * @tparam LM
 * @param graph
 * @param lengthMap
 * @return GR::ArcMap<int>*
 */
template <typename LS>
std::unique_ptr<typename LS::Graph::template ArcMap<double>>
corridorCentralityMap(const LS & landscape) {
    using Graph = typename LS::Graph;
    using PredMap = typename Graph::template NodeMap<typename Graph::Arc>;
    const Graph & graph = landscape.getNetwork();

    std::unique_ptr<typename Graph::template ArcMap<double>> centralityMap =
        std::make_unique<typename Graph::template ArcMap<double>>(graph, 0);

    lemon::Dijkstra<Graph, typename LS::ProbabilityMap> dijkstra(
        graph, landscape.getProbabilityMap());

    for(typename Graph::NodeIt s(graph); s != lemon::INVALID; ++s) {
        if(landscape.getQuality(s) == 0) continue;
        dijkstra.run(s);
        const PredMap & predMap = dijkstra.predMap();
        for(typename Graph::NodeIt t(graph); t != lemon::INVALID; ++t) {
            if(landscape.getQuality(t) == 0) continue;
            if(!dijkstra.reached(t)) continue;
            typename Graph::Node u = t;
            while(u != s) {
                (*centralityMap)[predMap[u]] += 1;
                u = graph.source(predMap[u]);
            }
        }
    }
    return centralityMap;
}

/**
 * @brief Computes the value of the ECA index of the specified landscape
 * graph.
 *
 * @time \f$O(n \cdot (m + n) \log n)\f$ where \f$n\f$ is the number of
 * nodes and \f$m\f$ the number of arcs
 * @space \f$O(m)\f$ where \f$m\f$ is the number of arcs
 */
template <typename GR, typename QM, typename PM>
std::vector<std::pair<typename GR::Node, double>> computeDistancePairs(
    const GR & graph, const QM & qualityMap, const PM & probabilityMap,
    const typename GR::Node s) {
    using Node = typename GR::Node;

    std::vector<std::pair<Node, double>> result;
    result.reserve(lemon::countNodes(graph));
    lemon::MultiplicativeSimplerDijkstra<GR, PM> dijkstra(graph,
                                                          probabilityMap);
    if(qualityMap[s] != 0) {
        dijkstra.init(s);
        while(!dijkstra.emptyQueue()) {
            result.emplace_back(dijkstra.processNextNode());
            if(result.back().second == 0.0) {
                result.pop_back();
                break;
            }
        }
    }
    return result;
}

template <typename LS>
std::vector<std::pair<typename LS::Node, double>> computeDistancePairs(
    const LS & landscape, const typename LS::Node s) {
    auto result =
        computeDistancePairs(landscape.getNetwork(), landscape.getQualityMap(),
                             landscape.getProbabilityMap(), s);
    return result;
}

template <typename LS>
double averageRatioOfNodesInECARealization(double ratio_of_eca,
                                           LS && landscape) {
    using Graph = typename std::remove_reference<LS>::type::Graph;
    using NodeIt = typename std::remove_reference<LS>::type::NodeIt;

    const Graph & graph = landscape.getNetwork();

    double average_sum = 0.0;
    int cpt = 0;
    for(NodeIt s(graph); s != lemon::INVALID; ++s) {
        const auto sorted_pairs = computeDistancePairs(landscape, s);

        if(sorted_pairs.empty()) continue;

        const double contribution_max = std::transform_reduce(
            sorted_pairs.begin(), sorted_pairs.end(), 0.0, std::plus<double>(),
            [&](const auto & p) {
                return landscape.getQuality(s) * landscape.getQuality(p.first) *
                       p.second;
            });

        int node_count = 0;
        double contribution_sum = 0.0;
        for(const auto & [t, p_st] : sorted_pairs) {
            if(contribution_sum / contribution_max >= ratio_of_eca) break;
            ++node_count;
            contribution_sum +=
                landscape.getQuality(s) * landscape.getQuality(t) * p_st;
        }

        average_sum += node_count / static_cast<double>(sorted_pairs.size());
        ++cpt;
    }
    return average_sum / cpt;
}

template <typename LS>
DecoredLandscape<LS> decore_landscape(const LS & landscape,
                                      const RestorationPlan<LS> & plan,
                                      const Solution & solution) {
    using Graph = typename LS::Graph;
    const Graph & graph = landscape.getNetwork();
    DecoredLandscape<LS> decored_landscape(landscape);

    for(typename Graph::NodeIt u(graph); u != lemon::INVALID; ++u)
        for(const auto & e : plan[u])
            decored_landscape.getQualityRef(u) +=
                solution[e.option] * e.quality_gain;
    for(typename Graph::ArcIt a(graph); a != lemon::INVALID; ++a)
        for(const auto & e : plan[a])
            decored_landscape.getProbabilityRef(a) = std::max(
                decored_landscape.getProbability(a),
                landscape.getProbability(a) +
                    solution[e.option] *
                        (e.restored_probability - landscape.getProbability(a)));

    return decored_landscape;
}

template <typename LS>
DecoredLandscape<LS> decore_landscape(const LS & landscape,
                                      const RestorationPlan<LS> & plan) {
    using Graph = typename LS::Graph;
    const Graph & graph = landscape.getNetwork();
    DecoredLandscape<LS> decored_landscape(landscape);

    for(typename Graph::NodeIt u(graph); u != lemon::INVALID; ++u)
        for(const auto & e : plan[u])
            decored_landscape.getQualityRef(u) += e.quality_gain;
    for(typename Graph::ArcIt a(graph); a != lemon::INVALID; ++a)
        for(const auto & e : plan[a])
            decored_landscape.getProbabilityRef(a) = std::max(
                decored_landscape.getProbability(a), e.restored_probability);

    return decored_landscape;
}

void printSolution(const MutableLandscape & landscape,
                   const RestorationPlan<MutableLandscape> & plan,
                   std::string name, concepts::Solver & solver, double B,
                   const Solution & solution);

template <typename LS_From, typename LS_To>
void copyPlan(
    RestorationPlan<LS_To> & contracted_plan,
    const RestorationPlan<LS_From> & plan,
    const typename LS_From::Graph::template NodeMap<typename LS_To::Node> &
        nodesRef,
    const typename LS_From::Graph::template ArcMap<typename LS_To::Arc> &
        arcsRef) {
    assert(contracted_plan.getNbOptions() == 0);
    for(const RestorationPlan<MutableLandscape>::Option i : plan.options())
        contracted_plan.addOption(plan.getCost(i));

    const typename LS_From::Graph & from_graph =
        plan.getLandscape().getNetwork();

    for(typename LS_From::Graph::NodeIt u(from_graph); u != lemon::INVALID; ++u)
        for(const auto & e : plan[u])
            contracted_plan.addNode(e.option, nodesRef[u], e.quality_gain);
    for(typename LS_From::Graph::ArcIt a(from_graph); a != lemon::INVALID; ++a)
        for(const auto & e : plan[a])
            contracted_plan.addArc(e.option, arcsRef[a],
                                   e.restored_probability);
}

// need to include the binary search tree for y-h , y+h search
std::pair<MutableLandscape::Node, MutableLandscape::Node> neerestNodes(
    const MutableLandscape & landscape);

void assert_well_formed(const MutableLandscape & landscape,
                        const RestorationPlan<MutableLandscape> & plan);

// template <typename GR>
// class NodeIterator {
// public:
//     using Node = typename GR::Node;
//     using NodeIt = typename GR::NodeIt;

//     using difference_type = std::ptrdiff_t;
//     using value_type = Node;
//     using pointer = void;
//     using reference = const Node&;
//     using iterator_category = std::input_iterator_tag;

// private:
//     NodeIt it;
// public:
//     NodeIterator() : it(lemon::INVALID) {}
//     NodeIterator(const GR & graph) : it(graph) {}
//     NodeIterator(const NodeIterator<GR> & o) : it(o.it) {}

//     NodeIterator& operator=(const NodeIterator<GR> & o) { it = o.it; return
//     *this; }

//     value_type operator*() const { return static_cast<value_type>(it); }
//     NodeIterator operator++(int) const { return ++NodeIterator<GR>(*this); }
//     NodeIterator& operator++() { ++it; return *this; }

//     bool operator==(const NodeIterator<GR> & o) const { return o.it == it; }
//     bool operator!=(const NodeIterator<GR> & o) const { return o.it != it; }
// };

// template <typename GR>
// std::pair<NodeIterator<GR>, NodeIterator<GR>> nodesRange(const GR & graph) {
//     return std::make_pair(NodeIterator(graph), NodeIterator<GR>());
// }
}  // namespace Helper

template <typename LS>
double max_flow_in(const LS & landscape, const RestorationPlan<LS> & plan,
                   typename LS::Node t) {
    using Graph = typename LS::Graph;
    using ProbabilityMap = typename LS::ProbabilityMap;
    using Reversed = lemon::ReverseDigraph<const Graph>;

    const Graph & original_g = landscape.getNetwork();
    Reversed reversed_g(original_g);
    ProbabilityMap probabilities(original_g);

    for(typename Graph::ArcIt b(original_g); b != lemon::INVALID; ++b) {
        probabilities[b] = landscape.getProbability(b);
        for(auto const & e : plan[b])
            probabilities[b] =
                std::max(probabilities[b], e.restored_probability);
    }

    lemon::MultiplicativeSimplerDijkstra<Reversed, ProbabilityMap> dijkstra(
        reversed_g, probabilities);
    double sum = 0;
    dijkstra.init(t);
    while(!dijkstra.emptyQueue()) {
        std::pair<typename Graph::Node, double> pair =
            dijkstra.processNextNode();
        typename Graph::Node v = pair.first;
        const double p_tv = pair.second;
        sum += landscape.getQuality(v) * p_tv;
        for(auto const & e : plan[v]) sum += e.quality_gain * p_tv;
    }
    return sum;
}

#endif  // HELPER


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/range.hpp>

#include <boost/hana/integral_constant.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/searchable.hpp>
#include <test/cnumeric.hpp>
using namespace boost::hana;

int main() {
    //////////////////////////////////////////////////////////////////////////
    // Setup for the laws below
    //////////////////////////////////////////////////////////////////////////
    auto ranges = make<Tuple>(
          range(int_<0>, int_<0>)
        , range(int_<0>, int_<1>)
        , range(int_<0>, int_<2>)
        , range(int_<1>, int_<1>)
        , range(int_<50>, long_<60>)
        , range(long_<50>, int_<60>)
    );

    auto integers = tuple_c<int, 0, 1, 900>;

    //////////////////////////////////////////////////////////////////////////
    // Searchable
    //////////////////////////////////////////////////////////////////////////
    {
        // find
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                find(range(int_<0>, int_<0>), test::cnumeric<int, 0>),
                nothing
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                find(range(int_<0>, int_<1>), test::cnumeric<int, 0>),
                just(int_<0>)
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                find(range(int_<0>, int_<10>), test::cnumeric<int, 3>),
                just(int_<3>)
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                find(range(int_<0>, int_<10>), test::cnumeric<int, 9>),
                just(int_<9>)
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                find(range(int_<-10>, int_<10>), test::cnumeric<int, -10>),
                just(int_<-10>)
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                find(range(int_<-10>, int_<10>), test::cnumeric<int, -5>),
                just(int_<-5>)
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                find(range(int_<-10>, int_<0>), test::cnumeric<int, 3>),
                nothing
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                find(range(int_<0>, int_<10>), test::cnumeric<int, 15>),
                nothing
            ));
        }

        // elem
        {
            BOOST_HANA_CONSTANT_CHECK(
                not_(elem(range(int_<0>, int_<0>), test::cnumeric<int, 0>))
            );

            BOOST_HANA_CONSTANT_CHECK(
                elem(range(int_<0>, int_<1>), test::cnumeric<int, 0>)
            );

            BOOST_HANA_CONSTANT_CHECK(
                elem(range(int_<0>, int_<10>), test::cnumeric<int, 3>)
            );

            BOOST_HANA_CONSTANT_CHECK(
                elem(range(int_<0>, int_<10>), test::cnumeric<int, 9>)
            );

            BOOST_HANA_CONSTANT_CHECK(
                elem(range(int_<-10>, int_<10>), test::cnumeric<int, -10>)
            );

            BOOST_HANA_CONSTANT_CHECK(
                elem(range(int_<-10>, int_<10>), test::cnumeric<int, -5>)
            );

            BOOST_HANA_CONSTANT_CHECK(
                not_(elem(range(int_<-10>, int_<0>), test::cnumeric<int, 3>))
            );

            BOOST_HANA_CONSTANT_CHECK(
                not_(elem(range(int_<0>, int_<10>), test::cnumeric<int, 15>))
            );
        }

        // laws
        test::TestSearchable<Range>{ranges, integers};
    }
}


// Copyright (c) Dietmar Wolz.
//
// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory.

// Eigen based implementation of differential evolution using on the DE/best/1 strategy.
// Uses two deviations from the standard DE algorithm:
// a) temporal locality introduced in
// https://www.researchgate.net/publication/309179699_Differential_evolution_for_protein_folding_optimization_based_on_a_three-dimensional_AB_off-lattice_model
// b) reinitialization of individuals based on their age.
// requires https://github.com/imneme/pcg-cpp

#include <Eigen/Core>
#include <iostream>
#include <float.h>
#include <stdint.h>
#include <ctime>
#include <random>
#include <queue>
#include <tuple>
#include "pcg_random.hpp"
#include "evaluator.h"

using namespace std;

namespace differential_evolution {

class DeOptimizer {

public:

    DeOptimizer(long runid_, Fitness *fitfun_, int dim_, int seed_,
            int popsize_, int maxEvaluations_, double keep_,
            double stopfitness_, double F_, double CR_) {
        // runid used to identify a specific run
        runid = runid_;
        // fitness function to minimize
        fitfun = fitfun_;
        // Number of objective variables/problem dimension
        dim = dim_;
        // Population size
        popsize = popsize_ > 0 ? popsize_ : 15 * dim;
        // maximal number of evaluations allowed.
        maxEvaluations = maxEvaluations_ > 0 ? maxEvaluations_ : 50000;
        // keep best young after each iteration.
        keep = keep_ > 0 ? keep_ : 30;
        // Limit for fitness value.
        stopfitness = stopfitness_;
        F = F0 = F_ > 0 ? F_ : 0.5;
        CR = CR0 = CR_ > 0 ? CR_ : 0.9;
        // Number of iterations already performed.
        iterations = 0;
        bestY = DBL_MAX;
        // stop criteria
        stop = 0;
        pos = 0;
        //std::random_device rd;
        rs = new pcg64(seed_);
        init();
    }

    ~DeOptimizer() {
        delete rs;
    }

    double rnd01() {
        return distr_01(*rs);
    }

    int rndInt(int max) {
        return (int) (max * distr_01(*rs));
    }

    vec nextX(int p, const vec &xp, const vec &xb) {
        if (p == 0) {
            iterations++;
            CR = iterations % 2 == 0 ? 0.5 * CR0 : CR0;
            F = iterations % 2 == 0 ? 0.5 * F0 : F0;
        }
        int r1, r2;
        do {
            r1 = rndInt(popsize);
        } while (r1 == p || r1 == bestI);
        do {
            r2 = rndInt(popsize);
        } while (r2 == p || r2 == bestI || r2 == r1);
        vec x1 = popX.col(r1);
        vec x2 = popX.col(r2);
        vec x = xb + (x1 - x2) * F;
        int r = rndInt(dim);
        for (int j = 0; j < dim; j++)
            if (j != r && rnd01() > CR)
                x[j] = xp[j];
        return fitfun->getClosestFeasible(x);
    }

    vec next_improve(const vec &xb, const vec &x, const vec &xi) {
        return fitfun->getClosestFeasible(xb + ((x - xi) * 0.5));
    }

    vec ask(int &p) {
        // ask for one new argument vector.
        if (improvesX.empty()) {
            p = pos;
            vec x = nextX(p, popX.col(p), popX.col(bestI));
            pos = (pos + 1) % popsize;
            return x;
        } else {
            p = improvesP.front();
            vec x = improvesX.front();
            improvesP.pop();
            improvesX.pop();
            return x;
        }
    }

    int tell(double y, const vec &x, int p) {
        //tell function value for a argument list retrieved by ask_one().
        if (isfinite(y) && y < popY[p]) {
            if (iterations > 1) {
                // temporal locality
                improvesP.push(p);
                improvesX.push(next_improve(popX.col(bestI), x, popX0.col(p)));
            }
            popX0.col(p) = popX.col(p);
            popX.col(p) = x;
            popY[p] = y;
            popIter[p] = iterations;
            if (y < popY[bestI]) {
                bestI = p;
                if (y < bestY) {
                    bestY = y;
                    bestX = x;
                    if (isfinite(stopfitness) && bestY < stopfitness)
                        stop = 1;
                }
            }
        } else {
            // reinitialize individual
            if (keep * rnd01() < iterations - popIter[p]) {
                popX.col(p) = fitfun->sample(*rs);
                popY[p] = DBL_MAX;
            }
        }
        return stop;
    }

    void doOptimize() {

        // -------------------- Generation Loop --------------------------------
        for (iterations = 1; fitfun->evaluations() < maxEvaluations;
                iterations++) {

            CR = iterations % 2 == 0 ? 0.5 * CR0 : CR0;
            F = iterations % 2 == 0 ? 0.5 * F0 : F0;

            for (int p = 0; p < popsize; p++) {
                vec xp = popX.col(p);
                vec xb = popX.col(bestI);
                int r1, r2;
                do {
                    r1 = rndInt(popsize);
                } while (r1 == p || r1 == bestI);
                do {
                    r2 = rndInt(popsize);
                } while (r2 == p || r2 == bestI || r2 == r1);
                vec x1 = popX.col(r1);
                vec x2 = popX.col(r2);
                int r = rndInt(dim);
                vec x = vec(xp);
                for (int j = 0; j < dim; j++) {
                    if (j == r || rnd01() < CR) {
                        x[j] = xb[j] + F * (x1[j] - x2[j]);
                        if (!fitfun->feasible(j, x[j]))
                            x[j] = fitfun->sample_i(j, *rs);
                    }
                }

                double y = fitfun->eval(x);
                if (isfinite(y) && y < popY[p]) {
                    // temporal locality
                    vec x2 = next_improve(xb, x, xp);
                    double y2 = fitfun->eval(x2);
                    if (isfinite(y2) && y2 < y) {
                        y = y2;
                        x = x2;
                    }
                    popX.col(p) = x;
                    popY(p) = y;
                    popIter[p] = iterations;
                    if (y < popY[bestI]) {
                        bestI = p;
                        if (y < bestY) {
                            bestY = y;
                            bestX = x;
                            if (isfinite(stopfitness) && bestY < stopfitness) {
                                stop = 1;
                                return;
                            }
                        }
                    }
                } else {
                    // reinitialize individual
                    if (keep * rnd01() < iterations - popIter[p]) {
                        popX.col(p) = fitfun->sample(*rs);
                        popY[p] = DBL_MAX;
                    }
                }
            }
        }
    }

    void do_optimize_delayed_update(int workers) {
    	 iterations = 0;
    	 fitfun->resetEvaluations();
         workers = std::min(workers, popsize); // workers <= popsize
    	 evaluator eval(fitfun, 1, workers);
         int evals_size = popsize*10;
    	 vec evals_x[evals_size];
   	     int evals_p[evals_size];
         int cp = 0;

	     // fill eval queue with initial population
    	 for (int i = 0; i < workers; i++) {
    		 int p;
    		 vec x = ask(p);
    		 eval.evaluate(x, cp);
    		 evals_x[cp] = x;
    		 evals_p[cp] = p;
             cp = (cp + 1) % evals_size;
    	 }
    	 while (fitfun->evaluations() < maxEvaluations) {
    		 vec_id* vid = eval.result();
    		 vec y = vec(vid->_v);
    		 int id = vid->_id;
    		 delete vid;
    		 vec x = evals_x[id];
             int p = evals_p[id];
    		 tell(y(0), x, p); // tell evaluated x
    		 if (fitfun->evaluations() >= maxEvaluations)
    			 break;
    		 x = ask(p);
    		 eval.evaluate(x, cp);
    		 evals_x[cp] = x;
    		 evals_p[cp] = p;
             cp = (cp + 1) % evals_size;
    	 }
	}

    void init() {
        popX = mat(dim, popsize);
        popX0 = mat(dim, popsize);
        popY = vec(popsize);
        for (int p = 0; p < popsize; p++) {
            popX0.col(p) = popX.col(p) = fitfun->sample(*rs);
            popY[p] = DBL_MAX; // compute fitness
        }
        bestI = 0;
        bestX = popX.col(bestI);
        popIter = zeros(popsize);
    }

    vec getBestX() {
        return bestX;
    }

    double getBestValue() {
        return bestY;
    }

    mat getX() {
        return popX;
    }

    mat getY() {
        return popY;
    }

    double getIterations() {
        return iterations;
    }

    double getStop() {
        return stop;
    }

    Fitness* getFitfun() {
        return fitfun;
    }

    int getDim() {
        return dim;
    }

private:
    long runid;
    Fitness *fitfun;
    int popsize; // population size
    int dim;
    int maxEvaluations;
    double keep;
    double stopfitness;
    int iterations;
    double bestY;
    vec bestX;
    int bestI;
    int stop;
    double F0;
    double CR0;
    double F;
    double CR;
    pcg64 *rs;
    mat popX;
    mat popX0;
    vec popY;
    vec popIter;
    queue<vec> improvesX;
    queue<int> improvesP;
    int pos;
};

// see https://cvstuff.wordpress.com/2014/11/27/wraping-c-code-with-python-ctypes-memory-and-pointers/

}

using namespace differential_evolution;

/*
 * Class:     fcmaes_core_Jni
 * Method:    optimizeDE
 * Signature: (Lfcmaes/core/Fitness;[D[D[DIDIDDDJII)I
 */
JNIEXPORT jint JNICALL Java_fcmaes_core_Jni_optimizeDE(JNIEnv *env, jclass cls,
        jobject func, jdoubleArray jlower, jdoubleArray jupper,
        jdoubleArray jresult, jint maxEvals, jdouble stopfitness, jint popsize,
        jdouble keep, jdouble F, jdouble CR, jlong seed, jint runid, jint workers) {

    double *result = env->GetDoubleArrayElements(jresult, JNI_FALSE);
    double *lower = env->GetDoubleArrayElements(jlower, JNI_FALSE);
    double *upper = env->GetDoubleArrayElements(jupper, JNI_FALSE);
    int dim = env->GetArrayLength(jlower);
    vec lower_limit(dim), upper_limit(dim);
    for (int i = 0; i < dim; i++) {
        lower_limit[i] = lower[i];
        upper_limit[i] = upper[i];
    }
    CallJava callJava(func, env);
    Fitness fitfun(&callJava, dim, 1, lower_limit, upper_limit);

    DeOptimizer opt(runid, &fitfun, dim, seed, popsize, maxEvals, keep,
            stopfitness, F, CR);
    try {
        if (workers <= 1)
            opt.doOptimize();
        else
            opt.do_optimize_delayed_update(workers);
        vec bestX = opt.getBestX();
        double bestY = opt.getBestValue();

        for (int i = 0; i < dim; i++)
            result[i] = bestX[i];

        env->SetDoubleArrayRegion(jresult, 0, dim, (jdouble*) result);
        env->ReleaseDoubleArrayElements(jresult, result, 0);
        env->ReleaseDoubleArrayElements(jupper, upper, 0);
        env->ReleaseDoubleArrayElements(jlower, lower, 0);
        return fitfun.evaluations();

    } catch (std::exception &e) {
        cout << e.what() << endl;
        return fitfun.evaluations();
    }
    return 0;
}

/*
 * Class:     fcmaes_core_Jni
 * Method:    initDE
 * Signature: (Lfcmaes/core/Fitness;[D[DIDDDJI)J
 */
JNIEXPORT jlong JNICALL Java_fcmaes_core_Jni_initDE(JNIEnv *env, jclass cls,
        jobject func, jdoubleArray jlower, jdoubleArray jupper,
        jint popsize, jdouble keep, jdouble F, jdouble CR, jlong seed,
        jint runid) {
    double *lower = env->GetDoubleArrayElements(jlower, JNI_FALSE);
    double *upper = env->GetDoubleArrayElements(jupper, JNI_FALSE);
    int dim = env->GetArrayLength(jlower);
    vec lower_limit(dim), upper_limit(dim);
    for (int i = 0; i < dim; i++) {
        lower_limit[i] = lower[i];
        upper_limit[i] = upper[i];
    }
    CallJava* callJava = new CallJava(func, env);
    Fitness* fitfun = new Fitness(callJava, dim, 1, lower_limit, upper_limit);
    DeOptimizer *opt = new DeOptimizer(runid, fitfun, dim, seed, popsize,
            INT_MAX, keep, -DBL_MAX, F, CR);
    env->ReleaseDoubleArrayElements(jupper, upper, 0);
    env->ReleaseDoubleArrayElements(jlower, lower, 0);
    return (intptr_t) opt;
}

/*
 * Class:     fcmaes_core_Jni
 * Method:    destroyDE
 * Signature: (J)V
 */
JNIEXPORT void JNICALL Java_fcmaes_core_Jni_destroyDE(JNIEnv *env, jclass cls, intptr_t ptr) {
    DeOptimizer* opt = (DeOptimizer*)ptr;
    Fitness* fitfun = opt->getFitfun();
    delete fitfun->getFunc();
    delete fitfun;
    delete opt;
}

/*
 * Class:     fcmaes_core_Jni
 * Method:    askDE
 * Signature: (J)[D
 */
JNIEXPORT jdoubleArray JNICALL Java_fcmaes_core_Jni_askDE(JNIEnv *env,
        jclass cls, intptr_t ptr) {
    DeOptimizer *opt = (DeOptimizer*) ptr;
    int dim = opt->getDim();
    jdoubleArray jx = env->NewDoubleArray(dim + 1);
    double x[dim + 1];
    int p;
    vec args = opt->ask(p);
    for (int i = 0; i < dim; i++)
        x[i] = args[i];
    x[dim] = p;
    env->SetDoubleArrayRegion(jx, 0, dim + 1, x);
    return jx;
}

/*
 * Class:     fcmaes_core_Jni
 * Method:    tellDE
 * Signature: (J[DDI)I
 */
JNIEXPORT jint JNICALL Java_fcmaes_core_Jni_tellDE(JNIEnv *env, jclass cls,
		intptr_t ptr, jdoubleArray jx, jdouble y, jint p) {
    DeOptimizer *opt = (DeOptimizer*) ptr;
    int dim = opt->getDim();
    double *x = env->GetDoubleArrayElements(jx, JNI_FALSE);
    vec args(dim);
    for (int i = 0; i < dim; i++)
        args[i] = x[i];
    opt->tell(y, args, p);
    env->ReleaseDoubleArrayElements(jx, x, 0);
    return opt->getStop();
}

/*
 * Class:     fcmaes_core_Jni
 * Method:    populationDE
 * Signature: (J)[D
 */
JNIEXPORT jdoubleArray JNICALL Java_fcmaes_core_Jni_populationDE(JNIEnv *env,
        jclass cls, intptr_t ptr) {
    DeOptimizer *opt = (DeOptimizer*) ptr;
    int size = opt->getX().size();
    double* xdata = opt->getX().data();
    jdoubleArray jres = env->NewDoubleArray(size);
    env->SetDoubleArrayRegion(jres, 0, size, (jdouble*) xdata);
    return jres;
}


/*
 * (C) Copyright 2020 UCAR
 *
 * This software is licensed under the terms of the Apache Licence Version 2.0
 * which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
 */

#include <memory>
#include <boost/optional/optional_io.hpp>

#include "ufo/filters/obsfunctions/ObsErrorModelStepwiseLinear.h"

#include "eckit/exception/Exceptions.h"

#include "ioda/ObsDataVector.h"

#include "oops/util/CompareNVectors.h"
#include "oops/util/Logger.h"
#include "oops/util/missingValues.h"
#include "oops/util/PropertiesOfNVectors.h"

#include "ufo/filters/ObsFilterData.h"

namespace ufo {

static ObsFunctionMaker<ObsErrorModelStepwiseLinear> makerSteps_("ObsErrorModelStepwiseLinear");

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

ObsErrorModelStepwiseLinear::ObsErrorModelStepwiseLinear(const eckit::LocalConfiguration config)
  : invars_() {
  // Initialize options
  options_.deserialize(config);

  // Get piece-wise parameters from options.
  const std::vector<float> &xvals = options_.xvals.value();
  const std::vector<float> &errors = options_.errors.value();

  // Ensure same size vectors (xvals and errors); Also ensure more than one value in each.
  if (!oops::allVectorsSameNonZeroSize(xvals, errors) ||
                ((xvals.size() <= 1 || errors.size() <= 1))) {
      std::ostringstream errString;
      errString << "At least one vector is the wrong size - either unequal or less than 2."
                << std::endl << "Vector sizes of xvals, errors: "
                << oops::listOfVectorSizes(xvals, errors) << std::endl;
      oops::Log::error() << errString.str();
      throw eckit::BadValue(errString.str());
  }

  // Initialize x-variable
  const Variable &xvar = options_.xvar.value();
  ASSERT(xvar.size() == 1);
  invars_ += xvar;

  // In the case of using a multiply operator, load it into invars as well.
  if (options_.scale_factor_var.value() != boost::none) {
    multiplicative_ = true;
    const boost::optional<Variable> &scale_factor_var = options_.scale_factor_var.value();
    invars_ += *scale_factor_var;
    oops::Log::debug() << " StepwiseLinear, scale_factor_var: " << *scale_factor_var << std::endl;
  }

  // Check that all errors >= 0
  for (size_t i = 0; i < errors.size(); ++i) {
    ASSERT(errors[i] >= 0.0);
  }

  // In order to check for beyond range of xvals, determine if ascending or descending.
  // Also ensure that the entire vector is consistent throughout and no consecutive elements
  // of xval are equal.
  if (xvals.back() < xvals.front()) {
    isAscending_ = false;
  }
  for (size_t kv = 1; kv < xvals.size(); ++kv) {
    if ((xvals[kv] >= xvals[kv-1] && !isAscending_) || (xvals[kv] <= xvals[kv-1] && isAscending_)) {
      std::ostringstream errString;
      errString << "The xvals vector of elements is NOT internally consistent."
                << " It must be entirely either ascending or descending order,"
                << " or two consecutive values are the same." << std::endl;
      oops::Log::error() << errString.str();
      throw eckit::BadValue(errString.str());
    }
  }
  oops::Log::debug() << "ObsErrorModelStepwiseLinear: config (constructor) = "
                     << config << std::endl;
}

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

ObsErrorModelStepwiseLinear::~ObsErrorModelStepwiseLinear() {}

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

void ObsErrorModelStepwiseLinear::compute(const ObsFilterData & data,
                                     ioda::ObsDataVector<float> & obserr) const {
  const float missing = util::missingValue(missing);
  // Linearly interpolate from y0 to y1 at xstar between x0 and x1 to arrive at error
  float x0, x1, y0, y1;
  float xstar, error;

  // Get the x-variable name and piece-wise parameters from options
  const Variable &xvar = options_.xvar.value();
  const std::vector<float> &xvals = options_.xvals.value();
  const std::vector<float> &errors = options_.errors.value();
  oops::Log::debug() << "  ObsErrorModelStepwiseLinear, x-variable name: " << xvar.variable()
                     << "  and group: " << xvar.group() << std::endl;

  // Populate the testdata array.  xstar is just the 0..nloc-1 value of testvar[iv]
  // At each nloc, find matching (x0,x1) and (y0,y1) pair for linear interp.
  ioda::ObsDataVector<float> testdata(data.obsspace(), xvar.toOopsVariables());
  data.get(xvar, testdata);

  // Declare a unique_ptr, but keep it empty for now (don’t assign anything to it).
  std::unique_ptr<ioda::ObsDataVector<float>> obvalues;

  // If using the multiply operator, load up the variable (scale_factor_var) to multiply by.
  if (multiplicative_) {
    const boost::optional<Variable> &scale_factor_var = options_.scale_factor_var.value();
    oops::Log::debug() << "  ObsErrorModelStepwiseLinear, scale_factor_var name: "
                       << (*scale_factor_var).variable() << "  and group: "
                       << (*scale_factor_var).group() << std::endl;
    obvalues.reset(new ioda::ObsDataVector<float>(data.obsspace(),
                                 (*scale_factor_var).toOopsVariables()));
    data.get(*scale_factor_var, *obvalues);
  }

  // The 1st index of data should have size 1 and 2nd index should be size nlocs.
  int iv = 0;
  if (testdata[iv].size() != obserr[iv].size()) {
    std::ostringstream errString;
    errString << "Something is wrong, testdata size not equal obserr size."
              << " Sizes: " << testdata[iv].size() << " and " << obserr[iv].size() << std::endl;
    oops::Log::error() << errString.str();
    throw eckit::BadValue(errString.str());
  }

  for (size_t jobs = 0; jobs < testdata[iv].size(); ++jobs) {
    error = 0.0;
    obserr[iv][jobs] = missing;
    if (testdata[iv][jobs] == missing) {
      continue;
    }
    xstar = testdata[iv][jobs];
    if ((xstar <= xvals[0] && isAscending_) || (xstar >= xvals[0] && !isAscending_)) {
      error = errors[0];
    } else if ((xstar >= xvals.back() && isAscending_)
              || (xstar <= xvals.back() && !isAscending_)) {
      error = errors[errors.size()-1];
    } else {
      for (size_t kv = 1; kv < xvals.size(); ++kv) {
        if ((isAscending_ && (xstar > xvals[kv-1]) && (xstar <= xvals[kv])) ||
              (!isAscending_ && (xstar < xvals[kv-1]) && (xstar >= xvals[kv]))) {
          x0 = xvals[kv-1];
          x1 = xvals[kv];
          y0 = errors[kv-1];
          y1 = errors[kv];
          error = y0 + (xstar-x0)*((y1-y0)/(x1-x0));
          break;
        }
      }
    }
    // TODO(gthompsn):  probably need this next line for when filtervariable is flagged missing
    // if (!flagged_[jv][jobs]) obserr[jv][jobs] = error;
    if (multiplicative_) {
      obserr[iv][jobs] = error*(*obvalues)[iv][jobs];
    } else {
      obserr[iv][jobs] = error;
    }
  }
}

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

const ufo::Variables & ObsErrorModelStepwiseLinear::requiredVariables() const {
  return invars_;
}

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

}  // namespace ufo


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// 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)
//
/// \file constelm_test.cpp
/// \brief Add your file description here.

// #define MCRL2_PBES_CONSTELM_DEBUG

#include <string>
#include <boost/test/minimal.hpp>
#include "mcrl2/utilities/logger.h"
#include "mcrl2/pbes/eqelm.h"
#include "mcrl2/pbes/rewriter.h"
#include "mcrl2/pbes/txt2pbes.h"
#include "mcrl2/pbes/detail/pbes_property_map.h"
#include "mcrl2/pbes/detail/pbes2bool.h"

using namespace mcrl2;
using namespace mcrl2::data;
using namespace mcrl2::pbes_system;

// Example provided by Frank Stappers
std::string t1 =
  "pbes nu X(m, n: Nat) =                  \n"
  "        val(n == m) && X(m + 1, n + 1); \n"
  "                                        \n"
  " init X(0, 0);                          \n"
  ;
std::string x1 = "binding_variables = X(n: Nat)";

// Example provided by Tim Willemse.
// The parameters n and m are not equivalent, and thus should not
// be removed.
std::string t2 =
  "pbes nu X(n,m: Nat) =          \n"
  "       forall p: Nat. X(m, p); \n"
  "                               \n"
  "init X(0, 0);                  \n"
  ;
std::string x2 = "binding_variables = X(n,m: Nat)";

void test_pbes(const std::string& pbes_spec,
               const std::string& expected_result,
               bool /* compute_conditions */,
               bool /* remove_equations = true */,
               const std::string& msg = "")
{
  typedef simplify_data_rewriter<data::rewriter> my_pbes_rewriter;

  pbes p = txt2pbes(pbes_spec);
  pbes q = p;

  // data rewriter
  data::rewriter datar(q.data());

  // pbes rewriter
  my_pbes_rewriter pbesr(datar);

  // constelm algorithm
  pbes_eqelm_algorithm<pbes_expression, data::rewriter, my_pbes_rewriter> algorithm(datar, pbesr);

  // run the algorithm
  algorithm.run(q);
  BOOST_CHECK(q.is_well_typed());
  if (!q.is_well_typed())
  {
    std::cerr << pbes_system::pp(q) << std::endl;
  }

  pbes_system::detail::pbes_property_map info1(q);
  pbes_system::detail::pbes_property_map info2(expected_result);
  std::string diff = info1.compare(info2);
  if (!diff.empty())
  {
    std::cerr << "\n------ FAILED TEST ------ " << msg << std::endl;
    std::cerr << "--- expected result" << std::endl;
    std::cerr << expected_result << std::endl;
    std::cerr << "--- found result" << std::endl;
    std::cerr << info1.to_string() << std::endl;
    std::cerr << "--- differences" << std::endl;
    std::cerr << diff << std::endl;
  }
  BOOST_CHECK(diff.empty());

}

void test_eqelm(const std::string& pbes_spec, const bool expected_outcome)
{
  typedef simplify_data_rewriter<data::rewriter> my_pbes_rewriter;

  pbes p = txt2pbes(pbes_spec);
  pbes q = p;

  data::rewriter datar(q.data());
  my_pbes_rewriter pbesr(datar);
  pbes_eqelm_algorithm<pbes_expression, data::rewriter, my_pbes_rewriter> algorithm(datar, pbesr);
  algorithm.run(q);

  bool solution_p = pbes_system::detail::pbes2bool(p);
  bool solution_q = pbes_system::detail::pbes2bool(q);
  BOOST_CHECK(solution_p == expected_outcome);
  BOOST_CHECK(solution_q == expected_outcome);
}

std::string random1 =
  "pbes mu X0(b: Bool) =                            \n"
  "       (X1(0, 0) || false) && true;              \n"
  "     mu X1(n,m: Nat) =                           \n"
  "       (X1(m + 1, 0) || false) || val(!(n < 2)); \n"
  "                                                 \n"
  "init X0(true);                                   \n"
  ;

int test_main(int argc, char** argv)
{
  log::mcrl2_logger::set_reporting_level(log::debug);

  bool compute_conditions = false;
  bool remove_equations = true;
  test_pbes(t1 , x1, compute_conditions, remove_equations, "test 1");
  test_pbes(t2 , x2, compute_conditions, remove_equations, "test 2");
  test_eqelm(random1,false);

  return 0;
}


#define BOOST_TEST_NO_LIB
#include <boost/test/auto_unit_test.hpp>

#include "coconut/milk/utils/bits.hpp"

using namespace coconut;
using namespace coconut::milk;
using namespace coconut::milk::utils;

namespace /* anonymous */ {

BOOST_AUTO_TEST_SUITE(MilkUtilsBitsTestSuite);

BOOST_AUTO_TEST_CASE(NumberOfBitsOnReturnsCorrectValue) {
	BOOST_CHECK_EQUAL(numberOfBitsOn(0b0), 0);
	BOOST_CHECK_EQUAL(numberOfBitsOn(0b1), 1);
	BOOST_CHECK_EQUAL(numberOfBitsOn(0b10), 1);
	BOOST_CHECK_EQUAL(numberOfBitsOn(0b11), 2);
	BOOST_CHECK_EQUAL(numberOfBitsOn(0b10110011100011110000), 10);
}

BOOST_AUTO_TEST_SUITE_END(/* MilkUtilsBitsTestSuite */);

} // anonymous namespace


#include <kjq_navigation/AStarPlanner.hpp>

#include <queue>
#include <unordered_set>

#include <Eigen/Geometry>

namespace kjq_navigation
{

void AStarPlanner::setMap(const nav_msgs::OccupancyGrid& msg) {
    if (map_initialized_) return;

    map_initialized_=true;
    grid_map::GridMapRosConverter::fromOccupancyGrid(msg, "global", map_);

    map_.add("inflation", grid_map::Matrix::Zero(map_.getSize()(0), map_.getSize()(1)) );

    // apply inflation to map
    grid_map::Position center;
    for (grid_map::GridMapIterator it(map_); !it.isPastEnd(); ++it) {
        if (map_.at("global", *it) > 0.0) {
            map_.getPosition(*it, center);
            for (grid_map::CircleIterator iterator(map_, center, robot_radius_); !iterator.isPastEnd(); ++iterator) {
                map_.at("inflation", *iterator) = 1.0;
            }
        }
    }
}

AStarPlanner::AStarPlanner() : map_(grid_map::GridMap({"global", "inflation"})) {
     map_.setFrameId("map");
}

// TODO use std::move for map
AStarPlanner::AStarPlanner(const float robot_radius, grid_map::GridMap& map) :
    robot_radius_(robot_radius), map_(map), map_initialized_(true)  {

    // create new layers for planning purposes
    map_.add("inflation", grid_map::Matrix::Zero(map_.getSize()(0), map_.getSize()(1)));

    // apply inflation to map
    grid_map::Position center;
    for (grid_map::GridMapIterator it(map_); !it.isPastEnd(); ++it) {
        if (map_.at("global", *it) > 0.5) {
            map_.getPosition(*it, center);
            for (grid_map::CircleIterator iterator(map_, center, robot_radius); !iterator.isPastEnd(); ++iterator) {
                map_.at("inflation", *iterator) = 1.0;
            }
        }
    }
}

nav_msgs::Path AStarPlanner::plan(const grid_map::Position& start, const grid_map::Position& end) {
    nav_msgs::Path path;

    // convert to indices
    grid_map::Index start_idx, end_idx;
    if (map_.getIndex(start, start_idx) == false ) {
        ROS_ERROR("INVALID START POSITION FOR PLANNING!!!!");
        return path;
    }
    if ( map_.getIndex(end, end_idx) == false) {
        ROS_ERROR("INVALID END POSITION FOR PLANNING!!!!");
        return path;
    }

    std::string start_key = index2String(start_idx);
    std::string end_key = index2String(end_idx);
    ROS_INFO_STREAM("Planning mission " << start_key << " to " << end_key);

    // initialize data structures for A*
    auto comp = [](const std::pair<float, grid_map::Index>& p1,
    const std::pair<float, grid_map::Index>& p2) {
        return p1.first > p2.first;
    };
    std::priority_queue<std::pair<float, grid_map::Index>,
        std::vector<std::pair<float, grid_map::Index>>, decltype(comp)> open(comp);
    std::unordered_map<std::string, std::string> came_from;
    std::unordered_map<std::string, float> cost_so_far;

    open.emplace(0.0f, start_idx);
    cost_so_far[start_key] = 0.0f;
    came_from[start_key] = start_key;

    const std::vector<std::pair<int, int>> dir = {{-1, 0}, {0, -1}, {1, 0}, {0, 1},
                                                  {1, 1}, {1, -1}, {-1, 1}, {-1, -1}};
    while(!open.empty()) {
        auto node = open.top(); open.pop();
        grid_map::Index idx = node.second;
        std::string idx_key = index2String(idx);

        if (idx.x() == end_idx.x() && idx.y() == end_idx.y())
            break;

        for (auto d : dir) {
            grid_map::Index neighbour(idx.x()+d.first, idx.y()+d.second);
            if (inMap(neighbour) && map_.isValid(neighbour, "inflation") && map_.at("inflation", neighbour) < 1.0) {
                float new_cost = cost_so_far[idx_key] + std::sqrt(d.first*d.first + d.second*d.second);
                std::string neighbour_key = index2String(neighbour);

                // if we've never been here before or the cost is lower than what we've seen
                if (cost_so_far.find(neighbour_key) == cost_so_far.end()
                    || new_cost < cost_so_far[neighbour_key]) {
                    cost_so_far[neighbour_key] = new_cost;
                    float priority = new_cost + calculateHeuristic(neighbour, end_idx);
                    open.emplace(priority, neighbour);
                    came_from[neighbour_key] = idx_key;
                }
            }
        }
    }

    if (cost_so_far.find(end_key) != cost_so_far.end())
        ROS_INFO_STREAM("Planning complete with cost " << std::to_string(cost_so_far[end_key])) ;

    // reconstruct path
    std::string curr_key = end_key;
    path.header.stamp = ros::Time::now();
    path.header.frame_id = map_.getFrameId();

    int x, y;
    grid_map::Position pos;

    while (curr_key != start_key) {
        std::istringstream iss(curr_key);
        iss >> x >> y;
        map_.getPosition(grid_map::Index(x, y), pos);

        geometry_msgs::PoseStamped pose;
        pose.header.frame_id = map_.getFrameId();
        pose.pose.position.x = pos.x();
        pose.pose.position.y = pos.y();
        pose.pose.position.z = 0.0;

        pose.pose.orientation.x = 0.0;
        pose.pose.orientation.y = 0.0;
        pose.pose.orientation.z = 0.0;
        pose.pose.orientation.w = 1.0;

        path.poses.push_back(pose);

        curr_key = came_from[curr_key];
    }

    std::reverse(path.poses.begin(), path.poses.end());

    // generate orientation
    for (size_t i = 0; i < path.poses.size()-1; ++i) {
        float dx = path.poses[i+1].pose.position.x - path.poses[i].pose.position.x;
        float dy = path.poses[i+1].pose.position.y - path.poses[i].pose.position.y;
        float yaw = std::atan2(dy, dx);

       Eigen::Quaternionf q = q = Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX())
                            * Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY())
                            * Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ());

        path.poses[i].pose.orientation.x = q.x();
        path.poses[i].pose.orientation.y = q.y();
        path.poses[i].pose.orientation.z = q.z();
        path.poses[i].pose.orientation.w = q.w();

        if (i+1 == path.poses.size()-1) {
            path.poses[i+1].pose.orientation.x = q.x();
            path.poses[i+1].pose.orientation.y = q.y();
            path.poses[i+1].pose.orientation.z = q.z();
            path.poses[i+1].pose.orientation.w = q.w();
        }

    }

    return path;

}

float AStarPlanner::calculateHeuristic(const grid_map::Index& start, const grid_map::Index& end) {
    return (start.matrix()-end.matrix()).norm();
}

} // namespace kjq_navigation


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/function/cyl_bessel_j1.hpp>
#include <eve/function/prev.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/platform.hpp>
#include <cmath>
#include <boost/math/special_functions/bessel.hpp>

EVE_TEST_TYPES( "Check return types of cyl_bessel_j1"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  TTS_EXPR_IS(eve::cyl_bessel_j1(T(0)), T);
  TTS_EXPR_IS(eve::cyl_bessel_j1(v_t(0)), v_t);
};

 EVE_TEST( "Check behavior of cyl_bessel_j1 on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 10.0))
         )
   <typename T>(T const& a0)
{
  using v_t = eve::element_type_t<T>;

  auto eve__cyl_bessel_j1 =  [](auto x) { return eve::cyl_bessel_j1(x); };
  auto std__cyl_bessel_j1 =  [](auto x)->v_t { return boost::math::cyl_bessel_j(v_t(1), x); };
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_j1(eve::minf(eve::as<v_t>())), eve::zero(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j1(eve::inf(eve::as<v_t>())), v_t(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j1(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j1(eve::minf(eve::as< T>())), eve::zero(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j1(eve::inf(eve::as< T>())),  T(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j1(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
  }
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(1000)) , std__cyl_bessel_j1( v_t(1000)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(10)), std__cyl_bessel_j1(v_t(10))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(5)),  std__cyl_bessel_j1(v_t(5))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(2)),  std__cyl_bessel_j1(v_t(2))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(1.5)),std__cyl_bessel_j1(v_t(1.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(0.5)),std__cyl_bessel_j1(v_t(0.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(1)),  std__cyl_bessel_j1(v_t(1))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(0)),  std__cyl_bessel_j1(v_t(0))   , 2.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(1000)) ,  T(std__cyl_bessel_j1( v_t(1000)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(10)) ,  T(std__cyl_bessel_j1( v_t(10)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(5))  ,  T(std__cyl_bessel_j1( v_t(5))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(2))  ,  T(std__cyl_bessel_j1( v_t(2))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(1.5)),  T(std__cyl_bessel_j1( v_t(1.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(0.5)),  T(std__cyl_bessel_j1( v_t(0.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(1))  ,  T(std__cyl_bessel_j1( v_t(1))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(0))  ,  T(std__cyl_bessel_j1( v_t(0))  ), 2.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_j1(a0), map(std__cyl_bessel_j1, a0), 20.0);
};


#include <iostream>
#include <fstream>
#include <array>
#include <vector>
#include <algorithm>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>

template<typename T, typename E>
bool contain(const T &c, E e)
{
    return std::find(c.begin(), c.end(), e) != c.end();
}

std::vector<std::string>splitBySpacesAndSort(std::string in)
{
    std::vector<std::string> ret;
    boost::split(ret, in, boost::is_any_of(" "));
    for (std::string &s : ret)
        std::sort(s.begin(), s.end());
    return ret;
}

size_t countCommon(const std::string f, const std::string s)
{
    return std::count_if(f.begin(), f.end(),
    [&](const auto &n) { return contain(s, n); });
}

void removeAndAddMappingIfEq(
    std::vector<std::string> &in,
    std::array<std::string, 10> &mapping,
    size_t out_map_idx,
    size_t cmp_map_idx,
    int val)
{
    for (int i = 0; i < in.size(); ++i) {
        if (countCommon(in[i], mapping[cmp_map_idx]) == val) {
            mapping[out_map_idx] = in[i];
            in.erase(std::next(in.begin(), i));
            return;
        }
    }
}

std::vector<std::string>
getWithLen(const std::vector<std::string> &in, const size_t len)
{
    std::vector<std::string>ret;
    std::copy_if(in.begin(), in.end(), std::back_inserter(ret),
    [=](std::string s) { return s.size() == len; });
    return ret;
}

int main (void)
{
    std::fstream file("input.txt");
    std::vector<std::vector<std::string>> outputs, signals;
    for (std::string line; getline(file, line);) {
        std::vector<std::string> tmp;
        boost::split(tmp, line, boost::is_any_of("|"));
        signals.push_back(splitBySpacesAndSort(tmp[0]));
        outputs.push_back(splitBySpacesAndSort(tmp[1]));
    }
    std::array<std::string, 10> mapping;
    int counter = 0, sum = 0;
    for (int i = 0; i < signals.size(); ++i) {
        mapping[1] = getWithLen(signals[i], 2)[0];
        mapping[4] = getWithLen(signals[i], 4)[0];
        mapping[7] = getWithLen(signals[i], 3)[0];
        mapping[8] = getWithLen(signals[i], 7)[0];
        std::vector<std::string>lenSix = getWithLen(signals[i], 6);
        removeAndAddMappingIfEq(lenSix, mapping, 9, 4, 4);
        removeAndAddMappingIfEq(lenSix, mapping, 0, 1, 2);
        mapping[6] = lenSix[0];
        std::vector<std::string>lenFive = getWithLen(signals[i], 5);
        removeAndAddMappingIfEq(lenFive, mapping, 2, 9, 4);
        removeAndAddMappingIfEq(lenFive, mapping, 5, 1, 1);
        mapping[3] = lenFive[0];

        int counter = 0;
        for (std::string s : outputs[i]) {
            for (int j = 0; j < mapping.size(); ++j) {
                if (mapping[j] == s) {
                    counter += j;
                    counter *= 10;
                    break;
                }
            }
        }
        sum += counter/10;
    }
    std::cout << sum << '\n';
}


/**
 * @file termination_policy_test.cpp
 * @author Ryan Curtin
 *
 * Tests for AMF termination policies.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/amf/amf.hpp>
#include <mlpack/methods/amf/termination_policies/max_iteration_termination.hpp>
#include <mlpack/methods/amf/update_rules/nmf_mult_div.hpp>

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

BOOST_AUTO_TEST_SUITE(TerminationPolicyTest);

using namespace std;
using namespace arma;
using namespace mlpack;
using namespace mlpack::amf;

/**
 * Simple test -- make sure termination happens after the right number of
 * iterations.
 */
BOOST_AUTO_TEST_CASE(MaxIterationTerminationTest)
{
  MaxIterationTermination mit(500);

  arma::mat x; // Just an argument to pass.
  for (size_t i = 0; i < 499; ++i)
    BOOST_REQUIRE_EQUAL(mit.IsConverged(x, x), false);

  // Should keep returning true once maximum iterations are reached.
  BOOST_REQUIRE_EQUAL(mit.IsConverged(x, x), true);
  BOOST_REQUIRE_EQUAL(mit.Iteration(), 500);
  BOOST_REQUIRE_EQUAL(mit.IsConverged(x, x), true);
  BOOST_REQUIRE_EQUAL(mit.IsConverged(x, x), true);
}

/**
 * Make sure that AMF properly terminates.
 */
BOOST_AUTO_TEST_CASE(AMFMaxIterationTerminationTest)
{
  mat w = randu<mat>(20, 12);
  mat h = randu<mat>(12, 20);
  mat v = w * h;
  size_t r = 12;

  // Custom tighter tolerance.
  MaxIterationTermination mit(10); // Only 10 iterations.
  AMF<MaxIterationTermination,
      RandomInitialization,
      NMFMultiplicativeDivergenceUpdate> nmf(mit);
  nmf.Apply(v, r, w, h);

  BOOST_REQUIRE_EQUAL(nmf.TerminationPolicy().Iteration(), 10);
}

BOOST_AUTO_TEST_SUITE_END();


///////////////////////////////////////////////////////////////////////////////
// acf.hpp                                                                   //
//                                                                           //
//  Copyright 2008 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_ACCUMULATORS_STATISTICS_ACF_HPP_ER_2008_04
#define BOOST_ACCUMULATORS_STATISTICS_ACF_HPP_ER_2008_04

#include <cmath>
#include <vector>
#include <algorithm>
#include <functional>
#include <boost/mpl/int.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/placeholders.hpp>

#include <boost/range/iterator_range.hpp>
#include <boost/type_traits/add_const.hpp>
#include <boost/bind.hpp>

#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/acvf.hpp>
#include <boost/accumulators/statistics/delay.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ////////////////////////////////////////////////////////////////////////////
    // acf_impl
    template<typename T,typename I>
    class acf_impl
      : public accumulator_base
    {
       //TODO via the feature for delay (?)
	   typedef acvf_impl<T,I>      acvf_type;
	   typedef typename acvf_type::result_type      acvfs_result_type;
       typedef std::vector<T>                  acfs_type;
    public:
       typedef boost::iterator_range<
            typename acfs_type::const_iterator>     result_type;

        acf_impl(dont_care):acfs(0,(T)(static_cast<T>(0))){}

        template<typename Args>
        void operator ()(Args const &args){
            const acvfs_result_type& range
                = acvf<I>(args[accumulator]);
            std::size_t sz = range.size();
            acfs.resize(sz);
            T acv0 = (*begin(range));
            if(acv0>static_cast<T>(0)){
                T div=(static_cast<T>(1)/acv0);
                transform(
                    begin(range),
                    end(range),
                    acfs.begin(),
                    boost::bind(
                        std::multiplies<T>(),
                        _1,
                        div
                    )
                );
            }else{
                std::fill_n(acfs.begin(),sz,static_cast<T>(0));
            }
        }
        result_type result(dont_care) const
        {
          return boost::make_iterator_range(acfs.begin(),acfs.end());
        }
    private:
        mutable acfs_type acfs;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::acf
//

namespace tag
{
    template <typename I = default_delay_discriminator>
    struct acf
      : depends_on<acvf<I> >
    {
        /// INTERNAL ONLY
      typedef accumulators::impl::acf_impl<mpl::_1,I> impl;

    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::acf
//

namespace extract
{

  template<typename I,typename AccumulatorSet>
  typename
    mpl::apply<AccumulatorSet,tag::acf<I> >::type::result_type
  acf(AccumulatorSet const& acc){
    typedef tag::acf<I> the_tag;
    return extract_result<the_tag>(acc);
  }//typical call://acf<default_delay_discriminator>(acc)

}

using extract::acf;

}} // namespace boost::accumulators

#endif


#include "subsystems/FlywheelSubsystem.h"

#include <Eigen/Core>
#include <frc/smartdashboard/SmartDashboard.h>
#include <frc2/command/button/JoystickButton.h>

#include "ctre/Phoenix.h"

FlywheelSubsystem::FlywheelSubsystem()
{
    m_leader.ConfigFactoryDefault();

    // Disable Talon FX velocity filtering so that our Kalman filter can do the
    // work

    // m_leader.ConfigVelocityMeasurementPeriod(SensorVelocityMeasPeriod::Period_1Ms);
    // m_leader.ConfigVelocityMeasurementWindow(1);
    m_leader.SetNeutralMode(Coast);

    m_follower.ConfigFactoryDefault();
    m_follower.SetNeutralMode(Coast);
    m_follower.SetInverted(true);

    m_leader.EnableVoltageCompensation(true);
    m_leader.ConfigVoltageCompSaturation(9);
    m_follower.EnableVoltageCompensation(true);
    m_follower.ConfigVoltageCompSaturation(9);

    m_leader.Config_kP(0, 0.025);
    m_follower.Config_kP(0, 0.025);
    m_leader.Config_kF(0, 0.068175);
    m_follower.Config_kF(0, 0.068175);
    // Set Feeder to coast and config Default
}

void FlywheelSubsystem::Periodic()
{
    auto ffVoltage = units::volt_t{m_feedforward.Calculate(Eigen::Vector<double, 1>(m_desiredVelocity.value()))[0]};
    if (units::math::abs(ffVoltage) < Constants::MinimumFFVoltage)
    {
        ffVoltage = 0_V;
    }
    if (m_desiredVelocity > 0_deg_per_s)
    {
        m_leader.Set(
            ControlMode::Velocity,
            m_desiredVelocity * Constants::VelocityFactor::TalonFX * Constants::TicksPerRevolution::TalonFX * Constants::Flywheel::FlyWheelGearing
            /*DemandType::DemandType_ArbitraryFeedForward,
            ffVoltage / 12_V*/);
        m_follower.Set(
            ControlMode::Velocity,
            m_desiredVelocity * Constants::VelocityFactor::TalonFX * Constants::TicksPerRevolution::TalonFX * Constants::Flywheel::FlyWheelGearing
            /*DemandType::DemandType_ArbitraryFeedForward,
            ffVoltage / 12_V*/);
    }
    else
    {
        m_leader.Set(ControlMode::PercentOutput, 0);
        m_follower.Set(ControlMode::PercentOutput, 0);
    }

    frc::SmartDashboard::PutNumber("Flywheel.MeasuredState", GetMeasuredVelocity().value());
    frc::SmartDashboard::PutNumber("Flywheel.DesiredState", GetDesiredVelocity().value());
}

void FlywheelSubsystem::SetSetpoint(rad_per_s_t setpoint)
{
    m_desiredVelocity = setpoint;
}

rad_per_s_t FlywheelSubsystem::GetDesiredVelocity()
{
    return m_desiredVelocity;
}

rad_per_s_t FlywheelSubsystem::GetMeasuredVelocity()
{
    return m_leader.GetSelectedSensorVelocity() / Constants::TicksPerRevolution::TalonFX /
        Constants::VelocityFactor::TalonFX / Constants::Flywheel::FlyWheelGearing;
}
rad_per_s_t FlywheelSubsystem::GetFollowerMeasuredVelocity()
{
    return m_follower.GetSelectedSensorVelocity() / Constants::TicksPerRevolution::TalonFX /
        Constants::VelocityFactor::TalonFX / Constants::Flywheel::FlyWheelGearing;
}


#include "NTtimescales.h"
#include "write.h"
#include "messages.h"
#include "globalVariables.h"
#include "adafFunctions.h"

#include <flosses/nonThermalLosses.h>
#include <flosses/lossesSyn.h>
#include <flosses/lossesIC.h>
#include <flosses/lossesBrem.h>
#include <flosses/lossesHadronics.h>
#include <flosses/lossesPhotoHadronic.h>
#include <fparameters/SpaceIterator.h>
#include <fparameters/Dimension.h>
#include <fparameters/parameters.h>

#include <boost/property_tree/ptree.hpp>

void nonThermalTimescales(Particle& p, State& st, const std::string& filename)
{
	if (p.id == "ntElectron")
		show_message(msgStart, Module_electronRadLosses);
	else if (p.id == "ntProton")
		show_message(msgStart, Module_protonRadLosses);

	std::ofstream file,file2;
	file.open(filename.c_str(), std::ios::out);
	if (p.id == "ntProton") file2.open("diffusion_adv.dat", std::ios::out);

	file << "r [2M]"
		<< "\t" << "Log(gamma)"
		<< "\t" << "Acc"
		<< "\t" << "Acc_SDA"
		<< "\t" << "Adv"
		<< "\t" << "Diff_K"
		<< "\t" << "Diff_B"
		<< "\t" << "Diff_|_"
		<< "\t" << "Diff_||"
        << "\t" << "EmaxHillas"
		<< "\t" << "Sy"
		<< "\t" << "IC/pp"
		<< "\t" << "pg/Bremss"
		<< "\t" << "Relax"

		<< std::endl;

	if (p.id == "ntProton") {
		file2   << "r [2M]"
				<< "\t" << "Log(gamma)"
				<< "\t" << "Diff_length"
				<< "\t" << "dR"
				<< "\t" << "Dlenght/dR"
				<< std::endl;
	}

	int flag1, flag2, flag3, flag4, flag5, flag6, flag7;
	flag1 = flag2 = flag3 = flag4 = flag5 = flag6 = flag7 = 0;
	double logr1,logr2,logr3,logr4,logr5,logr6,logr7;
	logr1 = log10(1.5);
	double aux = log10(st.denf_e.ps[DIM_R].last()/schwRadius)/7.0;
	logr2 = logr1+aux;
	logr3 = logr2+aux;
	logr4 = logr3+aux;
	logr5 = logr4+aux;
	logr6 = logr5+aux;
	logr7 = logr6+aux;
	p.ps.iterate([&](const SpaceIterator& iR) {
		double r = iR.val(DIM_R);
		double logr = log10(r/schwRadius);
		if (logr > logr1) flag1++;
		if (logr > logr2) flag2++;
		if (logr > logr3) flag3++;
		if (logr > logr4) flag4++;
		if (logr > logr5) flag5++;
		if (logr > logr6) flag6++;
		if (logr > 0.9*logr7) flag7++;

		if (flag1 == 1 || flag2 == 1 || flag3 == 1 || flag4 == 1 || flag5 == 1
				|| flag6 == 1 || flag7 == 1) {
			double tAdv = accretionTime(r);
			double vR = radialVel(r);
			double dR = r * (sqrt(paso_r)-1.0/sqrt(paso_r));
			double tCell = dR / abs(vR);
			double B = st.magf.get(iR);
			double height = height_fun(r);
			double rho = massDensityADAF(r);
			double eMaxHillas = electronCharge*B*height;
			p.ps.iterate([&](const SpaceIterator& iRE) {
				double E = iRE.val(DIM_E);
				double tAcc = 1.0/accelerationRate(E,B);
				double tAccSDA = accelerationTimeSDA(E,p,B,height,rho);
				double tDiffKol = diffusionTimeTurbulence(E,height,p,B);
				double tDiffBohm = height*height/BohmDiffusionCoeff(E,B);
				double tDiffParallel = diffusionTimeParallel(E,height,B);
				double tDiffPerpend = diffusionTimePerpendicular(E,height,B);

				file << (int)(r/schwRadius)												// 0
					 << "\t" << safeLog10(E/(p.mass*cLight2))							// 1
					 << "\t" << safeLog10(tAcc)											// 2
					 << "\t" << safeLog10(tAccSDA)										// 3
					 << "\t" << safeLog10(tAdv)											// 4
					 << "\t" << safeLog10(tDiffKol)										// 5
					 << "\t" << safeLog10(tDiffBohm)									// 6
					 << "\t" << safeLog10(tDiffParallel)								// 7
					 << "\t" << safeLog10(tDiffPerpend)									// 8
					 << "\t" << safeLog10(eMaxHillas/1.602e-12/(p.mass*cLight2));		// 9
				if (p.id == "ntProton") {
					double diff_length = diffLength(E/(p.mass*cLight2),p,r,height,B,vR);
					file2   << (int)(r/schwRadius)
							<< "\t" << safeLog10(E/(p.mass*cLight2))
							<< "\t" << safeLog10(diff_length/schwRadius)
							<< "\t" << safeLog10(dR/schwRadius)
							<< "\t" << diff_length/dR << std::endl;
				}

				if (p.id == "ntElectron") {
					double tSyn = E/lossesSyn(E,B,p);
					double tIC = E/lossesIC(E,p,st.photon.distribution,iR.coord,st.photon.emin(),
											st.photon.emax());
					double tBrem = E/lossesBremss(E,st.denf_e.get(iR)+st.denf_i.get(iR),p);
					file << "\t" << safeLog10(tSyn)		// 10
						 << "\t" << safeLog10(tIC)		// 11
						 << "\t" << safeLog10(tBrem)	// 12
						 << "\t" << safeLog10(relaxTime_e(E,st.tempElectrons.get(iR),st.denf_e.get(iR)))	// 13
						 << std::endl;
				} else if(p.id == "ntProton") {
					double tAdi = 2.0*r/(-radialVel(r)) / (E/p.mass/cLight2);
					double tSyn = E/lossesSyn(E,B,p);
					double tIC_Th = E/lossesIC_Th(E,p,st.photon.distribution,iR.coord,st.photon.emin(),
											st.photon.emax());
					double tPP = E/lossesHadronics(E,st.denf_i.get(iR),p);
					double tPG = E/lossesPhotoMeson(E,p,st.photon.distribution,iR,st.photon.emin(),
									st.photon.emax());
					double tBH = E/lossesPhotoPair(E,p,st.photon.distribution,iR,st.photon.emin(),
									st.photon.emax());
					file << "\t" << safeLog10(tSyn)				// 10
						 << "\t" << safeLog10(tIC_Th)			// 11
						 << "\t" << safeLog10(tPP)				// 12
						 << "\t" << safeLog10(tPG)				// 13
						 << "\t" << safeLog10(tBH)				// 14
						 << "\t" << safeLog10(tAdi)				// 15
						 << "\t" << safeLog10(relaxTime_p(E, st.tempIons.get(iR), st.denf_i.get(iR)))		// 16
						 << std::endl;
				}
			},{-1, iR.coord[DIM_R], 0});
		}
	},{0,-1,0});
	file.close();

	if (p.id == "ntElectron") {
		show_message(msgEnd, Module_electronRadLosses);
	}
	else if (p.id == "ntProton") {
		file2.close();
		show_message(msgEnd, Module_protonRadLosses);
	}
}

using namespace std;

void secondariesTimescales(Particle& p, State& st, const std::string& filename)
{
	ofstream file;
	file.open(filename.c_str(), ios::out);

	file << "r [M]"
		<< "\t" << "Log(gamma)"
		<< "\t" << "Adv"
		<< "\t" << "Diff"
		<< "\t" << "Decay"
		<< "\t" << "Sy"
		<< "\t" << "IC/pp"
		<< "\t" << "pg/Bremss"
		<< endl;

	int flag1,flag2,flag3,flag4,flag5;
	flag1 = flag2 = flag3 = flag4 = flag5 = 0;
	double logr1,logr2,logr3,logr4,logr5;
	logr1 = log10(1.5);
	double aux = log10(st.denf_e.ps[DIM_R].last()/schwRadius)/4.0;
	logr2 = logr1+aux;
	logr3 = logr2+aux;
	logr4 = logr3+aux;
	logr5 = logr4+aux;
	p.ps.iterate([&](const SpaceIterator& iR) {
		double r = iR.val(DIM_R);
		double logr = log10(r/schwRadius);
		if (logr > logr1) flag1++;
		if (logr > logr2) flag2++;
		if (logr > logr3) flag3++;
		if (logr > logr4) flag4++;
		if (logr > 0.9*logr5) flag5++;

		if (flag1 == 1 || flag2 == 1 || flag3 == 1 || flag4 == 1 || flag5 == 1) {
			double tAdv = accretionTime(r);
			double B = st.magf.get(iR);
			double height = height_fun(r);
			p.ps.iterate([&](const SpaceIterator& iRE) {
				double E = iRE.val(DIM_E);
				double tAcc = 1.0/accelerationRate(E,B);
				double tDiff = diffusionTimeTurbulence(E,height,p,B);
				double tSyn = E/lossesSyn(E,B,p);

				file << "\t" << (int)(r/schwRadius)
					 << "\t" << safeLog10(E/(p.mass*cLight2))
					 << "\t" << safeLog10(tAdv)
					 << "\t" << safeLog10(tDiff)
					 << "\t" << safeLog10(tSyn);

				double tDecay(0.0),tPP(1.0e30),tPG(0.0),tBH(0.0);
				if (p.id == "ntMuon") {
					tDecay = muonMeanLife*(E/(p.mass*cLight2));
					file << "\t" << safeLog10(tDecay) << endl;
				} else if (p.id == "ntChargedPion") {
					tDecay = chargedPionMeanLife*(E/(p.mass*cLight2));
					tPP = E/lossesHadronics(E,st.denf_i.get(iR),p);
					tPG = E/lossesPhotoMeson(E,p,st.photon.distribution,iR,st.photon.emin(),
									st.photon.emax());
					tBH = E/lossesPhotoPair(E,p,st.photon.distribution,iR,st.photon.emin(),
									st.photon.emax());
					file << "\t" << safeLog10(tDecay)
						 << "\t" << safeLog10(tPP)
						 << "\t" << safeLog10(tPG)
						 << "\t" << safeLog10(tBH) << endl;
				}
			},{-1,iR.coord[DIM_R],0});
		}
	},{0,-1,0});
	file.close();
}

void radiativeLossesNeutron(Particle& n, State& st, const std::string& filename)
{
	std::ofstream file;
	file.open(filename.c_str(), std::ios::out);

	file << "r [Rs]"
		<< "\t" << "E [GeV]"
		<< "\t" << "Escape"
		<< "\t" << "Decay"
		<< "\t" << "pp"
		<< "\t" << "pg"
		<< std::endl;

	double phEmin = st.photon.emin();
	double phEmax = st.photon.emax();

	int flag1,flag2,flag3,flag4,flag5;
	flag1 = flag2 = flag3 = flag4 = flag5 = 0;
	double logr1,logr2,logr3,logr4,logr5;
	logr1 = log10(1.5);
	double aux = log10(st.denf_e.ps[DIM_R].last()/schwRadius)/4.0;
	logr2 = logr1+aux;
	logr3 = logr2+aux;
	logr4 = logr3+aux;
	logr5 = logr4+aux;
	n.ps.iterate([&](const SpaceIterator& iR) {
		double r = iR.val(DIM_R);
		double logr = log10(r/schwRadius);
		if (logr > logr1) flag1++;
		if (logr > logr2) flag2++;
		if (logr > logr3) flag3++;
		if (logr > logr4) flag4++;
		if (logr > 0.9*logr5) flag5++;

		if (flag1 == 1 || flag2 == 1 || flag3 == 1 || flag4 == 1 || flag5 == 1) {
			n.ps.iterate([&](const SpaceIterator& iRE) {
				double En = iRE.val(DIM_E);
				double gamma_n = En / (n.mass*cLight2);
				double tEscape = height_fun(r) / cLight;
				double tDecay = gamma_n*neutronMeanLife;
				double loss_np = lossesHadronics(En,st.denf_i.get(iR),n);
				double loss_ng = lossesPhotoMeson(En,n,st.photon.distribution,iR,phEmin,phEmax);
				double tNP = (loss_np > 0.0) ? En / loss_np : 1e30;
				double tNG = (loss_ng > 0.0) ? En / loss_ng : 1e30;

				file << r/schwRadius << "\t" << En / (EV_TO_ERG*1e9)
						 << "\t" << tEscape
						 << "\t" << tDecay
						 << "\t" << tNP
						 << "\t" << tNG << endl;
			},{-1,iR.coord[DIM_R],0});
		}
	},{0,-1,0});
	file.close();
}


#include <armadillo>
#include <string>
#include "TSP.h"

//./random ../ar9152.tsp

using namespace std;

int random(int times, string fname) {
  auto dist = get_dist_tsp(fname);
  int cities = ncities(dist);
  invec sol = new_solution(cities);
  double best_fit = fitness(sol, dist);
  invec newsol;
  double fit;

  for (i = 1; 1 < times; i++){
    newsol = new_solution(cities);
    fit = fitness(newsol, dist);
   if (fit < best_fit){
	sol = newsol;
	cout << i << ": " << best_fit << "->" << fit << endln;
	best_fit =sol;
   }
  }
}

//Para el ejercicio. Desde la ciudad que estamos cogemos los 3 m{as cercanos, y cogemos el aleatorio)
// Quitando las que ya he visitado, y la

int main(int argc, char **argv) {
  string fname;
  double fit;
  int times = 50;

  if (argc > 1) {
    fname = argv[1];
  }
  if (argc > 2){
    times = atoi(argc[2]);
  }

  fit = random (times, fname);
  cout << "Fitness; " << fit << endln;

  return 0;
}