//
// Created by mqye on 2020/11/4.
//
#include <limits>
#include <iostream>
#include <boost/format.hpp>
#include "utils.h"

bool hasNaN(std::vector<float> &vec){
  for (int i = 0; i < vec.size(); ++i) {
    if(vec[i] != vec[i]){
      return true;
    }
  }
  return false;
}

bool hasInf(std::vector<float> &vec){
  for (int i = 0; i < vec.size(); ++i) {
    if (vec[i] == std::numeric_limits<float>::infinity() |
    vec[i] == -std::numeric_limits<float>::infinity()){
      return true;
    }
  }
  return false;
}

int main() {
  float max = std::numeric_limits<float>::max();
  float min = std::numeric_limits<float>::min();
  float quiet_nan = std::numeric_limits<float>::quiet_NaN();
  float signaling_nan = std::numeric_limits<float>::signaling_NaN();
  std::cout << boost::format("max : %f") % max << ' ' << max << std::endl
            << boost::format("min : %f") % min << ' ' << min << std::endl
            << boost::format("quiet_nan : %f") % quiet_nan << ' ' << quiet_nan << std::endl
            << boost::format("signaling_nan : %f") % signaling_nan << ' ' << signaling_nan << std::endl
            << std::endl;

  std::vector<float> vec = {
    .1, .2, .3, quiet_nan, .5, signaling_nan, .6
  };

  std::cout << vec << std::endl << hasNaN(vec) << std::endl << hasInf(vec);

  return 0;
}


// An interval object.
#pragma once

#include <Eigen/Core>
#include <tight_inclusion/Types.hpp>
#ifdef TIGHT_INCLUSION_USE_GMP
#include <tight_inclusion/Rational.hpp>
#endif

namespace inclusion_ccd
{

    // typedef std::array<std::pair<int,int>,3> Paraccd;//<k,n> pair present parameters (u,v,t) which are presented as k/(2^n)
    typedef std::pair<long, int>
        Numccd; //<k,n> pair present a number k/pow(2,n)
    typedef std::pair<Numccd, Numccd>
        Singleinterval; // a interval presented by two double numbers
    typedef std::array<Singleinterval, 3> Interval3; // 3 dimesional interval
#ifdef TIGHT_INCLUSION_USE_GMP
    typedef Eigen::Matrix<Rational, 3, 1, Eigen::ColMajor | Eigen::DontAlign>
        Vector3r;
#endif

} // namespace inclusion_ccd


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

#ifndef BOOST_HANA_TEST_LAWS_MONAD_PLUS_HPP
#define BOOST_HANA_TEST_LAWS_MONAD_PLUS_HPP

#include <boost/hana/assert.hpp>
#include <boost/hana/comparable.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/core/models.hpp>
#include <boost/hana/core/operators.hpp>
#include <boost/hana/core/when.hpp>
#include <boost/hana/monad_plus.hpp>

#include <laws/base.hpp>

namespace boost { namespace hana { namespace test {
    template <typename M, typename = when<true>>
    struct TestMonadPlus : TestMonadPlus<M, laws> {
        using TestMonadPlus<M, laws>::TestMonadPlus;
    };

    template <typename M>
    struct TestMonadPlus<M, laws> {
        static_assert(_models<MonadPlus, M>{}, "");

        template <typename Xs>
        TestMonadPlus(Xs xs) {
            auto f = hana::compose(lift<M>, test::_injection<0>{});

            foreach3(xs, [=](auto a, auto b, auto c) {

                // left identity
                BOOST_HANA_CHECK(hana::equal(
                    hana::concat(empty<M>(), a),
                    a
                ));

                // right identity
                BOOST_HANA_CHECK(hana::equal(
                    hana::concat(a, empty<M>()),
                    a
                ));

                // associativity
                BOOST_HANA_CHECK(hana::equal(
                    hana::concat(a, hana::concat(b, c)),
                    hana::concat(hana::concat(a, b), c)
                ));

                // absorption
                BOOST_HANA_CHECK(hana::equal(
                    hana::bind(empty<M>(), f),
                    empty<M>()
                ));

                BOOST_HANA_CHECK(hana::equal(
                    hana::bind(a, hana::always(empty<M>())),
                    empty<M>()
                ));

            });
        }
    };

    template <typename S>
    struct TestMonadPlus<S, when<_models<Sequence, S>{}>> : TestMonadPlus<S, laws> {
        template <int i>
        using eq = _constant<i>;

        template <typename Xs>
        TestMonadPlus(Xs xs) : TestMonadPlus<S, laws>{xs} {
            auto z = eq<999>{};
            constexpr auto list = make<S>;

            //////////////////////////////////////////////////////////////////
            // empty
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(equal(
                empty<S>(), list()
            ));

            //////////////////////////////////////////////////////////////////
            // concat
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(equal(
                concat(list(), list()),
                list()
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                concat(list(), list(eq<0>{})),
                list(eq<0>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                concat(list(), list(eq<0>{}, eq<1>{})),
                list(eq<0>{}, eq<1>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                concat(list(eq<0>{}), list()),
                list(eq<0>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                concat(list(eq<0>{}), list(eq<1>{})),
                list(eq<0>{}, eq<1>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                concat(list(eq<0>{}), list(eq<1>{}, eq<2>{})),
                list(eq<0>{}, eq<1>{}, eq<2>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                concat(list(eq<0>{}, eq<1>{}), list()),
                list(eq<0>{}, eq<1>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                concat(list(eq<0>{}, eq<1>{}), list(eq<2>{})),
                list(eq<0>{}, eq<1>{}, eq<2>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                concat(list(eq<0>{}, eq<1>{}), list(eq<2>{}, eq<3>{})),
                list(eq<0>{}, eq<1>{}, eq<2>{}, eq<3>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                concat(list(), list()),
                list()
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                concat(list(1), list()),
                list(1)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                concat(list(), list(1)),
                list(1)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                concat(list(1), list('2')),
                list(1, '2')
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                concat(list(1, '2'), list(3.3)),
                list(1, '2', 3.3)
            ));

            //////////////////////////////////////////////////////////////////
            // filter
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(equal(
                filter(list(), not_equal.to(z)),
                list()
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                filter(list(z), not_equal.to(z)),
                list()
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                filter(list(eq<1>{}), not_equal.to(z)),
                list(eq<1>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                filter(list(eq<1>{}, eq<2>{}), not_equal.to(z)),
                list(eq<1>{}, eq<2>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                filter(list(z, eq<2>{}), not_equal.to(z)),
                list(eq<2>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                filter(list(eq<1>{}, z), not_equal.to(z)),
                list(eq<1>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                filter(list(z, eq<2>{}, eq<3>{}), not_equal.to(z)),
                list(eq<2>{}, eq<3>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                filter(list(eq<1>{}, z, eq<3>{}), not_equal.to(z)),
                list(eq<1>{}, eq<3>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                filter(list(eq<1>{}, eq<2>{}, z), not_equal.to(z)),
                list(eq<1>{}, eq<2>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                filter(list(eq<1>{}, z, z), not_equal.to(z)),
                list(eq<1>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                filter(list(z, eq<2>{}, z), not_equal.to(z)),
                list(eq<2>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                filter(list(z, z, eq<3>{}), not_equal.to(z)),
                list(eq<3>{})
            ));

            //////////////////////////////////////////////////////////////////
            // prepend
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(equal(
                prepend(eq<0>{}, list()),
                list(eq<0>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                prepend(eq<0>{}, list(eq<1>{})),
                list(eq<0>{}, eq<1>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                prepend(eq<0>{}, list(eq<1>{}, eq<2>{})),
                list(eq<0>{}, eq<1>{}, eq<2>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                prepend(eq<0>{}, list(eq<1>{}, eq<2>{}, eq<3>{})),
                list(eq<0>{}, eq<1>{}, eq<2>{}, eq<3>{})
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                prepend(1, list()), list(1)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                prepend(1, list('2')), list(1, '2')
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                prepend(1, list('2', 3.3)), list(1, '2', 3.3)
            ));

            //////////////////////////////////////////////////////////////////
            // append
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(equal(
                append(list(), eq<0>{}),
                list(eq<0>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                append(list(eq<0>{}), eq<1>{}),
                list(eq<0>{}, eq<1>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                append(list(eq<0>{}, eq<1>{}), eq<2>{}),
                list(eq<0>{}, eq<1>{}, eq<2>{})
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                append(list(), 1), list(1)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                append(list(1), '2'), list(1, '2')
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                append(list(1, '2'), 3.3), list(1, '2', 3.3)
            ));

            //////////////////////////////////////////////////////////////////
            // cycle
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<0>, list()),
                list()
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<1>, list()),
                list()
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<2>, list()),
                list()
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<3>, list()),
                list()
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<0>, list(eq<0>{})),
                list()
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<1>, list(eq<0>{})),
                list(eq<0>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<2>, list(eq<0>{})),
                list(eq<0>{}, eq<0>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<3>, list(eq<0>{})),
                list(eq<0>{}, eq<0>{}, eq<0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<0>, list(eq<0>{}, eq<1>{})),
                list()
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<1>, list(eq<0>{}, eq<1>{})),
                list(eq<0>{}, eq<1>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<2>, list(eq<0>{}, eq<1>{})),
                list(eq<0>{}, eq<1>{}, eq<0>{}, eq<1>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<3>, list(eq<0>{}, eq<1>{})),
                list(eq<0>{}, eq<1>{}, eq<0>{}, eq<1>{}, eq<0>{}, eq<1>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<0>, list(eq<0>{}, eq<1>{}, eq<2>{})),
                list()
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<1>, list(eq<0>{}, eq<1>{}, eq<2>{})),
                list(eq<0>{}, eq<1>{}, eq<2>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<2>, list(eq<0>{}, eq<1>{}, eq<2>{})),
                list(eq<0>{}, eq<1>{}, eq<2>{}, eq<0>{}, eq<1>{}, eq<2>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                cycle(size_t<3>, list(eq<0>{}, eq<1>{}, eq<2>{})),
                list(eq<0>{}, eq<1>{}, eq<2>{}, eq<0>{}, eq<1>{}, eq<2>{}, eq<0>{}, eq<1>{}, eq<2>{})
            ));

            //////////////////////////////////////////////////////////////////
            // repeat
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(equal(
                repeat<S>(size_t<0>, eq<0>{}),
                list()
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                repeat<S>(size_t<1>, eq<0>{}),
                list(eq<0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                repeat<S>(size_t<2>, eq<0>{}),
                list(eq<0>{}, eq<0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                repeat<S>(size_t<3>, eq<0>{}),
                list(eq<0>{}, eq<0>{}, eq<0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                repeat<S>(size_t<4>, eq<0>{}),
                list(eq<0>{}, eq<0>{}, eq<0>{}, eq<0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                repeat<S>(size_t<5>, eq<0>{}),
                list(eq<0>{}, eq<0>{}, eq<0>{}, eq<0>{}, eq<0>{})
            ));

            //////////////////////////////////////////////////////////////////
            // prefix
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(equal(
                prefix(z, list()),
                list()
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                prefix(z, list(eq<0>{})),
                list(z, eq<0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                prefix(z, list(eq<0>{}, eq<1>{})),
                list(z, eq<0>{}, z, eq<1>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                prefix(z, list(eq<0>{}, eq<1>{}, eq<2>{})),
                list(z, eq<0>{}, z, eq<1>{}, z, eq<2>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                prefix(z, list(eq<0>{}, eq<1>{}, eq<2>{}, eq<3>{})),
                list(z, eq<0>{}, z, eq<1>{}, z, eq<2>{}, z, eq<3>{})
            ));

            //////////////////////////////////////////////////////////////////
            // suffix
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(equal(
                suffix(z, list()),
                list()
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                suffix(z, list(eq<0>{})),
                list(eq<0>{}, z)
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                suffix(z, list(eq<0>{}, eq<1>{})),
                list(eq<0>{}, z, eq<1>{}, z)
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                suffix(z, list(eq<0>{}, eq<1>{}, eq<2>{})),
                list(eq<0>{}, z, eq<1>{}, z, eq<2>{}, z)
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                suffix(z, list(eq<0>{}, eq<1>{}, eq<2>{}, eq<3>{})),
                list(eq<0>{}, z, eq<1>{}, z, eq<2>{}, z, eq<3>{}, z)
            ));
        }
    };
}}} // end namespace boost::hana::test

#endif // !BOOST_HANA_TEST_LAWS_MONAD_PLUS_HPP


// Copyright (C) 2019 T. Zachary Laine
//
// 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/stl_interfaces/iterator_interface.hpp>

#include <algorithm>
#include <array>
#include <vector>

#include <cassert>

//[ filtered_int_iterator_defn
template<typename Pred>
struct filtered_int_iterator : boost::stl_interfaces::iterator_interface<
                                   filtered_int_iterator<Pred>,
                                   std::bidirectional_iterator_tag,
                                   int>
{
    filtered_int_iterator() : it_(nullptr) {}
    filtered_int_iterator(int * it, int * last, Pred pred) :
        it_(it),
        last_(last),
        pred_(std::move(pred))
    {
        // We need to do this in the constructor so that operator== works
        // properly on two filtered_int_iterators, when they bound a sequence
        // in which none of the ints meets the predicate.
        it_ = std::find_if(it_, last_, pred_);
    }

    // A bidirectional iterator based on iterator_interface usually required
    // four user-defined operations.  since we are adapting an existing
    // iterator (an int *), we only need to define this one.  The others are
    // implemented by iterator_interface, using the underlying int *.
    filtered_int_iterator & operator++()
    {
        it_ = std::find_if(std::next(it_), last_, pred_);
        return *this;
    }

    // It is really common for iterator adaptors to have a base() member
    // function that returns the adapted iterator.
    int * base() const { return it_; }

private:
    // Provide access to these private members.
    friend boost::stl_interfaces::access;

    // These functions are picked up by iterator_interface, and used to
    // implement any operations that you don't define above.  They're not
    // called base() so that they do not collide with the base() member above.
    //
    // Note that the const overload does not strictly speaking need to be a
    // reference, as demonstrated here.
    constexpr int *& base_reference() noexcept { return it_; }
    constexpr int * base_reference() const noexcept { return it_; }

    int * it_;
    int * last_;
    Pred pred_;
};

// A make-function makes it easier to deal with the Pred parameter.
template<typename Pred>
auto make_filtered_int_iterator(int * it, int * last, Pred pred)
{
    return filtered_int_iterator<Pred>(it, last, std::move(pred));
}
//]

int main()
{
    //[ filtered_int_iterator_usage
    std::array<int, 8> ints = {{0, 1, 2, 3, 4, 5, 6, 7}};
    int * const ints_first = ints.data();
    int * const ints_last = ints.data() + ints.size();

    auto even = [](int x) { return (x % 2) == 0; };
    auto first = make_filtered_int_iterator(ints_first, ints_last, even);
    auto last = make_filtered_int_iterator(ints_last, ints_last, even);

    // This is an example only.  Obviously, we could have called
    // std::copy_if() here.
    std::vector<int> ints_copy;
    std::copy(first, last, std::back_inserter(ints_copy));
    assert(ints_copy == (std::vector<int>{0, 2, 4, 6}));
    //]
}


#include "lwtnn/Stack.hh"
#include <Eigen/Dense>

#include <set>

// internal utility functions
namespace {
  using namespace Eigen;
  using namespace lwt;
}
namespace lwt {

  // ______________________________________________________________________
  // Feed forward Stack class

  // dummy construction routine
  Stack::Stack() {
    m_layers.push_back(new DummyLayer);
    m_layers.push_back(new UnaryActivationLayer({Activation::SIGMOID}));
    m_layers.push_back(new BiasLayer(std::vector<double>{1, 1, 1, 1}));
    MatrixXd mat(4, 4);
    mat <<
      0, 0, 0, 1,
      0, 0, 1, 0,
      0, 1, 0, 0,
      1, 0, 0, 0;
    m_layers.push_back(new MatrixLayer(mat));
    m_n_outputs = 4;
  }

  // construct from LayerConfig
  Stack::Stack(size_t n_inputs, const std::vector<LayerConfig>& layers,
               size_t skip) {
    for (size_t nnn = skip; nnn < layers.size(); nnn++) {
      n_inputs = add_layers(n_inputs, layers.at(nnn));
    }
    // the final assigned n_inputs is the number of output nodes
    m_n_outputs = n_inputs;
  }

  Stack::~Stack() {
    for (auto& layer: m_layers) {
      delete layer;
      layer = 0;
    }
  }
  VectorXd Stack::compute(VectorXd in) const {
    for (const auto& layer: m_layers) {
      in = layer->compute(in);
    }
    return in;
  }
  size_t Stack::n_outputs() const {
    return m_n_outputs;
  }

  // Private Stack methods to add various types of layers
  //
  // top level add_layers method. This delegates to the other methods
  // below
  size_t Stack::add_layers(size_t n_inputs, const LayerConfig& layer) {
    if (layer.architecture == Architecture::DENSE) {
      return add_dense_layers(n_inputs, layer);
    } else if (layer.architecture == Architecture::NORMALIZATION){
      return add_normalization_layers(n_inputs, layer);
    } else if (layer.architecture == Architecture::HIGHWAY){
      return add_highway_layers(n_inputs, layer);
    } else if (layer.architecture == Architecture::MAXOUT) {
      return add_maxout_layers(n_inputs, layer);
    }
    throw NNConfigurationException("unknown architecture");
  }

  size_t Stack::add_dense_layers(size_t n_inputs, const LayerConfig& layer) {
    assert(layer.architecture == Architecture::DENSE);
    throw_if_not_dense(layer);

    size_t n_outputs = n_inputs;

    // add matrix layer
    if (layer.weights.size() > 0) {
      MatrixXd matrix = build_matrix(layer.weights, n_inputs);
      n_outputs = matrix.rows();
      m_layers.push_back(new MatrixLayer(matrix));
    };

    // add bias layer
    if (layer.bias.size() > 0) {
      if (n_outputs != layer.bias.size() ) {
        std::string problem = "tried to add a bias layer with " +
          std::to_string(layer.bias.size()) + " entries, previous layer"
          " had " + std::to_string(n_outputs) + " outputs";
        throw NNConfigurationException(problem);
      }
      m_layers.push_back(new BiasLayer(layer.bias));
    }

    // add activation layer
    if (layer.activation.function != Activation::LINEAR) {
      m_layers.push_back(get_raw_activation_layer(layer.activation));
    }

    return n_outputs;
  }

  size_t Stack::add_normalization_layers(size_t n_inputs, const LayerConfig& layer) {
    assert(layer.architecture == Architecture::NORMALIZATION);
    throw_if_not_normalization(layer);

    // Do some checks
    if ( layer.weights.size() < 1 || layer.bias.size() < 1 ) {
      std::string problem = "Either weights or bias layer size is < 1";
      throw NNConfigurationException(problem);
    };
    if ( layer.weights.size() != layer.bias.size() ) {
      std::string problem = "weights and bias layer are not equal in size!";
      throw NNConfigurationException(problem);
    };
    VectorXd v_weights = build_vector(layer.weights);
    VectorXd v_bias = build_vector(layer.bias);

    m_layers.push_back(
      new NormalizationLayer(v_weights, v_bias));
    return n_inputs;
  }

  size_t Stack::add_highway_layers(size_t n_inputs, const LayerConfig& layer) {
    auto& comps = layer.components;
    const auto& t = get_component(comps.at(Component::T), n_inputs);
    const auto& c = get_component(comps.at(Component::CARRY), n_inputs);

    m_layers.push_back(
      new HighwayLayer(t.W, t.b, c.W, c.b, layer.activation));
    return n_inputs;
  }

  size_t Stack::add_maxout_layers(size_t n_inputs, const LayerConfig& layer) {
    assert(layer.architecture == Architecture::MAXOUT);
    throw_if_not_maxout(layer);
    std::vector<MaxoutLayer::InitUnit> matrices;
    std::set<size_t> n_outputs;
    for (const auto& sublayer: layer.sublayers) {
      MatrixXd matrix = build_matrix(sublayer.weights, n_inputs);
      VectorXd bias = build_vector(sublayer.bias);
      n_outputs.insert(matrix.rows());
      matrices.push_back(std::make_pair(matrix, bias));
    }
    if (n_outputs.size() == 0) {
      throw NNConfigurationException("tried to build maxout withoutweights!");
    }
    else if (n_outputs.size() != 1) {
      throw NNConfigurationException("uneven matrices for maxout");
    }
    m_layers.push_back(new MaxoutLayer(matrices));
    return *n_outputs.begin();
  }

  // _______________________________________________________________________
  // Feed-forward layers

  VectorXd DummyLayer::compute(const VectorXd& in) const {
    return in;
  }

  // activation functions
  UnaryActivationLayer::UnaryActivationLayer(ActivationConfig act):
    m_func(get_activation(act))
  {
  }
  VectorXd UnaryActivationLayer::compute(const VectorXd& in) const {
    return in.unaryExpr(m_func);
  }

  VectorXd SoftmaxLayer::compute(const VectorXd& in) const {
    // More numerically stable softmax, as suggested in
    // http://stackoverflow.com/a/34969389
    size_t n_elements = in.rows();
    VectorXd exp(n_elements);
    double max = in.maxCoeff();
    for (size_t iii = 0; iii < n_elements; iii++) {
      exp(iii) = std::exp(in(iii) - max);
    }
    double sum_exp = exp.sum();
    return exp / sum_exp;
  }

  // bias layer
  BiasLayer::BiasLayer(const VectorXd& bias): m_bias(bias)
  {
  }
  BiasLayer::BiasLayer(const std::vector<double>& bias):
    m_bias(build_vector(bias))
  {
  }
  VectorXd BiasLayer::compute(const VectorXd& in) const {
    return in + m_bias;
  }

  // basic dense matrix layer
  MatrixLayer::MatrixLayer(const MatrixXd& matrix):
    m_matrix(matrix)
  {
  }
  VectorXd MatrixLayer::compute(const VectorXd& in) const {
    return m_matrix * in;
  }

  // maxout layer
  MaxoutLayer::MaxoutLayer(const std::vector<MaxoutLayer::InitUnit>& units):
    m_bias(units.size(), units.front().first.rows())
  {
    int out_pos = 0;
    for (const auto& unit: units) {
      m_matrices.push_back(unit.first);
      m_bias.row(out_pos) = unit.second;
      out_pos++;
    }
  }
  VectorXd MaxoutLayer::compute(const VectorXd& in) const {
    // eigen supports tensors, but only in the experimental component
    // for now just stick to matrix and vector classes
    const size_t n_mat = m_matrices.size();
    const size_t out_dim = m_matrices.front().rows();
    MatrixXd outputs(n_mat, out_dim);
    for (size_t mat_n = 0; mat_n < n_mat; mat_n++) {
      outputs.row(mat_n) = m_matrices.at(mat_n) * in;
    }
    outputs += m_bias;
    return outputs.colwise().maxCoeff();
  }

   // Normalization layer
   NormalizationLayer::NormalizationLayer(const VectorXd& W,
                                          const VectorXd& b):
    _W(W), _b(b)
  {
  }
  VectorXd NormalizationLayer::compute(const VectorXd& in) const {
    VectorXd shift = in + _b ;
    return _W.cwiseProduct(shift);
  }

  // highway layer
  HighwayLayer::HighwayLayer(const MatrixXd& W,
                             const VectorXd& b,
                             const MatrixXd& W_carry,
                             const VectorXd& b_carry,
                             ActivationConfig activation):
    m_w_t(W), m_b_t(b), m_w_c(W_carry), m_b_c(b_carry),
    m_act(get_activation(activation))
  {
  }
  VectorXd HighwayLayer::compute(const VectorXd& in) const {
    const std::function<double(double)> sig(nn_sigmoid);
    ArrayXd c = (m_w_c * in + m_b_c).unaryExpr(sig);
    ArrayXd t = (m_w_t * in + m_b_t).unaryExpr(m_act);
    return c * t + (1 - c) * in.array();
  }

  // ______________________________________________________________________
  // Recurrent Stack

  RecurrentStack::RecurrentStack(size_t n_inputs,
                                 const std::vector<lwt::LayerConfig>& layers)
  {
    using namespace lwt;
    const size_t n_layers = layers.size();
    for (size_t layer_n = 0; layer_n < n_layers; layer_n++) {
      auto& layer = layers.at(layer_n);

      // add recurrent layers (now LSTM and GRU!)
      if (layer.architecture == Architecture::LSTM) {
        n_inputs = add_lstm_layers(n_inputs, layer);
      } else if (layer.architecture == Architecture::GRU) {
        n_inputs = add_gru_layers(n_inputs, layer);
      } else if (layer.architecture == Architecture::EMBEDDING) {
        n_inputs = add_embedding_layers(n_inputs, layer);
      } else {
        throw NNConfigurationException("found non-recurrent layer");
      }
    }
    m_n_outputs = n_inputs;
  }
  RecurrentStack::~RecurrentStack() {
    for (auto& layer: m_layers) {
      delete layer;
      layer = 0;
    }
  }
  MatrixXd RecurrentStack::scan(MatrixXd in) const {
    for (auto* layer: m_layers) {
      in = layer->scan(in);
    }
    return in;
  }
  size_t RecurrentStack::n_outputs() const {
    return m_n_outputs;
  }

  size_t RecurrentStack::add_lstm_layers(size_t n_inputs,
                                         const LayerConfig& layer) {
    auto& comps = layer.components;
    const auto& i = get_component(comps.at(Component::I), n_inputs);
    const auto& o = get_component(comps.at(Component::O), n_inputs);
    const auto& f = get_component(comps.at(Component::F), n_inputs);
    const auto& c = get_component(comps.at(Component::C), n_inputs);
    m_layers.push_back(
      new LSTMLayer(layer.activation, layer.inner_activation,
                    i.W, i.U, i.b,
                    f.W, f.U, f.b,
                    o.W, o.U, o.b,
                    c.W, c.U, c.b));
    return o.b.rows();
  }

  size_t RecurrentStack::add_gru_layers(size_t n_inputs,
                                         const LayerConfig& layer) {
    auto& comps = layer.components;
    const auto& z = get_component(comps.at(Component::Z), n_inputs);
    const auto& r = get_component(comps.at(Component::R), n_inputs);
    const auto& h = get_component(comps.at(Component::H), n_inputs);
    m_layers.push_back(
      new GRULayer(layer.activation, layer.inner_activation,
                    z.W, z.U, z.b,
                    r.W, r.U, r.b,
                    h.W, h.U, h.b));
    return h.b.rows();
  }

  size_t RecurrentStack::add_embedding_layers(size_t n_inputs,
                                              const LayerConfig& layer) {
    for (const auto& emb: layer.embedding) {
      size_t n_wt = emb.weights.size();
      size_t n_cats = n_wt / emb.n_out;
      MatrixXd mat = build_matrix(emb.weights, n_cats);
      m_layers.push_back(new EmbeddingLayer(emb.index, mat));
      n_inputs += emb.n_out - 1;
    }
    return n_inputs;
  }

  ReductionStack::ReductionStack(size_t n_in,
                                 const std::vector<LayerConfig>& layers) {
    std::vector<LayerConfig> recurrent;
    std::vector<LayerConfig> feed_forward;
    std::set<Architecture> recurrent_arcs{
      Architecture::LSTM, Architecture::GRU, Architecture::EMBEDDING};
    for (const auto& layer: layers) {
      if (recurrent_arcs.count(layer.architecture)) {
        recurrent.push_back(layer);
      } else {
        feed_forward.push_back(layer);
      }
    }
    m_recurrent = new RecurrentStack(n_in, recurrent);
    m_stack = new Stack(m_recurrent->n_outputs(), feed_forward);
  }
  ReductionStack::~ReductionStack() {
    delete m_recurrent;
    delete m_stack;
  }
  VectorXd ReductionStack::reduce(MatrixXd in) const {
    in = m_recurrent->scan(in);
    return m_stack->compute(in.col(in.cols() -1));
  }
  size_t ReductionStack::n_outputs() const {
    return m_stack->n_outputs();
  }

  // __________________________________________________________________
  // Recurrent layers

  EmbeddingLayer::EmbeddingLayer(int var_row_index, MatrixXd W):
    m_var_row_index(var_row_index),
    m_W(W)
  {
    if(var_row_index < 0)
      throw NNConfigurationException(
        "EmbeddingLayer::EmbeddingLayer - can not set var_row_index<0,"
        " it is an index for a matrix row!");
  }

  MatrixXd EmbeddingLayer::scan( const MatrixXd& x) const {

    if( m_var_row_index >= x.rows() )
      throw NNEvaluationException(
        "EmbeddingLayer::scan - var_row_index is larger than input matrix"
        " number of rows!");

    MatrixXd embedded(m_W.rows(), x.cols());

    for(int icol=0; icol<x.cols(); icol++) {
      double vector_idx = x(m_var_row_index, icol);
      bool is_int = std::floor(vector_idx) == vector_idx;
      bool is_valid = (vector_idx >= 0) && (vector_idx < m_W.cols());
      if (!is_int || !is_valid) throw NNEvaluationException(
        "Invalid embedded index: " + std::to_string(vector_idx));
      embedded.col(icol) = m_W.col( vector_idx );
    }

    //only embed 1 variable at a time, so this should be correct size
    MatrixXd out(m_W.rows() + (x.rows() - 1), x.cols());

    //assuming m_var_row_index is an index with first possible value of 0
    if(m_var_row_index > 0)
      out.topRows(m_var_row_index) = x.topRows(m_var_row_index);

    out.block(m_var_row_index, 0, embedded.rows(), embedded.cols()) = embedded;

    if( m_var_row_index < (x.rows()-1) )
      out.bottomRows( x.cols() - 1 - m_var_row_index)
        = x.bottomRows( x.cols() - 1 - m_var_row_index);

    return out;
  }

  // LSTM layer
  LSTMLayer::LSTMLayer(ActivationConfig activation,
                       ActivationConfig inner_activation,
                       MatrixXd W_i, MatrixXd U_i, VectorXd b_i,
                       MatrixXd W_f, MatrixXd U_f, VectorXd b_f,
                       MatrixXd W_o, MatrixXd U_o, VectorXd b_o,
                       MatrixXd W_c, MatrixXd U_c, VectorXd b_c):
    m_W_i(W_i),
    m_U_i(U_i),
    m_b_i(b_i),
    m_W_f(W_f),
    m_U_f(U_f),
    m_b_f(b_f),
    m_W_o(W_o),
    m_U_o(U_o),
    m_b_o(b_o),
    m_W_c(W_c),
    m_U_c(U_c),
    m_b_c(b_c)
  {
    m_n_outputs = m_W_o.rows();

    m_activation_fun = get_activation(activation);
    m_inner_activation_fun = get_activation(inner_activation);
  }

  // internal structure created on each scan call
  struct LSTMState {
    LSTMState(size_t n_input, size_t n_outputs);
    MatrixXd C_t;
    MatrixXd h_t;
    int time;
  };
  LSTMState::LSTMState(size_t n_input, size_t n_output):
    C_t(MatrixXd::Zero(n_output, n_input)),
    h_t(MatrixXd::Zero(n_output, n_input)),
    time(0)
  {
  }

  void LSTMLayer::step(const VectorXd& x_t, LSTMState& s) const {
    // https://github.com/fchollet/keras/blob/master/keras/layers/recurrent.py#L740

    const auto& act_fun = m_activation_fun;
    const auto& in_act_fun = m_inner_activation_fun;

    int tm1 = s.time == 0 ? 0 : s.time - 1;
    VectorXd h_tm1 = s.h_t.col(tm1);
    VectorXd C_tm1 = s.C_t.col(tm1);

    VectorXd i  =  (m_W_i*x_t + m_b_i + m_U_i*h_tm1).unaryExpr(in_act_fun);
    VectorXd f  =  (m_W_f*x_t + m_b_f + m_U_f*h_tm1).unaryExpr(in_act_fun);
    VectorXd o  =  (m_W_o*x_t + m_b_o + m_U_o*h_tm1).unaryExpr(in_act_fun);
    VectorXd ct =  (m_W_c*x_t + m_b_c + m_U_c*h_tm1).unaryExpr(act_fun);

    s.C_t.col(s.time) = f.cwiseProduct(C_tm1) + i.cwiseProduct(ct);
    s.h_t.col(s.time) = o.cwiseProduct(s.C_t.col(s.time).unaryExpr(act_fun));
  }

  MatrixXd LSTMLayer::scan( const MatrixXd& x ) const {

    LSTMState state(x.cols(), m_n_outputs);

    for(state.time = 0; state.time < x.cols(); state.time++) {
      step( x.col( state.time ), state );
    }

    return state.h_t;
  }

  // GRU layer
  GRULayer::GRULayer(ActivationConfig activation,
                     ActivationConfig inner_activation,
                     MatrixXd W_z, MatrixXd U_z, VectorXd b_z,
                     MatrixXd W_r, MatrixXd U_r, VectorXd b_r,
                     MatrixXd W_h, MatrixXd U_h, VectorXd b_h):
    m_W_z(W_z),
    m_U_z(U_z),
    m_b_z(b_z),
    m_W_r(W_r),
    m_U_r(U_r),
    m_b_r(b_r),
    m_W_h(W_h),
    m_U_h(U_h),
    m_b_h(b_h)
  {
    m_n_outputs = m_W_h.rows();

    m_activation_fun = get_activation(activation);
    m_inner_activation_fun = get_activation(inner_activation);
  }
  // internal structure created on each scan call
  struct GRUState {
    GRUState(size_t n_input, size_t n_outputs);
    MatrixXd h_t;
    int time;
  };
  GRUState::GRUState(size_t n_input, size_t n_output):
    h_t(MatrixXd::Zero(n_output, n_input)),
    time(0)
  {
  }

  void GRULayer::step( const VectorXd& x_t, GRUState& s) const {
    // https://github.com/fchollet/keras/blob/master/keras/layers/recurrent.py#L547

    const auto& act_fun = m_activation_fun;
    const auto& in_act_fun = m_inner_activation_fun;

    int tm1 = s.time == 0 ? 0 : s.time - 1;
    VectorXd h_tm1 = s.h_t.col(tm1);
    VectorXd z  = (m_W_z*x_t + m_b_z + m_U_z*h_tm1).unaryExpr(in_act_fun);
    VectorXd r  = (m_W_r*x_t + m_b_r + m_U_r*h_tm1).unaryExpr(in_act_fun);
    VectorXd rh = r.cwiseProduct(h_tm1);
    VectorXd hh = (m_W_h*x_t + m_b_h + m_U_h*rh).unaryExpr(act_fun);
    VectorXd one = VectorXd::Ones(z.size());
    s.h_t.col(s.time)  = z.cwiseProduct(h_tm1) + (one - z).cwiseProduct(hh);
  }

  MatrixXd GRULayer::scan( const MatrixXd& x ) const {

    GRUState state(x.cols(), m_n_outputs);

    for(state.time = 0; state.time < x.cols(); state.time++) {
      step( x.col( state.time ), state );
    }

    return state.h_t;
  }

  // _____________________________________________________________________
  // Activation functions
  //
  // There are two functions below. In most cases the activation layer
  // can be implemented as a unary function, but in some cases
  // (i.e. softmax) something more complicated is reqired.

  // Note that in the first case you own this layer! It's your
  // responsibility to delete it.
  ILayer* get_raw_activation_layer(ActivationConfig activation) {
    // Check for special cases. If it's not one, use
    // UnaryActivationLayer
    switch (activation.function) {
    case Activation::SOFTMAX: return new SoftmaxLayer;
    default: return new UnaryActivationLayer(activation);
    }
  }

  // Most activation functions should be handled here.
  std::function<double(double)> get_activation(lwt::ActivationConfig act) {
    using namespace lwt;
    switch (act.function) {
    case Activation::SIGMOID: return nn_sigmoid;
    case Activation::HARD_SIGMOID: return nn_hard_sigmoid;
    case Activation::SWISH: return Swish(act.alpha);
    case Activation::TANH: return nn_tanh;
    case Activation::RECTIFIED: return nn_relu;
    case Activation::ELU: return ELU(act.alpha);
    case Activation::LEAKY_RELU: return LeakyReLU(act.alpha);
    case Activation::LINEAR: return [](double x){return x;};
    default: {
      throw NNConfigurationException("Got undefined activation function");
    }
    }
  }

  double nn_sigmoid( double x ){
    //github.com/Theano/Theano/blob/master/theano/tensor/nnet/sigm.py#L35
    if (x < -30.0) return 0.0;
    if (x >  30.0) return 1.0;
    return 1.0 / (1.0 + std::exp(-1.0*x));
  }

  double nn_hard_sigmoid( double x ){
    //github.com/Theano/Theano/blob/master/theano/tensor/nnet/sigm.py#L279
    double out = 0.2*x + 0.5;
    if (out < 0) return 0.0;
    if (out > 1) return 1.0;
    return out;
  }

  Swish::Swish(double alpha):
    m_alpha(alpha)
  {}
  double Swish::operator()(double x) const {
    return x * nn_sigmoid(m_alpha * x);
  }

  double nn_tanh( double x ){
    return std::tanh(x);
  }

  double nn_relu( double x) {
    if (std::isnan(x)) return x;
    else return x > 0 ? x : 0;
  }

  ELU::ELU(double alpha):
    m_alpha(alpha)
  {}
  double ELU::operator()( double x ) const {
    /* ELU function : https://arxiv.org/pdf/1511.07289.pdf
       f(x)=(x>=0)*x + ( (x<0)*alpha*(exp(x)-1) )
    */
    double exp_term = m_alpha * (std::exp(x)-1);
    return x>=0 ? x : exp_term;
  }

  LeakyReLU::LeakyReLU(double alpha):
    m_alpha(alpha)
  {}
  double LeakyReLU::operator()(double x) const {
    return x > 0 ? x : m_alpha * x;
  }

  // ________________________________________________________________________
  // utility functions
  MatrixXd build_matrix(const std::vector<double>& weights, size_t n_inputs)
  {
    size_t n_elements = weights.size();
    if ((n_elements % n_inputs) != 0) {
      std::string problem = "matrix elements not divisible by number"
        " of columns. Elements: " + std::to_string(n_elements) +
        ", Inputs: " + std::to_string(n_inputs);
      throw lwt::NNConfigurationException(problem);
    }
    size_t n_outputs = n_elements / n_inputs;
    MatrixXd matrix(n_outputs, n_inputs);
    for (size_t row = 0; row < n_outputs; row++) {
      for (size_t col = 0; col < n_inputs; col++) {
        double element = weights.at(col + row * n_inputs);
        matrix(row, col) = element;
      }
    }
    return matrix;
  }
  VectorXd build_vector(const std::vector<double>& bias) {
    VectorXd out(bias.size());
    size_t idx = 0;
    for (const auto& val: bias) {
      out(idx) = val;
      idx++;
    }
    return out;
  }

  // consistency checks
  void throw_if_not_maxout(const LayerConfig& layer) {
    bool wt_ok = layer.weights.size() == 0;
    bool bias_ok = layer.bias.size() == 0;
    bool maxout_ok = layer.sublayers.size() > 0;
    bool act_ok = layer.activation.function == Activation::NONE;
    if (wt_ok && bias_ok && maxout_ok && act_ok) return;
    throw NNConfigurationException("layer has wrong info for maxout");
  }
  void throw_if_not_dense(const LayerConfig& layer) {
    if (layer.sublayers.size() > 0) {
      throw NNConfigurationException("sublayers in dense layer");
    }
  }

  void throw_if_not_normalization(const LayerConfig& layer) {
    if (layer.sublayers.size() > 0) {
      throw NNConfigurationException("sublayers in normalization layer");
    }
  }

  // component-wise getters (for Highway, lstm, etc)
  DenseComponents get_component(const lwt::LayerConfig& layer, size_t n_in) {
    using namespace Eigen;
    using namespace lwt;
    MatrixXd weights = build_matrix(layer.weights, n_in);
    size_t n_out = weights.rows();
    VectorXd bias = build_vector(layer.bias);

    // the u element is optional
    size_t u_el = layer.U.size();
    MatrixXd U = u_el ? build_matrix(layer.U, n_out) : MatrixXd::Zero(0,0);

    size_t u_out = U.rows();
    size_t b_out = bias.rows();
    bool u_mismatch = (u_out != n_out) && (u_out > 0);
    if ( u_mismatch || b_out != n_out) {
      throw NNConfigurationException(
        "Output dims mismatch, W: " + std::to_string(n_out) +
        ", U: " + std::to_string(u_out) + ", b: " + std::to_string(b_out));
    }
    return {weights, U, bias};
  }

}


// Copyright (c) John Morrison, all rights reserved.  See the
// accompanying LICENSE file for more information.

#include <math.h>
#include <Accuracy.h>
#include <CartesianCoordinates.h>
#include <CoordinateConversionException.h>
#include <CoordinateConversionService.h>
#include <GeodeticCoordinates.h>
#include <GeodeticParameters.h>
#include <HeightType.h>
#include <LocalCartesianParameters.h>
#include <geocon/geodetic2local.h>
#include <Eigen/Eigen>
#include <miniglog/logging.h>
#include "geocon_init.h"

namespace geocon {

using namespace MSP::CCS;
using namespace Eigen;

static inline Matrix3d enu_to_ned() {
  // Rotation from ENU to NED.
  static Matrix3d r_en;
  r_en <<
      0, 1, 0,
      1, 0, 0,
      0, 0, -1;
  return r_en;
}

static GeodeticParameters kEllipsoidParameters(
    CoordinateType::geodetic,
    HeightType::ellipsoidHeight);

geodetic2local* geodetic2local::Create(double latitude,
                                       double longitude,
                                       double altitude) {
  if (!init()) {
    LOG(ERROR) << "Failed to initialize GeoCon";
    return nullptr;
  }
  LocalCartesianParameters localParameters(
      CoordinateType::localCartesian,
      // Longitude, latitude, altitude, orientation.
      longitude, latitude, altitude, 0);
  try {
    return new geodetic2local(
        new CoordinateConversionService("WGE", &kEllipsoidParameters,
                                        "WGE", &localParameters));
  } catch (CoordinateConversionException& e) {
    LOG(ERROR) << "Failed to create ccs_: " << e.getMessage();
  }
  return nullptr;
}

geodetic2local::geodetic2local(CoordinateConversionService* ccs) : ccs_(ccs) {}

bool geodetic2local::to_local(
    double latitude, double longitude, double altitude, double std,
    CartesianCoordinates* cart, Accuracy* acc) const {
  CHECK_NOTNULL(cart);
  CHECK_NOTNULL(acc);
  GeodeticCoordinates geo_coords(CoordinateType::geodetic,
                                 longitude, latitude, altitude);
  Accuracy geo_acc;

  // 90% confidence interval is 1.64 STD
  geo_acc.setSphericalError90(std * 1.64);

  try {
    ccs_->convertSourceToTarget(&geo_coords, &geo_acc, *cart, *acc);
  } catch (CoordinateConversionException& e) {
    LOG(ERROR) << "Failed to convert coordinates: " << e.getMessage();
    return false;
  }

  static Matrix3d r_enu_to_ned = enu_to_ned();
  Vector3d v_ned(r_enu_to_ned * Vector3d(cart->x(), cart->y(), cart->z()));
  cart->set(v_ned[0], v_ned[1], v_ned[2]);
  return true;
}

bool geodetic2local::to_geodetic(
    double x, double y, double z, double std,
    GeodeticCoordinates* geo, Accuracy* acc) const {
  CHECK(false);
  return false;
}
}  // namespace geocon


#include "astar.hpp"
#include "uat/agent.hpp"

#include <algorithm>
#include <cmath>
#include <limits>
#include <random>
#include <unordered_map>
#include <variant>

#include <boost/functional/hash.hpp>
#include <cool/compose.hpp>
#include <uat/permit.hpp>

using namespace uat;

struct score_t
{
  value_t g = std::numeric_limits<value_t>::infinity();
  value_t f = std::numeric_limits<value_t>::infinity();
};

template <typename T, typename Cmp> class heap
{
public:
  heap(Cmp cmp) : cmp_(std::move(cmp)) {}

  auto push(T value)
  {
    for (auto& v : values_)
      if (v.first == value)
        v.second = false;
    values_.push_back({std::move(value), true});
    std::push_heap(values_.begin(), values_.end(), [&](const auto& x, const auto& y) { return cmp_(x.first, y.first); });
  }

  auto pop() -> std::pair<T, bool>
  {
    while (values_.size() > 1) {
      std::pop_heap(values_.begin(), values_.end(), [&](const auto& x, const auto& y) { return cmp_(x.first, y.first); });
      auto current = std::move(values_.back());
      values_.pop_back();
      if (current.second)
        return current;
    }
    auto value = std::move(values_.back());
    values_.pop_back();
    return value;
  }

  auto empty() const { return values_.empty(); }

private:
  std::vector<std::pair<T, bool>> values_;
  Cmp cmp_;
};

struct step
{
  permit first;
  permit second;
  auto operator==(const step& other) const { return first == other.first && second == other.second; }
};

namespace std
{
template <> struct hash<step>
{
  auto operator()(const step& s) const noexcept -> size_t
  {
    size_t seed = 0;
    boost::hash_combine(seed, std::hash<permit>{}(s.first));
    boost::hash_combine(seed, std::hash<permit>{}(s.second));
    return seed;
  }
};
} // namespace std

auto astar(const region& from, const region& to, uint_t tstart, value_t bid_max_value, permit_public_status_fn& status, int seed)
  -> std::vector<uat::permit>
{
  using namespace uat::permit_public_status;
  if (std::holds_alternative<unavailable>(status(from, tstart)))
    return {};

  std::mt19937 gen(seed);

  const auto h = [to, bid_max_value](const region& s, uint_t t) -> value_t {
    return 2 * s.heuristic_distance(to) * bid_max_value;
  };

  const auto cost = [&](const permit& s) {
    return std::visit(
      cool::compose{[&](unavailable) { return std::numeric_limits<value_t>::infinity(); }, [&](owned) -> value_t { return 0; },
                    [&](available status) {
                      return status.min_value > bid_max_value ? std::numeric_limits<value_t>::infinity() : bid_max_value;
                    }},
      status(s.location(), s.time()));
  };

  std::unordered_map<step, step> came_from;
  std::unordered_map<step, score_t> score;

  const step first = {{from, tstart}, {from, tstart}};
  score[first] = {0, h(from, tstart)};

  const auto cmp = [&score](const step& a, const step& b) { return score[a].f > score[b].f; };

  heap<step, decltype(cmp)> open(cmp);
  open.push(first);

  const auto try_path = [&](const step& current, step next) {
    const auto d = cost(next.first) + cost(next.second);
    if (std::isinf(d))
      return;

    const auto tentative = score[current].g + d;
    const auto hnext = h(next.second.location(), next.second.time());

    if (tentative < score[next].g) {
      came_from[next] = current;
      score[next] = {tentative, tentative + hnext};

      open.push(std::move(next));
    }
  };

  while (!open.empty()) {
    const auto [current, valid] = open.pop();
    if (!valid)
      continue;

    if (current.second.location() == to) {
      std::vector<permit> path;
      auto s = current;
      for (; s.second.location() != from; s = came_from.at(s)) {
        path.push_back(s.second);
        path.push_back(s.first);
      }
      path.push_back(s.second);

      return path;
    }

    // XXX: should we keep forbiding staying still?
    // try_path(current, {current.location(), current.time() + 1});

    auto nei = current.second.location().adjacent_regions();
    std::shuffle(nei.begin(), nei.end(), gen);
    for (auto nslot : nei)
      try_path(current, {{current.second.location(), current.second.time() + 1}, {std::move(nslot), current.second.time() + 1}});
  }

  return {};
}


//
// immer: immutable data structures for C++
// Copyright (C) 2016, 2017, 2018 Juan Pedro Bolivar Puente
//
// This software is distributed under the Boost Software License, Version 1.0.
// See accompanying file LICENSE or copy at http://boost.org/LICENSE_1_0.txt
//

#pragma once

#include <immer/heap/heap_policy.hpp>
#include <immer/refcount/enable_intrusive_ptr.hpp>
#include <immer/refcount/refcount_policy.hpp>

#include <boost/intrusive_ptr.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/smart_ptr/intrusive_ref_counter.hpp>

#include <cassert>
#include <cstddef>
#include <limits>

namespace immer {
namespace detail {
namespace dvektor {

constexpr auto fast_log2(std::size_t x)
{
    return x == 0 ? 0 : sizeof(std::size_t) * 8 - 1 - __builtin_clzl(x);
}

template <int B, typename T = std::size_t>
constexpr T branches = T{1} << B;

template <int B, typename T = std::size_t>
constexpr T mask = branches<B, T> - 1;

template <int B, typename T = std::size_t>
constexpr auto
    max_depth = fast_log2(std::numeric_limits<std::size_t>::max()) / B;

template <typename T, int B, typename MP>
struct node;

template <typename T, int B, typename MP>
using node_ptr = boost::intrusive_ptr<node<T, B, MP>>;

template <typename T, int B>
using leaf_node = std::array<T, 1 << B>;

template <typename T, int B, typename MP>
using inner_node = std::array<node_ptr<T, B, MP>, 1 << B>;

template <typename T, int B, typename MP>
struct node
    : enable_intrusive_ptr<node<T, B, MP>, typename MP::refcount>
    , enable_optimized_heap_policy<node<T, B, MP>, typename MP::heap>
{
    using leaf_node_t  = leaf_node<T, B>;
    using inner_node_t = inner_node<T, B, MP>;

    enum
    {
        leaf_kind,
        inner_kind
    } kind;

    union data_t
    {
        leaf_node_t leaf;
        inner_node_t inner;
        data_t(leaf_node_t n)
            : leaf(std::move(n))
        {}
        data_t(inner_node_t n)
            : inner(std::move(n))
        {}
        ~data_t() {}
    } data;

    ~node()
    {
        switch (kind) {
        case leaf_kind:
            data.leaf.~leaf_node_t();
            break;
        case inner_kind:
            data.inner.~inner_node_t();
            break;
        }
    }

    node(leaf_node<T, B> n)
        : kind{leaf_kind}
        , data{std::move(n)}
    {}

    node(inner_node<T, B, MP> n)
        : kind{inner_kind}
        , data{std::move(n)}
    {}

    inner_node_t& inner() &
    {
        assert(kind == inner_kind);
        return data.inner;
    }
    const inner_node_t& inner() const&
    {
        assert(kind == inner_kind);
        return data.inner;
    }
    inner_node_t&& inner() &&
    {
        assert(kind == inner_kind);
        return std::move(data.inner);
    }

    leaf_node_t& leaf() &
    {
        assert(kind == leaf_kind);
        return data.leaf;
    }
    const leaf_node_t& leaf() const&
    {
        assert(kind == leaf_kind);
        return data.leaf;
    }
    leaf_node_t&& leaf() &&
    {
        assert(kind == leaf_kind);
        return std::move(data.leaf);
    }
};

template <typename T, int B, typename MP, typename... Ts>
auto make_node(Ts&&... xs) -> boost::intrusive_ptr<node<T, B, MP>>
{
    return new node<T, B, MP>(std::forward<Ts>(xs)...);
}

template <typename T, int B, typename MP>
struct ref
{
    using inner_t    = inner_node<T, B, MP>;
    using leaf_t     = leaf_node<T, B>;
    using node_t     = node<T, B, MP>;
    using node_ptr_t = node_ptr<T, B, MP>;

    unsigned depth;
    std::array<node_ptr_t, max_depth<B>> display;

    template <typename... Ts>
    static auto make_node(Ts&&... xs)
    {
        return dvektor::make_node<T, B, MP>(std::forward<Ts>(xs)...);
    }

    const T& get_elem(std::size_t index, std::size_t xr) const
    {
        auto display_idx = fast_log2(xr) / B;
        auto node        = display[display_idx].get();
        auto shift       = display_idx * B;
        while (display_idx--) {
            node = node->inner()[(index >> shift) & mask<B>].get();
            shift -= B;
        }
        return node->leaf()[index & mask<B>];
    }

    node_ptr_t null_slot_and_copy_inner(node_ptr_t& node, std::size_t idx)
    {
        auto& n = node->inner();
        auto x  = node_ptr_t{};
        x.swap(n[idx]);
        return copy_of_inner(x);
    }

    node_ptr_t null_slot_and_copy_leaf(node_ptr_t& node, std::size_t idx)
    {
        auto& n = node->inner();
        auto x  = node_ptr_t{};
        x.swap(n[idx]);
        return copy_of_leaf(x);
    }

    node_ptr_t copy_of_inner(const node_ptr_t& n)
    {
        return make_node(n->inner());
    }

    node_ptr_t copy_of_leaf(const node_ptr_t& n)
    {
        return make_node(n->leaf());
    }

    void stabilize(std::size_t index)
    {
        auto shift = B;
        for (auto i = 1u; i < depth; ++i) {
            display[i] = copy_of_inner(display[i]);
            display[i]->inner()[(index >> shift) & mask<B>] = display[i - 1];
            shift += B;
        }
    }

    void goto_pos_writable_from_clean(std::size_t old_index,
                                      std::size_t index,
                                      std::size_t xr)
    {
        assert(depth);
        auto d = depth - 1;
        if (d == 0) {
            display[0] = copy_of_leaf(display[0]);
        } else {
            IMMER_UNREACHABLE;
            display[d] = copy_of_inner(display[d]);
            auto shift = B * d;
            while (--d) {
                display[d] = null_slot_and_copy_inner(
                    display[d + 1], (index >> shift) & mask<B>);
                shift -= B;
            }
            display[0] =
                null_slot_and_copy_leaf(display[1], (index >> B) & mask<B>);
        }
    }

    void goto_pos_writable_from_dirty(std::size_t old_index,
                                      std::size_t new_index,
                                      std::size_t xr)
    {
        assert(depth);
        if (xr < (1 << B)) {
            display[0] = copy_of_leaf(display[0]);
        } else {
            auto display_idx = fast_log2(xr) / B;
            auto shift       = B;
            for (auto i = 1u; i <= display_idx; ++i) {
                display[i] = copy_of_inner(display[i]);
                display[i]->inner()[(old_index >> shift) & mask<B>] =
                    display[i - 1];
                shift += B;
            }
            for (auto i = display_idx - 1; i > 0; --i) {
                shift -= B;
                display[i] = null_slot_and_copy_inner(
                    display[i + 1], (new_index >> shift) & mask<B>);
            }
            display[0] =
                null_slot_and_copy_leaf(display[1], (new_index >> B) & mask<B>);
        }
    }

    void goto_fresh_pos_writable_from_clean(std::size_t old_index,
                                            std::size_t new_index,
                                            std::size_t xr)
    {
        auto display_idx = fast_log2(xr) / B;
        if (display_idx > 0) {
            auto shift = display_idx * B;
            if (display_idx == depth) {
                display[display_idx] = make_node(inner_t{});
                display[display_idx]->inner()[(old_index >> shift) & mask<B>] =
                    display[display_idx - 1];
                ++depth;
            }
            while (--display_idx) {
                auto node = display[display_idx + 1]
                                ->inner()[(new_index >> shift) & mask<B>];
                display[display_idx] =
                    node ? std::move(node) : make_node(inner_t{});
            }
            display[0] = make_node(leaf_t{});
        }
    }

    void goto_fresh_pos_writable_from_dirty(std::size_t old_index,
                                            std::size_t new_index,
                                            std::size_t xr)
    {
        stabilize(old_index);
        goto_fresh_pos_writable_from_clean(old_index, new_index, xr);
    }

    void goto_next_block_start(std::size_t index, std::size_t xr)
    {
        auto display_idx = fast_log2(xr) / B;
        auto shift       = display_idx * B;
        if (display_idx > 0) {
            display[display_idx - 1] =
                display[display_idx]->inner()[(index >> shift) & mask<B>];
            while (--display_idx)
                display[display_idx - 1] = display[display_idx]->inner()[0];
        }
    }

    void goto_pos(std::size_t index, std::size_t xr)
    {
        auto display_idx = fast_log2(xr) / B;
        auto shift       = display_idx * B;
        if (display_idx) {
            do {
                display[display_idx - 1] =
                    display[display_idx]->inner()[(index >> shift) & mask<B>];
                shift -= B;
            } while (--display_idx);
        }
    }
};

template <typename T, int B, typename MP>
struct impl
{
    using inner_t    = inner_node<T, B, MP>;
    using leaf_t     = leaf_node<T, B>;
    using node_t     = node<T, B, MP>;
    using node_ptr_t = node_ptr<T, B, MP>;
    using ref_t      = ref<T, B, MP>;

    std::size_t size;
    std::size_t focus;
    bool dirty;
    ref_t p;

    template <typename... Ts>
    static auto make_node(Ts&&... xs)
    {
        return dvektor::make_node<T, B, MP>(std::forward<Ts>(xs)...);
    }

    void goto_pos_writable(std::size_t old_index,
                           std::size_t new_index,
                           std::size_t xr)
    {
        if (dirty) {
            p.goto_pos_writable_from_dirty(old_index, new_index, xr);
        } else {
            p.goto_pos_writable_from_clean(old_index, new_index, xr);
            dirty = true;
        }
    }

    void goto_fresh_pos_writable(std::size_t old_index,
                                 std::size_t new_index,
                                 std::size_t xr)
    {
        if (dirty) {
            p.goto_fresh_pos_writable_from_dirty(old_index, new_index, xr);
        } else {
            p.goto_fresh_pos_writable_from_clean(old_index, new_index, xr);
            dirty = true;
        }
    }

    impl push_back(T value) const
    {
        if (size) {
            auto block_index = size & ~mask<B>;
            auto lo          = size & mask<B>;
            if (size != block_index) {
                auto s = impl{size + 1, block_index, dirty, p};
                s.goto_pos_writable(focus, block_index, focus ^ block_index);
                s.p.display[0]->leaf()[lo] = std::move(value);
                return s;
            } else {
                auto s = impl{size + 1, block_index, dirty, p};
                s.goto_fresh_pos_writable(
                    focus, block_index, focus ^ block_index);
                s.p.display[0]->leaf()[lo] = std::move(value);
                return s;
            }
        } else {
            return impl{
                1, 0, false, {1, {{make_node(leaf_t{{std::move(value)}})}}}};
        }
    }

    const T& get(std::size_t index) const
    {
        return p.get_elem(index, index ^ focus);
    }

    template <typename FnT>
    impl update(std::size_t idx, FnT&& fn) const
    {
        auto s = impl{size, idx, dirty, p};
        s.goto_pos_writable(focus, idx, focus ^ idx);
        auto& v = s.p.display[0]->leaf()[idx & mask<B>];
        v       = fn(std::move(v));
        return s;
    }

    impl assoc(std::size_t idx, T value) const
    {
        return update(idx, [&](auto&&) { return std::move(value); });
    }
};

template <typename T, int B, typename MP>
const impl<T, B, MP> empty = {0, 0, false, ref<T, B, MP>{1, {}}};

template <typename T, int B, typename MP>
struct iterator
    : boost::iterator_facade<iterator<T, B, MP>,
                             T,
                             boost::random_access_traversal_tag,
                             const T&>
{
    struct end_t
    {};

    iterator() = default;

    iterator(const impl<T, B, MP>& v)
        : p_{v.p}
        , i_{0}
        , base_{0}
    {
        if (v.dirty)
            p_.stabilize(v.focus);
        p_.goto_pos(0, 0 ^ v.focus);
        curr_ = p_.display[0]->leaf().begin();
    }

    iterator(const impl<T, B, MP>& v, end_t)
        : p_{v.p}
        , i_{v.size}
        , base_{(v.size - 1) & ~mask<B>}
    {
        if (v.dirty)
            p_.stabilize(v.focus);
        p_.goto_pos(base_, base_ ^ v.focus);
        curr_ = p_.display[0]->leaf().begin() + (i_ - base_);
    }

private:
    friend class boost::iterator_core_access;
    using leaf_iterator = typename leaf_node<T, B>::const_iterator;

    ref<T, B, MP> p_;
    std::size_t i_;
    std::size_t base_;
    leaf_iterator curr_;

    void increment()
    {
        ++i_;
        if (i_ - base_ < branches<B>) {
            ++curr_;
        } else {
            auto new_base = base_ + branches<B>;
            p_.goto_next_block_start(new_base, base_ ^ new_base);
            base_ = new_base;
            curr_ = p_.display[0]->leaf().begin();
        }
    }

    void decrement()
    {
        assert(i_ > 0);
        --i_;
        if (i_ >= base_) {
            --curr_;
        } else {
            auto new_base = base_ - branches<B>;
            p_.goto_pos(new_base, base_ ^ new_base);
            base_ = new_base;
            curr_ = std::prev(p_.display[0]->leaf().end());
        }
    }

    void advance(std::ptrdiff_t n)
    {
        i_ += n;
        if (i_ <= base_ && i_ - base_ < branches<B>) {
            curr_ += n;
        } else {
            auto new_base = i_ & ~mask<B>;
            p_.goto_pos(new_base, base_ ^ new_base);
            base_ = new_base;
            curr_ = p_.display[0]->leaf().begin() + (i_ - base_);
        }
    }

    bool equal(const iterator& other) const { return i_ == other.i_; }

    std::ptrdiff_t distance_to(const iterator& other) const
    {
        return other.i_ > i_ ? static_cast<std::ptrdiff_t>(other.i_ - i_)
                             : -static_cast<std::ptrdiff_t>(i_ - other.i_);
    }

    const T& dereference() const { return *curr_; }
};

} /* namespace dvektor */
} /* namespace detail */
} /* namespace immer */


// Copyright 2013 Daniel Parker
// Distributed under Boost license

#ifdef __linux__
#define BOOST_TEST_DYN_LINK
#endif

#include <boost/test/unit_test.hpp>
#include <sstream>
#include <vector>
#include <utility>
#include <ctime>
#include <new>
#include <jsoncons/json.hpp>
#include <jsoncons_ext/jsonpath/json_query.hpp>

using namespace jsoncons;
using namespace jsoncons::jsonpath;
using namespace jsoncons::jsonpath::detail;

BOOST_AUTO_TEST_SUITE(jsonpath_filter_tests)

struct jsonpath_filter_fixture
{
    static const char* store_text()
    {
        static const char* text = "{ \"store\": {\"book\": [ { \"category\": \"reference\",\"author\": \"Nigel Rees\",\"title\": \"Sayings of the Century\",\"price\": 8.95},{ \"category\": \"fiction\",\"author\": \"Evelyn Waugh\",\"title\": \"Sword of Honour\",\"price\": 12.99},{ \"category\": \"fiction\",\"author\": \"Herman Melville\",\"title\": \"Moby Dick\",\"isbn\": \"0-553-21311-3\",\"price\": 8.99},{ \"category\": \"fiction\",\"author\": \"J. R. R. Tolkien\",\"title\": \"The Lord of the Rings\",\"isbn\": \"0-395-19395-8\",\"price\": 22.99}],\"bicycle\": {\"color\": \"red\",\"price\": 19.95}}}";
        return text;
    }
    static const char* book_text()
    {
        static const char* text = "{ \"category\": \"reference\",\"author\": \"Nigel Rees\",\"title\": \"Sayings of the Century\",\"price\": 8.95}";
        return text;
    }

    json book()
    {
        json root = json::parse(jsonpath_filter_fixture::store_text());
        json book = root["store"]["book"];
        return book;
    }

    json bicycle()
    {
        json root = json::parse(jsonpath_filter_fixture::store_text());
        json bicycle = root["store"]["bicycle"];
        return bicycle;
    }
};

BOOST_AUTO_TEST_CASE(test_div)
{
    const char* pend;
    jsonpath_filter_parser<json> parser;

    json context = json::array();
    context.push_back(3);

    std::string s1 = "(3/1)";
    auto expr1 = parser.parse(context,s1.c_str(), s1.c_str()+ s1.length(), &pend);
    auto result1 = expr1.eval(context);
    BOOST_CHECK_EQUAL(json(3),result1);

    std::string s2 = "(3/@.length)";
    auto expr2 = parser.parse(context,s2.c_str(), s2.c_str()+ s2.length(), &pend);
    auto result2 = expr2.eval(context);
    BOOST_CHECK_EQUAL(json(3),result2);

    std::string s3 = "(5/2)";
    auto expr3 = parser.parse(context,s3.c_str(), s3.c_str()+ s3.length(), &pend);
    auto result3 = expr3.eval(context);
    BOOST_CHECK_EQUAL(json(2.5),result3);

    std::string s4 = "(@.length/3)";
    auto expr4 = parser.parse(context,s4.c_str(), s4.c_str()+ s4.length(), &pend);
    auto result4 = expr4.eval(context);
    BOOST_CHECK_CLOSE(0.333333,result4.as<double>(),0.001);

    std::string s5 = "(@.0/@.length)";
    auto expr5 = parser.parse(context,s5.c_str(), s5.c_str()+ s5.length(), &pend);
    auto result5 = expr5.eval(context);
    BOOST_CHECK_EQUAL(json(3),result5);
}

BOOST_AUTO_TEST_CASE(test_mult)
{
    const char* pend;
    jsonpath_filter_parser<json> parser;

    json context = json::array();
    context.push_back(1);
    context.push_back(2);

    std::string s1 = "(3*1)";
    auto expr1 = parser.parse(context, s1.c_str(), s1.c_str()+ s1.length(), &pend);
    auto result1 = expr1.eval(context);
    BOOST_CHECK_EQUAL(json(3),result1);

    std::string s2 = "(3*@.length)";
    auto expr2 = parser.parse(context, s2.c_str(), s2.c_str()+ s2.length(), &pend);
    auto result2 = expr2.eval(context);
    BOOST_CHECK_EQUAL(json(6),result2);

    std::string s3 = "(5*2)";
    auto expr3 = parser.parse(context, s3.c_str(), s3.c_str()+ s3.length(), &pend);
    auto result3 = expr3.eval(context);
    BOOST_CHECK_EQUAL(json(10),result3);

    std::string s4 = "(@.length*3)";
    auto expr4 = parser.parse(context, s4.c_str(), s4.c_str()+ s4.length(), &pend);
    auto result4 = expr4.eval(context);
    BOOST_CHECK_EQUAL(json(6),result4);

    std::string s5 = "(@.length*@.1)";
    auto expr5 = parser.parse(context, s5.c_str(), s5.c_str()+ s5.length(), &pend);
    auto result5 = expr5.eval(context);
    BOOST_CHECK_EQUAL(json(4),result5);
}

BOOST_AUTO_TEST_CASE(test_minus)
{
    const char* pend;
    jsonpath_filter_parser<json> parser;

    json context = json::array();
    context.push_back(10.0);

    std::string s1 = "(3-1)";
    auto expr1 = parser.parse(context, s1.c_str(), s1.c_str()+ s1.length(), &pend);
    auto result1 = expr1.eval(context);
    BOOST_CHECK_EQUAL(json(2),result1);

    std::string s2 = "(3-@.length)";
    auto expr2 = parser.parse(context, s2.c_str(), s2.c_str()+ s2.length(), &pend);
    auto result2 = expr2.eval(context);
    BOOST_CHECK_EQUAL(json(2),result2);

    std::string s3 = "(3.5-1.0)";
    auto expr3 = parser.parse(context, s3.c_str(), s3.c_str()+ s3.length(), &pend);
    auto result3 = expr3.eval(context);
    BOOST_CHECK_EQUAL(json(2.5),result3);

    std::string s4 = "(@.length-3)";
    auto expr4 = parser.parse(context, s4.c_str(), s4.c_str()+ s4.length(), &pend);
    auto result4 = expr4.eval(context);
    BOOST_CHECK_EQUAL(json(-2),result4);

    std::string s5 = "(@.length-@.0)";
    auto expr5 = parser.parse(context, s5.c_str(), s5.c_str()+ s5.length(), &pend);
    auto result5 = expr5.eval(context);
    BOOST_CHECK_EQUAL(json(-9),result5);
}

BOOST_AUTO_TEST_CASE(test_lt)
{
    const char* pend;
    jsonpath_filter_parser<json> parser;

    json context = json::array();
    context.push_back(100);
    context.push_back(1);

    std::string s1 = "(3 < 1)";
    auto expr1 = parser.parse(context, s1.c_str(), s1.c_str()+ s1.length(), &pend);
    auto result1 = expr1.eval(context);
    BOOST_CHECK_EQUAL(json(false),result1);

    std::string s2 = "(3 < @.length)";
    auto expr2 = parser.parse(context, s2.c_str(), s2.c_str()+ s2.length(), &pend);
    auto result2 = expr2.eval(context);
    BOOST_CHECK_EQUAL(json(false),result2);

    std::string s3 = "(@.length < 3)";
    auto expr3 = parser.parse(context, s3.c_str(), s3.c_str()+ s3.length(), &pend);
    auto result3 = expr3.eval(context);
    BOOST_CHECK_EQUAL(json(true),result3);

    std::string s4 = "(@.length < @.length)";
    auto expr4 = parser.parse(context, s4.c_str(), s4.c_str()+ s4.length(), &pend);
    auto result4 = expr4.eval(context);
    BOOST_CHECK_EQUAL(json(false),result4);

    std::string s5 = "(@.length < @.0)";
    auto expr5 = parser.parse(context, s5.c_str(), s5.c_str()+ s5.length(), &pend);
    auto result5 = expr5.eval(context);
    BOOST_CHECK_EQUAL(json(true),result5);

    std::string s6 = "(@.length < @.1)";
    auto expr6 = parser.parse(context, s6.c_str(), s6.c_str()+ s6.length(), &pend);
    auto result6 = expr6.eval(context);
    BOOST_CHECK_EQUAL(json(false),result6);
}

BOOST_AUTO_TEST_CASE(test_lte)
{
    const char* pend;
    jsonpath_filter_parser<json> parser;

    json context = json::array();
    context.push_back(1);

    std::string s1 = "(3 <= 1)";
    auto expr1 = parser.parse(context, s1.c_str(), s1.c_str()+ s1.length(), &pend);
    auto result1 = expr1.eval(context);
    BOOST_CHECK_EQUAL(json(false),result1);

    std::string s2 = "(3 <= @.length)";
    auto expr2 = parser.parse(context, s2.c_str(), s2.c_str()+ s2.length(), &pend);
    auto result2 = expr2.eval(context);
    BOOST_CHECK_EQUAL(json(false),result2);
}

BOOST_AUTO_TEST_CASE(test_gt)
{
    const char* pend;
    jsonpath_filter_parser<json> parser;

    json context = json::array();
    context.push_back(1);

    std::string s1 = "(3 > 1)";
    auto expr1 = parser.parse(context, s1.c_str(), s1.c_str()+ s1.length(), &pend);
    auto result1 = expr1.eval(context);
    BOOST_CHECK_EQUAL(json(true),result1);

    std::string s2 = "(3 > @.length)";
    auto expr2 = parser.parse(context, s2.c_str(), s2.c_str()+ s2.length(), &pend);
    auto result2 = expr2.eval(context);
    BOOST_CHECK_EQUAL(json(true),result2);
}

BOOST_AUTO_TEST_CASE(test_gte)
{
    const char* pend;
    jsonpath_filter_parser<json> parser;

    json context = json::array();
    context.push_back(1);

    std::string s1 = "(3 >= 1)";
    auto expr1 = parser.parse(context, s1.c_str(), s1.c_str()+ s1.length(), &pend);
    auto result1 = expr1.eval(context);
    BOOST_CHECK_EQUAL(json(true),result1);

    std::string s2 = "(3 >= @.length)";
    auto expr2 = parser.parse(context, s2.c_str(), s2.c_str()+ s2.length(), &pend);
    auto result2 = expr2.eval(context);
    BOOST_CHECK_EQUAL(json(true),result2);
}

BOOST_AUTO_TEST_CASE(test_eq)
{
    const char* pend;
    jsonpath_filter_parser<json> parser;

    json context = json::array();
    context.push_back(1);

    std::string s1 = "(3 == 1)";
    auto expr1 = parser.parse(context, s1.c_str(), s1.c_str()+ s1.length(), &pend);
    auto result1 = expr1.eval(context);
    BOOST_CHECK_EQUAL(json(false),result1);

    std::string s2 = "(3 == @.length)";
    auto expr2 = parser.parse(context, s2.c_str(), s2.c_str()+ s2.length(), &pend);
    auto result2 = expr2.eval(context);
    BOOST_CHECK_EQUAL(json(false),result2);

    std::string s3 = "(1 == 1)";
    auto expr3 = parser.parse(context, s3.c_str(), s3.c_str()+ s3.length(), &pend);
    auto result3 = expr3.eval(context);
    BOOST_CHECK_EQUAL(json(true),result3);

    std::string s4 = "(1 == @.length)";
    auto expr4 = parser.parse(context, s4.c_str(), s4.c_str()+ s4.length(), &pend);
    auto result4 = expr4.eval(context);
    BOOST_CHECK_EQUAL(json(true),result4);
}

BOOST_AUTO_TEST_CASE(test_precedence)
{
    const char* pend;
    jsonpath_filter_parser<json> parser;

    json context = json::array();
    context.push_back(1);
    context.push_back(2);

    std::string s1 = "(@.0 == 1 && @.1 == 2)";
    auto expr1 = parser.parse(context, s1.c_str(), s1.c_str()+ s1.length(), &pend);
    auto result1 = expr1.eval(context);
    BOOST_CHECK_EQUAL(json(true),result1);

    std::string s2 = "((@.0 == 1) && (@.1 == 2))";
    auto expr2 = parser.parse(context, s2.c_str(), s2.c_str()+ s2.length(), &pend);
    auto result2 = expr2.eval(context);
    BOOST_CHECK_EQUAL(json(true),result2);

    std::string s3 = "(@.0 == 2 && @.1 == 2)";
    auto expr3 = parser.parse(context, s3.c_str(), s3.c_str()+ s3.length(), &pend);
    auto result3 = expr3.eval(context);
    BOOST_CHECK_EQUAL(json(false),result3);

    std::string s4 = "((@.0 == 1) && (@.1 == 1))";
    auto expr4 = parser.parse(context, s4.c_str(), s4.c_str()+ s4.length(), &pend);
    auto result4 = expr4.eval(context);
    BOOST_CHECK_EQUAL(json(false),result4);
}

BOOST_AUTO_TEST_CASE(test_ne)
{
    const char* pend;
    jsonpath_filter_parser<json> parser;

    json context = json::array();
    context.push_back(1);

    std::string s1 = "(3 != 1)";
    auto expr1 = parser.parse(context, s1.c_str(), s1.c_str()+ s1.length(), &pend);
    auto result1 = expr1.eval(context);
    BOOST_CHECK_EQUAL(json(true),result1);

    std::string s2 = "(3 != @.length)";
    auto expr2 = parser.parse(context, s2.c_str(), s2.c_str()+ s2.length(), &pend);
    auto result2 = expr2.eval(context);
    BOOST_CHECK_EQUAL(json(true),result2);

    std::string s3 = "(1 != 1)";
    auto expr3 = parser.parse(context, s3.c_str(), s3.c_str()+ s3.length(), &pend);
    auto result3 = expr3.eval(context);
    BOOST_CHECK_EQUAL(json(false),result3);

    std::string s4 = "(1 != @.length)";
    auto expr4 = parser.parse(context, s4.c_str(), s4.c_str()+ s4.length(), &pend);
    auto result4 = expr4.eval(context);
    BOOST_CHECK_EQUAL(json(false),result4);
}

BOOST_AUTO_TEST_CASE(test_jsonpath_filter)
{
    const char* pend;
    jsonpath_filter_parser<json> parser;
    json parent = json::array();
    parent.push_back(1);
    parent.push_back(2);

    std::string expr1 = "(1 + 1)";
    auto res1 = parser.parse(parent, expr1.c_str(), expr1.c_str()+ expr1.length(), &pend);
    auto result1 = res1.eval(parent);
    BOOST_CHECK_EQUAL(json(2),result1);

    std::string expr2 = "(1 - 1)";
    auto res2 = parser.parse(parent, expr2.c_str(), expr2.c_str()+ expr2.length(), &pend);
    auto result2 = res2.eval(parent);
    BOOST_CHECK_EQUAL(json(0), result2);

    std::string expr3 = "(@.length - 1)";
    auto res3 = parser.parse(parent, expr3.c_str(), expr3.c_str()+ expr3.length(), &pend);
    auto result3 = res3.eval(parent);
    BOOST_CHECK_EQUAL(json(1), result3);

}

BOOST_AUTO_TEST_CASE(test_jsonpath_filter_exclaim)
{
    const char* pend;
    jsonpath_filter_parser<json> parser;
    json parent = json::array();
    parent.push_back(1);
    parent.push_back(2);

    std::string expr1 = "(!(1 + 1))";
    auto res1 = parser.parse(parent, expr1.c_str(), expr1.c_str()+ expr1.length(), &pend);
    auto result1 = res1.eval(parent);
    BOOST_CHECK_EQUAL(json(false),result1);

    std::string expr2 = "(!0)";
    auto res2 = parser.parse(parent, expr2.c_str(), expr2.c_str()+ expr2.length(), &pend);
    auto result2= res2.eval(parent);
    BOOST_CHECK_EQUAL(json(true),result2);
}

BOOST_AUTO_TEST_CASE(test_jsonpath_index_expression)
{
    json root = json::parse(jsonpath_filter_fixture::store_text());
    //std::cout << pretty_print(root) << std::endl;
    //std::cout << "$..book[(@.length-1)]" << std::endl;

    json result = json_query(root,"$..book[(@.length-1)]");

    BOOST_CHECK_EQUAL(1,result.size());
    BOOST_CHECK_EQUAL(root["store"]["book"][3],result[0]);

    //    std::cout << pretty_print(result) << std::endl;
}

BOOST_AUTO_TEST_CASE(test_jsonpath_filter_negative_numbers)
{
    const char* pend;
    jsonpath_filter_parser<json> parser;
    json parent = json::array();
    parent.push_back(1);
    parent.push_back(2);

    std::string expr1 = "(-1 + 1)";
    auto res1 = parser.parse(parent, expr1.c_str(), expr1.c_str()+ expr1.length(), &pend);
    auto result1 = res1.eval(parent);
    BOOST_CHECK_EQUAL(json(0),result1);

    std::string expr2 = "(1 + -1)";
    auto res2 = parser.parse(parent, expr2.c_str(), expr2.c_str()+ expr2.length(), &pend);
    auto result2 = res2.eval(parent);
    BOOST_CHECK_EQUAL(json(0), result2);

    std::string expr3 = "(-1 - -1)";
    auto res3 = parser.parse(parent, expr3.c_str(), expr3.c_str()+ expr3.length(), &pend);
    auto result3 = res3.eval(parent);
    BOOST_CHECK_EQUAL(json(0), result3);

    std::string expr4 = "(-1 - -3)";
    auto res4 = parser.parse(parent, expr4.c_str(), expr4.c_str()+ expr4.length(), &pend);
    auto result4 = res4.eval(parent);
    BOOST_CHECK_EQUAL(json(2), result4);

    std::string expr5 = "((-2 < -1) && (-3 > -4))";
    auto res5 = parser.parse(parent, expr5.c_str(), expr5.c_str()+ expr5.length(), &pend);
    auto result5 = res5.eval(parent);
    BOOST_CHECK_EQUAL(json(true), result5);

    std::string expr6 = "((-2 < -1) || (-4 > -3))";
    auto res6 = parser.parse(parent, expr6.c_str(), expr6.c_str()+ expr6.length(), &pend);
    auto result6 = res6.eval(parent);
    BOOST_CHECK_EQUAL(json(true), result6);

    std::string expr7 = "(-2 < -1 && -3 > -4)";
    auto res7 = parser.parse(parent, expr7.c_str(), expr7.c_str()+ expr7.length(), &pend);
    auto result7 = res7.eval(parent);
    BOOST_CHECK_EQUAL(json(true), result7);

    std::string expr8 = "(-2 < -1 || -4 > -3)";
    auto res8 = parser.parse(parent, expr8.c_str(), expr8.c_str()+ expr8.length(), &pend);
    auto result8 = res8.eval(parent);
    BOOST_CHECK_EQUAL(json(true), result8);
}

BOOST_AUTO_TEST_CASE(test_jsonpath_filter_uni)
{
    const char* pend;
    jsonpath_filter_parser<json> parser;
    json parent = json::array();
    parent.push_back(1);
    parent.push_back(2);

    std::string expr1 = "(0)";
    auto res = parser.parse(parent, expr1.c_str(), expr1.c_str()+ expr1.length(), &pend);
    auto result1 = res.eval(parent);

    //std::cout << (int)result1.major_type() << std::endl;
    std::cout << "result1=" << result1 << std::endl;
    BOOST_CHECK(result1 == json(0));

    BOOST_CHECK_EQUAL(json(0),result1);
}

#if defined(__GNUC__) && (__GNUC__ == 4 && __GNUC_MINOR__ < 9)
// GCC 4.8 has broken regex support: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=53631
BOOST_AUTO_TEST_CASE_EXPECTED_FAILURES(test_jsonpath_filter_regex, 2)
#endif

BOOST_AUTO_TEST_CASE(test_jsonpath_filter_regex)
{
    const char* pend;
    jsonpath_filter_parser<json> parser;
    json parent = json::array();
    parent.push_back(1);
    parent.push_back(2);

    std::string expr1 = "('today I go' =~ /today.*?/)";
    auto res1 = parser.parse(parent, expr1.c_str(), expr1.c_str()+ expr1.length(), &pend);
    auto result1 = res1.eval(parent);
    BOOST_CHECK_EQUAL(json(true),result1);

    std::string expr2 = "('today I go' =~ /Today.*?/)";
    auto res2 = parser.parse(parent, expr2.c_str(), expr2.c_str()+ expr2.length(), &pend);
    auto result2 = res2.eval(parent);
    BOOST_CHECK_EQUAL(json(false),result2);

    std::string expr3 = "('today I go' =~ /Today.*?/i)";
    auto res3 = parser.parse(parent, expr3.c_str(), expr3.c_str()+ expr3.length(), &pend);
    auto result3 = res3.eval(parent);
    BOOST_CHECK_EQUAL(json(true),result3);
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SINCOSD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SINCOSD_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object computes simultaneously and
    at lower cost the sind and cosd of the input

    @par Header <boost/simd/function/sincosd.hpp>

    @see sincos, sincospi

    @par Example:

      @snippet sincosd.cpp sincosd

    @par Possible output:

      @snippet sincosd.txt sincosd

  **/
  std::pair<IEEEValue, IEEEValue> sincosd(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/sincosd.hpp>
#include <boost/simd/function/simd/sincosd.hpp>

#endif


#include <iostream>
#include <Eigen/Dense>
#include "optitrack_client.hpp"

// 実行時にNatNetLib.dll, NatNetML.dll, NatNetML.xmlが必要

int main() {
  OptiTrackClient client;
  int cnt = 0;
  while (!client.connect()) {
    if (cnt++ > 20) {
      std::cout << "Failed to open NatNet connection" << std::endl;
      exit(1);
    }
    for (int i = 0; i < 100000000; i++) {
      ;
    }
  }
  for (int c = 0; c < 10; c++) {
    auto body = client.getBody(3);

    Eigen::Quaternion<double> q(body.qw, body.qx, body.qy, body.qz);
    std::cout << "rotation(global -> model)" << std::endl;
    auto mat = q.toRotationMatrix();
    for (int i = 0; i < 3; i++) {
      for (int j = 0; j < 3; j++) {
        std::cout << mat(i, j) << ", ";
      }
    }
    std::cout << std::endl;
    std::cout << "translation(global -> model)" << std::endl;
    auto vec = Eigen::Vector3d(body.x, body.y, body.z).transpose();
    std::cout << vec.x() << ", " << vec.y() << ", " << vec.z() << std::endl;

    for (int i = 0; i < 100000000; i++) {
      ;
    }
  }

  return 0;
}


#include "Prime.h"
#include <cassert>
#include <thread>
#include <utility>
#include <iostream>

#include "boost/filesystem.hpp"
#include "boost/filesystem/fstream.hpp"
#include <boost/iostreams/device/mapped_file.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/prime.hpp>
#include <boost/multiprecision/miller_rabin.hpp>
#ifndef OPENMIND_PRIME_TABLE_OM
#include <boost/tokenizer.hpp>
#endif

#include <rt/tasq.h>

#define PRIME_LIST_FILENAME "Primes.inc"
#ifdef OPENMIND_PRIME_TABLE_OM
static const boost::multiprecision::cpp_int Primes[] = {
#include "Primes.inc"
};
static const size_t PrimeItems = sizeof(Primes) / sizeof(Primes[0]);
#elif defined(OPENMIND_PRIME_TABLE_BOOST)
static constexpr size_t PrimeItems = boost::math::max_prime + 1;
#else
static_assert(!"Specify primes table");
#endif

namespace {
#ifdef rt_SRC_DIR
boost::filesystem::path PrimeListDir = rt_SRC_DIR;
auto PrimeListPath = PrimeListDir / PRIME_LIST_FILENAME;
#endif
} // namespace

namespace omnn::rt {

auto NextToCheckForPrime() {
    boost::filesystem::ifstream inpt(PrimeListPath);
    char c;
    std::string s;
    inpt.seekg(0, std::ios::end);
    auto pos = inpt.tellg();
    do {
        pos -= 1;
        inpt.seekg(pos, std::ios::beg);
        inpt.get(c);
        if (std::isdigit(c))
            s.insert(s.begin(), c);
    } while (c != ',');
    assert(s.length() > 1);

    return ++boost::multiprecision::cpp_int(s);
}

bool GrowPrime(const boost::multiprecision::cpp_int& upto,
               std::function<bool(boost::multiprecision::cpp_int)> is_prime) {
    static boost::multiprecision::cpp_int from = NextToCheckForPrime();
    auto prev = from - 1;
    if (upto <= prev)
        return {}; // another table is in use

    auto range = upto;
    range -= prev;
    auto chunks = std::thread::hardware_concurrency() - 1; // One thread left free for GC
    auto chunk = range;
    chunk /= chunks;
    if (from + chunk * chunks < upto) {
        ++chunk;
    }
    std::deque<std::future<std::string>> primining;
    std::cout << "new prime table target: " << upto << '(' << from + chunk * chunks << ')'<<std::endl;
    for (decltype(chunks) i = 0; i < chunks; ++i) {
        primining.emplace_back(std::async(std::launch::async, [=]() {
            std::stringstream ss;
            auto j = chunk;
            j *= i;
            j += from;
            auto up = j;
            up += chunk; // NOTE: preserving up type same as j. Expression (j+chunk)  type differs from j type.
            {
                static std::mutex m;
                std::lock_guard l(m);
                std::cout << '[' << j << ',' << up << ')';
            }
            for (; j < up; ++j) {
                if (is_prime(j)) {
                    std::cout << j << ',';
                    ss << j << ',';
                }
            }
            return std::move(ss.str());
        }));
    }

    auto i = -1;
    while (primining.size()) {
        auto line = primining.front().get();
        std::cout << "Chunk " << ++i << std::endl << line << std::endl << "\n chunk is ready. Appending." << std::endl;
        boost::filesystem::ofstream PrimesIncFile(PrimeListPath, std::ios_base::app);
        PrimesIncFile << std::endl << line;
        PrimesIncFile.close();
        primining.pop_front();
    }
    from++ = upto;
    return true;
}

const boost::multiprecision::cpp_int
#ifndef OPENMIND_PRIME_TABLE_BOOST
&
#endif
 prime(size_t idx) {
#ifndef NDEBUG
    auto haveThePrime = idx < PrimeItems;
    if (!haveThePrime) {
        std::cerr << "There is no " << idx
                  << "th prime in the table yet. Use GrowPrime call if you use debug version to increase the table for "
                     "next compile time. Currently the table has "
                  << PrimeItems << " elements" << std::endl;
        assert(haveThePrime);
    }
#endif
#ifdef OPENMIND_PRIME_TABLE_OM
    return Primes[idx];
#elif defined(OPENMIND_PRIME_TABLE_BOOST)
    return boost::math::prime(boost::numeric_cast<uint32_t>(idx));
#else
    static_assert(!"Specify primes table");
#endif // OPENMIND_PRIME_TABLE_BOOST
}

size_t primes() {
#ifdef OPENMIND_PRIME_TABLE_OM
    return PrimeItems - 1;
#elif defined(OPENMIND_PRIME_TABLE_BOOST)
    return boost::math::max_prime;
#else
    static_assert(!"Specify primes table");
#endif // OPENMIND_PRIME_TABLE_BOOST
}

auto GetNextToCheckForPrimeToMine() {
    auto p = NextToCheckForPrime();
    boost::filesystem::ofstream PrimesIncFile(PrimeListPath, std::ios_base::app);
    PrimesIncFile << '\n';
    PrimesIncFile.close();
    return p;
}

namespace {
boost::multiprecision::cpp_int factorial(const boost::multiprecision::cpp_int& n) {
    auto result = n;
    if (n == 0)
        ++result;
    else
        for (auto i = n; --i > 1;)
            result *= i;
    return result;
}
}

void MineNextPrime() {

    static auto next = NextToCheckForPrime();
    static auto i = next; --i;
    static auto prev = i; --prev;
    static auto nFact = factorial(prev);

	for (;
		i < boost::math::max_prime;
		++prev, ++i, ++next)
	{
        nFact *= i;
        auto fastPrimeTest = (prev * (nFact % next));
        if (fastPrimeTest != 0) {
            auto prime = fastPrimeTest / i + 2;
            std::cout << prime << ',';

                //boost::filesystem::ofstream PrimesIncFile(PrimeListPath, std::ios_base::app);
                //PrimesIncFile << ',' << prime;
                //PrimesIncFile.close();
                break;
        }
    }
}

} // namespace omnn::rt


#define BOOST_TEST_MODULE pcraster tab class_interval_map
#include <boost/test/unit_test.hpp>
#include "stddefx.h"
#include "com_statistics.h"
#include "com_intervaltypes.h"
#include "tab_classintervalmap.h"

BOOST_AUTO_TEST_CASE(class_interval_map)
{
  using namespace tab;

  std::vector<const com::Interval<float> *> v;
  v.push_back(new com::LessThan<float>(4));
  v.push_back(new com::GreaterThan<float>(4));

  {
   typedef ClassIntervalMap< com::IntervalMap< com::AverageSdMinMax< >, float > > M;
   M m(v);
   m.addClass(3);
   m.addClass(1);

   m.visit(1, 2);
   m.visit(1, 4); // in outside!
   m.visit(2, 2); // class implicit added
   m.visit(3, 2);
   m.visit(3, 5);

   size_t c=0;
   for(M::const_iterator i=m.begin();i!=m.end();++i) {
     BOOST_CHECK(i->second.size()==2); // < 4  and > 4
     switch(c) {
       case 0:
         BOOST_CHECK(i->first==1);
         BOOST_CHECK(i->second.outside().nr()==1);
         BOOST_CHECK(i->second.outside().maximum()==4);
         break;
       case 1:
         BOOST_CHECK(i->first==2);
         BOOST_CHECK(i->second.outside().nr()==0);
         break;
       case 2:
         BOOST_CHECK(i->first==3);
         BOOST_CHECK(i->second.outside().nr()==0);
         break;
     }
     c++;
   }
  }

  {
   v.push_back(new com::GreaterThan<float>(4.5));
   typedef tab::ClassIntervalMap< com::IntervalMultiMap< com::AverageSdMinMax<>,float > > M;
   M m(v);
   m.addClass(3);
   m.addClass(1);

   m.visit(1, 2);
   m.visit(1, 4); // in outside!
   m.visit(2, 2); // class implicit added
   m.visit(3, 2);
   m.visit(3, 5);

   size_t c=0;
   for(M::const_iterator i=m.begin();i!=m.end();++i) {
     BOOST_CHECK(i->second.size()==3); // < 4, > 4 > 4.5
     switch(c) {
       case 0:
         BOOST_CHECK(i->first==1);
         BOOST_CHECK(i->second.outside().nr()==1);
         BOOST_CHECK(i->second.outside().maximum()==4);
         break;
       case 1:
         BOOST_CHECK(i->first==2);
         BOOST_CHECK(i->second.outside().nr()==0);
         break;
       case 2:
         BOOST_CHECK(i->first==3);
         BOOST_CHECK(i->second.outside().nr()==0);
         break;
     }
     c++;
   }
  }
  clearClone(v);
}


// Copyright (c) 2014, NICTA.
// This file is licensed under the General Public License version 3 or later.
// See the COPYRIGHT file.

/**
 * The prior module produces distributions over model parameters.
 * Other parts of the program can sample and evaluate probabilities
 * that are mapped into an associated real vector space. In other words,
 * we 'vectorize' objects to a 'theta' vector of doubles, but only the parts
 * that are changing.
 *
 * @file prior.hpp
 * @author Lachlan McCalman
 * @date 2014
 */

#pragma once

// Standard Library
#include <string>
// Prerequisites
#include <glog/logging.h>
#include <Eigen/Core>
// Project
#include "datatype/datatypes.hpp"
#include "prior/world.hpp"
#include "prior/gravity.hpp"
#include "prior/magnetic.hpp"
#include "prior/mt.hpp"
#include "prior/seismic.hpp"
#include "prior/contactpoint.hpp"
#include "prior/thermal.hpp"
#include <random>

namespace obsidian
{
  class GlobalPrior
  {
  public:
//      GlobalPrior(const std::vector<distrib::MultiGaussian>& ctrlpts,
//                  const std::vector<Eigen::MatrixXi>& ctrlptMasks,
//                  const std::vector<distrib::MultiGaussian>& properties,
//                  const std::vector<Eigen::VectorXi>& propMasks);

    GlobalParams reconstruct(const Eigen::VectorXd& theta);
    double evaluate(const Eigen::VectorXd& theta);
    Eigen::VectorXd sample(std::mt19937 &gen);

    uint size();

//    private:
    prior::WorldParamsPrior world;
    prior::GravParamsPrior grav;
    prior::MagParamsPrior mag;
    prior::MtAnisoParamsPrior mt;
    prior::Seismic1dParamsPrior s1d;
    prior::ContactPointParamsPrior cpt;
    prior::ThermalParamsPrior thr;
  };

} // namespace obsidian


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_REDUCTION_FUNCTION_SIMD_SSE_SSSE3_SUM_HPP_INCLUDED
#define NT2_TOOLBOX_REDUCTION_FUNCTION_SIMD_SSE_SSSE3_SUM_HPP_INCLUDED
#include <nt2/sdk/constant/digits.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/sdk/meta/strip.hpp>

#include <nt2/include/functions/first.hpp>
#include <nt2/include/functions/second.hpp>
#include <boost/fusion/include/fold.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is int16_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::sum_, tag::cpu_,
                      (A0),
                      ((simd_<ints16_<A0>,tag::sse_>))
                     );

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::sum_(tag::simd_<tag::ints16_, tag::sse_> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename meta::scalar_of<A0>::type                 base;
      typedef typename std::tr1::result_of<meta::arithmetic(base)>::type  type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      A0 tmp = {_mm_hadd_epi16(a0, a0)};
      A0 tmp1 = {_mm_hadd_epi16(tmp, tmp)};
      return  first(simd::native_cast<A0>(_mm_hadd_epi16(tmp1, tmp1)));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::sum_, tag::cpu_,
                      (A0),
                      ((simd_<double_<A0>,tag::sse_>))
                     );

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::sum_(tag::simd_<tag::double_, tag::sse_> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename meta::scalar_of<A0>::type                 base;
      typedef typename std::tr1::result_of<meta::arithmetic(base)>::type  type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      return first(simd::native_cast<A0>(_mm_hadd_pd( a0, Zero<A0>())));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is int64_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::sum_, tag::cpu_,
                      (A0),
                      ((simd_<ints64_<A0>,tag::sse_>))
                     );

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::sum_(tag::simd_<tag::ints64_, tag::sse_> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename meta::scalar_of<A0>::type                 base;
      typedef typename std::tr1::result_of<meta::arithmetic(base)>::type  type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      return first(a0)+second(a0);
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::sum_, tag::cpu_,
                      (A0),
                      ((simd_<float_<A0>,tag::sse_>))
                     );

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::sum_(tag::simd_<tag::float_, tag::sse_> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename meta::scalar_of<A0>::type                 base;
      typedef typename std::tr1::result_of<meta::arithmetic(base)>::type  type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      A0 tmp = {_mm_hadd_ps(a0, Zero<A0>())};
      return  first(simd::native_cast<A0>(_mm_hadd_ps(tmp, Zero<A0>())));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is int8_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::sum_, tag::cpu_,
                      (A0),
                      ((simd_<ints8_<A0>,tag::sse_>))
                     );

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::sum_(tag::simd_<tag::ints8_, tag::sse_> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename meta::scalar_of<A0>::type                 base;
      typedef typename std::tr1::result_of<meta::arithmetic(base)>::type  type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      return boost::fusion::fold(a0,0,functor<tag::plus_>());
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is int32_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::sum_, tag::cpu_,
                      (A0),
                      ((simd_<ints32_<A0>,tag::sse_>))
                     );

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::sum_(tag::simd_<tag::ints32_, tag::sse_> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename meta::scalar_of<A0>::type                 base;
      typedef typename std::tr1::result_of<meta::arithmetic(base)>::type  type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      A0 tmp = {_mm_hadd_epi32(a0, a0)};
      return  first(simd::native_cast<A0>(_mm_hadd_epi32(tmp, tmp)));
    }
  };
} }

#endif
// modified by jt the 05/01/2011