// Boost.Geometry

// Copyright (c) 2018-2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_LINE_INTERPOLATE_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_LINE_INTERPOLATE_HPP

#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/strategies/line_interpolate.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/util/algorithm.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace line_interpolate
{

/*!
\brief Interpolate point on a cartesian segment.
\ingroup strategies
\tparam CalculationType \tparam_calculation
\tparam DistanceStrategy The underlying point-point distance strategy

\qbk{
[heading See also]
\* [link geometry.reference.algorithms.line_interpolate.line_interpolate_4_with_strategy line_interpolate (with strategy)]
}

*/
template
<
    typename CalculationType = void,
    typename DistanceStrategy = distance::pythagoras<CalculationType>
>
class cartesian
{
public:

    // point-point strategy getters
    struct distance_pp_strategy
    {
        typedef DistanceStrategy type;
    };

    inline typename distance_pp_strategy::type get_distance_pp_strategy() const
    {
        typedef typename distance_pp_strategy::type distance_type;
        return distance_type();
    }

    template <typename Point, typename Fraction, typename Distance>
    inline void apply(Point const& p0,
                      Point const& p1,
                      Fraction const& fraction,
                      Point & p,
                      Distance const&) const
    {
        typedef typename select_calculation_type_alt
            <
                CalculationType, Point
            >::type calc_t;
        typedef typename coordinate_type<Point>::type coord_t;

        //segment convex combination: p0*fraction + p1*(1-fraction)
        Fraction const one_minus_fraction = 1-fraction;
        geometry::detail::for_each_dimension<Point>([&](auto dimension)
        {
            // NOTE: numeric_cast is a leftover from convert, it could probably be ommited.
            // NOTE: the order of points is different than in the formula above
            //       this is also a leftover from the previous implementation
            calc_t coord0 = boost::numeric_cast<calc_t>(get<dimension>(p0));
            calc_t coord1 = boost::numeric_cast<calc_t>(get<dimension>(p1));
            calc_t result = calc_t(coord1 * fraction) + calc_t(coord0 * one_minus_fraction);
            set<dimension>(p, boost::numeric_cast<coord_t>(result));
        });
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <>
struct default_strategy<cartesian_tag>
{
    typedef strategy::line_interpolate::cartesian<> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::line_interpolate

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_LINE_INTERPOLATE_HPP


// Author(s): Koen Degeling
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/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 pgsolve_gjkw.cpp
/// Reads a parity game from input and reduces it using the Divergence-preserving
/// branching bisimulation equivalence using the O(m log m) algorithm
/// [Groote/Jansen/Keiren/Wijs 2017]

#include "liblts_bisim_gjkw_kripke.h"
#include "mcrl2/lts/lts_fsm.h"
#include "mcrl2/utilities/input_output_tool.h"
#include "pg.h"
#include "pgsolver_io.h"
#include "utilities.h"
#include "mcrl2/lts/lts_algorithm.h"
#include "scc.h"

#include <boost/graph/strong_components.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>

using namespace mcrl2::lts; // For reduce() function.
using namespace mcrl2::log; // Logging.

namespace mcrl2 {

class pg_convert
{

  protected:
    /** Storing Parity Game */
    parity_game_t m_pg;
    /** Reduced Kripke structure in LTS */
    lts::lts_fsm_t m_lts;

  public:
  pg_convert(parity_game_t pg)
    : m_pg(pg)
    {}

  parity_game_t get_parity_game()
  {
    return m_pg;
  }

  void convert_ks_to_pg()
  /*  Convert reduced Kripke structure in lts_fsm_t lts to a Parity Game in
      parity_game_t format. */
  {
    // Clear old Parity Game and construct new one from m_lts
    m_pg.clear();
    std::map<size_t, size_t> state_vertex_map;

    // Loop over states and add vertices to graph
    for(size_t i=0; i!=m_lts.num_states(); ++i )
    {
      std::vector<std::size_t> l = m_lts.state_label(i);
      printf("Label of the state: {%u,%u}.\n", l[0], l[1]);
      size_t v = boost::add_vertex(m_pg);
      printf("New vertex added: %u.\n", v);
      // Set properties of the new state
      m_pg[v].player = l[0] == 0 ? even : odd;
      m_pg[v].prio = l[1];
      // Safe the mapping from state to vertex for later when adding edges.
      state_vertex_map[i] = v;
    }

    for(const transition& t: m_lts.get_transitions())
    {
      size_t u = state_vertex_map[t.from()];
      size_t v = state_vertex_map[t.to()];
      boost::add_edge(u, v, m_pg);
    }
  }

  void reduce_pg_scc(std::string file)
  /* Reduce Parity Game graph to remove Strongly Connected Components. */
  {
    typedef typename boost::graph_traits<parity_game_t>::vertices_size_type vertex_size_t;
    parity_game_t* reduced = new parity_game_t(m_pg);
    boost::graph_traits<parity_game_t>::edge_iterator e, m;
    /* We (deep) copied the graph and go over all edges and remove any edge for which
      the source and target label are not the same (to ensure our SCC have same label)*/
    for(boost::tie(e,m) = edges(m_pg); e != m; ++e)
    {
      vertex_size_t s = source(*e, m_pg);
      vertex_size_t t = target(*e, m_pg);
      if(m_pg[s].prio != m_pg[t].prio || m_pg[s].player != m_pg[t].player)
      {
        // Since we aren't removing edges from m_pg, this is safe.
        boost::remove_edge(s,t,*reduced);
      }
    }
    /* We compute the strong components and go over the edges in our original graph.
      We merge all vertices that are in the same SCC. */
    std::vector<vertex_size_t> components (boost::num_vertices(*reduced), 0);
    size_t num = boost::strong_components(*reduced, &components[0]);
    std::map<size_t, vertex_size_t> component_map;
    mCRL2log(verbose) << "Shrinking the graph" << std::endl;
    boost::graph_traits<parity_game_t >::vertex_iterator i, n;
    for(boost::tie(i, n) = vertices(m_pg); i != n; ++i)
    {
      size_t group = components[*i];
      if(component_map.count(group)==1)
      // If already present, we shrink the graph by merging i into u.
      {
        vertex_size_t u = component_map[group];
        merge(m_pg, u, *i);
        m_pg[*i].unused = true;
      }
      else
      // We haven't seen any vertices of this group yet, so we add it to the map.
      {
        component_map[group] = *i;
      }
    }
    // We go over all the vertices and remove those which are unused.
    for(auto i = 0; i < boost::num_vertices(m_pg);)
    {
      if(m_pg[i].unused)
      {
        boost::clear_vertex(i, m_pg);
        boost::remove_vertex(i, m_pg);
      }
      else
      {
        i++;
      }
    }
  }

  typedef typename boost::graph_traits<parity_game_t>::
      vertices_size_type vertex_size_t;
  void merge(parity_game_t& pg, vertex_size_t s, vertex_size_t r)
  /* Merge vertex r into vertex s*/
  {
    boost::graph_traits<parity_game_t>::out_edge_iterator oe, m;
    for(boost::tie(oe,m) = out_edges(r, pg); oe != m; ++oe)
    {
      boost::add_edge(s, target(*oe, pg), pg); // Add new edge
    }
    boost::graph_traits<parity_game_t>::in_edge_iterator ie, n;
    for(boost::tie(ie,n) = in_edges(r, pg); ie != n; ++ie)
    {
      boost::add_edge(source(*ie, pg), s, pg); // Add new edge
    }
  }

  void convert_pg_to_ks()
  /* Convert Parity Game to a Kripke structure contained in lts_fsm_t. */
  {
    // LTS in which we save the Kripke structure
    std::map<size_t, size_t> vertex_state_map;
    std::map<size_t, size_t> mp;
    std::map<size_t, size_t> mp1;
    m_lts.add_process_parameter("Parity", "Nat");
    m_lts.add_process_parameter("Priority", "Nat");
    size_t zero = m_lts.add_state_element_value(0, "0");
    size_t one = m_lts.add_state_element_value(0, "1");
    mp1[0] = zero;
    mp1[1] = one;
    unsigned int max_priority = 8;
    for(unsigned int i = 0; i != max_priority; ++i)
    {
      size_t l = m_lts.add_state_element_value(1, std::to_string(i));
      mp[i] = l;
    }

    boost::graph_traits<parity_game_t >::vertex_iterator i, n;
    // We loop over vertices and add new states to the lts
    for(boost::tie(i, n) = vertices(m_pg); i != n; ++i)
    {
      size_t player = m_pg[*i].player;
      size_t priority = m_pg[*i].prio;
      state_label_fsm label =
          state_label_fsm(std::vector<std::size_t> {player,priority});
      size_t state = m_lts.add_state(label);
      // Add the (vertex,state) pair to map
      vertex_state_map[*i] = state;
    }
    m_lts.set_initial_state(0);
    size_t label = m_lts.add_action(action_label_string(" "));
    size_t divergence_label = m_lts.add_action(action_label_string("d"));
    // Loop over all edges in the graph and add transitions to our lts
    boost::graph_traits<parity_game_t>::edge_iterator e, m;
    for(boost::tie(e,m) = edges(m_pg); e != m; ++e)
    {
      size_t s = vertex_state_map[source(*e, m_pg)];
      size_t t = vertex_state_map[target(*e, m_pg)];
      if (s == t) // If we have a self-loop, add divergence-label
      {
        m_lts.add_transition(transition(s,divergence_label,t));
      }
      else
      {
        m_lts.add_transition(transition(s,label,t));
      }
    }
  }

  void run(std::string file)
  {
    reduce_pg_scc(file);
    convert_pg_to_ks();
    m_lts.save(file);
    // Call algorithm on m_lts.
    mCRL2log(verbose) << "Calling lts, with " << m_lts.num_states() << " states and " << m_lts.num_transitions() << " transitions." << std::endl;
    lts::detail::bisim_partitioner_gjkw_kripke(m_lts, true, true);
    mCRL2log(verbose) << "Replacing LTS!" << std::endl;
     convert_ks_to_pg();
    mCRL2log(verbose) << "Printed file" << std::endl;
  }
};

} // End mcrl2

using mcrl2::utilities::tools::input_output_tool;

template<class ParityGame>
class pgsolve_tool: public input_output_tool
{
  private:
    std::ifstream m_ifstream;
    std::ofstream m_ofstream;

  protected:
    typedef input_output_tool super;

  public:

    pgsolve_tool()
      : super(
          "pgsolve_gjkw",
          "Koen Degeling",
          "Reduce a parity game using the new m log n"
          "algorithm for stuttering bisimulation.",
          "Longer description"
      )
    {}

    bool run()
    {
      std::istream& is = open_input(input_filename(), m_ifstream);
      // Main program flow
      parity_game_t pg;
      mCRL2log(verbose) << "Loading PG from input file..." << std::endl;
      parse_pgsolver(pg, is, timer());
      mcrl2::pg_convert c = mcrl2::pg_convert(pg);
      c.run(output_filename());
      mCRL2log(verbose) << "Terminating" << std::endl;
      return true;
    }
};

int main(int argc, char* argv[])
{
  return pgsolve_tool<parity_game_t>().execute(argc, argv);
}


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

  Copyright 2016 NumScale SAS

  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/simd/pack.hpp>
#include <boost/simd/function/prev.hpp>
#include <boost/simd/function/bits.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/constant/halfeps.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/bitincrement.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/four.hpp>
#include <boost/simd/constant/mthree.hpp>

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : T(-i);
    b[i] = bs::prev(a1[i]) ;
  }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb (&b[0], &b[0]+N);
  STF_IEEE_EQUAL(bs::prev(aa1), bb);
}

STF_CASE_TPL("Check prev on pack" , STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

STF_CASE_TPL (" prev real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::prev;
  using p_t = bs::pack<T>;
  using r_t = decltype(prev(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(prev(bs::Inf<p_t>()), bs::Valmax<r_t>());
  STF_EQUAL(prev(bs::Minf<p_t>()), bs::Minf<r_t>());
  STF_IEEE_EQUAL(prev(bs::Nan<p_t>()), bs::Nan<r_t>());
  STF_EQUAL(prev(bs::Valmin<p_t>()), bs::Minf<r_t>());
#endif
  STF_EQUAL(prev(bs::Mone<p_t>()), bs::Mone<r_t>()-bs::Eps<r_t>());
  STF_EQUAL(prev(bs::One<p_t>()), bs::One<r_t>()-bs::Eps<r_t>()/2);
  STF_EQUAL(prev(bs::Zero<p_t>()), -bs::Bitincrement<p_t>());
}

STF_CASE_TPL (" prev unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::prev;
  using p_t = bs::pack<T>;
  using r_t = decltype(prev(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
  STF_EQUAL(prev(bs::One<p_t>()), bs::Zero<r_t>());
  STF_EQUAL(prev(bs::Valmin<p_t>()), bs::Valmax<r_t>());
  STF_EQUAL(prev(bs::Zero<p_t>()), bs::Valmax<r_t>());
}

STF_CASE_TPL (" prev signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::prev;
  using p_t = bs::pack<T>;
  using r_t = decltype(prev(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
  STF_EQUAL(prev(bs::Mone<p_t>()), -bs::Two<r_t>());
  STF_EQUAL(prev(bs::One<p_t>()), bs::Zero<r_t>());
  STF_EQUAL(prev(bs::Zero<p_t>()), bs::Mone<r_t>());
}


#include <fstream>
#include <iostream>
#include <string>
#include <unordered_map>
#include <unordered_set>

#include <boost/range/irange.hpp>

struct Point {
	std::int64_t x = 0;
	std::int64_t y = 0;
};

auto& operator+=(Point& lhs, const Point& rhs) {
	lhs.x += rhs.x;
	lhs.y += rhs.y;
	return lhs;
}

// this is not actually needed, but I'll keep it as a reference for
// how to implement operator+ for a struct in terms of operator+=
auto operator+(Point lhs, const Point& rhs) {
	lhs += rhs;
	return lhs;
}

auto operator==(const Point& lhs, const Point& rhs) {
	return ((lhs.x == rhs.x) && (lhs.y == rhs.y));
}

enum class Direction { NORTH, EAST, SOUTH, WEST };

struct Position {
	Point point;
	Direction direction = Direction::NORTH;
};

struct Situation {
	Direction direction;
	char turn;
};

auto operator==(const Situation& lhs, const Situation& rhs) {
	return ((lhs.direction == rhs.direction) && (lhs.turn == rhs.turn));
}

template<typename T>
auto get_hash(T value) {
	return std::hash<T>{}(value);
}

namespace std {
	template<>
	struct hash<Point> {
		auto operator()(const Point& point) const {
			return (get_hash(point.x) ^ get_hash(point.y));
		}
	};

	template<>
	struct hash<Situation> {
		auto operator()(const Situation& situation) const {
			return (get_hash(situation.direction) ^ get_hash(situation.turn));
		}
	};
}

int main() {

	const auto filename = std::string{"directions.txt"};
	auto file = std::fstream{filename};

	if(file.is_open()) {

		const auto left = 'L';
		const auto right = 'R';

		// get the new direction you are facing after the turn
		const auto guide = std::unordered_map<Situation, Direction>{
			{{Direction::NORTH, left}, Direction::WEST},
			{{Direction::NORTH, right}, Direction::EAST},
			{{Direction::EAST, left}, Direction::NORTH},
			{{Direction::EAST, right}, Direction::SOUTH},
			{{Direction::SOUTH, left}, Direction::EAST},
			{{Direction::SOUTH, right}, Direction::WEST},
			{{Direction::WEST, left}, Direction::SOUTH},
			{{Direction::WEST, right}, Direction::NORTH}
		};

		// converts a move in any direction to the corresponding
		// unit of distance, in this case, Point
		const auto point_units = std::unordered_map<Direction, Point>{
			{Direction::NORTH, {0, 1}},
			{Direction::EAST, {1, 0}},
			{Direction::SOUTH, {0, -1}},
			{Direction::WEST, {-1, 0}}
		};

		auto pos = Position{};
		auto& point = pos.point;
		auto& direction = pos.direction;

		auto locations = std::unordered_set<Point>{point};

		auto revisited = false;

		std::string instruction;

		while(!revisited && (file >> instruction)) {

			const auto turn = instruction.front();

			direction = guide.find({direction, turn})->second;

			auto blocks = std::stoul(instruction.substr(1, (instruction.size() - 1)));

			for(const auto i : boost::irange(blocks)) {

				point += point_units.find(direction)->second;

				// we make use of the fact that a set will fail to insert
				// an element if it's already present, so the first failed
				// insertion is our cue to leave the loop
				revisited = !(locations.emplace(point).second);

				if(revisited) {
					break;
				}
			}
		}

		const auto distance = (std::abs(point.x) + std::abs(point.y));

		std::cout << distance << std::endl;

	} else {
		std::cerr << "Error! Could not open \"" << filename << "\"!" << std::endl;
	}

	return 0;
}


/* boost random/linear_feedback_shift.hpp header file
 *
 * Copyright Jens Maurer 2002
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 */

#ifndef BOOST_RANDOM_LINEAR_FEEDBACK_SHIFT_HPP
#define BOOST_RANDOM_LINEAR_FEEDBACK_SHIFT_HPP

#include <iosfwd>
#include <stdexcept>
#include <boost/config.hpp>
#include <boost/cstdint.hpp>
#include <boost/static_assert.hpp>
#include <boost/integer/integer_mask.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/seed.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/detail/seed_impl.hpp>

namespace boost {
namespace random {

/**
 * Instatiations of @c linear_feedback_shift model a
 * \pseudo_random_number_generator.  It was originally
 * proposed in
 *
 *  @blockquote
 *  "Random numbers generated by linear recurrence modulo two.",
 *  Tausworthe, R. C.(1965), Mathematics of Computation 19, 201-209.
 *  @endblockquote
 */
template<class UIntType, int w, int k, int q, int s>
class linear_feedback_shift_engine
{
public:
    typedef UIntType result_type;
    BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
    BOOST_STATIC_CONSTANT(int, word_size = w);
    BOOST_STATIC_CONSTANT(int, exponent1 = k);
    BOOST_STATIC_CONSTANT(int, exponent2 = q);
    BOOST_STATIC_CONSTANT(int, step_size = s);
    BOOST_STATIC_CONSTANT(UIntType, default_seed = 341);

    /** Returns the smallest value that the generator can produce. */
    static result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () { return 0; }
    /** Returns the largest value that the generator can produce. */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return wordmask(); }

    BOOST_STATIC_ASSERT(w > 0);
    BOOST_STATIC_ASSERT(q > 0);
    BOOST_STATIC_ASSERT(k < w);
    BOOST_STATIC_ASSERT(0 < 2*q && 2*q < k);
    BOOST_STATIC_ASSERT(0 < s && s <= k-q);

    /** Constructs a @c linear_feedback_shift_engine, using the default seed. */
    linear_feedback_shift_engine() { seed(); }

    /** Constructs a @c linear_feedback_shift_engine, seeding it with s0. */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(linear_feedback_shift_engine,
        UIntType, s0)
    { seed(s0); }

    /** Constructs a @c linear_feedback_shift_engine, seeding it with seq. */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(linear_feedback_shift_engine,
        SeedSeq, seq)
    { seed(seq); }

    /**
     * Constructs a @c linear_feedback_shift_engine, seeding it with
     * values from the range [first, last).
     */
    template<class It> linear_feedback_shift_engine(It& first, It last)
    { seed(first, last); }

    /** Seeds a @c linear_feedback_shift_engine with the default seed. */
    void seed() {  seed(default_seed); }

    /** Seeds a @c linear_feedback_shift_engine with @c s0. */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(linear_feedback_shift_engine,
        UIntType, s0)
    {
        value = s0 & wordmask();
        if(value < (1 << (w-k))) {
            value += 1 << (w-k);
        }
    }

    /**
     * Seeds a @c linear_feedback_shift_engine with values
     * produced by @c seq.generate().
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(linear_feedback_shift_engine,
        SeedSeq, seq)
    { seed(detail::seed_one_int<UIntType, (UIntType(2) << (w - 1))>(seq)); }

    /**
     * Seeds a @c linear_feedback_shift_engine with values
     * from the range [first, last).
     */
    template<class It> void seed(It& first, It last)
    {
        seed(detail::get_one_int<UIntType, (UIntType(2) << (w - 1))>(first, last));
    }

    /** Returns the next value of the generator. */
    result_type operator()()
    {
        const UIntType b = (((value << q) ^ value) & wordmask()) >> (k-s);
        const UIntType mask = (wordmask() << (w-k)) & wordmask();
        value = ((value & mask) << s) ^ b;
        return value;
    }

    /** Fills a range with random values */
    template<class Iter>
    void generate(Iter first, Iter last)
    { detail::generate_from_int(*this, first, last); }

    /** Advances the state of the generator by @c z. */
    void discard(boost::uintmax_t z)
    {
        for(boost::uintmax_t j = 0; j < z; ++j) {
            (*this)();
        }
    }

    /**
     * Writes the textual representation of the generator to a @c std::ostream.
     */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, linear_feedback_shift_engine, x)
    {
        os << x.value;
        return os;
    }

    /**
     * Reads the textual representation of the generator from a @c std::istream.
     */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, linear_feedback_shift_engine, x)
    {
        is >> x.value;
        return is;
    }

    /**
     * Returns true if the two generators will produce identical
     * sequences of outputs.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(linear_feedback_shift_engine, x, y)
    { return x.value == y.value; }

    /**
     * Returns true if the two generators will produce different
     * sequences of outputs.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(linear_feedback_shift_engine)

private:
    /// \cond show_private
    static UIntType wordmask() { return boost::low_bits_mask_t<w>::sig_bits; }
    /// \endcond
    UIntType value;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class UIntType, int w, int k, int q, int s>
const bool linear_feedback_shift_engine<UIntType, w, k, q, s>::has_fixed_range;
template<class UIntType, int w, int k, int q, int s>
const int linear_feedback_shift_engine<UIntType, w, k, q, s>::word_size;
template<class UIntType, int w, int k, int q, int s>
const int linear_feedback_shift_engine<UIntType, w, k, q, s>::exponent1;
template<class UIntType, int w, int k, int q, int s>
const int linear_feedback_shift_engine<UIntType, w, k, q, s>::exponent2;
template<class UIntType, int w, int k, int q, int s>
const int linear_feedback_shift_engine<UIntType, w, k, q, s>::step_size;
template<class UIntType, int w, int k, int q, int s>
const UIntType linear_feedback_shift_engine<UIntType, w, k, q, s>::default_seed;
#endif

/// \cond show_deprecated

/** Provided for backwards compatibility. */
template<class UIntType, int w, int k, int q, int s, UIntType v = 0>
class linear_feedback_shift :
    public linear_feedback_shift_engine<UIntType, w, k, q, s>
{
    typedef linear_feedback_shift_engine<UIntType, w, k, q, s> base_type;
public:
    linear_feedback_shift() {}
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(linear_feedback_shift,
        SeedSeq, seq)
    { seed(seq); }
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(linear_feedback_shift,
        UIntType, val)
    { seed(val); }
    template<class It>
    linear_feedback_shift(It& first, It last) : base_type(first, last) {}
};

/// \endcond

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_LINEAR_FEEDBACK_SHIFT_HPP


/*
Brian Staber (brian.staber@gmail.com)
*/

#ifndef NRL_PCA_LIKELIHOOD
#define NRL_PCA_LIKELIHOOD

#include  <math.h>
#include "compressible_Mooney_Transverse_Isotropic_Random_Field.hpp"
#include "newtonRaphson.hpp"
#include "distributenrldata.hpp"
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/math/special_functions/gamma.hpp>

class nrl_PCA_Likelihood
{
private:

    Teuchos::ParameterList      _paramList;
    Teuchos::RCP<newtonRaphson> newton;
    std::string                 fullOutputPath;

    Epetra_Comm              * comm;
    Epetra_Map               * MapExpPoints;
    Epetra_SerialDenseVector lb, ub;

public:

    Teuchos::RCP<tiMooneyRandomField> interface;
    Teuchos::RCP<distributenrldata>    nrldata;

    nrl_PCA_Likelihood(Epetra_Comm & Comm, Teuchos::ParameterList & paramList){
        comm                   = &Comm;
        _paramList             = paramList;
        std::string pathnrl    = Teuchos::getParameter<std::string>(paramList.sublist("nrldata"),"pathnrl");
        std::string station    = Teuchos::getParameter<std::string>(paramList.sublist("nrldata"),"station");
        std::string outputpath = Teuchos::getParameter<std::string>(paramList.sublist("nrldata"),"outputpath");
        fullOutputPath         = outputpath + station + "/";
        interface              = Teuchos::rcp(new tiMooneyRandomField(Comm,paramList));
        newton                 = Teuchos::rcp(new newtonRaphson(*interface,paramList));
        nrldata                = Teuchos::rcp(new distributenrldata(*interface->Mesh,pathnrl));

        MapExpPoints           = new Epetra_Map(-1,nrldata->global_id_faces.size(),&nrldata->global_id_faces[0],0,Comm);
    }

    nrl_PCA_Likelihood(){
    }

    int rnd(unsigned int                  nmc                  ,
            Epetra_IntSerialDenseVector & seeds                ,
            Epetra_SerialDenseVector    & mean_parameters      ,
            Epetra_SerialDenseVector    & exponents            ,
            Epetra_SerialDenseVector    & correlation_lengths  ,
            Epetra_SerialDenseVector    & coeff_of_variation   ,
            double                      & plyagl_deg           ,
            bool                          printNewtonIterations,
            bool                          printDisplacements   ,
            bool                          printDeformations    ,
            bool                          printRandomVariableY ,
            bool                          printDeformationsOnFaces)
    {
        Epetra_SerialDenseVector omega(6);
        omega(0) = coeff_of_variation(0);
        omega(1) = coeff_of_variation(1);
        omega(2) = coeff_of_variation(2);
        omega(3) = coeff_of_variation(3);
        omega(4) = correlation_lengths(0);
        omega(5) = correlation_lengths(1);

        double plyagl = plyagl_deg*2.0*M_PI/360.0;
        interface->setParameters(mean_parameters,exponents,omega);
        interface->set_plyagl(plyagl);

        if (comm->MyPID()==0){
          std::cout << "Generation of the Gaussian fields...";
        }
        interface->RandomFieldGenerator(seeds);
        if (comm->MyPID()==0){
          std::cout << "done.\n";
        }
        Epetra_Vector RandomVariableX(*MapExpPoints);
        RandomVariableX.PutScalar(0.0);

        Epetra_Vector DeformationsOnFaces_xx(*MapExpPoints);
        Epetra_Vector DeformationsOnFaces_yy(*MapExpPoints);
        Epetra_Vector DeformationsOnFaces_xy(*MapExpPoints);
        DeformationsOnFaces_xx.PutScalar(0.0);
        DeformationsOnFaces_yy.PutScalar(0.0);
        DeformationsOnFaces_xy.PutScalar(0.0);

        int error;
        newton->Initialization();
        for (unsigned int i=0; i<nrldata->boundaryconditions.Length(); ++i){
            newton->setParameters(_paramList);
            if(i==0){
                newton->bc_disp=nrldata->boundaryconditions(i);
            }
            else{
                newton->bc_disp=nrldata->boundaryconditions(i)-nrldata->boundaryconditions(i-1);
            }

            error = newton->Solve_with_Aztec(printNewtonIterations);

            if (!error){

                if (printDisplacements){
                  std::string path = fullOutputPath + "u_nmc=" + std::to_string(nmc) + "_angle=" + std::to_string(int(plyagl_deg)) + "_load=" + std::to_string(i) + "_delta=" + std::to_string(omega(0)) + "_l1" + std::to_string(omega(4)) + "_l2" + std::to_string(omega(5)) + ".mtx";
                  int flag = newton->print_newton_solution(path);
                  if (flag){
                    if (comm->MyPID()==0){
                      std::cout << "Failed printing displacements.";
                    }
                  }
                }

                if (printDeformations){
                  std::string path = fullOutputPath + "e_nmc" + std::to_string(nmc) + "_angle=" + std::to_string(int(plyagl_deg)) + "_load=" + std::to_string(i) + "_delta=" + std::to_string(omega(0)) + "_l1" + std::to_string(omega(4)) + "_l2" + std::to_string(omega(5)) + ".mtx";
                  double xi = 0.0;
                  int flag = interface->compute_green_lagrange(*newton->x,xi,xi,xi,path);
                  if (flag){
                    if (comm->MyPID()==0){
                      std::cout << "Failed printing deformations.";
                    }
                  }
                }

                Epetra_SerialDenseMatrix eij(nrldata->local_id_faces.size(),3);
                compute_green_lagrange(*newton->x,eij);

                if (printDeformationsOnFaces){
                  for (unsigned int i=0; i<nrldata->global_id_faces.size(); ++i){
                    DeformationsOnFaces_xx[MapExpPoints->LID(nrldata->global_id_faces[i])] = eij(i,0);
                    DeformationsOnFaces_yy[MapExpPoints->LID(nrldata->global_id_faces[i])] = eij(i,1);
                    DeformationsOnFaces_xy[MapExpPoints->LID(nrldata->global_id_faces[i])] = eij(i,2);
                  }
                  std::string pathDeformationsOnFaces_xx = fullOutputPath + "exx_nmc=" + std::to_string(nmc) + "_plyagl=" + std::to_string(int(plyagl_deg)) + "_s=" + std::to_string(i) + ".mtx";
                  std::string pathDeformationsOnFaces_yy = fullOutputPath + "eyy_nmc=" + std::to_string(nmc) + "_plyagl=" + std::to_string(int(plyagl_deg)) + "_s=" + std::to_string(i) + ".mtx";
                  std::string pathDeformationsOnFaces_xy = fullOutputPath + "exy_nmc=" + std::to_string(nmc) + "_plyagl=" + std::to_string(int(plyagl_deg)) + "_s=" + std::to_string(i) + ".mtx";
                  comm->Barrier();
                  int error1 = printIndicatorY(pathDeformationsOnFaces_xx, DeformationsOnFaces_xx);
                  int error2 = printIndicatorY(pathDeformationsOnFaces_yy, DeformationsOnFaces_yy);
                  int error3 = printIndicatorY(pathDeformationsOnFaces_xy, DeformationsOnFaces_xy);
                }

                for (unsigned int j=0; j<nrldata->local_id_faces.size(); ++j){
                    RandomVariableX[j] += eij(j,0)*eij(j,0)+eij(j,1)*eij(j,1)+2.0*eij(j,2)*eij(j,2);
                }
            }
            else{
                if (comm->MyPID()==0){
                    std::cout << "Newton failed at loading " << i << ".\n";
                    std::cout << "GIndicator(" << i << ") set to 0.0.\n";
                }
                error = 1;
                break;
            }
        }
        if (printRandomVariableY && !error){
          std::string pathToRandomVariableX = fullOutputPath + "RandomVariableY_angle=" + std::to_string(int(plyagl_deg)) + "_nmc=" + std::to_string(nmc) + ".mtx";
          int error = printIndicatorY(pathToRandomVariableX, RandomVariableX);
          return error;
        }
        else{
          return error;
        }
    }

    int printIndicatorY(std::string filename, Epetra_Vector & X){
      int NumTargetElements = 0;
      if (comm->MyPID()==0){
          NumTargetElements = nrldata->npoints;
      }
      Epetra_Map         MapOnRoot(-1,NumTargetElements,0,*comm);
      Epetra_Export      ExportOnRoot(*MapExpPoints,MapOnRoot);
      Epetra_MultiVector lhs_root(MapOnRoot,true);
      lhs_root.Export(X,ExportOnRoot,Insert);

      int error = EpetraExt::MultiVectorToMatrixMarketFile(filename.c_str(),lhs_root,0,0,false);
      return error;
    }

    void compute_green_lagrange(Epetra_Vector & x, Epetra_SerialDenseMatrix & eij){
        Epetra_Vector u(*(interface->OverlapMap));
        u.Import(x, *(interface->ImportToOverlapMap), Insert);

        int e_gid, node;
        double det_jac;
        Epetra_SerialDenseMatrix matrix_X(2,interface->Mesh->face_type);
        Epetra_SerialDenseMatrix matrix_x(2,interface->Mesh->face_type);
        Epetra_SerialDenseMatrix D(interface->Mesh->face_type,2);
        Epetra_SerialDenseMatrix dx_shape_functions(interface->Mesh->face_type,2);
        Epetra_SerialDenseMatrix deformation_gradient(2,2);
        Epetra_SerialDenseMatrix JacobianMatrix(2,2);
        Epetra_SerialDenseMatrix InverseJacobianMatrix(2,2);
        Epetra_SerialDenseMatrix right_cauchy(2,2);

        for (unsigned int e=0; e<nrldata->local_id_faces.size(); ++e){
            e_gid = interface->Mesh->local_faces[nrldata->local_id_faces[e]];
            //if (interface->Mesh->faces_phases[e_gid]==92 || interface->Mesh->faces_phases[e_gid]==93){
              for (unsigned int inode=0; inode<interface->Mesh->face_type; ++inode){
                  node = interface->Mesh->faces_nodes[interface->Mesh->face_type*e_gid+inode];
                  matrix_X(0,inode) = interface->Mesh->nodes_coord[3*node+0];
                  matrix_X(1,inode) = interface->Mesh->nodes_coord[3*node+1];
                  matrix_x(0,inode) = u[interface->OverlapMap->LID(3*node+0)] + interface->Mesh->nodes_coord[3*node+0];
                  matrix_x(1,inode) = u[interface->OverlapMap->LID(3*node+1)] + interface->Mesh->nodes_coord[3*node+1];
              }
              switch (interface->Mesh->face_type){
                  case 3:
                      tri3::d_shape_functions(D, nrldata->local_xi[e], nrldata->local_eta[e]);
                      break;
                  case 4:
                      quad4::d_shape_functions(D, nrldata->local_xi[e], nrldata->local_eta[e]);
                      break;
                  case 6:
                      tri6::d_shape_functions(D, nrldata->local_xi[e], nrldata->local_eta[e]);
                      break;
              }
              jacobian_matrix(matrix_X,D,JacobianMatrix);
              det_jac = fabs(JacobianMatrix(0,0)*JacobianMatrix(1,1) - JacobianMatrix(1,0)*JacobianMatrix(0,1));
              InverseJacobianMatrix(0,0) =  (1.0/det_jac)*JacobianMatrix(1,1);
              InverseJacobianMatrix(1,1) =  (1.0/det_jac)*JacobianMatrix(0,0);
              InverseJacobianMatrix(0,1) = -(1.0/det_jac)*JacobianMatrix(0,1);
              InverseJacobianMatrix(1,0) = -(1.0/det_jac)*JacobianMatrix(1,0);
              dx_shape_functions.Multiply('N','N',1.0,D,InverseJacobianMatrix,0.0);

              deformation_gradient.Multiply('N','N',1.0,matrix_x,dx_shape_functions,0.0);
              right_cauchy.Multiply('T','N',1.0,deformation_gradient,deformation_gradient,0.0);

              eij(e,0) = (1.0/2.0)*(right_cauchy(0,0)-1.0);
              eij(e,1) = (1.0/2.0)*(right_cauchy(1,1)-1.0);
              eij(e,2) = (1.0/2.0)*right_cauchy(0,1);
          }
        //}
    }

    /*void compute_force_reaction(Epetra_Vector & x){
      Epetra_Vector u(*(interface->OverlapMap));
      u.Import(x, *(interface->ImportToOverlapMap), Insert);

      int e_gid, node;
      double det_jac;

      Epetra_SerialDenseMatrix matrix_X(2,interface->Mesh->face_type);
      Epetra_SerialDenseMatrix matrix_x(2,interface->Mesh->face_type);

      Epetra_SerialDenseMatrix D(interface->Mesh->face_type,2);
      Epetra_SerialDenseMatrix dx_shape_functions(interface->Mesh->face_type,2);
      Epetra_SerialDenseMatrix deformation_gradient(2,2);
      Epetra_SerialDenseMatrix JacobianMatrix(2,2);
      Epetra_SerialDenseMatrix InverseJacobianMatrix(2,2);

      Epetra_SerialDenseVector TResult(2);
      Epetra_SerialDenseMatrix PiolaS(2,2);
      Epetra_SerialDenseVector TStress(2);
      Epetra_SerianDenseVector Normal(2);

      for (unsigned int e=0; e<interface->Mesh->local_faces.size(); ++e){
          e_gid = interface->Mesh->local_faces[e];
          if (interface->Mesh->faces_phases[e_gid]==90){
            for (unsigned int inode=0; inode<interface->Mesh->face_type; ++inode){
                node = interface->Mesh->faces_nodes[interface->Mesh->face_type*e_gid+inode];
                matrix_X(0,inode) = interface->Mesh->nodes_coord[3*node+0];
                matrix_X(1,inode) = interface->Mesh->nodes_coord[3*node+2];
                matrix_x(0,inode) = u[interface->OverlapMap->LID(3*node+0)] + interface->Mesh->nodes_coord[3*node+0];
                matrix_x(1,inode) = u[interface->OverlapMap->LID(3*node+2)] + interface->Mesh->nodes_coord[3*node+2];
            }
            for (unsigned int gp=0; gp<interface->Mesh->n_gauss_faces; ++gp){
              jacobian_matrix(matrix_X,D,JacobianMatrix);
              det_jac = fabs(JacobianMatrix(0,0)*JacobianMatrix(1,1) - JacobianMatrix(1,0)*JacobianMatrix(0,1));
              InverseJacobianMatrix(0,0) =  (1.0/det_jac)*JacobianMatrix(1,1);
              InverseJacobianMatrix(1,1) =  (1.0/det_jac)*JacobianMatrix(0,0);
              InverseJacobianMatrix(0,1) = -(1.0/det_jac)*JacobianMatrix(0,1);
              InverseJacobianMatrix(1,0) = -(1.0/det_jac)*JacobianMatrix(1,0);
              dx_shape_functions.Multiply('N','N',1.0,D,InverseJacobianMatrix,0.0);
              deformation_gradient.Multiply('N','N',1.0,matrix_x,dx_shape_functions,0.0);

              TStress.Multiplt('N','N',1.0,PiolaS,Normal,0.0);
              TResult.Multiply('N','N',interface->Mesh->gauss_weight_faces[gp],deformation_gradient,TStress,1.0);
            }
          }
      }
    }
    */

};
#endif


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

#include <cmath>
#include <fstream>

#include <sycomore/magnetization.h>
#include <sycomore/como/Model.h>
#include <sycomore/Pulse.h>
#include <sycomore/sycomore.h>
#include <sycomore/units.h>

using namespace sycomore::units;

BOOST_AUTO_TEST_CASE(OffResonance, *boost::unit_test::tolerance(1e-9))
{
    sycomore::Species const species(0_Hz, 0_Hz, 0_um*um/ms);
    sycomore::Magnetization const m0{0,0,1};

    sycomore::Pulse const pulse(90_deg, M_PI*rad);
    auto const pulse_duration=1_ms;
    int const pulse_support_size = 101;
    int const zero_crossings = 2;

    // NOTE: in the absence of relaxation and diffusion, the TR is meaningless
    auto const TR=500_ms;
    auto const slice_thickness=1_mm;

    auto const t0 = pulse_duration/(2*zero_crossings);
    sycomore::HardPulseApproximation const sinc_pulse(
        pulse,
        sycomore::linspace(pulse_duration, pulse_support_size),
        sycomore::sinc_envelope(t0), 1/t0, slice_thickness, "rf");

    auto const frequencies = sycomore::linspace(60._rad/ms, 201);

    sycomore::como::Model model(
        species, m0, {
            {"rf", sinc_pulse.get_time_interval()},
            {"refocalization", {
                (TR-pulse_duration)/2., -sinc_pulse.get_gradient_moment()/2}}
    });

    model.apply_pulse(sinc_pulse);
    model.apply_time_interval("refocalization");

    std::vector<sycomore::Magnetization> magnetization;
    for(auto && frequency: frequencies)
    {
        auto const signal = model.isochromat({}, {}, frequency);
        magnetization.push_back(signal);
    }

    std::vector<double> baseline;
    std::string const root(getenv("SYCOMORE_TEST_DATA")?getenv("SYCOMORE_TEST_DATA"):"");
    if(root.empty())
    {
        throw std::runtime_error("SYCOMORE_TEST_DATA is undefined");
    }
    std::ifstream stream(root+"/baseline/off_resonance.dat", std::ios_base::binary);
    while(stream.good())
    {
        double value;
        stream.read(reinterpret_cast<char*>(&value), sizeof(value));
        if(stream.good())
        {
            baseline.push_back(value);
        }
    }

    // WARNING: we are using absolute tolerance, not relative to the value of
    // left and right
    BOOST_REQUIRE_EQUAL(baseline.size(), 2*magnetization.size());
    for(size_t i=0; i<magnetization.size(); ++i)
    {
        BOOST_TEST(
            sycomore::transversal(magnetization[i])-baseline[2*i] == 0.,
            "Error at " << i << " [ "
                << sycomore::transversal(magnetization[i]) <<
                " != " << baseline[2*i] << " ]");
        BOOST_TEST(
            magnetization[i][2]-baseline[2*i+1] == 0.,
            "Error at " << i << " [ "
                << magnetization[i][2] << " != " << baseline[2*i+1] << " ]");
    }
}


/*
Copyright 2017. All rights reserved.
Computer Vision Group, Visual Computing Institute
RWTH Aachen University, Germany

This file is part of the rwth_mot framework.
Authors: Aljosa Osep (osep -at- vision.rwth-aachen.de)

rwth_mot framework is free software; you can redistribute it and/or modify it under the
terms of the GNU General Public License as published by the Free Software
Foundation; either version 3 of the License, or any later version.

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

You should have received a copy of the GNU General Public License along with
rwth_mot framework; if not, write to the Free Software Foundation, Inc., 51 Franklin
Street, Fifth Floor, Boston, MA 02110-1301, USA
*/

#include <tracking/extended_kalman_filter.h>

#include <Eigen/LU>

#include <iostream>
#include <fstream>

namespace GOT {
    namespace tracking {

        // -------------------------------------------------------------------------------
        // +++ Implementation: EKF base class +++
        // -------------------------------------------------------------------------------

        ExtendedKalmanFilter::ExtendedKalmanFilter(const Parameters &params)
                : KalmanFilter(params) {
        }

        void ExtendedKalmanFilter::ComputePrediction(const Eigen::VectorXd &u, Eigen::VectorXd &x_prio,
                                                     Eigen::MatrixXd &P_prio) const {

            x_prio = NonlinearStateProjection(); // + u;

            if (x_prio.size() != this->params_.state_vector_dim_) {
                x_prio = Eigen::VectorXd();
                P_prio = Eigen::MatrixXd();
                return;
            }

            x_prio += u;

            // Will linearize A by computing Jacc. of the (non-linear) transition matrix
            Eigen::MatrixXd A_jacc = LinearizeTransitionMatrix();

            // Project covariance ahead. Note, that A has been linearized.
            P_prio = A_jacc*P_*A_jacc.transpose() + G_;
        }

        /// H is the observation model matrix (residual = z_t - H*x)
        void ExtendedKalmanFilter::Correction(const Eigen::VectorXd z_t, const Eigen::MatrixXd &observation_cov_t, const Eigen::MatrixXd &H) {

            Eigen::MatrixXd H_transposed = H.transpose();

            // Compute Kalman gain
            Eigen::MatrixXd HPH_plus_obs_cov= (H*P_*H_transposed)+observation_cov_t;
            // Eigen::MatrixXd K = P_*H_transposed*HPH_plus_obs_cov.inverse();
            Eigen::MatrixXd K = P_*H_transposed;
            Eigen::MatrixXd inv = HPH_plus_obs_cov.inverse();
            K = K * inv;
            Eigen::VectorXd measurement_residual = compute_measurement_residual(z_t, H);

            x_ = x_ + K*measurement_residual; //(z_t - H_*x_);

            // Update the covariance
            Eigen::MatrixXd  KH = K*H;
            P_ = (Eigen::MatrixXd::Identity(KH.rows(), KH.cols()) - KH)*P_;

            // Optionally, store whole history. Eg. for visualization.
            if (params_.store_history_) {
                this->state_corrections_.push_back(x_);
                this->cov_corrections_.push_back(P_);
                this->measurements_.push_back(z_t);
                this->cov_measurements_.push_back(observation_cov_t);

                this->kalman_gain_.push_back(K);
            }
        }

        // -------------------------------------------------------------------------------
        // +++ Implementation: 'Coupled state filter' base class +++
        // -------------------------------------------------------------------------------

        GOT::tracking::CoupledExtendedKalmanFilter::CoupledExtendedKalmanFilter(const GOT::tracking::KalmanFilter::Parameters &params)
                : ExtendedKalmanFilter(params) {
            use_2d_measurement_to_update_3d_state_ = true;
        }

        void CoupledExtendedKalmanFilter::set_camera_current_frame(const SUN::utils::Camera &camera) {
            this->camera_current_frame_ = camera;
        }

        void CoupledExtendedKalmanFilter::set_use_2d_measurement_to_update_3d_state(bool flag) {
            this->use_2d_measurement_to_update_3d_state_ = flag;
        }
    }
}


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

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#include "Day7.hpp"

int runIntcodeComputer2(std::vector<int>& values, int firstInput, int secondInput);

// Finished, firstTime, cursorPosition, Value returned
//std::tuple<bool, bool, int, int>

void day7() {
	std::ifstream inputFile("Day7.txt");
	std::string line;
	std::getline(inputFile, line);

	std::vector<std::string> strings;
	boost::split(strings, line, boost::is_any_of(","), boost::token_compress_on);

	std::vector<int> numbersOriginal;

	//std::transform(strings.begin(), strings.end(), std::back_inserter(numbers), [](const std::string& str) { return std::stoi(str); });
	std::transform(strings.begin(), strings.end(), std::back_inserter(numbersOriginal), &boost::lexical_cast<int, std::string>);

	// Reliquat, je garde lol
	std::vector<int> numbers1(numbersOriginal.size());
	std::vector<int> numbers2(numbersOriginal.size());
	std::vector<int> numbers3(numbersOriginal.size());
	std::vector<int> numbers4(numbersOriginal.size());
	std::vector<int> numbers5(numbersOriginal.size());
	//std::copy(numbersOriginal.begin(), numbersOriginal.end(), numbers.begin());

	int maxResult = 0;
	std::tuple<int, int, int, int, int> sequence;

	std::vector<int> range{ 5, 6, 7, 8, 9 };
	do {
		std::copy(numbersOriginal.begin(), numbersOriginal.end(), numbers1.begin());
		std::copy(numbersOriginal.begin(), numbersOriginal.end(), numbers2.begin());
		std::copy(numbersOriginal.begin(), numbersOriginal.end(), numbers3.begin());
		std::copy(numbersOriginal.begin(), numbersOriginal.end(), numbers4.begin());
		std::copy(numbersOriginal.begin(), numbersOriginal.end(), numbers5.begin());

		int result1 = 0;
		int result2 = 0;
		int result3 = 0;
		int result4 = 0;
		int result5 = 0;

		while (true) {
			result1 = runIntcodeComputer2(numbers1, range[0], result5);
			result2 = runIntcodeComputer2(numbers2, range[1], result1);
			result3 = runIntcodeComputer2(numbers3, range[2], result2);
			result4 = runIntcodeComputer2(numbers4, range[3], result3);
			result5 = runIntcodeComputer2(numbers5, range[4], result4);
		}

		if (result5 > maxResult) {
			maxResult = result5;
			sequence = std::make_tuple(range[0], range[1], range[2], range[3], range[4]);
		}
	} while (std::next_permutation(range.begin(), range.end()));

	std::cout << "Best sequence: " <<
		std::get<0>(sequence) << "-" <<
		std::get<1>(sequence) << "-" <<
		std::get<2>(sequence) << "-" <<
		std::get<3>(sequence) << "-" <<
		std::get<4>(sequence) <<
		" - Value: " << maxResult << std::endl;
}

int runIntcodeComputer2(std::vector<int>& values, int firstInput, int secondInput) {
	//int index = 0;
	//for (auto number = values.begin(); values != values.end(); ++number) {
	//	std::cout << *number;
	//	if (index % 4 == 3) {
	//		std::cout << std::endl;
	//	}
	//	else {
	//		std::cout << " ";
	//	}
	//	index++;
	//}

	bool firstTime = true;

	int cursorPosition = 0;
	while (true) {
		// Extract opcode and parameters
		int code = values[cursorPosition];
		int opcode = code % 100;

		int parametersValue = code / 100;
		std::vector<bool> parametersMode(3, 0);
		for (int i = 0; parametersValue > 0; i++) {
			parametersMode[i] = parametersValue % 10 == 1;
			parametersValue /= 10;
		}

		// Halt condition
		if (opcode == 99)
			break;

		int cursorIncrement = 0;
		switch (opcode)
		{
		case 1:
		case 2:
		{
			// Add
			int valueAtAddress1 = values[cursorPosition + 1];
			int value1 = parametersMode[0] ? valueAtAddress1 : values[valueAtAddress1];

			int valueAtAddress2 = values[cursorPosition + 2];
			int value2 = parametersMode[1] ? valueAtAddress2 : values[valueAtAddress2];

			int valueAtAddressTarget = values[cursorPosition + 3];

			if (opcode == 1) {
				values[valueAtAddressTarget] = value1 + value2;
			}
			else {
				values[valueAtAddressTarget] = value1 * value2;
			}

			cursorIncrement = 4;
			break;
		}
		case 3:
		{
			// Input
			int inputValue = 0;
			//std::cout << "Enter value :" << std::endl;
			//std::cin >> inputValue;
			//std::cout << "Thanks!" << std::endl << std::endl;
			if (firstTime) {
				inputValue = firstInput;
				firstTime = false;
			}
			else {
				inputValue = secondInput;
			}

			int addressTarget = values[cursorPosition + 1];
			values[addressTarget] = inputValue;

			cursorIncrement = 2;
			break;
		}
		case 4:
		{
			// Output
			int addressTarget = values[cursorPosition + 1];
			int targetValue = values[addressTarget];

			//std::cout << "Output is: " << targetValue << std::endl << std::endl;

			//cursorIncrement = 2;
			//break;
			return targetValue;
		}
		case 5:
		case 6:
		{
			int valueAtAddress1 = values[cursorPosition + 1];
			int value1 = parametersMode[0] ? valueAtAddress1 : values[valueAtAddress1];

			int valueAtAddress2 = values[cursorPosition + 2];
			int value2 = parametersMode[1] ? valueAtAddress2 : values[valueAtAddress2];

			if (opcode == 5 && value1 != 0 || opcode == 6 && value1 == 0) {
				cursorPosition = value2;
				cursorIncrement = 0;
			}
			else {
				cursorIncrement = 3;
			}
			break;
		}
		case 7:
		case 8:
		{
			// Add
			int valueAtAddress1 = values[cursorPosition + 1];
			int value1 = parametersMode[0] ? valueAtAddress1 : values[valueAtAddress1];

			int valueAtAddress2 = values[cursorPosition + 2];
			int value2 = parametersMode[1] ? valueAtAddress2 : values[valueAtAddress2];

			int valueAtAddressTarget = values[cursorPosition + 3];

			if ((opcode == 7 && value1 < value2) || (opcode == 8 && value1 == value2)) {
				values[valueAtAddressTarget] = 1;
			}
			else {
				values[valueAtAddressTarget] = 0;
			}

			cursorIncrement = 4;
			break;
		}
		}
		cursorPosition += cursorIncrement;
	}

	//index = 0;
	//for (auto number = values.begin(); number != values.end(); ++number) {
	//	std::cout << *number;
	//	if (index % 4 == 3) {
	//		std::cout << std::endl;
	//	}
	//	else {
	//		std::cout << " ";
	//	}
	//	index++;
	//}

	//std::cout << std::endl << std::endl << std::endl << values[0];

	std::cout << "Value at 0: " << values[0] << std::endl;

	return values[0];
}


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

// Copyright (c) 2012 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_SIDE_ON_CONVEX_RANGE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_SIDE_ON_CONVEX_RANGE_HPP

#include <boost/range.hpp>
#include <boost/geometry/algorithms/detail/overlay/segment_identifier.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace buffer
{

template <int D>
struct collinear_point_on_segment_check
{
	template <typename T>
    static inline bool apply_sorted(T const& subject, T const& c1, T const& c2)
    {
        return subject >= c1 && subject <= c2;
    }

    template <typename P0, typename P1, typename P2>
    static inline bool apply(P0 const& subject, P1 const& p1, P2 const& p2)
    {
	    typedef typename geometry::coordinate_type<P0>::type coordinate_type;
        coordinate_type const cs = geometry::get<D>(subject);
        coordinate_type const c1 = geometry::get<D>(p1);
        coordinate_type const c2 = geometry::get<D>(p2);
        return c1 > c2
            ? apply_sorted(cs, c2, c1)
            : apply_sorted(cs, c1, c2)
            ;
    }
};

// Checks is subject-point is on the segment, provided that it is already determined that it was collinear
template <typename P0, typename P1, typename P2>
inline bool collinear_point_on_segment(P0 const& subject, P1 const& p1, P2 const& p2)
{
    return collinear_point_on_segment_check<0>::apply(subject, p1, p2)
        && collinear_point_on_segment_check<1>::apply(subject, p1, p2);
}

template <typename SideStrategy, typename Point, typename Range>
inline int side_on_convex_range(Point const& subject, Range const& range)
{
    bool has_collinear = false;

    typedef typename boost::range_iterator
        <
            Range const
        >::type iterator_type;

    iterator_type it = boost::begin(range);
    for (iterator_type prev = it++;
        it != boost::end(range);
        prev = it++)
    {
        int const side = SideStrategy::apply(subject, *prev, *it);
        switch(side)
        {
            case 1 :
                return 1;
            case 0 :
                // Check if it is really on the segment.
                // If not, it is either on the left (because polygon is convex)
                // or it is still on one of the other segments (if segments are collinear)
                if (collinear_point_on_segment(subject, *prev, *it))
                {
                    return 0;
                }
                has_collinear = true;
                break;
        }
    }
    return has_collinear ? 1 : -1;
}

template <typename SideStrategy, typename Point, typename Iterator>
static inline int side_on_convex_range(Point const& subject,
			Iterator first, Iterator last,
			/* by value: */ segment_identifier seg_id,
			segment_identifier& on_segment_seg_id)
{
    bool has_collinear = false;
    Iterator it = first;
    for (Iterator prev = it++; it != last; prev = it++, seg_id.segment_index++)
    {
        int side = SideStrategy::apply(subject, *prev, *it);
        switch(side)
        {
            case 1 :
                return 1;
            case 0 :
                // Check if it is REALLY on the segment.
                // If not, it is either on the left (because polygon is convex)
                // or it is still on one of the other segments (if segments are collinear)
                if (collinear_point_on_segment(subject, *prev, *it))
                {
					on_segment_seg_id = seg_id;
                    return 0;
                }
                has_collinear = true;
                break;
        }
    }
    return has_collinear ? 1 : -1;
}

}} // namespace detail::buffer
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_SIDE_ON_CONVEX_RANGE_HPP


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// 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.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE basic_components_test

#include <boost/test/unit_test.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/curves/mnt4.hpp>
#include <nil/crypto3/algebra/curves/mnt6.hpp>

#include <nil/crypto3/zk/components/disjunction.hpp>
#include <nil/crypto3/zk/components/conjunction.hpp>
#include <nil/crypto3/zk/components/comparison.hpp>
#include <nil/crypto3/zk/components/inner_product.hpp>
#include <nil/crypto3/zk/components/loose_multiplexing.hpp>

using namespace nil::crypto3;
using namespace nil::crypto3::zk;
using namespace nil::crypto3::algebra;

template<typename FieldType>
void test_disjunction_component(size_t n) {
    components::blueprint<FieldType> bp;
    components::blueprint_variable_vector<FieldType> inputs;
    inputs.allocate(bp, n);

    components::blueprint_variable<FieldType> output;
    output.allocate(bp);

    components::disjunction<FieldType> d(bp, inputs, output);
    d.generate_r1cs_constraints();

    for (std::size_t w = 0; w < 1ul << n; ++w) {
        for (std::size_t j = 0; j < n; ++j) {
            bp.val(inputs[j]) = typename FieldType::value_type((w & (1ul << j)) ? 1 : 0);
        }

        d.generate_r1cs_witness();

        BOOST_CHECK(bp.val(output) == (w ? FieldType::value_type::one() : FieldType::value_type::zero()));
        BOOST_CHECK(bp.is_satisfied());

        bp.val(output) = (w ? FieldType::value_type::zero() : FieldType::value_type::one());
        BOOST_CHECK(!bp.is_satisfied());
    }
}

template<typename FieldType>
void test_conjunction_component(size_t n) {
    components::blueprint<FieldType> bp;
    components::blueprint_variable_vector<FieldType> inputs;
    inputs.allocate(bp, n);

    components::blueprint_variable<FieldType> output;
    output.allocate(bp);

    components::conjunction<FieldType> c(bp, inputs, output);
    c.generate_r1cs_constraints();

    for (std::size_t w = 0; w < 1ul << n; ++w) {
        for (std::size_t j = 0; j < n; ++j) {
            bp.val(inputs[j]) = (w & (1ul << j)) ? FieldType::value_type::one() : FieldType::value_type::zero();
        }

        c.generate_r1cs_witness();

        BOOST_CHECK(bp.val(output) ==
                    (w == (1ul << n) - 1 ? FieldType::value_type::one() : FieldType::value_type::zero()));
        BOOST_CHECK(bp.is_satisfied());

        bp.val(output) = (w == (1ul << n) - 1 ? FieldType::value_type::zero() : FieldType::value_type::one());
        BOOST_CHECK(!bp.is_satisfied());
    }
}

template<typename FieldType>
void test_comparison_component(size_t n) {
    components::blueprint<FieldType> bp;

    components::blueprint_variable<FieldType> A, B, less, less_or_eq;
    A.allocate(bp);
    B.allocate(bp);
    less.allocate(bp);
    less_or_eq.allocate(bp);

    components::comparison<FieldType> cmp(bp, n, A, B, less, less_or_eq);
    cmp.generate_r1cs_constraints();

    for (std::size_t a = 0; a < 1ul << n; ++a) {
        for (std::size_t b = 0; b < 1ul << n; ++b) {
            bp.val(A) = typename FieldType::value_type(a);
            bp.val(B) = typename FieldType::value_type(b);

            cmp.generate_r1cs_witness();

            BOOST_CHECK(bp.val(less) == (a < b ? FieldType::value_type::one() : FieldType::value_type::zero()));
            BOOST_CHECK(bp.val(less_or_eq) == (a <= b ? FieldType::value_type::one() : FieldType::value_type::zero()));
            BOOST_CHECK(bp.is_satisfied());
        }
    }
}

template<typename FieldType>
void test_inner_product_component(size_t n) {
    components::blueprint<FieldType> bp;
    components::blueprint_variable_vector<FieldType> A;
    A.allocate(bp, n);
    components::blueprint_variable_vector<FieldType> B;
    B.allocate(bp, n);

    components::blueprint_variable<FieldType> result;
    result.allocate(bp);

    components::inner_product<FieldType> g(bp, A, B, result);
    g.generate_r1cs_constraints();

    for (std::size_t i = 0; i < 1ul << n; ++i) {
        for (std::size_t j = 0; j < 1ul << n; ++j) {
            std::size_t correct = 0;
            for (std::size_t k = 0; k < n; ++k) {
                bp.val(A[k]) = (i & (1ul << k) ? FieldType::value_type::one() : FieldType::value_type::zero());
                bp.val(B[k]) = (j & (1ul << k) ? FieldType::value_type::one() : FieldType::value_type::zero());
                correct += ((i & (1ul << k)) && (j & (1ul << k)) ? 1 : 0);
            }

            g.generate_r1cs_witness();

            BOOST_CHECK(bp.val(result) == typename FieldType::value_type(correct));
            BOOST_CHECK(bp.is_satisfied());

            bp.val(result) = typename FieldType::value_type(100 * n + 19);
            BOOST_CHECK(!bp.is_satisfied());
        }
    }
}

template<typename FieldType>
void test_loose_multiplexing_component(size_t n) {
    components::blueprint<FieldType> bp;

    components::blueprint_variable_vector<FieldType> arr;
    arr.allocate(bp, 1ul << n);
    components::blueprint_variable<FieldType> index, result, success_flag;
    index.allocate(bp);
    result.allocate(bp);
    success_flag.allocate(bp);

    components::loose_multiplexing<FieldType> g(bp, arr, index, result, success_flag);
    g.generate_r1cs_constraints();

    for (std::size_t i = 0; i < 1ul << n; ++i) {
        bp.val(arr[i]) = typename FieldType::value_type((19 * i) % (1ul << n));
    }

    for (int idx = -1; idx <= (int)(1ul << n); ++idx) {

        bp.val(index) = typename FieldType::value_type(idx);
        g.generate_r1cs_witness();

        if (0 <= idx && idx <= (int)(1ul << n) - 1) {
            BOOST_CHECK(bp.val(result) == typename FieldType::value_type((19 * idx) % (1ul << n)));
            BOOST_CHECK(bp.val(success_flag) == FieldType::value_type::one());
            BOOST_CHECK(bp.is_satisfied());
            bp.val(result) -= FieldType::value_type::one();
            BOOST_CHECK(!bp.is_satisfied());
        } else {
            BOOST_CHECK(bp.val(success_flag) == FieldType::value_type::zero());
            BOOST_CHECK(bp.is_satisfied());
            bp.val(success_flag) = FieldType::value_type::one();
            BOOST_CHECK(!bp.is_satisfied());
        }
    }
}

BOOST_AUTO_TEST_SUITE(basic_components_test_suite)

BOOST_AUTO_TEST_CASE(basic_components_disjunction_test) {
    std::cout << "Disjunction component test started" << std::endl;
    std::cout << "Started for bls12<381>" << std::endl;
    test_disjunction_component<fields::bls12<381>>(10);
    std::cout << "Started for mnt4<298>" << std::endl;
    test_disjunction_component<fields::mnt4<298>>(10);
    std::cout << "Started for mnt6<298>" << std::endl;
    test_disjunction_component<fields::mnt6<298>>(10);
}

BOOST_AUTO_TEST_CASE(basic_components_conjunction_test) {
    std::cout << "Conjunction component test started" << std::endl;
    std::cout << "Started for bls12<381>" << std::endl;
    test_conjunction_component<fields::bls12<381>>(10);
    std::cout << "Started for mnt4<298>" << std::endl;
    test_conjunction_component<fields::mnt4<298>>(10);
    std::cout << "Started for mnt6<298>" << std::endl;
    test_conjunction_component<fields::mnt6<298>>(10);
}

BOOST_AUTO_TEST_CASE(basic_components_comparison_test) {
    std::cout << "Comparison component test started" << std::endl;
    std::cout << "Started for bls12<381>" << std::endl;
    test_comparison_component<fields::bls12<381>>(5);
    std::cout << "Started for mnt4<298>" << std::endl;
    test_comparison_component<fields::mnt4<298>>(5);
    std::cout << "Started for mnt6<298>" << std::endl;
    test_comparison_component<fields::mnt6<298>>(5);
}

BOOST_AUTO_TEST_CASE(basic_components_inner_product_test) {
    std::cout << "Inner product component test started" << std::endl;
    std::cout << "Started for bls12<381>" << std::endl;
    test_inner_product_component<fields::bls12<381>>(5);
    std::cout << "Started for mnt4<298>" << std::endl;
    test_inner_product_component<fields::mnt4<298>>(5);
    std::cout << "Started for mnt6<298>" << std::endl;
    test_inner_product_component<fields::mnt6<298>>(5);
}

BOOST_AUTO_TEST_CASE(basic_components_loose_multiplexing_test) {
    std::cout << "Loose multiplexing component test started" << std::endl;
    std::cout << "Started for bls12<381>" << std::endl;
    test_loose_multiplexing_component<fields::bls12<381>>(5);
    std::cout << "Started for mnt4<298>" << std::endl;
    test_loose_multiplexing_component<fields::mnt4<298>>(5);
    std::cout << "Started for mnt6<298>" << std::endl;
    test_loose_multiplexing_component<fields::mnt6<298>>(5);
}

BOOST_AUTO_TEST_SUITE_END()


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

#include <iostream>

#include <boost/math/distributions.hpp>
#include <boost/math/distributions/non_central_chi_squared.hpp>
#include <ql/math/interpolations/loginterpolation.hpp>
#include <ql/termstructures/credit/interpolatedsurvivalprobabilitycurve.hpp>
#include <ql/time/daycounters/actualactual.hpp>

#include <qle/models/crcirpp.hpp>

namespace QuantExt {

namespace {
Real nccs(const Real df, const Real ncp, const Real x, const bool cumulative) {
    QL_REQUIRE(std::isfinite(df) && df > 0, "CrCirpp::density(): illegal df=" << df);
    // boost
    boost::math::non_central_chi_squared_distribution<> d(df, ncp);
    return cumulative ? cdf(d, x) : pdf(d, x);
    // ql (use density from boost, there is none in ql?)
    // return cumulative ? NonCentralCumulativeChiSquareDistribution(df, ncp)(x) : pdf(d, x);
}
} // namespace

CrCirpp::CrCirpp(
    const boost::shared_ptr<CrCirppParametrization>& parametrization)
    : parametrization_(parametrization) {

    // FIXME hardcoded discretisation schema
    stateProcess_ =
        boost::make_shared<CrCirppStateProcess>(this, CrCirppStateProcess::Discretization::BrigoAlfonsi);
    QL_REQUIRE(stateProcess_ != NULL, "stateProcess has null pointer in CrCirpp ctor!");

    arguments_.resize(4);
    arguments_[0] = parametrization_->parameter(0);
    arguments_[1] = parametrization_->parameter(1);
    arguments_[2] = parametrization_->parameter(2);
    arguments_[3] = parametrization_->parameter(3);
    registerWith(parametrization_->termStructure());
}

Handle<DefaultProbabilityTermStructure> CrCirpp::defaultCurve(std::vector<Date> dateGrid) const {
    if (parametrization_->shifted()) {
        QL_REQUIRE(!parametrization_->termStructure().empty(), "default curve not set");
        QL_REQUIRE(dateGrid.size() == 0, "dateGrid without effect for shifted model");
        return parametrization_->termStructure();
    } else {
        // build one on the fly
        Date today = Settings::instance().evaluationDate();
        std::vector<Real> survivalProbabilities(1, 1.0);
        std::vector<Date> dates;
        DayCounter tsDayCounter = Actual365Fixed();
        if (dateGrid.size() > 0) {
            QL_REQUIRE(dateGrid.front() == today, "front date must be today");
            dates = dateGrid;
        } else {
            dates.push_back(today);
            // up to one year in monthly steps
            for (Size i = 1; i <= 12; i++)
                dates.push_back(today + i * Months);
            // starts in year two annually
            for (Size i = 2; i <= 10; i++)
                dates.push_back(today + i * Years);
        }
        for (Size i = 1; i < dates.size(); i++) {
            Real t = tsDayCounter.yearFraction(today, dates[i]);
            survivalProbabilities.push_back(survivalProbability(0.0, t, parametrization_->y0(t)));
        }
        boost::shared_ptr<DefaultProbabilityTermStructure> defaultCurve(
            new InterpolatedSurvivalProbabilityCurve<LogLinear>(dates, survivalProbabilities, tsDayCounter));
        defaultCurve->enableExtrapolation();
        return Handle<DefaultProbabilityTermStructure>(defaultCurve);
    }
}

Real CrCirpp::A(Real t, Real T) const {
    Real kappa = parametrization_->kappa(t);
    Real theta = parametrization_->theta(t);
    Real sigma = parametrization_->sigma(t);
    Real sigma2 = sigma * sigma;
    Real h = sqrt(kappa * kappa + 2.0 * sigma2);

    Real nominator = 2.0 * h * exp((kappa + h) * (T - t) / 2.0);
    Real denominator = 2.0 * h + (kappa + h) * (exp((T - t) * h) - 1.0);
    Real exponent = 2.0 * kappa * theta / sigma2;

    Real value = std::pow((nominator / denominator), exponent);

    return value;
}

Real CrCirpp::B(Real t, Real T) const {

    Real kappa = parametrization_->kappa(t);
    Real sigma = parametrization_->sigma(t);
    Real sigma2 = sigma * sigma;
    Real h = sqrt(kappa * kappa + 2.0 * sigma2);

    Real nominator = 2.0 * (exp((T - t) * h) - 1.0);
    Real denominator = 2.0 * h + (kappa + h) * (exp((T - t) * h) - 1.0);

    Real value = nominator / denominator;

    return value;
}

Real CrCirpp::zeroBond(Real t, Real T, Real y) const { return A(t, T) * exp(-B(t, T) * y); }

Real CrCirpp::survivalProbability(Real t, Real T, Real y) const {
    Real P_Cir = zeroBond(t, T, y); // A(t,T) * exp(-B(t,T) * y);
    if (parametrization_->shifted()) {
        Probability SP_t = parametrization_->termStructure()->survivalProbability(t);
        // std::cout<<" SP_t  " << SP_t << std::endl;
        Probability SP_T = parametrization_->termStructure()->survivalProbability(T);
        // std::cout<<" SP_T " << SP_T << std::endl;
        // std::cout<<" y0  " << parametrization_->y0(t) << std::endl;
        Real A_bar = (SP_T * A(0, t) * exp(-B(0, t) * parametrization_->y0(t))) /
                     (SP_t * A(0, T) * exp(-B(0, T) * parametrization_->y0(t)));
        // std::cout<<"A_bar " <<A_bar<< std::endl;
        return A_bar * P_Cir;
    } else
        return P_Cir;
}

Real CrCirpp::density(Real x, Real t) {

    Real kappa = parametrization_->kappa(t);
    Real theta = parametrization_->theta(t);
    Real sigma = parametrization_->sigma(t);
    Real y0 = parametrization_->y0(t);
    Real sigma2 = sigma * sigma;

    Real c = 4 * kappa / (sigma2 * (1 - exp(-kappa * t)));
    // degrees of freedom
    Real df = 4 * kappa * theta / (sigma2);
    // non-central parameter
    Real ncp = c * y0 * exp(-kappa * t);

    return c * nccs(df, ncp, c * x, false);
}

Real CrCirpp::cumulative(Real x, Real t) {

    Real kappa = parametrization_->kappa(t);
    Real theta = parametrization_->theta(t);
    Real sigma = parametrization_->sigma(t);
    Real y0 = parametrization_->y0(t);
    Real sigma2 = sigma * sigma;

    Real c = 4 * kappa / (sigma2 * (1 - exp(-kappa * t)));
    // degrees of freedom
    Real df = 4 * kappa * theta / (sigma2);
    // non-central parameter
    Real ncp = c * y0 * exp(-kappa * t);

    return c * nccs(df, ncp, c * x, true);
}

Real CrCirpp::densityForwardMeasure(Real x, Real t) {

    Real kappa = parametrization_->kappa(t);
    Real theta = parametrization_->theta(t);
    Real sigma = parametrization_->sigma(t);
    Real y0 = parametrization_->y0(t);
    Real sigma2 = sigma * sigma;

    Real h = sqrt(kappa * kappa + 2.0 * sigma2);
    Real rho = 2 * h / (sigma2 * (exp(h * t) - 1));
    // q(t,s) in Brigo-Mercuri in (3.28)
    Real c = 2 * (rho + (kappa + h) / sigma2 + 0.0); // 0.0 stands for the B term which is neglectible
    // degrees of freedom
    Real df = 4 * kappa * theta / (sigma2);
    // non-central parameter
    // delta(t,s) in Brigo-Mercurio in (3.28)
    Real ncp = 4 * (rho * rho * y0 * exp(h * t)) / c;

    return c * nccs(df, ncp, c * x, false);
}

// Brigo-Mercurio 2nd Edition, page 67
Real CrCirpp::cumulativeForwardMeasure(Real x, Real t) {

    Real kappa = parametrization_->kappa(t);
    Real theta = parametrization_->theta(t);
    Real sigma = parametrization_->sigma(t);
    Real y0 = parametrization_->y0(t);
    Real sigma2 = sigma * sigma;
    Real h = sqrt(kappa * kappa + 2.0 * sigma2);
    Real rho = 2 * h / (sigma2 * (exp(h * t) - 1));

    // q(t,s) in Brigo-Mercurii (3.28)
    Real c = 2 * (rho + (kappa + h) / sigma2 + 0.0); // 0.0 stands for the B term which is neglectible
    // degrees of freedom
    Real df = 4 * kappa * theta / (sigma2);
    // non-central parameter
    // delta(t,s) in Brigo-Mercurio in (3.28)
    Real ncp = 4 * (rho * rho * y0 * exp(h * t)) / c;

    return c * nccs(df, ncp, c * x, true);
}

// Brigo-Mercurio (3.26) and (3.78)
Real CrCirpp::zeroBondOption(Real eval_t, Real expiry_T, Real maturity_tau, Real strike_k, Real y_t,
                                              Real w) {

    Real value = 0.0;
    Real kappa = parametrization_->kappa(eval_t);
    Real theta = parametrization_->theta(eval_t);
    Real sigma = parametrization_->sigma(eval_t);
    Real y0 = parametrization_->y0(eval_t);
    Real sigma2 = sigma * sigma;

    Real h = sqrt(kappa * kappa + 2.0 * sigma2);
    Real rho = 2.0 * h / (sigma2 * (exp(h * (expiry_T - eval_t)) - 1.0));
    Real Psi = (kappa + h) / sigma2;

    Probability SP_T;
    Probability SP_tau;
    if (parametrization_->shifted()) {
        SP_T = parametrization()->termStructure()->survivalProbability(expiry_T);
        SP_tau = parametrization()->termStructure()->survivalProbability(maturity_tau);
    } else {
        SP_T = survivalProbability(0.0, expiry_T, y0);
        SP_tau = survivalProbability(0.0, maturity_tau, y0);
    }

    Real r_hat = 1.0 / B(expiry_T, maturity_tau) *
                 (log(A(expiry_T, maturity_tau) / strike_k) -
                  log((SP_T * A(0.0, maturity_tau) * exp(-B(0.0, maturity_tau) * y0)) /
                      (SP_tau * A(0.0, expiry_T) * exp(-B(0.0, expiry_T) * y0))));

    Real denom = rho + Psi + B(expiry_T, maturity_tau);
    Real df = 4.0 * kappa * theta / sigma2;
    Real ncp = 2.0 * rho * rho * y_t * exp(h * (expiry_T - eval_t)) / denom;
    QL_REQUIRE(std::isfinite(df) && df > 0, "CrCirpp::zeroBondOption(): illegal df="
                                                << df << ", kappa=" << kappa << ", theta= " << theta
                                                << ", sigma=" << sigma);

    value += nccs(df, ncp, 2 * r_hat * denom, true) * SP_tau;

    denom = rho + Psi;
    // df unchanged
    ncp = 2 * rho * rho * y_t * exp(h * (expiry_T - eval_t)) / denom;

    value -= nccs(df, ncp, 2 * r_hat * denom, true) * SP_T * strike_k;

    if (w < 0.0) {
        // PUT via put-call-paritiy
        // C - P = S_M(T) - S_M(t)*K
        // P = C - S_M(T) + S_M(t)*K
        value -= SP_tau - SP_T * strike_k;
    }

    return value;
}

} // namespace QuantExt


// SPDX-FileCopyrightText: 2015 - 2021 Marcin Łoś <marcin.los.91@gmail.com>
// SPDX-License-Identifier: MIT

#ifndef ADS_SIMULATION_SIMULATION_2D_HPP
#define ADS_SIMULATION_SIMULATION_2D_HPP

#include <array>
#include <cstddef>

#include <boost/range/counting_range.hpp>

#include "ads/lin/tensor.hpp"
#include "ads/projection.hpp"
#include "ads/simulation/basic_simulation_2d.hpp"
#include "ads/simulation/dimension.hpp"
#include "ads/simulation/simulation_base.hpp"
#include "ads/solver.hpp"
#include "ads/util/function_value.hpp"
#include "ads/util/iter/product.hpp"

namespace ads {

class simulation_2d : public basic_simulation_2d, public simulation_base {
protected:
    using basic_simulation_2d::dof_global_to_local;
    using basic_simulation_2d::dofs;
    using basic_simulation_2d::dofs_on_element;
    using basic_simulation_2d::elements;
    using basic_simulation_2d::elements_supporting_dof;
    using basic_simulation_2d::eval;
    using basic_simulation_2d::eval_basis;
    using basic_simulation_2d::jacobian;
    using basic_simulation_2d::point;
    using basic_simulation_2d::quad_points;
    using basic_simulation_2d::update_global_rhs;
    using basic_simulation_2d::weight;

    dimension x, y;
    vector_type buffer;

    void solve(vector_type& rhs) { ads_solve(rhs, buffer, x.data(), y.data()); }

    template <typename Function>
    void projection(vector_type& v, Function f) {
        compute_projection(v, x.basis, y.basis, f);
    }

    double grad_dot(value_type a, value_type b) const { return a.dx * b.dx + a.dy * b.dy; }

    std::array<int, 2> shape() const { return {x.dofs(), y.dofs()}; }

    std::array<int, 2> local_shape() const {
        return {x.basis.dofs_per_element(), y.basis.dofs_per_element()};
    }

    void prepare_matrices() {
        x.factorize_matrix();
        y.factorize_matrix();
    }

    index_range elements() const {
        return util::product_range<index_type>(x.element_indices(), y.element_indices());
    }

    index_range dofs() const {
        auto rx = boost::counting_range(0, x.basis.dofs);
        auto ry = boost::counting_range(0, y.basis.dofs);
        return util::product_range<index_type>(rx, ry);
    }

    index_range quad_points() const {
        auto rx = boost::counting_range(0, x.basis.quad_order);
        auto ry = boost::counting_range(0, y.basis.quad_order);
        return util::product_range<index_type>(rx, ry);
    }

    index_range dofs_on_element(index_type e) const {
        auto rx = x.basis.dof_range(e[0]);
        auto ry = y.basis.dof_range(e[1]);
        return util::product_range<index_type>(rx, ry);
    }

    index_range elements_supporting_dof(index_type dof) const {
        auto rx = x.basis.element_range(dof[0]);
        auto ry = y.basis.element_range(dof[1]);
        return util::product_range<index_type>(rx, ry);
    }

    double jacobian(index_type e) const { return x.basis.J[e[0]] * y.basis.J[e[1]]; }

    double weight(index_type q) const { return x.basis.w[q[0]] * y.basis.w[q[1]]; }

    point_type point(index_type e, index_type q) const {
        double px = x.basis.x[e[0]][q[0]];
        double py = y.basis.x[e[1]][q[1]];
        return {px, py};
    }

    value_type eval_basis(index_type e, index_type q, index_type a) const {
        auto loc = dof_global_to_local(e, a);

        const auto& bx = x.basis;
        const auto& by = y.basis;

        double B1 = bx.b[e[0]][q[0]][0][loc[0]];
        double B2 = by.b[e[1]][q[1]][0][loc[1]];
        double dB1 = bx.b[e[0]][q[0]][1][loc[0]];
        double dB2 = by.b[e[1]][q[1]][1][loc[1]];

        double v = B1 * B2;
        double dxv = dB1 * B2;
        double dyv = B1 * dB2;

        return {v, dxv, dyv};
    }

    value_type eval_fun(const vector_type& v, index_type e, index_type q) const {
        value_type u{};
        for (auto b : dofs_on_element(e)) {
            double c = v(b[0], b[1]);
            value_type B = eval_basis(e, q, b);
            u += c * B;
        }
        return u;
    }

    index_type dof_global_to_local(index_type e, index_type a) const {
        const auto& bx = x.basis;
        const auto& by = y.basis;
        return {{a[0] - bx.first_dof(e[0]), a[1] - by.first_dof(e[1])}};
    }

    vector_type element_rhs() const { return vector_type{local_shape()}; }

    void update_global_rhs(vector_type& global, const vector_type& local, index_type e) const {
        for (auto a : dofs_on_element(e)) {
            auto loc = dof_global_to_local(e, a);
            global(a[0], a[1]) += local(loc[0], loc[1]);
        }
    }

public:
    explicit simulation_2d(const config_2d& config);

    simulation_2d(const dimension& x, const dimension& y, const timesteps_config& steps);
};

}  // namespace ads

#endif  // ADS_SIMULATION_SIMULATION_2D_HPP


#ifndef SH_WRAP_HPP
#define SH_WRAP_HPP

#include <string>
#include <vector>
#include <iostream>
//#include <numpy/arrayobject.h>
#include "sh/spherical_harmonics.h"
#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <memory>
#include <algorithm>

#include <omp.h>

class SH_Wrapper {
public:
  SH_Wrapper() : SH_Wrapper(0) {};

  SH_Wrapper(int _verbose);

  int run(
    int K,
    std::vector< double > vecs,
    std::vector< double > scalars,
    std::vector< int > vecs_shape,
    std::vector< int > scalars_shape,
    std::vector< double >& output,
    bool only_even=false
  );

  int run_all_same_vecs(
    int K,
    std::vector< double > vecs,
    std::vector< double > scalars,
    std::vector< int > vecs_shape,
    std::vector< int > scalars_shape,
    std::vector< double >& output,
    bool only_even=false
  );

  int run_even(
    int K,
    std::vector< double > vecs,
    std::vector< double > scalars,
    std::vector< int > vecs_shape,
    std::vector< int > scalars_shape,
    std::vector< double >& output,
    bool all_same_vecs=false
  );

  int project_from_SH(
    int K,
    std::vector<double> sh_coefs,
    std::vector<double> vecs,
    std::vector<int> vecs_shape,
    std::vector<double>& output
  );

  int project_from_even_SH(
    int K,
    std::vector<double> sh_coefs,
    std::vector<double> vecs,
    std::vector<int> vecs_shape,
    std::vector<double>& output
  );

  int projection_matrix(
    int K,
    std::vector<double> vecs,
    std::vector<int> vecs_shape,
    std::vector<double>& output,
    std::vector<int>& out_shape
  );

  int projection_matrix_even(
    int K,
    std::vector<double> vecs,
    std::vector<int> vecs_shape,
    std::vector<double>& output,
    std::vector<int>& out_shape
  );

  int check_num_coef(
    int K
  );

  int check_num_even_coef(
    int K
  );

private:

  int n_vox;
  int n_points;

  int verbose;

};

#endif


#ifndef CUDA_HOG_DETECTOR_H
#define CUDA_HOG_DETECTOR_H
#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>
#include <opencv2/cudaobjdetect.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <random>
#include <chrono>
#include <fstream>
#include "../likelihood/logistic_regression.hpp"

using namespace cv;
using namespace std;
using namespace Eigen;

struct Args {
	bool make_gray = true;
    bool resize_src = true;
    int width, height;
    double scale;
    int nlevels;
    int gr_threshold;
    double hit_threshold;
    bool hit_threshold_auto;
    int win_width;
    int win_stride_width, win_stride_height;
    int block_width;
    int block_stride_width, block_stride_height;
    int cell_width;
    int nbins;
    bool gamma_corr;
    double overlap_threshold;
    double lambda, epsilon, tolerance;
    int n_iterations;
} ;

class CUDA_HOGDetector
{
public:
	CUDA_HOGDetector();
	CUDA_HOGDetector(int group_threshold, double hit_threshold);
	CUDA_HOGDetector(int group_threshold, double hit_threshold, Rect reference_roi);
	vector<Rect> detect(Mat &frame);
	void train(Mat &frame,Rect reference_roi);
	void draw();
	MatrixXd getFeatureValues(Mat &frame);
	MatrixXd getFeatureValues();
	VectorXd getDetectionWeights();
	void loadPreTrainedModel();
	Args args;
	MatrixXd compute(Mat &frame);
private:
	MatrixXd feature_values;
	int group_threshold;
	double hit_threshold;
	Ptr<cuda::HOG> gpu_hog;
	vector<Rect> detections;
	VectorXd detection_weights;
	VectorXd labels;
	VectorXd weights;
	Mat frame;
	LogisticRegression logistic_regression;
	mt19937 generator;
};

#endif