// Copyright Nick Thompson, 2019
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)
#ifndef BOOST_MATH_QUADRATURE_DETAIL_OOURA_FOURIER_INTEGRALS_DETAIL_HPP
#define BOOST_MATH_QUADRATURE_DETAIL_OOURA_FOURIER_INTEGRALS_DETAIL_HPP
#include <utility> // for std::pair.
#include <mutex>
#include <atomic>
#include <vector>
#include <iostream>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/math/special_functions/sin_pi.hpp>
#include <boost/math/special_functions/cos_pi.hpp>
#include <boost/math/constants/constants.hpp>

namespace boost { namespace math { namespace quadrature { namespace detail {

// Ooura and Mori, A robust double exponential formula for Fourier-type integrals,
// eta is the argument to the exponential in equation 3.3:
template<class Real>
std::pair<Real, Real> ooura_eta(Real x, Real alpha) {
    using std::expm1;
    using std::exp;
    using std::abs;
    Real expx = exp(x);
    Real eta_prime = 2 + alpha/expx + expx/4;
    Real eta;
    // This is the fast branch:
    if (abs(x) > 0.125) {
        eta = 2*x - alpha*(1/expx - 1) + (expx - 1)/4;
    }
    else {// this is the slow branch using expm1 for small x:
        eta = 2*x - alpha*expm1(-x) + expm1(x)/4;
    }
    return {eta, eta_prime};
}

// Ooura and Mori, A robust double exponential formula for Fourier-type integrals,
// equation 3.6:
template<class Real>
Real calculate_ooura_alpha(Real h)
{
    using boost::math::constants::pi;
    using std::log1p;
    using std::sqrt;
    Real x = sqrt(16 + 4*log1p(pi<Real>()/h)/h);
    return 1/x;
}

template<class Real>
std::pair<Real, Real> ooura_sin_node_and_weight(long n, Real h, Real alpha)
{
    using std::expm1;
    using std::exp;
    using std::abs;
    using boost::math::constants::pi;
    using std::isnan;

    if (n == 0) {
        // Equation 44 of https://arxiv.org/pdf/0911.4796.pdf
        // Fourier Transform of the Stretched Exponential Function: Analytic Error Bounds,
        // Double Exponential Transform, and Open-Source Implementation,
        // Joachim Wuttke,
        // The C library libkww provides functions to compute the Kohlrausch-Williams-Watts function,
        // the Laplace-Fourier transform of the stretched (or compressed) exponential function exp(-t^beta)
        // for exponent beta between 0.1 and 1.9 with sixteen decimal digits accuracy.

        Real eta_prime_0 = Real(2) + alpha + Real(1)/Real(4);
        Real node = pi<Real>()/(eta_prime_0*h);
        Real weight = pi<Real>()*boost::math::sin_pi(1/(eta_prime_0*h));
        Real eta_dbl_prime = -alpha + Real(1)/Real(4);
        Real phi_prime_0 = (1 - eta_dbl_prime/(eta_prime_0*eta_prime_0))/2;
        weight *= phi_prime_0;
        return {node, weight};
    }
    Real x = n*h;
    auto p = ooura_eta(x, alpha);
    auto eta = p.first;
    auto eta_prime = p.second;

    Real expm1_meta = expm1(-eta);
    Real exp_meta = exp(-eta);
    Real node = -n*pi<Real>()/expm1_meta;

    // I have verified that this is not a significant source of inaccuracy in the weight computation:
    Real phi_prime = -(expm1_meta + x*exp_meta*eta_prime)/(expm1_meta*expm1_meta);

    // The main source of inaccuracy is in computation of sin_pi.
    // But I've agonized over this, and I think it's as good as it can get:
    Real s = pi<Real>();
    Real arg;
    if(eta > 1) {
        arg = n/( 1/exp_meta - 1 );
        s *= boost::math::sin_pi(arg);
        if (n&1) {
            s *= -1;
        }
    }
    else if (eta < -1) {
        arg = n/(1-exp_meta);
        s *= boost::math::sin_pi(arg);
    }
    else {
        arg = -n*exp_meta/expm1_meta;
        s *= boost::math::sin_pi(arg);
        if (n&1) {
            s *= -1;
        }
    }

    Real weight = s*phi_prime;
    return {node, weight};
}

#ifdef BOOST_MATH_INSTRUMENT_OOURA
template<class Real>
void print_ooura_estimate(size_t i, Real I0, Real I1, Real omega) {
    using std::abs;
    std::cout << std::defaultfloat
              << std::setprecision(std::numeric_limits<Real>::digits10)
              << std::fixed;
    std::cout << "h = " << Real(1)/Real(1<<i) << ", I_h = " << I0/omega
              << " = " << std::hexfloat << I0/omega << ", absolute error estimate = "
              << std::defaultfloat << std::scientific << abs(I0-I1)  << std::endl;
}
#endif

template<class Real>
std::pair<Real, Real> ooura_cos_node_and_weight(long n, Real h, Real alpha)
{
    using std::expm1;
    using std::exp;
    using std::abs;
    using boost::math::constants::pi;

    Real x = h*(n-Real(1)/Real(2));
    auto p = ooura_eta(x, alpha);
    auto eta = p.first;
    auto eta_prime = p.second;
    Real expm1_meta = expm1(-eta);
    Real exp_meta = exp(-eta);
    Real node = pi<Real>()*(Real(1)/Real(2)-n)/expm1_meta;

    Real phi_prime = -(expm1_meta + x*exp_meta*eta_prime)/(expm1_meta*expm1_meta);

    // Takuya Ooura and Masatake Mori,
    // Journal of Computational and Applied Mathematics, 112 (1999) 229-241.
    // A robust double exponential formula for Fourier-type integrals.
    // Equation 4.6
    Real s = pi<Real>();
    Real arg;
    if (eta < -1) {
        arg = -(n-Real(1)/Real(2))/expm1_meta;
        s *= boost::math::cos_pi(arg);
    }
    else {
        arg = -(n-Real(1)/Real(2))*exp_meta/expm1_meta;
        s *= boost::math::sin_pi(arg);
        if (n&1) {
            s *= -1;
        }
    }

    Real weight = s*phi_prime;
    return {node, weight};
}

template<class Real>
class ooura_fourier_sin_detail {
public:
    ooura_fourier_sin_detail(const Real relative_error_goal, size_t levels) {
#ifdef BOOST_MATH_INSTRUMENT_OOURA
      std::cout << "ooura_fourier_sin with relative error goal " << relative_error_goal
        << " & " << levels << " levels." << std::endl;
#endif // BOOST_MATH_INSTRUMENT_OOURA
        if (relative_error_goal < std::numeric_limits<Real>::epsilon() * 2) {
            throw std::domain_error("The relative error goal cannot be smaller than the unit roundoff.");
        }
        using std::abs;
        requested_levels_ = levels;
        starting_level_ = 0;
        rel_err_goal_ = relative_error_goal;
        big_nodes_.reserve(levels);
        bweights_.reserve(levels);
        little_nodes_.reserve(levels);
        lweights_.reserve(levels);

        for (size_t i = 0; i < levels; ++i) {
            if (std::is_same<Real, float>::value) {
                add_level<double>(i);
            }
            else if (std::is_same<Real, double>::value) {
                add_level<long double>(i);
            }
            else {
                add_level<Real>(i);
            }
        }
    }

    std::vector<std::vector<Real>> const & big_nodes() const {
        return big_nodes_;
    }

    std::vector<std::vector<Real>> const & weights_for_big_nodes() const {
        return bweights_;
    }

    std::vector<std::vector<Real>> const & little_nodes() const {
        return little_nodes_;
    }

    std::vector<std::vector<Real>> const & weights_for_little_nodes() const {
        return lweights_;
    }

    template<class F>
    std::pair<Real,Real> integrate(F const & f, Real omega) {
        using std::abs;
        using std::max;
        using boost::math::constants::pi;

        if (omega == 0) {
            return {Real(0), Real(0)};
        }
        if (omega < 0) {
            auto p = this->integrate(f, -omega);
            return {-p.first, p.second};
        }

        Real I1 = std::numeric_limits<Real>::quiet_NaN();
        Real relative_error_estimate = std::numeric_limits<Real>::quiet_NaN();
        // As we compute integrals, we learn about their structure.
        // Assuming we compute f(t)sin(wt) for many different omega, this gives some
        // a posteriori ability to choose a refinement level that is roughly appropriate.
        size_t i = starting_level_;
        do {
            Real I0 = estimate_integral(f, omega, i);
#ifdef BOOST_MATH_INSTRUMENT_OOURA
            print_ooura_estimate(i, I0, I1, omega);
#endif
            Real absolute_error_estimate = abs(I0-I1);
            Real scale = (max)(abs(I0), abs(I1));
            if (!isnan(I1) && absolute_error_estimate <= rel_err_goal_*scale) {
                starting_level_ = (max)(long(i) - 1, long(0));
                return {I0/omega, absolute_error_estimate/scale};
            }
            I1 = I0;
        } while(++i < big_nodes_.size());

        // We've used up all our precomputed levels.
        // Now we need to add more.
        // It might seems reasonable to just keep adding levels indefinitely, if that's what the user wants.
        // But in fact the nodes and weights just merge into each other and the error gets worse after a certain number.
        // This value for max_additional_levels was chosen by observation of a slowly converging oscillatory integral:
        // f(x) := cos(7cos(x))sin(x)/x
        size_t max_additional_levels = 4;
        while (big_nodes_.size() < requested_levels_ + max_additional_levels) {
            size_t ii = big_nodes_.size();
            if (std::is_same<Real, float>::value) {
                add_level<double>(ii);
            }
            else if (std::is_same<Real, double>::value) {
                add_level<long double>(ii);
            }
            else {
                add_level<Real>(ii);
            }
            Real I0 = estimate_integral(f, omega, ii);
            Real absolute_error_estimate = abs(I0-I1);
            Real scale = (max)(abs(I0), abs(I1));
#ifdef BOOST_MATH_INSTRUMENT_OOURA
            print_ooura_estimate(ii, I0, I1, omega);
#endif
            if (absolute_error_estimate <= rel_err_goal_*scale) {
                starting_level_ = (max)(long(ii) - 1, long(0));
                return {I0/omega, absolute_error_estimate/scale};
            }
            I1 = I0;
            ++ii;
        }

        starting_level_ = static_cast<long>(big_nodes_.size() - 2);
        return {I1/omega, relative_error_estimate};
    }

private:

    template<class PreciseReal>
    void add_level(size_t i) {
        using std::abs;
        size_t current_num_levels = big_nodes_.size();
        Real unit_roundoff = std::numeric_limits<Real>::epsilon()/2;
        // h0 = 1. Then all further levels have h_i = 1/2^i.
        // Since the nodes don't nest, we could conceivably divide h by (say) 1.5, or 3.
        // It's not clear how much benefit (or loss) would be obtained from this.
        PreciseReal h = PreciseReal(1)/PreciseReal(1<<i);

        std::vector<Real> bnode_row;
        std::vector<Real> bweight_row;

        // This is a pretty good estimate for how many elements will be placed in the vector:
        bnode_row.reserve((static_cast<size_t>(1)<<i)*sizeof(Real));
        bweight_row.reserve((static_cast<size_t>(1)<<i)*sizeof(Real));

        std::vector<Real> lnode_row;
        std::vector<Real> lweight_row;

        lnode_row.reserve((static_cast<size_t>(1)<<i)*sizeof(Real));
        lweight_row.reserve((static_cast<size_t>(1)<<i)*sizeof(Real));

        Real max_weight = 1;
        auto alpha = calculate_ooura_alpha(h);
        long n = 0;
        Real w;
        do {
            auto precise_nw = ooura_sin_node_and_weight(n, h, alpha);
            Real node = static_cast<Real>(precise_nw.first);
            Real weight = static_cast<Real>(precise_nw.second);
            w = weight;
            if (bnode_row.size() == bnode_row.capacity()) {
                bnode_row.reserve(2*bnode_row.size());
                bweight_row.reserve(2*bnode_row.size());
            }

            bnode_row.push_back(node);
            bweight_row.push_back(weight);
            if (abs(weight) > max_weight) {
                max_weight = abs(weight);
            }
            ++n;
            // f(t)->0 as t->infty, which is why the weights are computed up to the unit roundoff.
        } while(abs(w) > unit_roundoff*max_weight);

        // This class tends to consume a lot of memory; shrink the vectors back down to size:
        bnode_row.shrink_to_fit();
        bweight_row.shrink_to_fit();
        // Why we are splitting the nodes into regimes where t_n >> 1 and t_n << 1?
        // It will create the opportunity to sensibly truncate the quadrature sum to significant terms.
        n = -1;
        do {
            auto precise_nw = ooura_sin_node_and_weight(n, h, alpha);
            Real node = static_cast<Real>(precise_nw.first);
            if (node <= 0) {
                break;
            }
            Real weight = static_cast<Real>(precise_nw.second);
            w = weight;
            using std::isnan;
            if (isnan(node)) {
                // This occurs at n = -11 in quad precision:
                break;
            }
            if (lnode_row.size() > 0) {
                if (lnode_row[lnode_row.size()-1] == node) {
                    // The nodes have fused into each other:
                    break;
                }
            }
            if (lnode_row.size() == lnode_row.capacity()) {
                lnode_row.reserve(2*lnode_row.size());
                lweight_row.reserve(2*lnode_row.size());
            }
            lnode_row.push_back(node);
            lweight_row.push_back(weight);
            if (abs(weight) > max_weight) {
                max_weight = abs(weight);
            }
            --n;
            // f(t)->infty is possible as t->0, hence compute up to the min.
        } while(abs(w) > (std::numeric_limits<Real>::min)()*max_weight);

        lnode_row.shrink_to_fit();
        lweight_row.shrink_to_fit();

        // std::scoped_lock once C++17 is more common?
        std::lock_guard<std::mutex> lock(node_weight_mutex_);
        // Another thread might have already finished this calculation and appended it to the nodes/weights:
        if (current_num_levels == big_nodes_.size()) {
            big_nodes_.push_back(bnode_row);
            bweights_.push_back(bweight_row);

            little_nodes_.push_back(lnode_row);
            lweights_.push_back(lweight_row);
        }
    }

    template<class F>
    Real estimate_integral(F const & f, Real omega, size_t i) {
        // Because so few function evaluations are required to get high accuracy on the integrals in the tests,
        // Kahan summation doesn't really help.
        //auto cond = boost::math::tools::summation_condition_number<Real, true>(0);
        Real I0 = 0;
        auto const & b_nodes = big_nodes_[i];
        auto const & b_weights = bweights_[i];
        // Will benchmark if this is helpful:
        Real inv_omega = 1/omega;
        for(size_t j = 0 ; j < b_nodes.size(); ++j) {
            I0 += f(b_nodes[j]*inv_omega)*b_weights[j];
        }

        auto const & l_nodes = little_nodes_[i];
        auto const & l_weights = lweights_[i];
        // If f decays rapidly as |t|->infty, not all of these calls are necessary.
        for (size_t j = 0; j < l_nodes.size(); ++j) {
            I0 += f(l_nodes[j]*inv_omega)*l_weights[j];
        }
        return I0;
    }

    std::mutex node_weight_mutex_;
    // Nodes for n >= 0, giving t_n = pi*phi(nh)/h. Generally t_n >> 1.
    std::vector<std::vector<Real>> big_nodes_;
    // The term bweights_ will indicate that these are weights corresponding
    // to the big nodes:
    std::vector<std::vector<Real>> bweights_;

    // Nodes for n < 0: Generally t_n << 1, and an invariant is that t_n > 0.
    std::vector<std::vector<Real>> little_nodes_;
    std::vector<std::vector<Real>> lweights_;
    Real rel_err_goal_;
    std::atomic<long> starting_level_;
    size_t requested_levels_;
};

template<class Real>
class ooura_fourier_cos_detail {
public:
    ooura_fourier_cos_detail(const Real relative_error_goal, size_t levels) {
#ifdef BOOST_MATH_INSTRUMENT_OOURA
      std::cout << "ooura_fourier_cos with relative error goal " << relative_error_goal
        << " & " << levels << " levels." << std::endl;
      std::cout << "epsilon for type = " << std::numeric_limits<Real>::epsilon() << std::endl;
#endif // BOOST_MATH_INSTRUMENT_OOURA
        if (relative_error_goal < std::numeric_limits<Real>::epsilon() * 2) {
            throw std::domain_error("The relative error goal cannot be smaller than the unit roundoff!");
        }

        using std::abs;
        requested_levels_ = levels;
        starting_level_ = 0;
        rel_err_goal_ = relative_error_goal;
        big_nodes_.reserve(levels);
        bweights_.reserve(levels);
        little_nodes_.reserve(levels);
        lweights_.reserve(levels);

        for (size_t i = 0; i < levels; ++i) {
            if (std::is_same<Real, float>::value) {
                add_level<double>(i);
            }
            else if (std::is_same<Real, double>::value) {
                add_level<long double>(i);
            }
            else {
                add_level<Real>(i);
            }
        }

    }

    template<class F>
    std::pair<Real,Real> integrate(F const & f, Real omega) {
        using std::abs;
        using std::max;
        using boost::math::constants::pi;

        if (omega == 0) {
            throw std::domain_error("At omega = 0, the integral is not oscillatory. The user must choose an appropriate method for this case.\n");
        }

        if (omega < 0) {
            return this->integrate(f, -omega);
        }

        Real I1 = std::numeric_limits<Real>::quiet_NaN();
        Real absolute_error_estimate = std::numeric_limits<Real>::quiet_NaN();
        Real scale = std::numeric_limits<Real>::quiet_NaN();
        size_t i = starting_level_;
        do {
            Real I0 = estimate_integral(f, omega, i);
#ifdef BOOST_MATH_INSTRUMENT_OOURA
            print_ooura_estimate(i, I0, I1, omega);
#endif
            absolute_error_estimate = abs(I0-I1);
            scale = (max)(abs(I0), abs(I1));
            if (!isnan(I1) && absolute_error_estimate <= rel_err_goal_*scale) {
                starting_level_ = (max)(long(i) - 1, long(0));
                return {I0/omega, absolute_error_estimate/scale};
            }
            I1 = I0;
        } while(++i < big_nodes_.size());

        size_t max_additional_levels = 4;
        while (big_nodes_.size() < requested_levels_ + max_additional_levels) {
            size_t ii = big_nodes_.size();
            if (std::is_same<Real, float>::value) {
                add_level<double>(ii);
            }
            else if (std::is_same<Real, double>::value) {
                add_level<long double>(ii);
            }
            else {
                add_level<Real>(ii);
            }
            Real I0 = estimate_integral(f, omega, ii);
#ifdef BOOST_MATH_INSTRUMENT_OOURA
            print_ooura_estimate(ii, I0, I1, omega);
#endif
            absolute_error_estimate = abs(I0-I1);
            scale = (max)(abs(I0), abs(I1));
            if (absolute_error_estimate <= rel_err_goal_*scale) {
                starting_level_ = (max)(long(ii) - 1, long(0));
                return {I0/omega, absolute_error_estimate/scale};
            }
            I1 = I0;
            ++ii;
        }

        starting_level_ = static_cast<long>(big_nodes_.size() - 2);
        return {I1/omega, absolute_error_estimate/scale};
    }

private:

    template<class PreciseReal>
    void add_level(size_t i) {
        using std::abs;
        size_t current_num_levels = big_nodes_.size();
        Real unit_roundoff = std::numeric_limits<Real>::epsilon()/2;
        PreciseReal h = PreciseReal(1)/PreciseReal(1<<i);

        std::vector<Real> bnode_row;
        std::vector<Real> bweight_row;
        bnode_row.reserve((static_cast<size_t>(1)<<i)*sizeof(Real));
        bweight_row.reserve((static_cast<size_t>(1)<<i)*sizeof(Real));

        std::vector<Real> lnode_row;
        std::vector<Real> lweight_row;

        lnode_row.reserve((static_cast<size_t>(1)<<i)*sizeof(Real));
        lweight_row.reserve((static_cast<size_t>(1)<<i)*sizeof(Real));

        Real max_weight = 1;
        auto alpha = calculate_ooura_alpha(h);
        long n = 0;
        Real w;
        do {
            auto precise_nw = ooura_cos_node_and_weight(n, h, alpha);
            Real node = static_cast<Real>(precise_nw.first);
            Real weight = static_cast<Real>(precise_nw.second);
            w = weight;
            if (bnode_row.size() == bnode_row.capacity()) {
                bnode_row.reserve(2*bnode_row.size());
                bweight_row.reserve(2*bnode_row.size());
            }

            bnode_row.push_back(node);
            bweight_row.push_back(weight);
            if (abs(weight) > max_weight) {
                max_weight = abs(weight);
            }
            ++n;
            // f(t)->0 as t->infty, which is why the weights are computed up to the unit roundoff.
        } while(abs(w) > unit_roundoff*max_weight);

        bnode_row.shrink_to_fit();
        bweight_row.shrink_to_fit();
        n = -1;
        do {
            auto precise_nw = ooura_cos_node_and_weight(n, h, alpha);
            Real node = static_cast<Real>(precise_nw.first);
            // The function cannot be singular at zero,
            // so zero is not a unreasonable node,
            // unlike in the case of the Fourier Sine.
            // Hence only break if the node is negative.
            if (node < 0) {
                break;
            }
            Real weight = static_cast<Real>(precise_nw.second);
            w = weight;
            if (lnode_row.size() > 0) {
                if (lnode_row.back() == node) {
                    // The nodes have fused into each other:
                    break;
                }
            }
            if (lnode_row.size() == lnode_row.capacity()) {
                lnode_row.reserve(2*lnode_row.size());
                lweight_row.reserve(2*lnode_row.size());
            }

            lnode_row.push_back(node);
            lweight_row.push_back(weight);
            if (abs(weight) > max_weight) {
                max_weight = abs(weight);
            }
            --n;
        } while(abs(w) > (std::numeric_limits<Real>::min)()*max_weight);

        lnode_row.shrink_to_fit();
        lweight_row.shrink_to_fit();

        std::lock_guard<std::mutex> lock(node_weight_mutex_);
        // Another thread might have already finished this calculation and appended it to the nodes/weights:
        if (current_num_levels == big_nodes_.size()) {
            big_nodes_.push_back(bnode_row);
            bweights_.push_back(bweight_row);

            little_nodes_.push_back(lnode_row);
            lweights_.push_back(lweight_row);
        }
    }

    template<class F>
    Real estimate_integral(F const & f, Real omega, size_t i) {
        Real I0 = 0;
        auto const & b_nodes = big_nodes_[i];
        auto const & b_weights = bweights_[i];
        Real inv_omega = 1/omega;
        for(size_t j = 0 ; j < b_nodes.size(); ++j) {
            I0 += f(b_nodes[j]*inv_omega)*b_weights[j];
        }

        auto const & l_nodes = little_nodes_[i];
        auto const & l_weights = lweights_[i];
        for (size_t j = 0; j < l_nodes.size(); ++j) {
            I0 += f(l_nodes[j]*inv_omega)*l_weights[j];
        }
        return I0;
    }

    std::mutex node_weight_mutex_;
    std::vector<std::vector<Real>> big_nodes_;
    std::vector<std::vector<Real>> bweights_;

    std::vector<std::vector<Real>> little_nodes_;
    std::vector<std::vector<Real>> lweights_;
    Real rel_err_goal_;
    std::atomic<long> starting_level_;
    size_t requested_levels_;
};

}}}}
#endif


/* Copyright (C) 2012-2020 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
#include <algorithm>
#include <tuple>
#include <NTL/BasicThreadPool.h>

#include <helib/helib.h>

#include <helib/intraSlot.h>
#include <helib/binaryArith.h>
#include <helib/binaryCompare.h>

#include "gtest/gtest.h"
#include "test_common.h"

#include <helib/debugging.h>

// define flags FLAG_PRINT_ZZX, FLAG_PRINT_POLY, FLAG_PRINT_VEC, functions
//        decryptAndPrint(ostream, ctxt, sk, ea, flags)
//        decryptAndCompare(ctxt, sk, ea, pa);

namespace {

struct Parameters
{
  Parameters(long prm, long bitSize, bool bootstrap, long seed, long nthreads) :
      prm(prm),
      bitSize(bitSize),
      bootstrap(bootstrap),
      seed(seed),
      nthreads(nthreads){};

  long prm;       // parameter size (0-tiny,...,4-huge)
  long bitSize;   // bitSize of input integers (<=32)
  bool bootstrap; // test comparison with bootstrapping
  long seed;      // PRG seed
  long nthreads;  // number of threads

  // Tell google test how to print Parameters
  friend std::ostream& operator<<(std::ostream& os, const Parameters& params)
  {
    return os << "{"
              << "prm=" << params.prm << ","
              << "bitSize=" << params.bitSize << ","
              << "bootstrap=" << params.bootstrap << ","
              << "seed=" << params.seed << ","
              << "nthreads=" << params.nthreads << "}";
  };
};

class GTestBinaryCompare :
    public ::testing::TestWithParam<std::tuple<Parameters, int>>
{
protected:
  static std::vector<helib::zzX> unpackSlotEncoding;
  constexpr static long mValues[][15] = {
      // { p, phi(m),   m,   d, m1, m2, m3,    g1,   g2,   g3, ord1,ord2,ord3,
      // B,c}
      {2, 48, 105, 12, 3, 35, 0, 71, 76, 0, 2, 2, 0, 25, 2},
      {2, 600, 1023, 10, 11, 93, 0, 838, 584, 0, 10, 6, 0, 25, 2},
      {2, 2304, 4641, 24, 7, 3, 221, 3979, 3095, 3760, 6, 2, -8, 25, 3},
      {2, 15004, 15709, 22, 23, 683, 0, 4099, 13663, 0, 22, 31, 0, 25, 3},
      // clang-format off
      {2, 27000, 32767, 15, 31, 7, 151, 11628, 28087, 25824, 30, 6, -10, 28, 4}
      // clang-format on
  };

  static long correctBitSize(long minimum, long oldBitSize)
  {
    long newBitSize;
    if (oldBitSize <= 0)
      newBitSize = minimum;
    else if (oldBitSize > 32)
      newBitSize = 32;
    else
      newBitSize = oldBitSize;
    return newBitSize;
  };

  // Validates the prm value, throwing if invalid
  static long validatePrm(long prm)
  {
    if (prm < 0 || prm >= 5)
      throw std::invalid_argument("prm must be in the interval [0, 4]");
    return prm;
  };

  static NTL::Vec<long> calculateMvec(const long* vals)
  {
    NTL::Vec<long> mvec;
    append(mvec, vals[4]);
    if (vals[5] > 1)
      append(mvec, vals[5]);
    if (vals[6] > 1)
      append(mvec, vals[6]);
    return mvec;
  };

  static std::vector<long> calculateGens(const long* vals)
  {
    std::vector<long> gens;
    gens.push_back(vals[7]);
    if (vals[8] > 1)
      gens.push_back(vals[8]);
    if (vals[9] > 1)
      gens.push_back(vals[9]);
    return gens;
  };

  static std::vector<long> calculateOrds(const long* vals)
  {
    std::vector<long> ords;
    ords.push_back(vals[10]);
    if (abs(vals[11]) > 1)
      ords.push_back(vals[11]);
    if (abs(vals[12]) > 1)
      ords.push_back(vals[12]);
    return ords;
  };

  static long calculateLevels(bool bootstrap, long bitSize)
  {
    return bootstrap
               ? 900
               : 30 * (7 + NTL::NumBits(bitSize + 2)); // that should be enough
  };

  helib::Context& prepareContext(helib::Context& context)
  {
    if (helib_test::verbose) {
      std::cout << "input bitSize=" << bitSize << std::endl;
      if (nthreads > 1)
        std::cout << "  using " << NTL::AvailableThreads() << " threads\n";
      std::cout << "computing key-independent tables..." << std::flush;
    }
    buildModChain(context, L, c, /*willBeBootstrappable=*/bootstrap);
    if (bootstrap) {
      context.enableBootStrapping(mvec);
    }
    buildUnpackSlotEncoding(unpackSlotEncoding, *context.ea);

    if (helib_test::verbose) {
      std::cout << " done.\n";
      context.zMStar.printout();
    }

    return context;
  }

  void prepareSecKey(helib::SecKey& secKey)
  {
    if (helib_test::verbose) {
      std::cout << "\ncomputing key-dependent tables..." << std::flush;
    }
    secKey.GenSecKey();
    addSome1DMatrices(secKey); // compute key-switching matrices
    addFrbMatrices(secKey);
    if (bootstrap)
      secKey.genRecryptData();
    if (helib_test::verbose)
      std::cout << " done\n";
  };

  const long prm;
  const long bitSize;
  const bool bootstrap;
  const long seed;
  const long nthreads;
  const long* vals;
  const long p;
  const long m;
  const NTL::Vec<long> mvec;
  const std::vector<long> gens;
  const std::vector<long> ords;
  const long c;
  const long L;
  helib::Context context;
  helib::SecKey secKey;

  GTestBinaryCompare() :
      prm(validatePrm(std::get<0>(GetParam()).prm)),
      bitSize(correctBitSize(5, std::get<0>(GetParam()).bitSize)),
      bootstrap(std::get<0>(GetParam()).bootstrap),
      seed(std::get<0>(GetParam()).seed),
      nthreads(std::get<0>(GetParam()).nthreads),
      vals(mValues[prm]),
      p(vals[0]),
      m(vals[2]),
      mvec(calculateMvec(vals)),
      gens(calculateGens(vals)),
      ords(calculateOrds(vals)),
      c(vals[14]),
      L(calculateLevels(bootstrap, bitSize)),
      context(m, p, /*r=*/1, gens, ords),
      secKey(prepareContext(context)){};

  void SetUp() override
  {
    if (seed)
      NTL::SetSeed(NTL::ZZ(seed));
    if (nthreads > 1)
      NTL::SetNumThreads(nthreads);

    prepareSecKey(secKey);

    helib::activeContext = &context; // make things a little easier sometimes

    helib::setupDebugGlobals(&secKey, context.ea);
  };

  virtual void TearDown() override
  {
#ifdef HELIB_DEBUG
    helib::cleanupDebugGlobals();
#endif
  }

public:
  static void TearDownTestCase()
  {
    if (helib_test::verbose)
      helib::printAllTimers(std::cout);
  };
};

std::vector<helib::zzX> GTestBinaryCompare::unpackSlotEncoding;
constexpr long GTestBinaryCompare::mValues[5][15];

TEST_P(GTestBinaryCompare, comparison)
{
  const helib::EncryptedArray& ea = *context.ea;

  // Choose two random n-bit integers
  long pa = NTL::RandomBits_long(bitSize);
  long pb = NTL::RandomBits_long(bitSize + 1);
  long pMax = std::max(pa, pb);
  long pMin = std::min(pa, pb);
  bool pMu = pa > pb;
  bool pNi = pa < pb;

  // Encrypt the individual bits
  NTL::Vec<helib::Ctxt> eMax, eMin, enca, encb;

  helib::Ctxt mu(secKey), ni(secKey);
  resize(enca, bitSize, mu);
  resize(encb, bitSize + 1, ni);
  for (long i = 0; i <= bitSize; i++) {
    if (i < bitSize)
      secKey.Encrypt(enca[i], NTL::ZZX((pa >> i) & 1));
    secKey.Encrypt(encb[i], NTL::ZZX((pb >> i) & 1));
    if (bootstrap) { // put them at a lower level
      if (i < bitSize)
        enca[i].bringToSet(context.getCtxtPrimes(5));
      encb[i].bringToSet(context.getCtxtPrimes(5));
    }
  }
#ifdef HELIB_DEBUG
  decryptAndPrint((std::cout << " before comparison: "),
                  encb[0],
                  secKey,
                  ea,
                  0);
#endif

  std::vector<long> slotsMin, slotsMax, slotsMu, slotsNi;
  // comparison only
  compareTwoNumbers(mu,
                    ni,
                    helib::CtPtrs_VecCt(enca),
                    helib::CtPtrs_VecCt(encb),
                    false,
                    &unpackSlotEncoding);
  ea.decrypt(mu, secKey, slotsMu);
  ea.decrypt(ni, secKey, slotsNi);
  EXPECT_EQ(std::make_pair(slotsMu[0], slotsNi[0]),
            std::make_pair((long)pMu, (long)pNi))
      << "Comparison (without min max) error: a=" << pa << ", b=" << pb
      << ", mu=" << slotsMu[0] << ", ni=" << slotsNi[0] << std::endl;
  if (helib_test::verbose) {
    std::cout << "Comparison (without min max) succeeded: ";
    std::cout << '(' << pa << ',' << pb << ")=> mu=" << slotsMu[0]
              << ", ni=" << slotsNi[0] << std::endl;
  }

  {
    helib::CtPtrs_VecCt wMin(eMin),
        wMax(eMax); // A wrappers around output vectors
    // comparison with max and min
    compareTwoNumbers(wMax,
                      wMin,
                      mu,
                      ni,
                      helib::CtPtrs_VecCt(enca),
                      helib::CtPtrs_VecCt(encb),
                      false,
                      &unpackSlotEncoding);
    decryptBinaryNums(slotsMax, wMax, secKey, ea);
    decryptBinaryNums(slotsMin, wMin, secKey, ea);
  } // get rid of the wrapper
  ea.decrypt(mu, secKey, slotsMu);
  ea.decrypt(ni, secKey, slotsNi);

  EXPECT_EQ(std::make_tuple(pMax, pMin, pMu, pNi),
            std::make_tuple(slotsMax[0], slotsMin[0], slotsMu[0], slotsNi[0]))
      << "Comparison (with min max) error: a=" << pa << ", b=" << pb
      << ", but min=" << slotsMin[0] << ", max=" << slotsMax[0]
      << ", mu=" << slotsMu[0] << ", ni=" << slotsNi[0] << std::endl;

  if (helib_test::verbose) {
    std::cout << "Comparison (with min max) succeeded: ";
    std::cout << '(' << pa << ',' << pb << ")=>(" << slotsMin[0] << ','
              << slotsMax[0] << "), mu=" << slotsMu[0] << ", ni=" << slotsNi[0]
              << std::endl;
  }

#ifdef HELIB_DEBUG
  const helib::Ctxt* minLvlCtxt = nullptr;
  long minLvl = 1000;
  for (const helib::Ctxt& c : eMax) {
    long lvl = c.logOfPrimeSet();
    if (lvl < minLvl) {
      minLvlCtxt = &c;
      minLvl = lvl;
    }
  }
  decryptAndPrint((std::cout << " after comparison: "),
                  *minLvlCtxt,
                  secKey,
                  ea,
                  0);
  std::cout << std::endl;
#endif
}

TEST_P(GTestBinaryCompare,
       comparingTwoPositiveNumbersInTwosComplementWorksCorrectly)
{
  const helib::EncryptedArray& ea = *context.ea;

  // Choose random n-bit numbers in 2's complement
  long pa = NTL::RandomBits_long(bitSize - 1);
  long pb = NTL::RandomBits_long(bitSize - 1);
  long pMax = std::max(pa, pb);
  long pMin = std::min(pa, pb);
  bool pMu = pa > pb;
  bool pNi = pa < pb;

  // Encrypt the individual bits
  NTL::Vec<helib::Ctxt> eMax, eMin, enca, encb;

  helib::Ctxt mu(secKey), ni(secKey);
  resize(enca, bitSize, mu);
  resize(encb, bitSize, ni);
  for (long i = 0; i < bitSize; i++) {
    secKey.Encrypt(enca[i], NTL::ZZX((pa >> i) & 1));
    secKey.Encrypt(encb[i], NTL::ZZX((pb >> i) & 1));
    if (bootstrap) { // put them at a lower level
      if (i < bitSize)
        enca[i].bringToSet(context.getCtxtPrimes(5));
      encb[i].bringToSet(context.getCtxtPrimes(5));
    }
  }

  std::vector<long> slotsMin, slotsMax, slotsMu, slotsNi;
  // comparison only
  compareTwoNumbers(mu,
                    ni,
                    helib::CtPtrs_VecCt(enca),
                    helib::CtPtrs_VecCt(encb),
                    true,
                    &unpackSlotEncoding);
  ea.decrypt(mu, secKey, slotsMu);
  ea.decrypt(ni, secKey, slotsNi);
  EXPECT_EQ(std::make_pair(slotsMu[0], slotsNi[0]),
            std::make_pair((long)pMu, (long)pNi))
      << "Comparison (without min max) error: a=" << pa << ", b=" << pb
      << ", mu=" << slotsMu[0] << ", ni=" << slotsNi[0] << std::endl;
  if (helib_test::verbose) {
    std::cout << "Comparison (without min max) succeeded: ";
    std::cout << '(' << pa << ',' << pb << ")=> mu=" << slotsMu[0]
              << ", ni=" << slotsNi[0] << std::endl;
  }

  {
    helib::CtPtrs_VecCt wMin(eMin),
        wMax(eMax); // A wrappers around output vectors
    // comparison with max and min
    compareTwoNumbers(wMax,
                      wMin,
                      mu,
                      ni,
                      helib::CtPtrs_VecCt(enca),
                      helib::CtPtrs_VecCt(encb),
                      true,
                      &unpackSlotEncoding);
    decryptBinaryNums(slotsMax, wMax, secKey, ea, true);
    decryptBinaryNums(slotsMin, wMin, secKey, ea, true);
  } // get rid of the wrapper
  ea.decrypt(mu, secKey, slotsMu);
  ea.decrypt(ni, secKey, slotsNi);

  EXPECT_EQ(std::make_tuple(pMax, pMin, pMu, pNi),
            std::make_tuple(slotsMax[0], slotsMin[0], slotsMu[0], slotsNi[0]))
      << "Comparison (with min max) error: a=" << pa << ", b=" << pb
      << ", but min=" << slotsMin[0] << ", max=" << slotsMax[0]
      << ", mu=" << slotsMu[0] << ", ni=" << slotsNi[0] << std::endl;
}

TEST_P(GTestBinaryCompare,
       comparingTwoNegativeNumbersInTwosComplementWorksCorrectly)
{
  const helib::EncryptedArray& ea = *context.ea;

  // Choose random n-bit numbers in 2's complement
  long pa_data = NTL::RandomBits_long(bitSize - 1);
  long pb_data = NTL::RandomBits_long(bitSize - 1);
  pa_data |= (1 << (bitSize - 1));
  pb_data |= (1 << (bitSize - 1));
  long pa = helib::bitSetToLong(pa_data, bitSize);
  long pb = helib::bitSetToLong(pb_data, bitSize);
  long pMax = std::max(pa, pb);
  long pMin = std::min(pa, pb);
  bool pMu = pa > pb;
  bool pNi = pa < pb;

  // Encrypt the individual bits
  NTL::Vec<helib::Ctxt> eMax, eMin, enca, encb;

  helib::Ctxt mu(secKey), ni(secKey);
  resize(enca, bitSize, mu);
  resize(encb, bitSize, ni);
  for (long i = 0; i < bitSize; i++) {
    secKey.Encrypt(enca[i], NTL::ZZX((pa >> i) & 1));
    secKey.Encrypt(encb[i], NTL::ZZX((pb >> i) & 1));
    if (bootstrap) { // put them at a lower level
      if (i < bitSize)
        enca[i].bringToSet(context.getCtxtPrimes(5));
      encb[i].bringToSet(context.getCtxtPrimes(5));
    }
  }

  std::vector<long> slotsMin, slotsMax, slotsMu, slotsNi;
  // comparison only
  compareTwoNumbers(mu,
                    ni,
                    helib::CtPtrs_VecCt(enca),
                    helib::CtPtrs_VecCt(encb),
                    true,
                    &unpackSlotEncoding);
  ea.decrypt(mu, secKey, slotsMu);
  ea.decrypt(ni, secKey, slotsNi);
  EXPECT_EQ(std::make_pair(slotsMu[0], slotsNi[0]),
            std::make_pair((long)pMu, (long)pNi))
      << "Comparison (without min max) error: a=" << pa << ", b=" << pb
      << ", mu=" << slotsMu[0] << ", ni=" << slotsNi[0] << std::endl;
  if (helib_test::verbose) {
    std::cout << "Comparison (without min max) succeeded: ";
    std::cout << '(' << pa << ',' << pb << ")=> mu=" << slotsMu[0]
              << ", ni=" << slotsNi[0] << std::endl;
  }

  {
    helib::CtPtrs_VecCt wMin(eMin),
        wMax(eMax); // A wrappers around output vectors
    // comparison with max and min
    compareTwoNumbers(wMax,
                      wMin,
                      mu,
                      ni,
                      helib::CtPtrs_VecCt(enca),
                      helib::CtPtrs_VecCt(encb),
                      true,
                      &unpackSlotEncoding);
    decryptBinaryNums(slotsMax, wMax, secKey, ea, true);
    decryptBinaryNums(slotsMin, wMin, secKey, ea, true);
  } // get rid of the wrapper
  ea.decrypt(mu, secKey, slotsMu);
  ea.decrypt(ni, secKey, slotsNi);

  EXPECT_EQ(std::make_tuple(pMax, pMin, pMu, pNi),
            std::make_tuple(slotsMax[0], slotsMin[0], slotsMu[0], slotsNi[0]))
      << "Comparison (with min max) error: a=" << pa << ", b=" << pb
      << ", but min=" << slotsMin[0] << ", max=" << slotsMax[0]
      << ", mu=" << slotsMu[0] << ", ni=" << slotsNi[0] << std::endl;
}

TEST_P(GTestBinaryCompare,
       comparingNegativeAndPositiveNumbersInTwosComplementWorksCorrectly)
{
  const helib::EncryptedArray& ea = *context.ea;

  // Choose random n-bit numbers in 2's complement
  long pa_data = NTL::RandomBits_long(bitSize - 1);
  long pb_data = NTL::RandomBits_long(bitSize - 1);
  // pa is non-negative, pb is negative
  pb_data |= (1 << (bitSize - 1));
  long pa = helib::bitSetToLong(pa_data, bitSize);
  long pb = helib::bitSetToLong(pb_data, bitSize);
  long pMax = pa;
  long pMin = pb;
  bool pMu = pa > pb; // Always true
  bool pNi = pa < pb; // Always false

  // Encrypt the individual bits
  NTL::Vec<helib::Ctxt> eMax, eMin, enca, encb;

  helib::Ctxt mu(secKey), ni(secKey);
  resize(enca, bitSize, mu);
  resize(encb, bitSize, ni);
  for (long i = 0; i < bitSize; i++) {
    secKey.Encrypt(enca[i], NTL::ZZX((pa >> i) & 1));
    secKey.Encrypt(encb[i], NTL::ZZX((pb >> i) & 1));
    if (bootstrap) { // put them at a lower level
      if (i < bitSize)
        enca[i].bringToSet(context.getCtxtPrimes(5));
      encb[i].bringToSet(context.getCtxtPrimes(5));
    }
  }

  std::vector<long> slotsMin, slotsMax, slotsMu, slotsNi;
  // comparison only
  compareTwoNumbers(mu,
                    ni,
                    helib::CtPtrs_VecCt(enca),
                    helib::CtPtrs_VecCt(encb),
                    true,
                    &unpackSlotEncoding);
  ea.decrypt(mu, secKey, slotsMu);
  ea.decrypt(ni, secKey, slotsNi);
  EXPECT_EQ(std::make_pair(slotsMu[0], slotsNi[0]),
            std::make_pair((long)pMu, (long)pNi))
      << "Comparison (without min max) error: a=" << pa << ", b=" << pb
      << ", mu=" << slotsMu[0] << ", ni=" << slotsNi[0] << std::endl;
  if (helib_test::verbose) {
    std::cout << "Comparison (without min max) succeeded: ";
    std::cout << '(' << pa << ',' << pb << ")=> mu=" << slotsMu[0]
              << ", ni=" << slotsNi[0] << std::endl;
  }

  {
    helib::CtPtrs_VecCt wMin(eMin),
        wMax(eMax); // A wrappers around output vectors
    // comparison with max and min
    compareTwoNumbers(wMax,
                      wMin,
                      mu,
                      ni,
                      helib::CtPtrs_VecCt(enca),
                      helib::CtPtrs_VecCt(encb),
                      true,
                      &unpackSlotEncoding);
    decryptBinaryNums(slotsMax, wMax, secKey, ea, true);
    decryptBinaryNums(slotsMin, wMin, secKey, ea, true);
  } // get rid of the wrapper
  ea.decrypt(mu, secKey, slotsMu);
  ea.decrypt(ni, secKey, slotsNi);

  EXPECT_EQ(std::make_tuple(pMax, pMin, pMu, pNi),
            std::make_tuple(slotsMax[0], slotsMin[0], slotsMu[0], slotsNi[0]))
      << "Comparison (with min max) error: a=" << pa << ", b=" << pb
      << ", but min=" << slotsMin[0] << ", max=" << slotsMax[0]
      << ", mu=" << slotsMu[0] << ", ni=" << slotsNi[0] << std::endl;
}

INSTANTIATE_TEST_SUITE_P(
    smallParamaterSizesRepeated,
    GTestBinaryCompare,
    ::testing::Combine(::testing::Values(
                           // SLOW
                           Parameters(1, 5, false, 0, 1)
                           // FAST
                           // Parameters(0, 5, false, 0, 1)
                           ),
                       ::testing::Range(0, 3)) // The range is for repeats
);

} // namespace

#if 0
e2=a2+b2+1
e1=a1+b1+1
e0=a0+b0+1, ne0 = a0+b0

e*2 = e2
e*1 = e2 e1
e*0 = e*1 e0, ne*0 = e*1 ne0

a*2 = a2    , b*2 = b2
a*1 = e2 a1 , b*1 = e*1 -e2 - a*1
a*0 = e*1 a0, b*0 = ne*0 -a*0

c2 = a2 b2
c1 = a1 b1
c0 = a0 b0

c*2 = c2
c*1 = e2 c1
c*0 = e*1 c0

A2 = a2,      B2 = b2,      C2 = c2
A1 = a2 +a*1, B1 = b2 +b*1, C1 = c2 + c*1
A0 = A1 +a*0, B0 = B1 +b*0, C0 = C1 + c*0

(a>b) = A0+C0
(a<b) = B0+C0

mx2 = a2+b2+c2
mx1 = a*1+b*1+c1 + a1(a2+c2) + b1(b2+c)

X ab01 = b1 a0
X b10 = b1 b0

mx0 = a0+b0+c*0
     + a0(A1+C1) = a0 A1 + a0(c*2 +a*1 b1) = a0(A1+c*2) + a*1 ab01
     + b0(B1+C1) = b0 B1 + b0(c*2 +a*1 b1) = b0(B1+c*2) + a*1 b10
    = a0(1+A1+c*2) + b0(1+B1+c*2) + a*1(b10+ab01) + c*0

mn0 = c0
     + a0(B1+C1) = a0 B1 + a0(c*2 +a*1 b1) = a0(B1+c*2) + a*1 ab01
     + b0(A1+C1) = b0 A1 + b0(c*2 +a*1 b1) = b0(A1+c*2) + a*1 b10
    = a0(B1+c*2) + b0(A1+C*2) + a*1(b10 + ab01) + c*0

e20 = e2 ne0

      a*1(b10+ab01)=(a2+b2+1)a1 b1(a0+b0) = e20 c1

mx0 = a0(1+A1+c*2) + b0(1+B1+c*2) + e20 c1 + c*0
mn0 = a0(B1+c*2) + b0(A1+c*2) + e20 c1 + c*0

#endif


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 1, 3> test_double_1_3(const Eigen::Matrix<double, 1, 3> & M)
{
	return M;
}
void export_double_1_3()
{
	boost::python::def("test_double_1_3",test_double_1_3);
}


#include "urdf_parser/urdf_parser.h"
#include <iostream>
#include <iomanip>
#include <cmath>

// the name of our test module
#define BOOST_TEST_MODULE URDF_UNIT_TEST
// needed for automatic generation of the main()
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

bool quat_are_near(urdf::Rotation left, urdf::Rotation right)
{
  static const double epsilon = 1e-3; // quite loose epsilon
  double l[4], r[4];
  left.getQuaternion(l[0], l[1], l[2], l[3]);
  right.getQuaternion(r[0], r[1], r[2], r[3]);
  return (std::abs(l[0] - r[0]) < epsilon &&
          std::abs(l[1] - r[1]) < epsilon &&
          std::abs(l[2] - r[2]) < epsilon &&
          std::abs(l[3] - r[3]) < epsilon) ||
         (std::abs(l[0] + r[0]) < epsilon &&
          std::abs(l[1] + r[1]) < epsilon &&
          std::abs(l[2] + r[2]) < epsilon &&
          std::abs(l[3] + r[3]) < epsilon);
}

std::ostream &operator<<(std::ostream &os, const urdf::Rotation& rot)
{
  double roll, pitch, yaw;
  double x, y, z, w;
  rot.getRPY(roll, pitch, yaw);
  rot.getQuaternion(x, y, z, w);
  os << std::setprecision(9)
     << "x: " << x << " y: " << y << " z: " << z << " w: " <<  w
     << "  roll: "  << roll << " pitch: " << pitch << " yaw: "<< yaw;
  return os;
}

void check_get_set_rpy_is_idempotent(double x, double y, double z, double w)
{
  urdf::Rotation rot0;
  rot0.setFromQuaternion(x, y, z, w);
  double roll, pitch, yaw;
  rot0.getRPY(roll, pitch, yaw);
  urdf::Rotation rot1;
  rot1.setFromRPY(roll, pitch, yaw);
  if (true) {
    std::cout << "\n"
              << "before  " << rot0 << "\n"
              << "after   " << rot1 << "\n"
              << "ok      " << quat_are_near(rot0, rot1) << "\n";
  }
  BOOST_CHECK(quat_are_near(rot0, rot1));
}

BOOST_AUTO_TEST_CASE(test_rotation_get_set_rpy_idempotent)
{
  double x0 = 0.5, y0 = -0.5, z0 = 0.5,  w0 = 0.5;
  check_get_set_rpy_is_idempotent(x0, y0, z0, w0);
  double delta = 2.2e-8;
  check_get_set_rpy_is_idempotent(x0, y0, z0+delta, w0-delta);
}


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Col;
using arma::span;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedInPlaceGenColVecElemIndRangeGenColVec: public Expected {
    public:
      ExpectedInPlaceGenColVecElemIndRangeGenColVec() {
        cout << "Compute ExpectedInPlaceGenColVecElemIndRangeGenColVec(): " << endl;

        vector<vector<pair<string, void*>>> inputs = Input::getTestParameters( {
            InputClass::GenColVec,
            InputClass::ElemIndRange,
            InputClass::GenColVec
          });

        for (vector<pair<string, void*>> input : inputs) {
          _fileSuffix = "";

          int n = 0;
          for (pair<string, void*> value : input) {
            switch (n) {
              case 0:
                _fileSuffix += value.first;
                _genColVecA = *static_cast<Col<double>*>(value.second);
                break;
              case 1:
                _fileSuffix += "," + value.first;
                _elemIndRange = *static_cast<span*>(value.second);
                break;
              case 2:
                _fileSuffix += "," + value.first;
                _genColVecB = *static_cast<Col<double>*>(value.second);
                break;
            }
            ++n;
          }

          cout << "Using input: " << _fileSuffix << endl;

          _copyOfGenColVecA = _genColVecA;
          _copyOfElemIndRange = _elemIndRange;
          _copyOfGenColVecB = _genColVecB;

          expectedColVecRowsEqual();

          _genColVecA = _copyOfGenColVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genColVecB = _copyOfGenColVecB;
          expectedColVecRowsPlus();

          _genColVecA = _copyOfGenColVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genColVecB = _copyOfGenColVecB;
          expectedColVecRowsMinus();

          _genColVecA = _copyOfGenColVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genColVecB = _copyOfGenColVecB;
          expectedColVecRowsElemTimes();

          _genColVecA = _copyOfGenColVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genColVecB = _copyOfGenColVecB;
          expectedColVecRowsElemDivide();

          _genColVecA = _copyOfGenColVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genColVecB = _copyOfGenColVecB;
          expectedColVecSubvecEqual();

          _genColVecA = _copyOfGenColVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genColVecB = _copyOfGenColVecB;
          expectedColVecSubvecPlus();

          _genColVecA = _copyOfGenColVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genColVecB = _copyOfGenColVecB;
          expectedColVecSubvecMinus();

          _genColVecA = _copyOfGenColVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genColVecB = _copyOfGenColVecB;
          expectedColVecSubvecElemTimes();

          _genColVecA = _copyOfGenColVecA;
          _elemIndRange = _copyOfElemIndRange;
          _genColVecB = _copyOfGenColVecB;
          expectedColVecSubvecElemDivide();
        }

        cout << "done." << endl;
      }

    protected:
      Col<double> _genColVecA;
      Col<double> _copyOfGenColVecA;

      span _elemIndRange;
      span _copyOfElemIndRange;

      Col<double> _genColVecB;
      Col<double> _copyOfGenColVecB;

      void expectedColVecRowsEqual() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genColVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedColVecRowsEqual() ... ";

        _genColVecA.rows(_elemIndRange.a, _elemIndRange.b) = _genColVecB;

        save<double>("Col.rowsEqual", _genColVecA);
        cout << "done." << endl;
      }

      void expectedColVecRowsPlus() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genColVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedColVecRowsPlus() ... ";

        _genColVecA.rows(_elemIndRange.a, _elemIndRange.b) += _genColVecB;

        save<double>("Col.rowsPlus", _genColVecA);
        cout << "done." << endl;
      }

      void expectedColVecRowsMinus() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genColVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedColVecRowsMinus() ... ";

        _genColVecA.rows(_elemIndRange.a, _elemIndRange.b) -= _genColVecB;

        save<double>("Col.rowsMinus", _genColVecA);
        cout << "done." << endl;
      }

      void expectedColVecRowsElemTimes() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genColVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedColVecRowsElemTimes() ... ";

        _genColVecA.rows(_elemIndRange.a, _elemIndRange.b) %= _genColVecB;

        save<double>("Col.rowsElemTimes", _genColVecA);
        cout << "done." << endl;
      }

      void expectedColVecRowsElemDivide() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genColVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedColVecRowsElemDivide() ... ";

        _genColVecA.rows(_elemIndRange.a, _elemIndRange.b) /= _genColVecB;

        save<double>("Col.rowsElemDivide", _genColVecA);
        cout << "done." << endl;
      }

      void expectedColVecSubvecEqual() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genColVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedColVecSubvecEqual() ... ";

        _genColVecA.subvec(_elemIndRange.a, _elemIndRange.b) = _genColVecB;

        save<double>("Col.subvecEqual", _genColVecA);
        cout << "done." << endl;
      }

      void expectedColVecSubvecPlus() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genColVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedColVecSubvecPlus() ... ";

        _genColVecA.subvec(_elemIndRange.a, _elemIndRange.b) += _genColVecB;

        save<double>("Col.subvecPlus", _genColVecA);
        cout << "done." << endl;
      }

      void expectedColVecSubvecMinus() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genColVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedColVecSubvecMinus() ... ";

        _genColVecA.subvec(_elemIndRange.a, _elemIndRange.b) -= _genColVecB;

        save<double>("Col.subvecMinus", _genColVecA);
        cout << "done." << endl;
      }

      void expectedColVecSubvecElemTimes() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVecA.in_range(_elemIndRange)) {
          return;
        }

        if (_genColVecB.n_elem != _elemIndRange.b - _elemIndRange.a + 1) {
          return;
        }

        cout << "- Compute expectedColVecSubvecElemTimes() ... ";

        _genColVecA.subvec(_elemIndRange.a, _elemIndRange.b) %= _genColVecB;

        save<double>("Col.subvecElemTimes", _genColVecA);
        cout << "done." << endl;
      }

      void expectedColVecSubvecElemDivide() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVecA.in_range(_elemIndRange) || _genColVecB.n_elem != (_elemIndRange.b - _elemIndRange.a) + 1) {
          return;
        }

        cout << "- Compute expectedColVecSubvecElemDivide() ... ";

        _genColVecA.subvec(_elemIndRange.a, _elemIndRange.b) /= _genColVecB;

        save<double>("Col.subvecElemDivide", _genColVecA);
        cout << "done." << endl;
      }

  };
}


/*
 [auto_generated]
 libs/numeric/odeint/test/fusion_algebra.cpp

 [begin_description]
 This file tests the Fusion algebra.
 [end_description]

 Copyright 2011-2012 Karsten Ahnert
 Copyright 2011-2013 Mario Mulansky

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

#define BOOST_TEST_MODULE odeint_fusion_algebra

// using fusion vectors as state types requires increased macro variables
#define BOOST_FUSION_INVOKE_MAX_ARITY 15
#define BOOST_RESULT_OF_NUM_ARGS 15

#include <cmath>
#include <complex>
#include <utility>
#include <functional>

#include <boost/numeric/odeint/config.hpp>

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

#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <boost/units/systems/si/acceleration.hpp>
#include <boost/units/systems/si/io.hpp>

#include <boost/fusion/container.hpp>

#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/fusion_algebra.hpp>

namespace units = boost::units;
namespace si = boost::units::si;
namespace fusion = boost::fusion;

using boost::numeric::odeint::default_operations;
using boost::numeric::odeint::fusion_algebra;

typedef double value_type;
typedef units::quantity< si::time , value_type > time_type;
typedef units::quantity< si::length , value_type > length_type;
typedef units::quantity< si::velocity , value_type > velocity_type;
typedef units::quantity< si::acceleration , value_type > acceleration_type;
typedef fusion::vector< length_type , velocity_type > state_type;
typedef fusion::vector< velocity_type , acceleration_type > deriv_type;

const time_type dt = 0.1 * si::second;

struct fusion_fixture
{
    fusion_fixture( void )
        : res( 0.0 * si::meter , 0.0 * si::meter_per_second ) ,
          x( 1.0 * si::meter , 1.0 * si::meter_per_second ) ,
          k1( 1.0 * si::meter_per_second , 1.0 * si::meter_per_second_squared ) ,
          k2( 2.0 * si::meter_per_second , 2.0 * si::meter_per_second_squared ) ,
          k3( 3.0 * si::meter_per_second , 3.0 * si::meter_per_second_squared ) ,
          k4( 4.0 * si::meter_per_second , 4.0 * si::meter_per_second_squared ) ,
          k5( 5.0 * si::meter_per_second , 5.0 * si::meter_per_second_squared ) ,
          k6( 6.0 * si::meter_per_second , 6.0 * si::meter_per_second_squared ) { }

    ~fusion_fixture( void )
    {
        BOOST_CHECK_CLOSE( fusion::at_c< 0 >( x ).value() , 1.0 , 1.0e-10 );
        BOOST_CHECK_CLOSE( fusion::at_c< 1 >( x ).value() , 1.0 , 1.0e-10 );
        BOOST_CHECK_CLOSE( fusion::at_c< 0 >( k1 ).value() , 1.0 , 1.0e-10 );
        BOOST_CHECK_CLOSE( fusion::at_c< 1 >( k1 ).value() , 1.0 , 1.0e-10 );
        BOOST_CHECK_CLOSE( fusion::at_c< 0 >( k2 ).value() , 2.0 , 1.0e-10 );
        BOOST_CHECK_CLOSE( fusion::at_c< 1 >( k2 ).value() , 2.0 , 1.0e-10 );
        BOOST_CHECK_CLOSE( fusion::at_c< 0 >( k3 ).value() , 3.0 , 1.0e-10 );
        BOOST_CHECK_CLOSE( fusion::at_c< 1 >( k3 ).value() , 3.0 , 1.0e-10 );
        BOOST_CHECK_CLOSE( fusion::at_c< 0 >( k4 ).value() , 4.0 , 1.0e-10 );
        BOOST_CHECK_CLOSE( fusion::at_c< 1 >( k4 ).value() , 4.0 , 1.0e-10 );
        BOOST_CHECK_CLOSE( fusion::at_c< 0 >( k5 ).value() , 5.0 , 1.0e-10 );
        BOOST_CHECK_CLOSE( fusion::at_c< 1 >( k5 ).value() , 5.0 , 1.0e-10 );
        BOOST_CHECK_CLOSE( fusion::at_c< 0 >( k6 ).value() , 6.0 , 1.0e-10 );
        BOOST_CHECK_CLOSE( fusion::at_c< 1 >( k6 ).value() , 6.0 , 1.0e-10 );
    }

    state_type res , x;
    deriv_type k1 , k2 , k3 , k4 , k5 , k6 ;
};

BOOST_AUTO_TEST_SUITE( fusion_algebra_test )

fusion_algebra algebra;

BOOST_AUTO_TEST_CASE( for_each2 )
{
    fusion_fixture f;
    algebra.for_each2( f.res , f.k1 ,
                       default_operations::scale_sum1< time_type >( dt ) );
    BOOST_CHECK_CLOSE( fusion::at_c< 0 >( f.res ).value() , 0.1 , 1.0e-10 );
    BOOST_CHECK_CLOSE( fusion::at_c< 1 >( f.res ).value() , 0.1 , 1.0e-10 );
}

BOOST_AUTO_TEST_CASE( for_each3 )
{
    fusion_fixture f;
    algebra.for_each3( f.res , f.x , f.k1 ,
                       default_operations::scale_sum2< value_type , time_type >( 1.0 , dt ) );
    BOOST_CHECK_CLOSE( fusion::at_c< 0 >( f.res ).value() , 1.1 , 1.0e-10 );
    BOOST_CHECK_CLOSE( fusion::at_c< 1 >( f.res ).value() , 1.1 , 1.0e-10 );
}

BOOST_AUTO_TEST_CASE( for_each4 )
{
    fusion_fixture f;
    algebra.for_each4( f.res , f.x , f.k1 , f.k2 ,
                       default_operations::scale_sum3< value_type , time_type , time_type >( 1.0 , dt , dt ) );
    BOOST_CHECK_CLOSE( fusion::at_c< 0 >( f.res ).value() , 1.3 , 1.0e-10 );
    BOOST_CHECK_CLOSE( fusion::at_c< 1 >( f.res ).value() , 1.3 , 1.0e-10 );
}

BOOST_AUTO_TEST_CASE( for_each5 )
{
    fusion_fixture f;
    algebra.for_each5( f.res , f.x , f.k1 , f.k2 , f.k3 ,
                       default_operations::scale_sum4< value_type , time_type , time_type , time_type >( 1.0 , dt , dt , dt ) );
    BOOST_CHECK_CLOSE( fusion::at_c< 0 >( f.res ).value() , 1.6 , 1.0e-10 );
    BOOST_CHECK_CLOSE( fusion::at_c< 1 >( f.res ).value() , 1.6 , 1.0e-10 );
}

BOOST_AUTO_TEST_CASE( for_each6 )
{
    fusion_fixture f;
    algebra.for_each6( f.res , f.x , f.k1 , f.k2 , f.k3 , f.k4 ,
                       default_operations::scale_sum5< value_type , time_type , time_type , time_type , time_type >( 1.0 , dt , dt , dt , dt ) );
    BOOST_CHECK_CLOSE( fusion::at_c< 0 >( f.res ).value() , 2.0 , 1.0e-10 );
    BOOST_CHECK_CLOSE( fusion::at_c< 1 >( f.res ).value() , 2.0 , 1.0e-10 );
}

BOOST_AUTO_TEST_CASE( for_each7 )
{
    fusion_fixture f;
    algebra.for_each7( f.res , f.x , f.k1 , f.k2 , f.k3 , f.k4 , f.k5 ,
                       default_operations::scale_sum6< value_type , time_type , time_type , time_type , time_type , time_type >( 1.0 , dt , dt , dt , dt , dt ) );
    BOOST_CHECK_CLOSE( fusion::at_c< 0 >( f.res ).value() , 2.5 , 1.0e-10 );
    BOOST_CHECK_CLOSE( fusion::at_c< 1 >( f.res ).value() , 2.5 , 1.0e-10 );
}

BOOST_AUTO_TEST_CASE( for_each8 )
{
    fusion_fixture f;
    algebra.for_each8( f.res , f.x , f.k1 , f.k2 , f.k3 , f.k4 , f.k5 , f.k6 ,
                       default_operations::scale_sum7< value_type , time_type , time_type , time_type , time_type , time_type , time_type >( 1.0 , dt , dt , dt , dt , dt , dt ) );
    BOOST_CHECK_CLOSE( fusion::at_c< 0 >( f.res ).value() , 3.1 , 1.0e-10 );
    BOOST_CHECK_CLOSE( fusion::at_c< 1 >( f.res ).value() , 3.1 , 1.0e-10 );
}

BOOST_AUTO_TEST_CASE( for_each15 )
{
    fusion_fixture f;
    algebra.for_each15( f.res , f.x , f.k1 , f.k2 , f.k3 , f.k4 , f.k5 , f.k6 , f.k1 , f.k2 , f.k3 , f.k4 , f.k5 , f.k6 , f.k1 ,
                        default_operations::scale_sum14< value_type , time_type , time_type , time_type , time_type , time_type , time_type ,
                                                         time_type , time_type , time_type , time_type , time_type , time_type , time_type >( 1.0 , dt , dt , dt , dt , dt , dt , dt , dt , dt , dt , dt , dt , dt ) );
    //BOOST_CHECK_CLOSE( fusion::at_c< 0 >( f.res ).value() , 3.1 , 1.0e-10 );
    //BOOST_CHECK_CLOSE( fusion::at_c< 1 >( f.res ).value() , 3.1 , 1.0e-10 );
}

BOOST_AUTO_TEST_CASE( norm_inf )
{
    double nrm = algebra.norm_inf( fusion::make_vector( 1.0 , 2.0 , 3.0 ) );
    BOOST_CHECK_CLOSE( nrm , 3.0 , 1.0e-10 );
}

BOOST_AUTO_TEST_SUITE_END()


/*!
 * \file ErrorTermEuclidean.hpp
 *
 * \author diemf
 *
 * A Pose ErrorTerm
 */
#ifndef ASLAM_BACKEND_ERROR_EUCLIDEAN_HPP
#define ASLAM_BACKEND_ERROR_EUCLIDEAN_HPP

#include <Eigen/Core>
#include <aslam/backend/ErrorTerm.hpp>
#include <aslam/backend/EuclideanExpression.hpp>

namespace aslam {
namespace backend {

/*!
 * \class ErrorTermEuclidean
 *
 * \brief An ErrorTerm implementation for the deviation of a translation pose wrt. a prior
 */

class ErrorTermEuclidean : public aslam::backend::ErrorTermFs<3> {
  public:
    // This is important. The superclass holds some fixed-sized Eigen types
    // For more information, see:
    // http://eigen.tuxfamily.org/dox-devel/TopicStructHavingEigenMembers.html
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    ErrorTermEuclidean(const aslam::backend::EuclideanExpression& t, const Eigen::Vector3d& prior,
                       const Eigen::Matrix<double, 3, 3>& N, int debug = 0);
    ErrorTermEuclidean(const aslam::backend::EuclideanExpression& t, const Eigen::Vector3d& prior, double weight,
                       int debug = 0);

    virtual ~ErrorTermEuclidean();

  protected:
    /// This is the interface required by ErrorTermFs<>

    /// \brief evaluate the error term and return the weighted squared error e^T invR e
    virtual double evaluateErrorImplementation();

    /// \brief evaluate the jacobian
    virtual void evaluateJacobiansImplementation(JacobianContainer& J);

  private:
    aslam::backend::EuclideanExpression _t;
    Eigen::Vector3d _prior;
    int _debug;
};

}  // namespace backend
}  // namespace aslam

#endif /* ASLAM_BACKEND_ERROR_EUCLIDEAN_HPP */


#pragma once

#if defined(_OPENMP)
#include <omp.h>
#endif

#include <cstdint>
#include <functional>
#include <limits>
#include <stdexcept>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/openmp_helpers.hpp>

namespace common_robotics_utilities
{
/// Compute Hausdorff distance between two distributions.
/// "The Hausdorff distance is the longest distance you can be forced to travel
/// by an adversary who chooses a point in one of the two sets, from where you
/// then must travel to the other set. In other words, it is the greatest of all
/// the distances from a point in one set to the closest point in the other
/// set." - from Wikipedia
namespace simple_hausdorff_distance
{
/// Compute Hausdorff distance between @param first_distribution and @param
/// second_distribution using @param distance_function to compute distance
/// between an element in @param first_distribution and an element in @param
/// second_distribution. @return distance between distributions.
/// Computation is performed in parallel, and the larger of the two input
/// distributions is automatically selected to be the outer (parallelized) loop.
template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
inline double ComputeDistanceParallel(
    const FirstContainer& first_distribution,
    const SecondContainer& second_distribution,
    const std::function<double(const FirstDatatype&,
                               const SecondDatatype&)>& distance_fn)
{
  if (first_distribution.empty())
  {
    throw std::invalid_argument("first_distribution is empty");
  }
  if (second_distribution.empty())
  {
    throw std::invalid_argument("second_distribution is empty");
  }
  // Swap if needed to make the outer parallel loop be larger
  const bool swap = (first_distribution.size() < second_distribution.size());
  const auto& outer_distribution
      = (swap) ? second_distribution : first_distribution;
  const auto& inner_distribution
      = (swap) ? first_distribution : second_distribution;
  // Make per-thread storage
  std::vector<double> per_thread_storage(
      openmp_helpers::GetNumOmpThreads(), 0.0);
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t idx = 0; idx < outer_distribution.size(); idx++)
  {
    const FirstDatatype& first = outer_distribution[idx];
    double minimum_distance = std::numeric_limits<double>::infinity();
    for (size_t jdx = 0; jdx < inner_distribution.size(); jdx++)
    {
      const SecondDatatype& second = inner_distribution[jdx];
      const double current_distance = distance_fn(first, second);
      if (current_distance < minimum_distance)
      {
        minimum_distance = current_distance;
      }
    }
    const auto current_thread_id = openmp_helpers::GetContextOmpThreadNum();
    if (minimum_distance > per_thread_storage.at(current_thread_id))
    {
      per_thread_storage.at(current_thread_id) = minimum_distance;
    }
  }
  double maximum_minimum_distance = 0.0;
  for (const double& temp_minimum_distance : per_thread_storage)
  {
    if (temp_minimum_distance > maximum_minimum_distance)
    {
      maximum_minimum_distance = temp_minimum_distance;
    }
  }
  return maximum_minimum_distance;
}

/// Compute Hausdorff distance between @param first_distribution and @param
/// second_distribution using @param distance_function to compute distance
/// between an element in @param first_distribution and an element in @param
/// second_distribution. @return distance between distributions.
template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
inline double ComputeDistanceSerial(
    const FirstContainer& first_distribution,
    const SecondContainer& second_distribution,
    const std::function<double(const FirstDatatype&,
                               const SecondDatatype&)>& distance_fn)
{
  if (first_distribution.empty())
  {
    throw std::invalid_argument("first_distribution is empty");
  }
  if (second_distribution.empty())
  {
    throw std::invalid_argument("second_distribution is empty");
  }
  double maximum_minimum_distance = 0.0;
  for (size_t idx = 0; idx < first_distribution.size(); idx++)
  {
    const FirstDatatype& first = first_distribution[idx];
    double minimum_distance = std::numeric_limits<double>::infinity();
    for (size_t jdx = 0; jdx < second_distribution.size(); jdx++)
    {
      const SecondDatatype& second = second_distribution[jdx];
      const double current_distance = distance_fn(first, second);
      if (current_distance < minimum_distance)
      {
        minimum_distance = current_distance;
      }
    }
    if (minimum_distance > maximum_minimum_distance)
    {
      maximum_minimum_distance = minimum_distance;
    }
  }
  return maximum_minimum_distance;
}

/// Compute Hausdorff distance between @param first_distribution and @param
/// second_distribution using @param distance_matrix which stores the pairwise
/// distance between all elements in @param first_distribution and @param
/// second_distribution. @return distance between distributions.
/// Computation is performed in parallel, and the larger of the two input
/// distributions is automatically selected to be the outer (parallelized) loop.
template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
inline double ComputeDistanceParallel(
    const FirstContainer& first_distribution,
    const SecondContainer& second_distribution,
    const Eigen::MatrixXd& distance_matrix)
{
  if (first_distribution.empty())
  {
    throw std::invalid_argument("first_distribution is empty");
  }
  if (second_distribution.empty())
  {
    throw std::invalid_argument("second_distribution is empty");
  }
  if (static_cast<size_t>(distance_matrix.rows()) != first_distribution.size()
      || static_cast<size_t>(distance_matrix.cols())
         != second_distribution.size())
  {
    throw std::invalid_argument("distance_matrix is the wrong size");
  }
  // Swap if needed to make the outer parallel loop be larger
  const bool swap = (first_distribution.size() < second_distribution.size());
  const auto& outer_distribution
      = (swap) ? second_distribution : first_distribution;
  const auto& inner_distribution
      = (swap) ? first_distribution : second_distribution;
  // Make per-thread storage
  std::vector<double> per_thread_storage(
      openmp_helpers::GetNumOmpThreads(), 0.0);
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t idx = 0; idx < outer_distribution.size(); idx++)
  {
    double minimum_distance = std::numeric_limits<double>::infinity();
    for (size_t jdx = 0; jdx < inner_distribution.size(); jdx++)
    {
      // Swap the lookup if we've swapped the loops
      const ssize_t row = (swap) ? static_cast<ssize_t>(jdx)
                                 : static_cast<ssize_t>(idx);
      const ssize_t col = (swap) ? static_cast<ssize_t>(idx)
                                 : static_cast<ssize_t>(jdx);
      const double current_distance = distance_matrix(row, col);
      if (current_distance < minimum_distance)
      {
        minimum_distance = current_distance;
      }
    }
    const auto current_thread_id = openmp_helpers::GetContextOmpThreadNum();
    if (minimum_distance > per_thread_storage.at(current_thread_id))
    {
      per_thread_storage.at(current_thread_id) = minimum_distance;
    }
  }
  double maximum_minimum_distance = 0.0;
  for (const double& temp_minimum_distance : per_thread_storage)
  {
    if (temp_minimum_distance > maximum_minimum_distance)
    {
      maximum_minimum_distance = temp_minimum_distance;
    }
  }
  return maximum_minimum_distance;
}

/// Compute Hausdorff distance between @param first_distribution and @param
/// second_distribution using @param distance_matrix which stores the pairwise
/// distance between all elements in @param first_distribution and @param
/// second_distribution. @return distance between distributions.
template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
inline double ComputeDistanceSerial(
    const FirstContainer& first_distribution,
    const SecondContainer& second_distribution,
    const Eigen::MatrixXd& distance_matrix)
{
  if (first_distribution.empty())
  {
    throw std::invalid_argument("first_distribution is empty");
  }
  if (second_distribution.empty())
  {
    throw std::invalid_argument("second_distribution is empty");
  }
  if (static_cast<size_t>(distance_matrix.rows()) != first_distribution.size()
      || static_cast<size_t>(distance_matrix.cols())
         != second_distribution.size())
  {
    throw std::invalid_argument("distance_matrix is the wrong size");
  }
  double maximum_minimum_distance = 0.0;
  for (size_t idx = 0; idx < first_distribution.size(); idx++)
  {
    double minimum_distance = std::numeric_limits<double>::infinity();
    for (size_t jdx = 0; jdx < second_distribution.size(); jdx++)
    {
      const double current_distance
          = distance_matrix(static_cast<ssize_t>(idx),
                            static_cast<ssize_t>(jdx));
      if (current_distance < minimum_distance)
      {
        minimum_distance = current_distance;
      }
    }
    if (minimum_distance > maximum_minimum_distance)
    {
      maximum_minimum_distance = minimum_distance;
    }
  }
  return maximum_minimum_distance;
}

/// Compute Hausdorff distance between @param first_distribution and @param
/// second_distribution using @param distance_function to compute distance
/// between an element in @param first_distribution and an element in @param
/// second_distribution. @return distance between distributions.
/// @param use_parallel selects if the computation should be performed in
/// parallel. If computation is performed in parallel, the larger of the two
/// input distributions is automatically selected to be the outer (parallelized)
/// loop.
template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
inline double ComputeDistance(
    const FirstContainer& first_distribution,
    const SecondContainer& second_distribution,
    const std::function<double(const FirstDatatype&,
                               const SecondDatatype&)>& distance_fn,
    const bool use_parallel = false)
{
  if (use_parallel)
  {
    return ComputeDistanceParallel
        <FirstDatatype, SecondDatatype, FirstContainer, SecondContainer>(
            first_distribution, second_distribution, distance_fn);
  }
  else
  {
    return ComputeDistanceSerial
        <FirstDatatype, SecondDatatype, FirstContainer, SecondContainer>(
            first_distribution, second_distribution, distance_fn);
  }
}

/// Compute Hausdorff distance between @param first_distribution and @param
/// second_distribution using @param distance_matrix which stores the pairwise
/// distance between all elements in @param first_distribution and @param
/// second_distribution. @return distance between distributions.
/// @param use_parallel selects if the computation should be performed in
/// parallel. If computation is performed in parallel, the larger of the two
/// input distributions is automatically selected to be the outer (parallelized)
/// loop.
template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
inline double ComputeDistance(
    const FirstContainer& first_distribution,
    const SecondContainer& second_distribution,
    const Eigen::MatrixXd& distance_matrix,
    const bool use_parallel = false)
{
  if (use_parallel)
  {
    return ComputeDistanceParallel
        <FirstDatatype, SecondDatatype, FirstContainer, SecondContainer>(
            first_distribution, second_distribution, distance_matrix);
  }
  else
  {
    return ComputeDistanceSerial
        <FirstDatatype, SecondDatatype, FirstContainer, SecondContainer>(
            first_distribution, second_distribution, distance_matrix);
  }
}
}  // namespace simple_hausdorff_distance
}  // namespace common_robotics_utilities


#pragma once
#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>
#include <boost/container/vector.hpp>
#include <boost/heap/priority_queue.hpp>
#include <gameai/inverse_comparator.hpp>
#include <gameai/astar_vertex.hpp>

namespace gameai {
	struct astar_pos_t { typedef boost::vertex_property_tag kind; };

	template<typename HeuristicF, typename GraphT> class astar {
	public:
		typedef GraphT graph_type;
		typedef typename boost::graph_traits<graph_type> graph_traits_type;
		typedef typename boost::graph_traits<graph_type>::vertex_descriptor vertex_type;
		typedef astar_vertex<vertex_type> astar_vertex_type;
	private:
		HeuristicF heuristic;
		const graph_type &g;
	public:
		astar(const graph_type &g) : g(g) {}

		boost::container::vector<boost::shared_ptr<astar_vertex_type>>
		path(const typename graph_traits_type::vertices_size_type &src,
		     const typename graph_traits_type::vertices_size_type &dst) {
			boost::unordered_map<typename graph_traits_type::vertices_size_type, vertex_type> vertex_map;

			BGL_FORALL_VERTICES_T(v, g, graph_type) {
				vertex_map.emplace(boost::get(boost::vertex_name, g, v), v);
			}

			/* a heap-based priority queue is used to retrieve the lowest-cost open node
			 * the heap is a max heap so an inverse comparator is used to ensure O(1) retrieval
			 * hash-based sets are used to lookup nodes in O(1)
			 */
			boost::heap::priority_queue<
				boost::shared_ptr<astar_vertex_type>,
				boost::heap::compare<inverse_shared_ptr_comparator<astar_vertex_type>>
			> open_heap;
			boost::unordered_set<boost::shared_ptr<astar_vertex_type>> open_set;
			boost::unordered_set<boost::shared_ptr<astar_vertex_type>> closed_set;

			auto v = boost::make_shared<astar_vertex_type>(0, 0, src, vertex_map.at(src));
			open_heap.emplace(v);
			open_set.emplace(v);

			boost::unordered_map<boost::shared_ptr<astar_vertex_type>,
			                     boost::shared_ptr<astar_vertex_type>> came_from;

			while(!open_heap.empty()) {
				auto current = open_heap.top();
				open_heap.pop();
				open_set.erase(current);

				auto n = boost::get(boost::vertex_name, g, current->vertex);
				if(n == dst) {
					return reconstruct_path(came_from, current);
				}

				BOOST_FOREACH(auto edge, boost::out_edges(current->vertex, g)) {
					auto target = boost::target(edge, g);

					auto w = boost::get(boost::edge_weight, g, edge);
					auto dist_to_end = heuristic(boost::get(astar_pos_t(), g, target),
					                             boost::get(astar_pos_t(), g, vertex_map.at(dst)));
					auto next = boost::make_shared<astar_vertex_type>(current->base_cost + w, dist_to_end, n, target);

					auto open_find = open_set.find(next);
					if(open_find != open_set.end() && (*open_find)->total_cost() < next->total_cost()) { continue; }

					auto closed_find = closed_set.find(next);
					if(closed_find != closed_set.end() && (*closed_find)->total_cost() < next->total_cost()) { continue; }

					came_from.emplace(next, current);
					open_heap.emplace(next);
					open_set.emplace(next);
				}
				closed_set.emplace(current);
			}
			return {};
		}

		boost::container::vector<boost::shared_ptr<astar_vertex_type>>
		reconstruct_path(boost::unordered_map<boost::shared_ptr<astar_vertex_type>,
		                                      boost::shared_ptr<astar_vertex_type>> came_from,
		                 boost::shared_ptr<astar_vertex_type> target) {
			boost::container::vector<boost::shared_ptr<astar_vertex_type>> path;
			path.emplace_back(target);
			while(came_from.find(target) != came_from.end()) {
				target = came_from.at(target);
				path.emplace_back(target);
			}
			return path;
		}
	};
	template<typename HeuristicF, typename GraphT> astar<HeuristicF, GraphT> make_astar(const GraphT &g) {
		return astar<HeuristicF, GraphT>(g);
	}
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include <nil/crypto3/multiprecision/cpp_int.hpp>
#include <boost/random.hpp>
#include <boost/functional/hash.hpp>
#include <unordered_set>
#include <city.h>

//[hash1

/*`
All of the types in this library support hashing via boost::hash or std::hash.
That means we can use multiprecision types directly in hashed containers such as std::unordered_set:
*/
//]

void t1() {
    //[hash2
    using namespace nil::crypto3::multiprecision;
    using namespace boost::random;

    mt19937 mt;
    uniform_int_distribution<uint256_t> ui;

    std::unordered_set<uint256_t> set;
    // Put 1000 random values into the container:
    for (unsigned i = 0; i < 1000; ++i)
        set.insert(ui(mt));

    //]
}

//[hash3

/*`
Or we can define our own hash function, for example in this case based on
Google's CityHash:
*/

struct cityhash {
    std::size_t operator()(const nil::crypto3::multiprecision::uint256_t& val) const {
        // create a hash from all the limbs of the argument, this function is probably x64 specific,
        // and requires that we access the internals of the data type:
        std::size_t result = CityHash64(reinterpret_cast<const char*>(val.backend().limbs()),
                                        val.backend().size() * sizeof(val.backend().limbs()[0]));
        // modify the returned hash based on sign:
        return val < 0 ? ~result : result;
    }
};

//]

void t2() {
    //[hash4

    /*`As before insert some values into a container, this time using our custom hasher:*/

    std::unordered_set<uint256_t, cityhash> set2;
    for (unsigned i = 0; i < 1000; ++i)
        set2.insert(ui(mt));

    //]
}

int main() {
    t1();
    t2();
    return 0;
}