//////////////////////////////////////////////////////////////////////////////////////////////
/// \file ComposeInverseTransformEvaluator.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_COMPOSE_INVERSE_TRANSFORM_EVALUATOR_HPP
#define STEAM_COMPOSE_INVERSE_TRANSFORM_EVALUATOR_HPP

#include <Eigen/Core>

#include <steam/evaluator/blockauto/transform/TransformEvaluator.hpp>

namespace steam {
namespace se3 {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Evaluator for the composition of two transformation matrices (with one inverted)
//////////////////////////////////////////////////////////////////////////////////////////////
class ComposeInverseTransformEvaluator : public TransformEvaluator
{
public:

  /// Convenience typedefs
  typedef std::shared_ptr<ComposeInverseTransformEvaluator> Ptr;
  typedef std::shared_ptr<const ComposeInverseTransformEvaluator> ConstPtr;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor -- T_ba = T_bx * inv(T_ax)
  //////////////////////////////////////////////////////////////////////////////////////////////
  ComposeInverseTransformEvaluator(const TransformEvaluator::ConstPtr& transform_bx,
                                   const TransformEvaluator::ConstPtr& transform_ax);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Pseudo constructor - return a shared pointer to a new instance
  //////////////////////////////////////////////////////////////////////////////////////////////
  static Ptr MakeShared(const TransformEvaluator::ConstPtr& transform_bx,
                        const TransformEvaluator::ConstPtr& transform_ax);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Returns whether or not an evaluator contains unlocked state variables
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual bool isActive() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Adds references (shared pointers) to active state variables to the map output
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual void getActiveStateVariables(
      std::map<unsigned int, steam::StateVariableBase::Ptr>* outStates) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Evaluate the resultant transformation matrix (transform_bx*inv(transform_ax))
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual lgmath::se3::Transformation evaluate() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Evaluate the transformation matrix tree
  ///
  /// ** Note that the returned pointer belongs to the memory pool EvalTreeNode<TYPE>::pool,
  ///    and should be given back to the pool, rather than being deleted.
  //////////////////////////////////////////////////////////////////////////////////////////////
#ifdef STEAM_USE_OBJECT_POOL
  virtual EvalTreeNode<lgmath::se3::Transformation>* evaluateTree() const;
#else
  virtual EvalTreeNode<lgmath::se3::Transformation>::Ptr evaluateTree() const;
#endif

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Evaluate the Jacobian tree
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual void appendBlockAutomaticJacobians(const Eigen::MatrixXd& lhs,
#ifdef STEAM_USE_OBJECT_POOL
                               EvalTreeNode<lgmath::se3::Transformation>* evaluationTree,
#else
                               EvalTreeNode<lgmath::se3::Transformation>::Ptr evaluationTree,
#endif
                               std::vector<Jacobian<> >* outJacobians) const;

  virtual void appendBlockAutomaticJacobians(const Eigen::Matrix<double,1,6>& lhs,
#ifdef STEAM_USE_OBJECT_POOL
                               EvalTreeNode<lgmath::se3::Transformation>* evaluationTree,
#else
                               EvalTreeNode<lgmath::se3::Transformation>::Ptr evaluationTree,
#endif
                               std::vector<Jacobian<1,6> >* outJacobians) const;

  virtual void appendBlockAutomaticJacobians(const Eigen::Matrix<double,2,6>& lhs,
#ifdef STEAM_USE_OBJECT_POOL
                               EvalTreeNode<lgmath::se3::Transformation>* evaluationTree,
#else
                               EvalTreeNode<lgmath::se3::Transformation>::Ptr evaluationTree,
#endif
                               std::vector<Jacobian<2,6> >* outJacobians) const;

  virtual void appendBlockAutomaticJacobians(const Eigen::Matrix<double,3,6>& lhs,
#ifdef STEAM_USE_OBJECT_POOL
                                EvalTreeNode<lgmath::se3::Transformation>* evaluationTree,
#else
                                EvalTreeNode<lgmath::se3::Transformation>::Ptr evaluationTree,
#endif
                                std::vector<Jacobian<3,6> >* outJacobians) const;

  virtual void appendBlockAutomaticJacobians(const Eigen::Matrix<double,4,6>& lhs,
#ifdef STEAM_USE_OBJECT_POOL
                                EvalTreeNode<lgmath::se3::Transformation>* evaluationTree,
#else
                                EvalTreeNode<lgmath::se3::Transformation>::Ptr evaluationTree,
#endif
                                std::vector<Jacobian<4,6> >* outJacobians) const;

  virtual void appendBlockAutomaticJacobians(const Eigen::Matrix<double,6,6>& lhs,
#ifdef STEAM_USE_OBJECT_POOL
                                EvalTreeNode<lgmath::se3::Transformation>* evaluationTree,
#else
                                EvalTreeNode<lgmath::se3::Transformation>::Ptr evaluationTree,
#endif
                                std::vector<Jacobian<6,6> >* outJacobians) const;

private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Implementation for Block Automatic Differentiation
  //////////////////////////////////////////////////////////////////////////////////////////////
  template<int LHS_DIM, int INNER_DIM, int MAX_STATE_SIZE>
  void appendJacobiansImpl(const Eigen::Matrix<double,LHS_DIM,INNER_DIM>& lhs,
#ifdef STEAM_USE_OBJECT_POOL
                           EvalTreeNode<lgmath::se3::Transformation>* evaluationTree,
#else
                           EvalTreeNode<lgmath::se3::Transformation>::Ptr evaluationTree,
#endif
                           std::vector<Jacobian<LHS_DIM,MAX_STATE_SIZE> >* outJacobians) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief First transform evaluator
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformEvaluator::ConstPtr transform_bx_;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Second transform evaluator - to be inverted
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformEvaluator::ConstPtr transform_ax_;

};

} // se3
} // steam

#endif // STEAM_COMPOSE_INVERSE_TRANSFORM_EVALUATOR_HPP


///////////////////////////////////////////////////////////////////////////////
// vector_space::algorithm::find.hpp                                         //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_VECTOR_SPACE_FIND_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_VECTOR_SPACE_FIND_HPP_ER_2009
#include <numeric>
#include <cmath>
#include <boost/vector_space/functional/equal.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace vector_space{

    // This is a version of find where the elements in *[b,e) are ranges.
    // The value eps is a minimum proximity distance that if not met
    // causes the algorithm to return e.
    template<template<typename> class F,typename In,typename X>
    In find(
        In b, // *In is a range
        In e,
        const X& x,
        const typename range_value<X>::type& eps
    ){
        return std::find_if(
            b,
            e,
            functional::equal(x,eps)
        );
    }

    template<typename In,typename X>
    In find(
        In b,
        In e,
        const X& x
    ){
        return std::find_if(
            b,
            e,
            functional::equal(x)
        );
    }

}// vector_space
}// detail
}// statistics
}// boost

#endif


#include <mex.h>
#include "eigenlab.h"
#include <Eigen/Dense>
#include <iostream>

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
    Eigen::MatrixXd out;
    mxArrayToEigen(prhs[0], out, 3, 4);
    std::cout << out << std::endl;
    return;
}


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <util/test_macros.hpp>
#include <string>
#include <iostream>
#include <vector>
#include <cmath>
#include <map>
#include <unordered_map>
#include <sketches/countsketch.hpp>
#include <sketches/countmin.hpp>
#include <random/random.hpp>
#include <timer/timer.hpp>

struct countsketch_test {

  /**
   * Create a set of random integers to be used to benchmark
   * the countsketch sketch.
   * One can choose the number of unique values and the distribution
   * of each element's frequency.
   * See the documentation for test_benchmark for more details.
   */
  std::vector<std::pair<size_t, size_t>> item_counts(size_t num_unique_items,
      size_t count_per_item,
      bool exponential) {

    std::vector<std::pair<size_t, size_t>> v;
    float alpha = 1.0;  // parameter for exponential distribution

    for (size_t i = 0; i < num_unique_items; ++i) {
      size_t count = count_per_item;
      if (exponential) count = std::floor(count * turi::random::gamma(alpha));
      v.push_back(std::pair<size_t, size_t>(i, count));
    }

    std::sort(v.begin(), v.end(),
         [](const std::pair<int, int>& lhs, const std::pair<int, int>& rhs) {
            return lhs.second < rhs.second; } );
    return v;
  }

  /**
   * Run an experiment (described more fully in the documentation for test_benchmark).
   *
   * \param m a synthetic dataset
   * \param sketch a sketch object
   * \param num_to_compare the number of objects for which we want to compute RMSE
   * \param verbose
   */
  template<typename T>
  std::map<std::string, double> run_experiment(std::vector<std::pair<size_t, size_t>> m,
                                               T sketch,
                                               size_t num_to_compare,
                                               bool verbose) {

    std::map<std::string, double> result;

    // Compute sketch
    turi::timer ti;
    ti.start();
    for (auto kv : m) {
      sketch.add(kv.first, kv.second);
    }
    result["elapsed"] = ti.current_time();
    result["count"] = m.size();

    // Compute RMSE for least common items
    std::vector<size_t> items;
    std::vector<int64_t> estimated;
    std::vector<int64_t> actual;
    for (size_t i = 0; i < num_to_compare; ++i) {
      size_t item = m[i].first;
      size_t true_value = m[i].second;
      int64_t estimate = sketch.estimate(item);

      items.push_back(item);
      estimated.push_back(estimate);
      actual.push_back((int64_t) true_value);
      if (verbose)
        std::cout << item << " : " << true_value << " : " << estimate << std::endl;
    }
    result["rmse_rare"] = rmse<int64_t>(estimated, actual);

    // Compute RMSE for most common items
    items.clear();
    estimated.clear();
    actual.clear();
    for (size_t i = m.size()-1; i > m.size() - num_to_compare; --i) {
      size_t item = m[i].first;
      size_t true_value = m[i].second;
      int64_t estimate = sketch.estimate(item);

      items.push_back(item);
      estimated.push_back(estimate);
      actual.push_back((int64_t) true_value);
      if (verbose)
        std::cout << item << " : " << true_value << " : " << estimate << std::endl;

    }
    result["rmse_common"] = rmse<int64_t>(estimated, actual);

    // Compute the density of the sketch, i.e. the ratio of nonzeros
    result["density"] = sketch.density();

    if (verbose) sketch.print();

    return result;
  }

  // Helper function that computes RMSE of two vectors of objects.
  template <typename T>
  double rmse(std::vector<T> y, std::vector<T> yhat) {
    DASSERT_EQ(y.size(), yhat.size());
    double rmse = 0;
    for (size_t i = 0; i < y.size(); ++i) {
      rmse += std::pow((double) y[i] - (double) yhat[i], 2.0);
    }
    return std::sqrt(rmse / (double) y.size());
  }

 public:

  /**
   * Small example to use for debugging.
   */
  void test_small_example() {

    size_t num_unique = 20;
    size_t mean_count_per_item = 5;
    bool expo = true;
    auto items = item_counts(num_unique, mean_count_per_item, expo);

    size_t num_bits = 4;
    size_t num_hash = 3;

    turi::sketches::countmin<size_t> cm(num_bits, num_hash);
    turi::sketches::countsketch<size_t> cs(num_bits, num_hash);

    for (auto kv : items) {
      std::cout << std::endl;
      for (size_t i = 0; i < kv.second; ++i) {
        cm.add(kv.first);
        cs.add(kv.first);
      }
      cm.print();
      cs.print();
    }

    for (auto kv : items) {
      std::cout << kv.first << ":" << kv.second << ":" << cm.estimate(kv.first) << std::endl;
    }

    for (auto kv : items) {
      std::cout << kv.first << ":" << kv.second << ":" << cs.estimate(kv.first) << std::endl;
    }
  }

  /**
   * This benchmark compares the RMSE for predicting the frequency of objects in a stream when
   * for two sketches: the CountMin sketch and the CountSketch.
   * The synthetic data set we create has a fixed number of objects (in this case simply integers)
   * and we create a stream where each object is observed a given number of times. We consider
   * the situation where the frequency is uniform across all items and where the frequency
   * has a geometric distribution (more or less); we keep the expected frequency per user fixed.
   *
   * Two metrics are chosen at this point: RMSE for the 20 most common items and RMSE for the 20
   * least common items.
   *
   * We vary the width and depth of each sketch.
   *
   * The columns of the results table are:
   *   - type of sketch
   *   - number of hash functions (depth)
   *   - number of bits (2^b is the number of bins, i.e. width)
   *   - number of unique objects included in sketch
   *   - 0 if all objects appear with the same frequecy; 1 if exponentially distributed
   *   - RMSE of the observed vs. predicted frequency for the 20 most rare items
   *   - RMSE of the observed vs. predicted frequency for the 20 most common items
   *   - # updates / second (in millions)
   *   - "compression ratio": The size of the sketch / the number of unique elements
   *   - density of the sketch: proportion of nonzero elements in the counts matrix
   */
  void test_benchmark() {

    bool verbose = false;
    turi::random::seed(1002);

    // Set up synthetic data
    size_t num_to_compare = 20;               // number of items to use when computing RMSE
    size_t num_unique = 100000;   // number of unique objects
    size_t mean_count_per_item = 15;          // expected number of observations per object

    // Set up experiment
    std::vector<size_t> num_hash{5, 10};      // number of hash functions to use for each sketch
    std::vector<size_t> bits{8, 10, 12, 14};  // number of bins to use for each sketch (2^bits)

    // Set up reporting
    std::cout.precision(5);
    std::cout << "\nsketch\t# hash\t# bits\t# uniq\texpon.\trmse_r\trmse_c\t"
              << "#items(M)/s\tratio\tdensity" << std::endl;

    // Consider both uniformly distributed and exponentially distributed per-object frequecies
    for (auto expo : {true, false}) {

      // Generate data
      auto items = item_counts(num_unique, mean_count_per_item, expo);

      for (auto h: num_hash) {
        for (auto b: bits) {

          std::vector<std::string> sketch_names = {"CountSketch", "CountMinSketch"};
          for (auto sk : sketch_names) {
            std::map<std::string, double> res;

            // Create sketch
            if (sk == "CountSketch") {
              turi::sketches::countsketch<size_t> cs(b, h);
              res = run_experiment(items, cs, num_to_compare, verbose);
            } else {
              turi::sketches::countmin<size_t> cm(b, h);
              res = run_experiment(items, cm, num_to_compare, verbose);
            }

            // Compute number of updates per second
            double rate = (double) res["count"] / res["elapsed"] / 1000000;

            // Compute "compression ratio": The size of the sketch / the number of unique elements
            double ratio = (double) (h * (1<<b)) / (double) num_unique;

            std::cout << sk << "\t" << h << "\t" << b << "\t" <<
              num_unique << "\t" << expo << "\t" <<
              res["rmse_rare"] << "\t" << res["rmse_common"] << "\t" <<
              rate << "\t" << ratio << "\t" << res["density"] << std::endl;
          }
        }
      }
    }

    }
};

BOOST_FIXTURE_TEST_SUITE(_countsketch_test, countsketch_test)
BOOST_AUTO_TEST_CASE(test_small_example) {
  countsketch_test::test_small_example();
}
BOOST_AUTO_TEST_CASE(test_benchmark) {
  countsketch_test::test_benchmark();
}
BOOST_AUTO_TEST_SUITE_END()


#ifndef GEOMETRY_UTILS_HPP
#define GEOMETRY_UTILS_HPP
/**
 * @file geometry_utils.hpp
 * @author Rafael Rey (rreyarc@upo.es)
 * @brief A set of geometry utilities functions
 * @version 0.1
 * @date 2021-06-29
 *
 * @copyright Copyright (c) 2021
 *
 */
#include <iostream>
#include <vector>
#include <math.h>
#include <Eigen/Dense>

#include "utils/utils.hpp"
#include "utils/LineOfSight.hpp"
#include "utils/world.hpp"

namespace Planners
{
    namespace utils
    {
        namespace geometry
        {
            /**
             * @brief Return the integrated distance alongside the _path object with the _resolution given
             *
             * @param _path CoordinateList object of points (discrete)
             * @param _resolution
             * @return float continous path length
             */
            float calculatePathLength(const CoordinateList &_path, const double &_resolution);

            /**
             * @brief Get the Adjacent Path object
             *
             * @param _path
             * @param _algorithm
             * @return utils::CoordinateList
             */
            utils::CoordinateList getAdjacentPath(const utils::CoordinateList &_path, const utils::DiscreteWorld &_world);
            /**
             * @brief Discrete distance multiplied by dist_scale_factor_
             *
             * @param n1
             * @param n2
             * @return unsigned int
             */
            unsigned int distanceBetween2Nodes(const Node &_n1, const Node &_n2);
            /**
             * @brief  Discrete distance multiplied by dist_scale_factor_
             *
             * @param n1
             * @param n2
             * @return unsigned int
             */
            unsigned int distanceBetween2Nodes(const Node *_n1, const Node *_n2);

            /**
             * @brief
             *
             * @param _v1
             * @param _v2
             * @return unsigned int
             */
            unsigned int distanceBetween2Nodes(const Vec3i &_v1, const Vec3i &_v2);
            /**
             * @brief Discrete distance
             *
             * @param n1
             * @param n2
             * @return unsigned int
             */
            unsigned int NodesBetween2Nodes(const Node &_n1, const Node &_n2);
            /**
             * @brief  Discrete distance             *
             * @param n1
             * @param n2
             * @return unsigned int
             */
            unsigned int NodesBetween2Nodes(const Node *_n1, const Node *_n2);

            /**
             * @brief
             *
             * @param _v1
             * @param _v2
             * @return unsigned int
             */
            unsigned int NodesBetween2Nodes(const Vec3i &_v1, const Vec3i &_v2);

            /**
             * @brief Returns the absolute value vector
             *
             * @param _vec
             * @return Vec3i
             */
            Vec3i abs(const Vec3i &_vec);

            int dotProduct(const Vec3i &_v1, const Vec3i &_v2);

            double moduleVector(const Vec3i &_v);

            double angleBetweenThreePoints(const Vec3i &_v1, const Vec3i &_v2, const Vec3i &_v3);

            double angleBetweenThreePoints(const Eigen::Vector3d &_v1, const Eigen::Vector3d &_v2, const Eigen::Vector3d &_v3);

            double getCircunferenceRadius(const Vec3i &_v1, const Vec3i &_v2, const Vec3i &_v3);

            double getCircunferenceRadius(const Eigen::Vector3d &_v1, const Eigen::Vector3d &_v2, const Eigen::Vector3d &_v3);

        }//namespace geometry
    }//namespace utils
}

#endif


#ifndef __PROBABILITY_DISTRIBUTIONS__ASYMMETRIC_LAPLACE_HPP__
#define __PROBABILITY_DISTRIBUTIONS__ASYMMETRIC_LAPLACE_HPP__

#include "asymmetric_distribution.hpp"
#include "laplace.hpp"

#include <boost/random/exponential_distribution.hpp>

namespace ProbabilityDistributions {
  template <class D, class W = D, class T = W>
  class AsymmetricLaplace:
    public AsymmetricDistribution<AsymmetricLaplace<D,W,T>,D,W,T> {
    public:
      AsymmetricLaplace(T p, T mu, T lambda);

      static constexpr unsigned int sample_size = 1;

      void fix_lambda(bool fixed = true) { fixed_lambda_ = fixed; }
      bool is_lambda_fixed() const { return fixed_lambda_; }
      void set_lambda(T lambda) { assert(lambda > 0); lambda_ = lambda; }
      T get_lambda() const { return lambda_; }

      AsymmetricLaplace<D,W,T> const&
        operator=(AsymmetricLaplace<D,W,T> const& other) {
          base_class::set_p(other.get_p());
          base_class::set_mu(other.get_mu());
          set_lambda(other.get_lambda());
          base_class::fixed_p_ = other.is_p_fixed();
          base_class::fixed_mu_ = other.is_mu_fixed();
          fixed_lambda_ = other.is_lambda_fixed();
          return *this;
        }
      AsymmetricLaplace<D,W,T> const&
        operator=(Laplace<D,W,T> const& other) {
          base_class::set_p(0.5);
          base_class::set_mu(other.get_mu());
          set_lambda(other.get_lambda());
          base_class::fixed_p_ = true;
          base_class::fixed_mu_ = other.is_mu_fixed();
          fixed_lambda_ = other.is_lambda_fixed();
          return *this;
        }

    private:
      typedef AsymmetricDistribution<AsymmetricLaplace<D,W,T>,D,W,T> base_class;
      friend class AsymmetricDistribution<AsymmetricLaplace<D,W,T>,D,W,T>;

      boost::random::exponential_distribution<T> create_gamma_plus() const;
      boost::random::exponential_distribution<T> create_gamma_minus() const;

      T constant_likelihood() const;
      T negative_ll(T s, T mu) const;
      T positive_ll(T s, T mu) const;
      void set_parameter_vector(std::vector<T> const& p) {
        assert(p.size() == 1); lambda_ = p[0]; }
      std::vector<T> get_parameter_vector() const { return {lambda_}; }

      void init_MLE(MA::ConstArray<D> const& data,
          MA::ConstArray<W> const& weight, std::vector<size_t> const& indexes);
      void end_MLE();
      void updated_p();

      void MLE_fixed_p(MA::ConstArray<D> const& data,
          MA::ConstArray<W> const& weight, std::vector<size_t> const& indexes);

      bool fixed_lambda_;
      T lambda_, alpha_, alpha_inv_;
      std::vector<T> pos_sum_all_0_, pos_sum_all_1_;
      std::vector<T> neg_sum_all_0_, neg_sum_all_1_;
  };
};

#include "asymmetric_laplace_impl.hpp"

#endif


#include <NTL/vec_ZZ_pE.h>

NTL_START_IMPL

void InnerProduct(ZZ_pE& x, const vec_ZZ_pE& a, const vec_ZZ_pE& b)
{
   long n = min(a.length(), b.length());
   long i;
   ZZ_pX accum, t;

   clear(accum);
   for (i = 0; i < n; i++) {
      mul(t, rep(a[i]), rep(b[i]));
      add(accum, accum, t);
   }

   conv(x, accum);
}

void InnerProduct(ZZ_pE& x, const vec_ZZ_pE& a, const vec_ZZ_pE& b,
                  long offset)
{
   if (offset < 0) LogicError("InnerProduct: negative offset");
   if (NTL_OVERFLOW(offset, 1, 0)) ResourceError("InnerProduct: offset too big");

   long n = min(a.length(), b.length()+offset);
   long i;
   ZZ_pX accum, t;

   clear(accum);
   for (i = offset; i < n; i++) {
      mul(t, rep(a[i]), rep(b[i-offset]));
      add(accum, accum, t);
   }

   conv(x, accum);
}

void mul(vec_ZZ_pE& x, const vec_ZZ_pE& a, const ZZ_pE& b_in)
{
   ZZ_pE b = b_in;
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      mul(x[i], a[i], b);
}

void mul(vec_ZZ_pE& x, const vec_ZZ_pE& a, const ZZ_p& b_in)
{
   NTL_ZZ_pRegister(b);
   b = b_in;
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      mul(x[i], a[i], b);
}

void mul(vec_ZZ_pE& x, const vec_ZZ_pE& a, long b_in)
{
   NTL_ZZ_pRegister(b);
   b = b_in;
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      mul(x[i], a[i], b);
}

void add(vec_ZZ_pE& x, const vec_ZZ_pE& a, const vec_ZZ_pE& b)
{
   long n = a.length();
   if (b.length() != n) LogicError("vector add: dimension mismatch");

   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      add(x[i], a[i], b[i]);
}

void sub(vec_ZZ_pE& x, const vec_ZZ_pE& a, const vec_ZZ_pE& b)
{
   long n = a.length();
   if (b.length() != n) LogicError("vector sub: dimension mismatch");

   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      sub(x[i], a[i], b[i]);
}

void negate(vec_ZZ_pE& x, const vec_ZZ_pE& a)
{
   long n = a.length();

   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      negate(x[i], a[i]);
}

void clear(vec_ZZ_pE& x)
{
   long n = x.length();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

long IsZero(const vec_ZZ_pE& a)
{
   long n = a.length();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

vec_ZZ_pE operator+(const vec_ZZ_pE& a, const vec_ZZ_pE& b)
{
   vec_ZZ_pE res;
   add(res, a, b);
   NTL_OPT_RETURN(vec_ZZ_pE, res);
}

vec_ZZ_pE operator-(const vec_ZZ_pE& a, const vec_ZZ_pE& b)
{
   vec_ZZ_pE res;
   sub(res, a, b);
   NTL_OPT_RETURN(vec_ZZ_pE, res);
}

vec_ZZ_pE operator-(const vec_ZZ_pE& a)
{
   vec_ZZ_pE res;
   negate(res, a);
   NTL_OPT_RETURN(vec_ZZ_pE, res);
}

ZZ_pE operator*(const vec_ZZ_pE& a, const vec_ZZ_pE& b)
{
   ZZ_pE res;
   InnerProduct(res, a, b);
   return res;
}

void VectorCopy(vec_ZZ_pE& x, const vec_ZZ_pE& a, long n)
{
   if (n < 0) LogicError("VectorCopy: negative length");
   if (NTL_OVERFLOW(n, 1, 0)) ResourceError("overflow in VectorCopy");

   long m = min(n, a.length());

   x.SetLength(n);

   long i;

   for (i = 0; i < m; i++)
      x[i] = a[i];

   for (i = m; i < n; i++)
      clear(x[i]);
}

void random(vec_ZZ_pE& x, long n)
{
   x.SetLength(n);
   for (long i = 0; i < n; i++) random(x[i]);
}

NTL_END_IMPL


/*
 * Polygon.hpp
 *
 *  Created on: Nov 7, 2014
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#pragma once

#include <grid_map_core/TypeDefs.hpp>

// STD
#include <vector>

// Eigen
#include <Eigen/Core>

namespace grid_map {

class Polygon
{
 public:

  enum class TriangulationMethods {
    FAN // Fan triangulation (only for convex polygons).
  };

  /*!
   * Default constructor.
   */
  Polygon();

  /*!
   * Constructor with vertices.
   * @param vertices the points of the polygon.
   */
  Polygon(std::vector<Position> vertices);

  /*!
   * Destructor.
   */
  virtual ~Polygon();

  /*!
   * Check if point is inside polygon.
   * @param point the point to be checked.
   * @return true if inside, false otherwise.
   */
  bool isInside(const Position& point) const;

  /*!
   * Add a vertex to the polygon
   * @param vertex the point to be added.
   */
  void addVertex(const Position& vertex);

  /*!
   * Get the vertex with index.
   * @param index the index of the requested vertex.
   * @return the requested vertex.
   */
  const Position& getVertex(const size_t index) const;

  /*!
   * Removes all vertices from the polygon.
   */
  void removeVertices();

  /*!
   * Get vertex operator overload.
   * @param index the index of the requested vertex.
   * @return the requested vertex.
   */
  const Position& operator [](const size_t index) const;

  /*!
   * Returns the vertices of the polygon.
   * @return the vertices of the polygon.
   */
  const std::vector<Position>& getVertices() const;

  /*!
   * Returns the number of vertices.
   * @return the number of vertices.
   */
  size_t nVertices() const;

  /*!
   * Set the timestamp of the polygon.
   * @param timestamp the timestamp to set (in  nanoseconds).
   */
  void setTimestamp(const uint64_t timestamp);

  /*!
   * Get the timestamp of the polygon.
   * @return timestamp in nanoseconds.
   */
  uint64_t getTimestamp() const;

  /*!
   * Resets the timestamp of the polygon (to zero).
   */
  void resetTimestamp();

  /*!
   * Set the frame id of the polygon.
   * @param frameId the frame id to set.
   */
  void setFrameId(const std::string& frameId);

  /*!
   * Get the frameId of the polygon.
   * @return frameId.
   */
  const std::string& getFrameId() const;

  /*!
   * Get the area of the polygon. The polygon has to be
   * "simple", i.e. not crossing itself.
   * @return area of the polygon.
   */
  double getArea() const;

  /*!
   * Get the centroid of polygon. The polygon has to be
   * "simple", i.e. not crossing itself.
   * @return centroid of polygon.
   */
  Position getCentroid() const;

  /*!
   * Gets the bounding box of the polygon.
   * @param center the center of the bounding box.
   * @param length the side lengths of the bounding box.
   */
  void getBoundingBox(Position& center, Length& length) const;

  /*!
   * Convert polygon to inequality constraints which most tightly contain the points; i.e.,
   * create constraints to bound the convex hull of polygon. The inequality constraints are
   * represented as A and b, a set of constraints such that A*x <= b defining the region of
   * space enclosing the convex hull.
   * Based on the VERT2CON MATLAB method by Michael Kleder:
   * http://www.mathworks.com/matlabcentral/fileexchange/7895-vert2con-vertices-to-constraints
   * @param A the A matrix in of the inequality constraint.
   * @param b the b matrix in of the inequality constraint.
   * @return true if conversion successful, false otherwise.
   */
  bool convertToInequalityConstraints(Eigen::MatrixXd& A,
                                      Eigen::VectorXd& b) const;

  /*!
   * Offsets the polygon inward (buffering) by a margin.
   * Use a negative margin to offset the polygon outward.
   * @param margin the margin to offset the polygon by (in [m]).
   * @return true if successful, false otherwise.
   */
  bool offsetInward(const double margin);

  /*!
   * If only two verices are given, this methods generates a
   * `thickened` line polygon with four vertices.
   * @param thickness the desired thickness of the line.
   * @return true if successful, false otherwise.
   */
  bool thickenLine(const double thickness);

  /*!
   * Return a triangulated version of the polygon.
   * @return a list of triangle polygons covering the same polygon.
   */
  std::vector<Polygon> triangulate(const TriangulationMethods& method = TriangulationMethods::FAN) const;

  /*!
   * Approximates a circle with a polygon.
   * @param[in] center the center position of the circle.
   * @param[in] radius radius of the circle.
   * @param[in] nVertices number of vertices of the approximation polygon. Default = 20.
   * @return circle as polygon.
   */
  static Polygon fromCircle(const Position center, const double radius,
                            const int nVertices = 20);

  /*!
   * Approximates two circles with a convex hull and returns it as polygon.
   * @param[in] center1 the center position of the first circle.
   * @param[in] center2 the center position of the second circle.
   * @param[in] radius radius of the circles.
   * @param[in] nVertices number of vertices of the approximation polygon. Default = 20.
   * @return convex hull of the two circles as polygon.
   */
  static Polygon convexHullOfTwoCircles(const Position center1,
                                        const Position center2,
                                        const double radius,
                                        const int nVertices = 20);

  /*!
   * Computes the convex hull of two polygons and returns it as polygon.
   * @param[in] polygon1 the first input polygon.
   * @param[in] polygon2 the second input polygon.
   * @return convex hull as polygon.
   */
  static Polygon convexHull(Polygon& polygon1, Polygon& polygon2);

 protected:

  /*!
   * Returns true if the vector1 and vector2 are sorted lexicographically.
   * @param[in] vector1 the first input vector.
   * @param[in] vector2 the second input vector.
   */
  static bool sortVertices(const Eigen::Vector2d& vector1,
                           const Eigen::Vector2d& vector2);

  /*!
   * Returns the 2D cross product of vector1 and vector2.
   * @param[in] vector1 the first input vector.
   * @param[in] vector2 the second input vector.
   */
  static double computeCrossProduct2D(const Eigen::Vector2d& vector1,
                                      const Eigen::Vector2d& vector2);

  //! Frame id of the polygon.
  std::string frameId_;

  //! Timestamp of the polygon (nanoseconds).
  uint64_t timestamp_;

  //! Vertices of the polygon.
  std::vector<Position> vertices_;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} /* namespace grid_map */


// ------------------------------------------------------------
// Copyright (c) Microsoft Corporation.  All rights reserved.
// Licensed under the MIT License (MIT). See License.txt in the repo root for license information.
// ------------------------------------------------------------

#include "stdafx.h"

#include <boost/test/unit_test.hpp>
#include "Common/boost-taef.h"
#include "Common/Math.h"
#include <stdlib.h>

namespace Common
{
    BOOST_AUTO_TEST_SUITE2(MathTests)

    BOOST_AUTO_TEST_CASE(RoundToTest)
    {
        double value = 1234.5678;
        double result = RoundTo(value, 2);
        VERIFY_ARE_EQUAL2(result, 1234.57);

        value = 1234.5678;
        result = RoundTo(value, 4);
        VERIFY_ARE_EQUAL2(result, 1234.5678);

        value = 1234;
        result = RoundTo(value, 0);
        VERIFY_ARE_EQUAL2(result, 1234);

        value = 1234.5;
        result = RoundTo(value, 1);
        VERIFY_ARE_EQUAL2(result, 1234.5);

        value = -45.67;
        result = RoundTo(value, 1);
        VERIFY_ARE_EQUAL2(result, -45.7);

        value = -1.12;
        result = RoundTo(value, 1);
        VERIFY_ARE_EQUAL2(result, -1.1);
    }

    BOOST_AUTO_TEST_SUITE_END()
};


#include "matheval.hpp"

#include <boost/math/constants/constants.hpp>

#include <string>

#define EXPRTEST(casename, expr, expected)                             \
BOOST_AUTO_TEST_CASE( casename )                                       \
{                                                                      \
    std::string const s = expr;                                        \
    std::map<std::string, double> st;                        \
    double result;                                                     \
    BOOST_CHECK_NO_THROW(result = matheval::parse(s, st));     \
    BOOST_CHECK_CLOSE(result, (expected),                              \
                      std::numeric_limits<double>::epsilon());         \
}

#define SYMEXPRTEST(casename, expr, st, expected)                      \
BOOST_AUTO_TEST_CASE( casename )                                       \
{                                                                      \
    std::string const s = expr;                                        \
    double result;                                                     \
    BOOST_CHECK_NO_THROW(result = matheval::parse(s, st));     \
    BOOST_CHECK_CLOSE(result, (expected),                              \
                      std::numeric_limits<double>::epsilon());         \
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_SFUNCTOR_INCLUDE
#define MTL_SFUNCTOR_INCLUDE

#include <cmath>
#include <complex>

#include <boost/numeric/mtl/concept/std_concept.hpp>
#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/mtl/concept/static_functor.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>
#include <boost/type_traits.hpp>

namespace mtl { namespace sfunctor {

template <typename Value1, typename Value2>
struct plus
{
    typedef const Value1&                                 first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef typename Addable<Value1, Value2>::result_type result_type;

    static inline result_type apply(const Value1& v1, const Value2& v2)
    {
  return v1 + v2;
    }

    result_type operator() (const Value1& v1, const Value2& v2) const
    {
  vampir_trace<23> tracer;
  return v1 + v2;
    }
};

template <typename Value1, typename Value2>
struct minus
{
    typedef const Value1&                                 first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef typename Subtractable<Value1, Value2>::result_type result_type;

    static inline result_type apply(const Value1& v1, const Value2& v2)
    {
  return v1 - v2;
    }

    result_type operator() (const Value1& v1, const Value2& v2) const
    {
  vampir_trace<24> tracer;
  return v1 - v2;
    }
};

template <typename Value1, typename Value2>
struct times
{
    typedef const Value1&                                 first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef typename Multiplicable<Value1, Value2>::result_type result_type;

    static inline result_type apply(const Value1& v1, const Value2& v2)
    {
  return v1 * v2;
    }

    result_type operator() (const Value1& v1, const Value2& v2) const
    {
  vampir_trace<25> tracer;
  return v1 * v2;
    }
};

template <typename Value1, typename Value2>
struct divide
{
    typedef const Value1&                                 first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef typename Divisible<Value1, Value2>::result_type result_type;

    static inline result_type apply(const Value1& v1, const Value2& v2)
    {
  return v1 / v2;
    }

    result_type operator() (const Value1& v1, const Value2& v2) const
    {
  vampir_trace<26> tracer;
  return v1 / v2;
    }
};

template <typename Value1, typename Value2>
struct assign
{
    typedef Value1&                                       first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef Value1&                                       result_type;

    static inline result_type apply(Value1& v1, const Value2& v2)
    {
  return v1= Value1(v2);
    }

    result_type operator() (Value1& v1, const Value2& v2) const
    {
  vampir_trace<27> tracer;
  return v1= v2;
    }
};

template <typename Value1, typename Value2>
struct plus_assign
{
    typedef Value1&                                       first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef Value1&                                       result_type;

    static inline result_type apply(Value1& v1, const Value2& v2)
    {
  return v1+= v2;
    }

    result_type operator() (Value1& v1, const Value2& v2) const
    {
  vampir_trace<28> tracer;
  return v1+= v2;
    }
};

template <typename Value1, typename Value2>
struct minus_assign
{
    typedef Value1&                                       first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef Value1&                                       result_type;

    static inline result_type apply(Value1& v1, const Value2& v2)
    {
  return v1-= v2;
    }

    result_type operator() (Value1& v1, const Value2& v2) const
    {
  vampir_trace<29> tracer;
  return v1-= v2;
    }
};

template <typename Value1, typename Value2>
struct times_assign
{
    typedef Value1&                                       first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef Value1&                                       result_type;

    static inline result_type apply(Value1& v1, const Value2& v2)
    {
  return v1*= v2;
    }

    result_type operator() (Value1& v1, const Value2& v2) const
    {
  vampir_trace<30> tracer;
  return v1*= v2;
    }
};

template <typename Value1, typename Value2>
struct divide_assign
{
    typedef Value1&                                       first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef Value1&                                       result_type;

    static inline result_type apply(Value1& v1, const Value2& v2)
    {
  return v1/= v2;
    }

    result_type operator() (Value1& v1, const Value2& v2) const
    {
  vampir_trace<31> tracer;
  return v1/= v2;
    }
};

// Might be helpful for surplus functor arguments
template <typename Value>
struct identity
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
  return v;
    }

    result_type operator() (const Value& v) const
    {
  vampir_trace<32> tracer;
  return v;
    }
};

template <typename Value>
struct abs
{
    typedef const Value&                                  argument_type;
    typedef typename Magnitude<Value>::type               result_type;

    static inline result_type apply(const Value& v)
    {
  using std::abs;
  return abs(v);
    }

    result_type operator() (const Value& v)  const
    {
  vampir_trace<33> tracer;
  return apply(v);
    }
};

template <typename Value>
struct sqrt
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
  using std::sqrt;
  return sqrt(v);
    }

    result_type operator() (const Value& v)  const
    {
  vampir_trace<34> tracer;
  return apply(v);
    }
};

template <typename Value>
struct square
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
  return v * v;
    }

    result_type operator() (const Value& v) const
    {
  vampir_trace<35> tracer;
  return apply(v);
    }
};

template <typename Value>
struct negate
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v) { return -v;  }
    result_type operator() (const Value& v) const
    {
  vampir_trace<36> tracer;
  return -v;
    }
};

template <typename Value>
struct acos
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::acos;
        return acos(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

# ifdef MTL_WITH_MATH_ELEVEN
template <typename Value>
struct acosh
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::acosh;
        return acosh(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};
# endif

template <typename Value>
struct asin
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::asin;
        return asin(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

# ifdef MTL_WITH_MATH_ELEVEN
template <typename Value>
struct asinh
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::asinh;
        return asinh(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};
# endif

template <typename Value>
struct atan
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::atan;
        return atan(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

# ifdef MTL_WITH_MATH_ELEVEN
template <typename Value>
struct atanh
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::atanh;
        return atanh(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};
# endif

template <typename Value>
struct cos
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::cos;
        return cos(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

template <typename Value>
struct cosh
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::cosh;
        return cosh(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

template <typename Value>
struct sin
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::sin;
        return sin(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

template <typename Value>
struct sinh
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::sinh;
        return sinh(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

template <typename Value>
struct tan
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::tan;
        return tan(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

template <typename Value>
struct tanh
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::tanh;
        return tanh(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

// Rounding functions

template <typename Value>
struct ceil
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        return apply(v, boost::is_integral<Value>());
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }

private:
    static inline result_type apply(const Value& v, boost::integral_constant<bool, false>)
    {
        using std::ceil;
        return ceil(v);
    }

    // return value directly for integer values
    static inline result_type apply(const Value& v, boost::integral_constant<bool, true>)
    {
        return v;
    };
};

template <typename Value>
struct floor
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        return apply(v, boost::is_integral<Value>());
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }

private:
    static inline result_type apply(const Value& v, boost::integral_constant<bool, false>)
    {
        using std::floor;
        return floor(v);
    }

    // return value directly for integer values
    static inline result_type apply(const Value& v, boost::integral_constant<bool, true>)
    {
        return v;
    };
};

# ifdef MTL_WITH_MATH_ELEVEN

    template <typename Value>
    struct round
    {
        typedef const Value&                                  argument_type;
        typedef Value                                         result_type;

        static inline result_type apply(const Value& v)
        {
            return apply(v, boost::is_integral<Value>());
        }
        result_type operator() (const Value& v) const
        {
            return apply(v);
        }

    private:
        static inline result_type apply(const Value& v, boost::integral_constant<bool, false>)
        {
            using std::round;
            return round(v);
        }

        // return value directly for integer values
        static inline result_type apply(const Value& v, boost::integral_constant<bool, true>)
        {
            return v;
        };
    };

    template <typename Value>
    struct trunc
    {
        typedef const Value&                                  argument_type;
        typedef Value                                         result_type;

        static inline result_type apply(const Value& v)
        {
            return apply(v, boost::is_integral<Value>());
        }
        result_type operator() (const Value& v) const
        {
            return apply(v);
        }

    private:
        static inline result_type apply(const Value& v, boost::integral_constant<bool, false>)
        {
            using std::trunc;
            return trunc(v);
        }

        // return value directly for integer values
        static inline result_type apply(const Value& v, boost::integral_constant<bool, true>)
        {
            return v;
        };
    };

# endif

// Logarithmic functions

template <typename Value>
struct log
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::log;
        return log(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

# ifdef MTL_WITH_MATH_ELEVEN
template <typename Value>
struct log2
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::log2;
        return log2(v);
    }
    result_type operator() (const Value& v) const
    {
        return apply(v);
    }
};
# endif

template <typename Value>
struct log10
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::log10;
        return log10(v);
    }
    result_type operator() (const Value& v) const
    {
        return apply(v);
    }
};

// Exponential functions

template <typename Value>
struct exp
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::exp;
        return exp(v);
    }
    result_type operator() (const Value& v) const
    {
        return apply(v);
    }
};

# ifdef MTL_WITH_MATH_ELEVEN
template <typename Value>
struct exp2
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::exp2;
        return exp2(v);
    }
    result_type operator() (const Value& v) const
    {
        return apply(v);
    }
};
#endif

template <typename Value>
struct exp10
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::exp;
        return exp(v * 2.302585092994045684017991454684364207601101488628772976033);
    }
    result_type operator() (const Value& v) const
    {
        return apply(v);
    }
};

// Inverse square root functions

template <typename Value>
struct rsqrt
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::pow;
        return pow(v, -0.5);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

// Error functions

# ifdef MTL_WITH_MATH_ELEVEN
template <typename Value>
struct erf
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::erf;
        return erf(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

template <typename Value>
struct erfc
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::erfc;
        return erfc(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};
# endif

/// Compose functors \p F and \p G, i.e. compute f(g(x)).
/** Functors must be models of StaticUnaryFunctor,
    StaticUnaryFunctor<G>::result_type must be convertible to
    StaticUnaryFunctor<F>::argument_type.
    Under these conditions compose<F, G> will be a model of StaticUnaryFunctor.
**/
template <typename F, typename G>
struct compose
{
    typedef typename StaticUnaryFunctor<G>::argument_type argument_type;
    typedef typename StaticUnaryFunctor<F>::result_type   result_type;

    static inline result_type apply(argument_type x)
    {
  return F::apply(G::apply(x));
    }

    result_type operator()(argument_type x)
    {
  vampir_trace<37> tracer;
  return apply(x);
    }
};

/// Compose functors \p F and \p G with G in F's first argument, i.e. compute f(g(x), y).
/** F/G must be models of StaticBinaryFunctor/StaticUnaryFunctor,
    StaticUnaryFunctor<G>::result_type must be convertible to
    StaticBinaryFunctor<F>::first_argument_type.
    Under these conditions compose_first<F, G> will be a model of StaticBinaryFunctor.
**/
template <typename F, typename G>
struct compose_first
{
    typedef typename StaticUnaryFunctor<G>::argument_type         first_argument_type;
    typedef typename StaticBinaryFunctor<F>::second_argument_type second_argument_type;
    typedef typename StaticBinaryFunctor<F>::result_type          result_type;

    static inline result_type apply(first_argument_type x, second_argument_type y)
    {
  return F::apply(G::apply(x), y);
    }

    result_type operator()(first_argument_type x, second_argument_type y)
    {
  vampir_trace<38> tracer;
  return apply(x, y);
    }
};

/// Compose functors \p F and \p G with G in F's second argument, i.e. compute f(x, g(y)).
/** F/G must be models of StaticBinaryFunctor/StaticUnaryFunctor,
    StaticUnaryFunctor<G>::result_type must be convertible to
    StaticBinaryFunctor<F>::second_argument_type.
    Under these conditions compose_second<F, G> will be a model of StaticBinaryFunctor.
**/
template <typename F, typename G>
struct compose_second
{
    typedef typename StaticBinaryFunctor<F>::first_argument_type  first_argument_type;
    typedef typename StaticUnaryFunctor<G>::argument_type         second_argument_type;
    typedef typename StaticBinaryFunctor<F>::result_type          result_type;

    static inline result_type apply(first_argument_type x, second_argument_type y)
    {
  return F::apply(x, G::apply(y));
    }

    result_type operator()(first_argument_type x, second_argument_type y)
    {
  vampir_trace<39> tracer;
  return apply(x, y);
    }
};

/// Compose functors \p F, \p G, and \p H with G/H in F's first/second argument, i.e. compute f(g(x), h(y)).
/** F/G must be models of StaticBinaryFunctor/StaticUnaryFunctor,
    StaticUnaryFunctor<G>::result_type must be convertible to
    StaticBinaryFunctor<F>::first_argument_type and
    StaticUnaryFunctor<H>::result_type must be convertible to
    StaticBinaryFunctor<F>::second_argument_type.
    Under these conditions compose_both<F, G, H> will be a model of StaticBinaryFunctor.
**/
template <typename F, typename G, typename H>
struct compose_both
{
    typedef typename StaticUnaryFunctor<G>::argument_type         first_argument_type;
    typedef typename StaticUnaryFunctor<H>::argument_type         second_argument_type;
    typedef typename StaticBinaryFunctor<F>::result_type          result_type;

    static inline result_type apply(first_argument_type x, second_argument_type y)
    {
  return F::apply(G::apply(x), H::apply(y));
    }

    result_type operator()(first_argument_type x, second_argument_type y)
    {
  vampir_trace<40> tracer;
  return apply(x, y);
    }
};

/// Compose unary functor \p F with binary functor \p G, i.e. compute f(g(x, y)).
/** F/G must be models of StaticUnaryFunctor/StaticBinaryFunctor,
    StaticBinaryFunctor<G>::result_type must be convertible to
    StaticUnaryFunctor<F>::argument_type.
    Under these conditions compose_binary<F, G> will be a model of StaticBinaryFunctor.
**/
template <typename F, typename G>
struct compose_binary
{
    typedef typename StaticBinaryFunctor<G>::first_argument_type  first_argument_type;
    typedef typename StaticBinaryFunctor<G>::second_argument_type second_argument_type;
    typedef typename StaticUnaryFunctor<F>::result_type           result_type;

    static inline result_type apply(first_argument_type x, second_argument_type y)
    {
  return F::apply(G::apply(x, y));
    }

    result_type operator()(first_argument_type x, second_argument_type y)
    {
  vampir_trace<41> tracer;
  return apply(x, y);
    }
};

/// Templatized example of composition, computes l_2 norm in 2D, i.e. sqrt(abs(x*x + y*y))
template <typename T>
struct l_2_2D
  : public compose_binary<sqrt<typename abs<T>::result_type>,
        compose_binary<abs<T>,
           compose_both<plus<T, T>,
                  square<T>,
                  square<T>  >
                                        >
                         >
{};

}} // namespace mtl::sfunctor

#endif // MTL_SFUNCTOR_INCLUDE


/**
 * @file EigenDef.hpp
 * @author wenyan4work (wenyan4work@gmail.com)
 * @brief useful definitions of eigen
 * @version 1.0
 * @date 2016-10-27
 *
 * Do not use auto with Eigen
 *
 * @copyright Copyright (c) 2018
 *
 */
#ifndef EIGENDEF_HPP_
#define EIGENDEF_HPP_

#include <cmath>
#include <vector>

#include <Eigen/Cholesky>
#include <Eigen/Dense>
#include <Eigen/Sparse>

/*
 * Do not use auto with Eigen
 *
 * Eigen defaults to column-major storage.
 * Mat(i,j) is not affected by row major or column major storage format
 * */

constexpr double Pi = 3.141592653589793238462643383279;

// unaligned fixed size vectors
using Evec2 = Eigen::Matrix<double, 2, 1, Eigen::DontAlign>;
using Evec3 = Eigen::Matrix<double, 3, 1, Eigen::DontAlign>;
using Evec4 = Eigen::Matrix<double, 4, 1, Eigen::DontAlign>;
using Evec6 = Eigen::Matrix<double, 6, 1, Eigen::DontAlign>;

// unaligned fixed size matrices
using Emat2 = Eigen::Matrix<double, 2, 2, Eigen::DontAlign>;
using Emat3 = Eigen::Matrix<double, 3, 3, Eigen::DontAlign>;
using Emat4 = Eigen::Matrix<double, 4, 4, Eigen::DontAlign>;
using Emat6 = Eigen::Matrix<double, 6, 6, Eigen::DontAlign>;
using EmatPsi = Eigen::Matrix<double, 4, 3, Eigen::DontAlign>; // psi mat for quaternion rotation

// unaligned quaternion
using Equatn = Eigen::Quaternion<double, Eigen::DontAlign>;

// aligned fixed size vectors
using EAvec2 = Eigen::Vector2d;
using EAvec3 = Eigen::Vector3d;
using EAvec4 = Eigen::Vector4d;
using EAvec6 = Eigen::Matrix<double, 6, 1>;

// aligned fixed size matrices
using EAmat2 = Eigen::Matrix2d;
using EAmat3 = Eigen::Matrix3d;
using EAmat4 = Eigen::Matrix4d;
using EAmat6 = Eigen::Matrix<double, 6, 6>;
using EAmatPsi = Eigen::Matrix<double, 4, 3, Eigen::DontAlign>;

// aligned quaternion
using EAquatn = Eigen::Quaterniond;

// map from raw pointer to fixed size type
using Emap3 = Eigen::Map<Evec3, Eigen::Unaligned>;
using ECmap3 = Eigen::Map<const Evec3, Eigen::Unaligned>;
using Emapq = Eigen::Map<Eigen::Quaternion<double>, Eigen::Unaligned>;
using ECmapq = Eigen::Map<const Eigen::Quaternion<double>, Eigen::Unaligned>;

// dynamic size types
using Evec = Eigen::VectorXd;
using Emat = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>; // default to column major
using Espmat = Eigen::SparseMatrix<double>;

/**
 * @brief swap by copy
 *
 * @tparam T
 * @param a
 * @param b
 */
template <typename T>
void copySwap(T &a, T &b) {
    T temp = b;
    b = a;
    a = b;
}

#endif /* EIGENDEF_HPP_ */


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
Copyright (C) 2005, 2007 StatPro Italia srl
Copyright (C) 2015 CompatibL

This file is part of QuantLib, a free-software/open-source library
for financial quantitative analysts and developers - http://quantlib.org/

QuantLib is free software: you can redistribute it and/or modify it
under the terms of the QuantLib license.  You should have received a
copy of the license along with this program; if not, please email
<quantlib-dev@lists.sf.net>. The license is also available online at
<http://quantlib.org/license.shtml>.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

// Based on bermudanswaption.cpp file from test-suite.

#ifndef cl_adjoint_berbudan_swaption_impl_hpp
#define cl_adjoint_berbudan_swaption_impl_hpp
#pragma once

#include "adjointbermudanswaptiontest.hpp"
#include "utilities.hpp"
#include "adjointtestutilities.hpp"
#include "adjointtestbase.hpp"
#include <ql/instruments/swaption.hpp>
#include <ql/pricingengines/swaption/treeswaptionengine.hpp>
#include <ql/pricingengines/swap/discountingswapengine.hpp>
#include <ql/pricingengines/swaption/fdhullwhiteswaptionengine.hpp>
#include <ql/models/shortrate/onefactormodels/hullwhite.hpp>
#include <ql/cashflows/coupon.hpp>
#include <ql/time/daycounters/thirty360.hpp>
#include <ql/indexes/ibor/euribor.hpp>
#include <ql/time/schedule.hpp>
#include <boost/make_shared.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;

#define OUTPUT_FOLDER_NAME "AdjointBermudanSwaption"

namespace
{
    enum
    {
#if defined CL_GRAPH_GEN
        // Number of points for dependency plots.
        pointNo = 100,
        // Number of points for performance plot.
        iterNo = 30,
        // Step for portfolio size for performance testing .
        step = 1,
#else
        // Number of points for dependency plots.
        pointNo = 0,
        // Number of points for performance plot.
        iterNo = 0,
        // Step for portfolio size for performance testing .
        step = 1,
#endif
        // Defines performance accuracy. Its value is a minimum number
        // of calling of O(1) complexity methods per one performance test.
        iterNumFactor = 0,
    };

    enum EngineType { tree, fdHullWhite };

    std::string toString(EngineType engine)
    {
        switch (engine)
        {
        case tree:
            return "TreeSwaptionEngine";
        case fdHullWhite:
            return "FdHullWhiteSwaptionEngine";
        default:
            return "";
        }
    }

    struct CommonVars
    {
        CommonVars()
        : backup_()
        , startYears_(1)
        , length_(5)
        , type_(VanillaSwap::Payer)
        , nominal_(1000.0)
        , fixedConvention_(Unadjusted)
        , floatingConvention_(ModifiedFollowing)
        , fixedFrequency_(Annual)
        , floatingFrequency_(Semiannual)
        , fixedDayCount_(Thirty360())
        , termStructure_()
        , index_(boost::shared_ptr<IborIndex>(new Euribor6M(termStructure_)))
        , settlementDays_(2)
        , calendar_(index_->fixingCalendar())
        , today_(calendar_.adjust(Date::todaysDate()))
        , settlement_(calendar_.advance(today_, settlementDays_, Days))
        , fixedRate_(0.4)
        {
        }

        // Provides swaps which are different only by their fixed rate strike.
        boost::shared_ptr<VanillaSwap> makeSwap(Spread spread)
        {
            Date start = calendar_.advance(settlement_, startYears_, Years);
            Date maturity = calendar_.advance(start, length_, Years);
            Schedule fixedSchedule(start, maturity,
                                   Period(fixedFrequency_),
                                   calendar_,
                                   fixedConvention_,
                                   fixedConvention_,
                                   DateGeneration::Forward, false);
            Schedule floatSchedule(start, maturity,
                                   Period(floatingFrequency_),
                                   calendar_,
                                   floatingConvention_,
                                   floatingConvention_,
                                   DateGeneration::Forward, false);
            boost::shared_ptr<VanillaSwap> swap(
                new VanillaSwap(type_, nominal_,
                fixedSchedule, fixedRate_, fixedDayCount_,
                floatSchedule, index_, spread,
                index_->dayCounter()));
            swap->setPricingEngine(boost::shared_ptr<PricingEngine>(
                new DiscountingSwapEngine(termStructure_)));
            return swap;
        }

        // cleanup
        SavedSettings backup_;

        // underlying swap parameters
        Integer startYears_;
        Integer length_;
        VanillaSwap::Type type_;
        Real nominal_;
        BusinessDayConvention fixedConvention_;
        BusinessDayConvention floatingConvention_;
        Frequency fixedFrequency_;
        Frequency floatingFrequency_;
        DayCounter fixedDayCount_;
        RelinkableHandle<YieldTermStructure> termStructure_;
        boost::shared_ptr<IborIndex> index_;
        Natural settlementDays_;

        // global data
        Calendar calendar_;
        Date today_;
        Date settlement_;
        Rate fixedRate_;
    };

    struct SwaptionsData
        : public CommonVars
    {
        explicit SwaptionsData(EngineType engine)
        : CommonVars()
        , engineId_(engine)
        {
            today_ = Date(15, February, 2002);
            Settings::instance().evaluationDate() = today_;
            settlement_ = Date(19, February, 2002);

            // flat yield term structure impling 1x5 swap at 5%
            termStructure_.linkTo(flatRate(settlement_,
                0.04875825,
                Actual365Fixed()));
            atmRate_ = makeSwap(0.0)->fairRate();
            Real a = 0.048696;
            Real sigma = 0.0058904;
            boost::shared_ptr<VanillaSwap> atmSwap = makeSwap(atmRate_);
            const Leg& leg = atmSwap->fixedLeg();
            model_ = boost::make_shared<HullWhite>(termStructure_, a, sigma);
            std::vector<Date> exerciseDates(leg.size());
            for (Size i = 0; i < leg.size(); i++)
            {
                boost::shared_ptr<Coupon> coupon =
                    boost::dynamic_pointer_cast<Coupon>(leg[i]);
                exerciseDates[i] = coupon->accrualStartDate();
            }
            exercise_ = boost::make_shared<BermudanExercise>(exerciseDates);
            switch (engine)
            {
                case tree:
                    engine_ = boost::make_shared<TreeSwaptionEngine>(model_, 200);
                    break;
                case fdHullWhite:
                    engine_ = boost::make_shared<FdHullWhiteSwaptionEngine>(model_, 100, 100, 0, 1e-7);
                    break;
            }
        }

        // Returns net present value (NPV) of swaption with given fixed rate strike and other
        // parameters determined from class instance.
        Real swaptionNVP(Spread spread)
        {
            auto swap = makeSwap(spread);
            Swaption swaption(swap, exercise_);
            swaption.setPricingEngine(engine_);
            return swaption.NPV();
        }

        Rate atmRate_;
        EngineType engineId_;
        boost::shared_ptr<HullWhite> model_;
        boost::shared_ptr<Exercise> exercise_;
        boost::shared_ptr<PricingEngine> engine_;
    };

    // Struct for plots recording.
    struct StrikeSens
    {
        static std::deque<std::string > get_columns()
        {
            static std::deque<std::string > columns =
            {
                "Spread", ""
            };

            return columns;
        }

        template <typename stream_type>
        friend inline stream_type&
            operator << (stream_type& stm, StrikeSens& v)
        {
                stm << v.strike_
                    << ";" << v.sensitivity_
                    << std::endl;
                return stm;
            }

        Real strike_;
        Real sensitivity_;
    };

    struct SwaptionTest
        : public cl::AdjointTest<SwaptionTest>
    {
        SwaptionTest(Size size, SwaptionsData* data, cl::tape_empty_test_output* logger = nullptr)
        : AdjointTest()
        , size_(size)
        , data_(data)
        , spread_(size)
        , prices_()
        , totalPrice_()
        {
            setLogger(logger);

            for (Size i = 0; i < size_; i++)
            {
                spread_[i] = 0.300909 + 0.1 * i / (size_ - 1);
            }
        }

        Size indepVarNumber() { return size_; }

        Size minPerfIteration() { return iterNumFactor; }

        void recordTape()
        {
            cl::Independent(spread_);
            calculatePrices();
            f_ = std::make_unique<cl::tape_function<double>>(spread_, totalPrice_);
        }

        // Calculates price of portfolio and each option.
        void calculatePrices()
        {
            totalPrice_.resize(1, 0);
            prices_.resize(size_);
            for (Size i = 0; i < size_; i++)
            {
                prices_[i] = data_->swaptionNVP(spread_[i]);
                totalPrice_.front() += prices_[i];
            }
        }

        // Calculates derivatives using finite difference method.
        void calcAnalytical()
        {
            Real h = 1e-6 ;  // shift for finite diff. method
            analyticalResults_.resize(size_);
            for (Size i = 0; i < size_; i++)
            {
                analyticalResults_[i] = (data_->swaptionNVP(spread_[i] + h)
                                         - data_->swaptionNVP(spread_[i] - h)) / (2 * h);
            }
        }

        double relativeTol() const { return 1e-2; }

        double absTol() const { return 1e-10; }

        Size size_;
        SwaptionsData* data_;
        std::vector<cl::tape_double> spread_;
        std::vector<Real> prices_;
        std::vector<cl::tape_double> totalPrice_;
    };

    struct TestData
        : public SwaptionsData
    {
        explicit TestData(EngineType engine)
        : SwaptionsData(engine)
        , outPerform_(OUTPUT_FOLDER_NAME "//" + toString(engine), {
            { "title", "Bermudan swaption NPV differentiation performance with respect to spread" }
            , { "not_clear", "Not" }
            , { "ylabel", "Time (s)" }
            , { "xlabel", "Number of spreads" }
            , { "line_box_width", "-5" }
            , { "cleanlog", "true" }
            , { "smooth", "default" }
        })
            , outAdjoint_(OUTPUT_FOLDER_NAME "//" + toString(engine), {
                { "title", "Swaption NPV adjoint differentiation with respect to spread" }
            , { "filename", "Adjoint" }
            , { "not_clear", "Not" }
            , { "ylabel", "Time (s)" }
            , { "xlabel", "Number of spreads" }
            , { "cleanlog", "false" }
            , { "smooth", "default" }
        })
            , outSize_(OUTPUT_FOLDER_NAME "//" + toString(engine), {
                { "title", "Tape size dependence on number of spreads" }
            , { "filename", "TapeSize" }
            , { "not_clear", "Not" }
            , { "ylabel", "Memory (MB)" }
            , { "cleanlog", "false" }
        })
            , out_(OUTPUT_FOLDER_NAME "//" + toString(engine) + "//output", {
                { "filename", "NVPonSpread" }
            , { "not_clear", "Not" }
            , { "title", "Swaption NPV dependence on spread" }
            , { "ylabel", "Swaption NPV" }
            , { "xlabel", "Spread" }
            , { "cleanlog", "false" }
        })
        {
        }

        bool makeOutput()
        {
            bool ok = true;
            if (pointNo > 0)
            {
                recordDependencePlot(pointNo);
            }
            ok &= cl::recordPerformance(*this, iterNo, step);
            return ok;
        }

        std::shared_ptr<SwaptionTest> getTest(size_t size)
        {
            return std::make_shared<SwaptionTest>(size, this, &outPerform_);
        }

        // Makes plots for strike sensitivity dependence.
        void recordDependencePlot(size_t pointNo)
        {
            std::vector<StrikeSens> outData(pointNo);
            auto test = getTest(pointNo);
            test->recordTape();
            for (Size i = 0; i < pointNo; i++)
            {
                outData[i] = { test->spread_[i], test->prices_[i] };
            }
            out_ << outData;
        }

        cl::tape_empty_test_output outPerform_;
        cl::tape_empty_test_output outAdjoint_;
        cl::tape_empty_test_output outSize_;
        cl::tape_empty_test_output out_;
    };
}

#endif


#pragma once

#ifndef MOTION_CORRECTION_HPP
#define MOTION_CORRECTION_HPP

#include <dvs_msgs/Event.h>
#include <ros/time.h>

#include <Eigen/Dense>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/video/tracking.hpp>
#include <string>
#include <vector>

namespace motion_correction {

using EventArray = std::vector<dvs_msgs::Event>;

/**
 * Custom floor function, reduces overhead avoiding overflows check (not
 * required in our use cases)
 *
 * @param x number to round to the floor
 *
 * @return rounded value
 */
static inline int int_floor(float x) {
    int i = (int)x;     /* truncate */
    return i - (i > x); /* convert trunc to floor */
}

/**
 * Resets matrix if already initialized, otherwise instantiates a zero matrix
 *
 * @param arr matrix to reset
 * @param size size of the matrix
 * @param type type of the matrix (default: CV_32F)
 */
static inline void resetMat(cv::Mat& arr, const cv::Size& size,
                            int type = CV_32F) {
    if (arr.cols == 0 || arr.rows == 0) {
        arr = cv::Mat::zeros(size, type);
    } else {
        arr.setTo(0);
    }
}

/**
 * Embeds all the parameters required for the estimation of the warp
 */
class WarpUpdateParams {
   public:
    int warp_mode;              ///< currently supported: cv::MOTION_HOMOGRAPHY
    int num_pyramid_levels;     ///< # pyramid levels used to estimate the warp
    cv::Size sensor_size;       ///< image size
    cv::TermCriteria criteria;  ///< termination criteria for the optimization
    WarpUpdateParams() {}

    WarpUpdateParams(int nIt, double eps, int mode, int lvls,
                     cv::Size sensor_size)
        : warp_mode(mode),
          num_pyramid_levels(lvls),
          criteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, nIt, eps),
          sensor_size(sensor_size) {}
};

/**
 * Initializes warp matrix for the multi-level analysis
 *
 * @param warp warp matrix to process
 * @param params optimization parameters (# pyramid levels)
 */
void initWarp(cv::Mat& warp, const WarpUpdateParams& params);

/**
 * Estimates the warp the better aligns img0 and img1
 *
 * @param warp warp estimate
 * @param img0 first frame
 * @param img1 second frame
 * @param params optimization parameters
 */
void updateWarp(cv::Mat& warp, const cv::Mat& img0, const cv::Mat& img1,
                const WarpUpdateParams& params);

/**
 * Computes optical flow from warp
 *
 * @param warp warp from which to compute the optical flow
 * @param dt time to estimate the velocity field
 * @param sensor_size optical flow image size
 * @param rectified_points rectification table
 *
 * @return computed optical flow
 */
cv::Mat computeFlowFromWarp(const cv::Mat& warp, double dt,
                            cv::Size sensor_size,
                            std::vector<cv::Point2f> rectified_points);

/**
 * Draw undistorted events on image
 *
 * @param ev_first iterator to first event
 * @param ev_last last iterator (included)
 * @param out output events frame
 * @param rectified_points rectification table
 * @param use_polarity whether to account for polarity when drawing events
 */
void drawEventsUndistorted(EventArray::iterator ev_first,
                           EventArray::iterator ev_last, cv::Mat& out,
                           cv::Size sensor_size,
                           const std::vector<cv::Point2f>& rectified_points,
                           const bool use_polarity);

/**
 * Draw motion corrected events from optical flow
 *
 * @param ev_first iterator to first event
 * @param ev_last last iterator (included)
 * @param flow_field optical flow to use to undistort events
 * @param out output events frame
 * @param sensor_size size of the events frame
 * @param rectified_points rectification table
 * @param use_polarity whether to account for polarity when drawing events
 */
void drawEventsMotionCorrectedOpticalFlow(
    EventArray::iterator ev_first, EventArray::iterator ev_last,
    const cv::Mat& flow_field, cv::Mat& out, cv::Size sensor_size,
    const std::vector<cv::Point2f>& rectified_points, const bool use_polarity);

}  // namespace motion_correction

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_DETAIL_PRIMES_HPP
#define CRYPTO3_DETAIL_PRIMES_HPP

#include <boost/integer.hpp>

namespace boost {
    namespace crypto3 {
        namespace detail {

            template<int Bits>
            struct all_ones {
                typedef typename boost::uint_t<Bits>::least type;
                static type const value = (all_ones<Bits - 1>::value << 1) | 1;
            };
            template<>
            struct all_ones<0> {
                typedef boost::uint_t<0>::least type;
                static type const value = 0;
            };

            template<int Bits>
            struct largest_prime;

#define CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(B, D)                              \
    template<>                                                                         \
    struct largest_prime<B> {                                                          \
        constexpr static boost::uint_t<B>::least const value = all_ones<B>::value - D; \
    };                                                                                \
    constexpr boost::uint_t<B>::least const largest_prime<B>::value;

            // http://primes.utm.edu/lists/2small/0bit.html or
            // http://www.research.att.com/~njas/sequences/A013603
            // Though those offets are from 2**b; This code is offsets from 2**b-1
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(2, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(3, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(4, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(5, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(6, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(7, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(8, 4);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(9, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(10, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(11, 8);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(12, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(13, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(14, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(15, 18);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(16, 14);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(17, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(18, 4);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(19, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(20, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(21, 8);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(22, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(23, 14);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(24, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(25, 38);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(26, 4);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(27, 38);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(28, 56);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(29, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(30, 34);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(31, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(32, 4);

        }    // namespace detail
    }        // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_HASH_DETAIL_PRIMES_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file cclgmprocess.hpp
    \brief cross currency lgm model process
           the fx processes are modeled in log spot here
           (different from BlackScholesProcess where it is mixed
           log spot / spot depending on the method)
*/

#ifndef quantlib_cclgm_process_hpp
#define quantlib_cclgm_process_hpp

#include <ql/math/matrixutilities/pseudosqrt.hpp>
#include <ql/experimental/models/cclgmparametrization.hpp>

#include <boost/unordered_map.hpp>

namespace QuantLib {

template <class Impl, class ImplFx, class ImplLgm>
class CcLgmProcess : public StochasticProcess {
  public:
    CcLgmProcess(const boost::shared_ptr<detail::CcLgmParametrization<
                     Impl, ImplFx, ImplLgm> > &parametrization,
                 const std::vector<Handle<Quote> > &fxSpots,
                 const std::vector<Handle<YieldTermStructure> > &curves);

    //! Stochastic process interface
    Size size() const;
    Size factors() const;
    Disposable<Array> initialValues() const;
    Disposable<Array> drift(Time t, const Array &x) const;      // not available
    Disposable<Matrix> diffusion(Time t, const Array &x) const; // not available
    Disposable<Array> expectation(Time t0, const Array &x0, Time dt) const;
    Disposable<Matrix> stdDeviation(Time t0, const Array &x0, Time dt) const;
    Disposable<Matrix> covariance(Time t0, const Array &x0, Time dt) const;

    //! clear cache
    void flushCache() const;

    //! curves inspector
    Handle<YieldTermStructure> termStructure(const Size i) const {
        QL_REQUIRE(i <= n_, "term structure index ("
                                << i << ") out of range 0..." << n_);
        return curves_[i];
    }

  private:
    const boost::shared_ptr<
        detail::CcLgmParametrization<Impl, ImplFx, ImplLgm> > p_;
    const std::vector<Handle<Quote> > fxSpots_;
    const std::vector<Handle<YieldTermStructure> > curves_;
    const Size n_;

    struct cache_key {
        double t0;
        double dt;
        const bool operator==(const cache_key &o) const {
            return (t0 == o.t0) && (dt == o.dt);
        }
    };

    struct cache_hasher : std::unary_function<cache_key, std::size_t> {
        std::size_t operator()(cache_key const &x) const {
            std::size_t seed = 0;
            boost::hash_combine(seed, x.t0);
            boost::hash_combine(seed, x.dt);
            return seed;
        }
    };

    mutable boost::unordered_map<cache_key, Array, cache_hasher> cache_e_;
    mutable boost::unordered_map<cache_key, Matrix, cache_hasher> cache_v_,
        cache_s_;
};

// inline

template <class Impl, class ImplFx, class ImplLgm>
void CcLgmProcess<Impl, ImplFx, ImplLgm>::flushCache() const {
    cache_v_.clear();
    cache_s_.clear();
    cache_e_.clear();
}

template <class Impl, class ImplFx, class ImplLgm>
inline Size CcLgmProcess<Impl, ImplFx, ImplLgm>::size() const {
    return 2 * n_ + 1;
}

template <class Impl, class ImplFx, class ImplLgm>
inline Size CcLgmProcess<Impl, ImplFx, ImplLgm>::factors() const {
    return 2 * n_ + 1;
}

template <class Impl, class ImplFx, class ImplLgm>
inline Disposable<Array>
CcLgmProcess<Impl, ImplFx, ImplLgm>::initialValues() const {
    Array res(2 * n_ + 1);
    for (Size i = 0; i < n_; ++i) {
        res[i] = fxSpots_[i]->value();
    }
    for (Size i = n_; i < 2 * n_ + 1; ++i) {
        res[i] = 0.0;
    }
    return res;
}

template <class Impl, class ImplFx, class ImplLgm>
inline Disposable<Array>
CcLgmProcess<Impl, ImplFx, ImplLgm>::drift(Time t, const Array &x) const {
    QL_FAIL("drift not implemented");
}

template <class Impl, class ImplFx, class ImplLgm>
inline Disposable<Matrix>
CcLgmProcess<Impl, ImplFx, ImplLgm>::diffusion(Time t, const Array &x) const {
    QL_FAIL("diffusion not implemented");
}

template <class Impl, class ImplFx, class ImplLgm>
inline Disposable<Array>
CcLgmProcess<Impl, ImplFx, ImplLgm>::expectation(Time t0, const Array &x0,
                                                 Time dt) const {
    cache_key k = {t0, dt};
    typename boost::unordered_map<cache_key, Array>::iterator it =
        cache_e_.find(k);
    Array res(2 * n_ + 1, 0.0);
    if (it == cache_e_.end()) {
        // fx
        for (Size i = 0; i < n_; ++i) {
            res[i] =
                std::log(curves_[i + 1]->discount(t0 + dt) /
                         curves_[i + 1]->discount(t0) *
                         curves_[0]->discount(t0) /
                         curves_[0]->discount(t0 + dt)) -
                0.5 * p_->int_sigma_i_sigma_j(i, i, t0, t0 + dt) +
                0.5 * (p_->H_i(0, t0 + dt) * p_->H_i(0, t0 + dt) *
                           p_->zeta_i(0, t0 + dt) -
                       p_->H_i(0, t0) * p_->H_i(0, t0) * p_->zeta_i(0, t0) -
                       p_->int_H_i_H_j_alpha_i_alpha_j(0, 0, t0, t0 + dt)) -
                0.5 * (p_->H_i(i + 1, t0 + dt) * p_->H_i(i + 1, t0 + dt) *
                           p_->zeta_i(i + 1, t0 + dt) -
                       p_->H_i(i + 1, t0) * p_->H_i(i + 1, t0) *
                           p_->zeta_i(i + 1, t0) -
                       p_->int_H_i_H_j_alpha_i_alpha_j(i + 1, i + 1, t0,
                                                       t0 + dt)) +
                p_->int_H_i_alpha_i_sigma_j(0, i, t0, t0 + dt) -
                p_->H_i(i + 1, t0 + dt) *
                    (-p_->int_H_i_alpha_i_alpha_j(i + 1, i + 1, t0, t0 + dt) +
                     p_->int_H_i_alpha_i_alpha_j(0, i + 1, t0, t0 + dt) -
                     p_->int_alpha_i_sigma_j(i + 1, i, t0, t0 + dt)) -
                p_->int_H_i_H_j_alpha_i_alpha_j(i + 1, i + 1, t0, t0 + dt) +
                p_->int_H_i_H_j_alpha_i_alpha_j(0, i + 1, t0, t0 + dt) -
                p_->int_H_i_alpha_i_sigma_j(i + 1, i, t0, t0 + dt);
        }
        // lgm
        for (Size i = 1; i < n_ + 1; ++i) {
            res[n_ + i] = -p_->int_H_i_alpha_i_alpha_j(i, i, t0, t0 + dt) -
                          p_->int_alpha_i_sigma_j(i, i - 1, t0, t0 + dt) +
                          p_->int_H_i_alpha_i_alpha_j(0, i, t0, t0 + dt);
        }
        cache_e_.insert(std::make_pair(k, res));
    } else {
        res = it->second;
    }
    for (Size i = 0; i < n_; ++i) {
        res[i] +=
            x0[i] + (p_->H_i(0, t0 + dt) - p_->H_i(0, t0)) * x0[n_] -
            (p_->H_i(i + 1, t0 + dt) - p_->H_i(i + 1, t0)) * x0[n_ + i + 1];
    }
    for (Size i = 0; i < n_ + 1; ++i) {
        res[n_ + i] += x0[n_ + i];
    }
    return res;
}

template <class Impl, class ImplFx, class ImplLgm>
inline Disposable<Matrix>
CcLgmProcess<Impl, ImplFx, ImplLgm>::covariance(Time t0, const Array &x0,
                                                Time dt) const {
    cache_key k = {t0, dt};
    typename boost::unordered_map<cache_key, Matrix>::iterator i =
        cache_v_.find(k);
    if (i == cache_v_.end()) {
        Matrix res(2 * n_ + 1, 2 * n_ + 1, 0.0);
        // fx-fx
        for (Size i = 0; i < n_; ++i) {
            for (Size j = 0; j <= i; ++j) {
                res[i][j] = res[j][i] =
                    // row 1
                    p_->H_i(0, t0 + dt) * p_->H_i(0, t0 + dt) *
                        p_->int_alpha_i_alpha_j(0, 0, t0, t0 + dt) -
                    2.0 * p_->H_i(0, t0 + dt) *
                        p_->int_H_i_alpha_i_alpha_j(0, 0, t0, t0 + dt) +
                    p_->int_H_i_H_j_alpha_i_alpha_j(0, 0, t0, t0 + dt) -
                    // row 2
                    p_->H_i(0, t0 + dt) * p_->H_i(j + 1, t0 + dt) *
                        p_->int_alpha_i_alpha_j(0, j + 1, t0, t0 + dt) +
                    p_->H_i(j + 1, t0 + dt) *
                        p_->int_H_i_alpha_i_alpha_j(0, j + 1, t0, t0 + dt) +
                    p_->H_i(0, t0 + dt) *
                        p_->int_H_i_alpha_i_alpha_j(j + 1, 0, t0, t0 + dt) -
                    p_->int_H_i_H_j_alpha_i_alpha_j(0, j + 1, t0, t0 + dt) -
                    // row 3
                    p_->H_i(0, t0 + dt) * p_->H_i(i + 1, t0 + dt) *
                        p_->int_alpha_i_alpha_j(0, i + 1, t0, t0 + dt) +
                    p_->H_i(i + 1, t0 + dt) *
                        p_->int_H_i_alpha_i_alpha_j(0, i + 1, t0, t0 + dt) +
                    p_->H_i(0, t0 + dt) *
                        p_->int_H_i_alpha_i_alpha_j(i + 1, 0, t0, t0 + dt) -
                    p_->int_H_i_H_j_alpha_i_alpha_j(0, i + 1, t0, t0 + dt) +
                    // row 4
                    p_->H_i(0, t0 + dt) *
                        p_->int_alpha_i_sigma_j(0, j, t0, t0 + dt) -
                    p_->int_H_i_alpha_i_sigma_j(0, j, t0, t0 + dt) +
                    // row 5
                    p_->H_i(0, t0 + dt) *
                        p_->int_alpha_i_sigma_j(0, i, t0, t0 + dt) -
                    p_->int_H_i_alpha_i_sigma_j(0, i, t0, t0 + dt) -
                    // row 6
                    p_->H_i(i + 1, t0 + dt) *
                        p_->int_alpha_i_sigma_j(i + 1, j, t0, t0 + dt) +
                    p_->int_H_i_alpha_i_sigma_j(i + 1, j, t0, t0 + dt) -
                    // row 7
                    p_->H_i(j + 1, t0 + dt) *
                        p_->int_alpha_i_sigma_j(j + 1, i, t0, t0 + dt) +
                    p_->int_H_i_alpha_i_sigma_j(j + 1, i, t0, t0 + dt) +
                    // row 8
                    p_->H_i(i + 1, t0 + dt) * p_->H_i(j + 1, t0 + dt) *
                        p_->int_alpha_i_alpha_j(i + 1, j + 1, t0, t0 + dt) -
                    p_->H_i(j + 1, t0 + dt) *
                        p_->int_H_i_alpha_i_alpha_j(i + 1, j + 1, t0, t0 + dt) -
                    p_->H_i(i + 1, t0 + dt) *
                        p_->int_H_i_alpha_i_alpha_j(j + 1, i + 1, t0, t0 + dt) +
                    p_->int_H_i_H_j_alpha_i_alpha_j(i + 1, j + 1, t0, t0 + dt) +
                    // row 9
                    p_->int_sigma_i_sigma_j(i, j, t0, t0 + dt);
            }
        }
        // fx-lgm
        for (Size i = 0; i < n_ + 1; ++i) {
            for (Size j = 0; j < n_; ++j) {
                res[j][i + n_] = res[i + n_][j] =
                    p_->H_i(0, t0 + dt) *
                        p_->int_alpha_i_alpha_j(0, i, t0, t0 + dt) -
                    p_->int_H_i_alpha_i_alpha_j(0, i, t0, t0 + dt) -
                    p_->H_i(j + 1, t0 + dt) *
                        p_->int_alpha_i_alpha_j(j + 1, i, t0, t0 + dt) +
                    p_->int_H_i_alpha_i_alpha_j(j + 1, i, t0, t0 + dt) +
                    p_->int_alpha_i_sigma_j(i, j, t0, t0 + dt);
            }
        }
        // lgm-lgm
        for (Size i = 0; i < n_ + 1; ++i) {
            for (Size j = 0; j <= i; ++j) {
                res[i + n_][j + n_] = res[j + n_][i + n_] =
                    p_->int_alpha_i_alpha_j(i, j, t0, t0 + dt);
            }
        }
        cache_v_.insert(std::make_pair(k, res));
        return res;
    } else {
        // we need to make a copy here since a disposable is returned
        Matrix tmp = i->second;
        return tmp;
    }
}

template <class Impl, class ImplFx, class ImplLgm>
inline Disposable<Matrix>
CcLgmProcess<Impl, ImplFx, ImplLgm>::stdDeviation(Time t0, const Array &x0,
                                                  Time dt) const {
    cache_key k = {t0, dt};
    typename boost::unordered_map<cache_key, Matrix>::iterator i =
        cache_s_.find(k);
    if (i == cache_s_.end()) {
        Matrix tmp =
            pseudoSqrt(covariance(t0, x0, dt), SalvagingAlgorithm::Spectral);
        cache_s_.insert(std::make_pair(k, tmp));
        return tmp;
    } else {
        // we need to make a copy here since a disposable is returned
        Matrix tmp = i->second;
        return tmp;
    }
}

// implementation

template <class Impl, class ImplFx, class ImplLgm>
CcLgmProcess<Impl, ImplFx, ImplLgm>::CcLgmProcess(
    const boost::shared_ptr<
        detail::CcLgmParametrization<Impl, ImplFx, ImplLgm> > &parametrization,
    const std::vector<Handle<Quote> > &fxSpots,
    const std::vector<Handle<YieldTermStructure> > &curves)
    : p_(parametrization), fxSpots_(fxSpots), curves_(curves), n_(p_->n()) {

    QL_REQUIRE(fxSpots_.size() == n_,
               fxSpots_.size()
                   << " fx spots given, while parametrization suggests " << n_);
    QL_REQUIRE(curves_.size() == n_ + 1,
               curves_.size()
                   << " curves given, while parametrization suggests "
                   << (n_ + 1));

    for (Size i = 0; i < n_; ++i)
        registerWith(fxSpots_[i]);
}

} // namesapce QuantLib

#endif