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

#ifndef BOOST_STATS_rayleigh_HPP
#define BOOST_STATS_rayleigh_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/config/no_tr1/cmath.hpp>

#ifdef BOOST_MSVC
# pragma warning(push)
# pragma warning(disable: 4702) // unreachable code (return after domain_error throw).
#endif

#include <utility>

namespace boost{ namespace math{

namespace detail
{ // Error checks:
  template <class RealType, class Policy>
  inline bool verify_sigma(const char* function, RealType sigma, RealType* presult, const Policy& pol)
  {
     if(sigma <= 0)
     {
        *presult = policies::raise_domain_error<RealType>(
           function,
           "The scale parameter \"sigma\" must be > 0, but was: %1%.", sigma, pol);
        return false;
     }
     return true;
  } // bool verify_sigma

  template <class RealType, class Policy>
  inline bool verify_rayleigh_x(const char* function, RealType x, RealType* presult, const Policy& pol)
  {
     if(x < 0)
     {
        *presult = policies::raise_domain_error<RealType>(
           function,
           "The random variable must be >= 0, but was: %1%.", x, pol);
        return false;
     }
     return true;
  } // bool verify_rayleigh_x
} // namespace detail

template <class RealType = double, class Policy = policies::policy<> >
class rayleigh_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   rayleigh_distribution(RealType sigma = 1)
      : m_sigma(sigma)
   {
      RealType err;
      detail::verify_sigma("boost::math::rayleigh_distribution<%1%>::rayleigh_distribution", sigma, &err, Policy());
   } // rayleigh_distribution

   RealType sigma()const
   { // Accessor.
     return m_sigma;
   }

private:
   RealType m_sigma;
}; // class rayleigh_distribution

typedef rayleigh_distribution<double> rayleigh;

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const rayleigh_distribution<RealType, Policy>& /*dist*/)
{ // Range of permissible values for random variable x.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(static_cast<RealType>(0), max_value<RealType>());
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const rayleigh_distribution<RealType, Policy>& /*dist*/)
{ // Range of supported values for random variable x.
   // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>((0),  max_value<RealType>());
}

template <class RealType, class Policy>
inline RealType pdf(const rayleigh_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING // for ADL of std function exp.

   RealType sigma = dist.sigma();
   RealType result;
   static const char* function = "boost::math::pdf(const rayleigh_distribution<%1%>&, %1%)";
   if(false == detail::verify_sigma(function, sigma, &result, Policy()))
   {
      return result;
   }
   if(false == detail::verify_rayleigh_x(function, x, &result, Policy()))
   {
      return result;
   }
   RealType sigmasqr = sigma * sigma;
   result = x * (exp(-(x * x) / ( 2 * sigmasqr))) / sigmasqr;
   return result;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const rayleigh_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   RealType result;
   RealType sigma = dist.sigma();
   static const char* function = "boost::math::cdf(const rayleigh_distribution<%1%>&, %1%)";
   if(false == detail::verify_sigma(function, sigma, &result, Policy()))
   {
      return result;
   }
   if(false == detail::verify_rayleigh_x(function, x, &result, Policy()))
   {
      return result;
   }
   result = -boost::math::expm1(-x * x / ( 2 * sigma * sigma), Policy());
   return result;
} // cdf

template <class RealType, class Policy>
inline RealType quantile(const rayleigh_distribution<RealType, Policy>& dist, const RealType& p)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   RealType result;
   RealType sigma = dist.sigma();
   static const char* function = "boost::math::quantile(const rayleigh_distribution<%1%>&, %1%)";
   if(false == detail::verify_sigma(function, sigma, &result, Policy()))
      return result;
   if(false == detail::check_probability(function, p, &result, Policy()))
      return result;

   if(p == 0)
   {
      return 0;
   }
   if(p == 1)
   {
     return policies::raise_overflow_error<RealType>(function, 0, Policy());
   }
   result = sqrt(-2 * sigma * sigma * boost::math::log1p(-p, Policy()));
   return result;
} // quantile

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<rayleigh_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   RealType result;
   RealType sigma = c.dist.sigma();
   static const char* function = "boost::math::cdf(const rayleigh_distribution<%1%>&, %1%)";
   if(false == detail::verify_sigma(function, sigma, &result, Policy()))
   {
      return result;
   }
   RealType x = c.param;
   if(false == detail::verify_rayleigh_x(function, x, &result, Policy()))
   {
      return result;
   }
   result =  exp(-x * x / ( 2 * sigma * sigma));
   return result;
} // cdf complement

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<rayleigh_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING // for ADL of std functions, log & sqrt.

   RealType result;
   RealType sigma = c.dist.sigma();
   static const char* function = "boost::math::quantile(const rayleigh_distribution<%1%>&, %1%)";
   if(false == detail::verify_sigma(function, sigma, &result, Policy()))
   {
      return result;
   }
   RealType q = c.param;
   if(false == detail::check_probability(function, q, &result, Policy()))
   {
      return result;
   }
   if(q == 1)
   {
      return 0;
   }
   if(q == 0)
   {
     return policies::raise_overflow_error<RealType>(function, 0, Policy());
   }
   result = sqrt(-2 * sigma * sigma * log(q));
   return result;
} // quantile complement

template <class RealType, class Policy>
inline RealType mean(const rayleigh_distribution<RealType, Policy>& dist)
{
   RealType result;
   RealType sigma = dist.sigma();
   static const char* function = "boost::math::mean(const rayleigh_distribution<%1%>&, %1%)";
   if(false == detail::verify_sigma(function, sigma, &result, Policy()))
   {
      return result;
   }
   using boost::math::constants::root_half_pi;
   return sigma * root_half_pi<RealType>();
} // mean

template <class RealType, class Policy>
inline RealType variance(const rayleigh_distribution<RealType, Policy>& dist)
{
   RealType result;
   RealType sigma = dist.sigma();
   static const char* function = "boost::math::variance(const rayleigh_distribution<%1%>&, %1%)";
   if(false == detail::verify_sigma(function, sigma, &result, Policy()))
   {
      return result;
   }
   using boost::math::constants::four_minus_pi;
   return four_minus_pi<RealType>() * sigma * sigma / 2;
} // variance

template <class RealType, class Policy>
inline RealType mode(const rayleigh_distribution<RealType, Policy>& dist)
{
   return dist.sigma();
}

template <class RealType, class Policy>
inline RealType median(const rayleigh_distribution<RealType, Policy>& dist)
{
   using boost::math::constants::root_ln_four;
   return root_ln_four<RealType>() * dist.sigma();
}

template <class RealType, class Policy>
inline RealType skewness(const rayleigh_distribution<RealType, Policy>& /*dist*/)
{
  // using namespace boost::math::constants;
  return static_cast<RealType>(0.63111065781893713819189935154422777984404221106391L);
  // Computed using NTL at 150 bit, about 50 decimal digits.
  // return 2 * root_pi<RealType>() * pi_minus_three<RealType>() / pow23_four_minus_pi<RealType>();
}

template <class RealType, class Policy>
inline RealType kurtosis(const rayleigh_distribution<RealType, Policy>& /*dist*/)
{
  // using namespace boost::math::constants;
  return static_cast<RealType>(3.2450893006876380628486604106197544154170667057995L);
  // Computed using NTL at 150 bit, about 50 decimal digits.
  // return 3 - (6 * pi<RealType>() * pi<RealType>() - 24 * pi<RealType>() + 16) /
  // (four_minus_pi<RealType>() * four_minus_pi<RealType>());
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const rayleigh_distribution<RealType, Policy>& /*dist*/)
{
  //using namespace boost::math::constants;
  // Computed using NTL at 150 bit, about 50 decimal digits.
  return static_cast<RealType>(0.2450893006876380628486604106197544154170667057995L);
  // return -(6 * pi<RealType>() * pi<RealType>() - 24 * pi<RealType>() + 16) /
  //   (four_minus_pi<RealType>() * four_minus_pi<RealType>());
} // kurtosis

} // namespace math
} // namespace boost

#ifdef BOOST_MSVC
# pragma warning(pop)
#endif

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_rayleigh_HPP


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#include <algorithm>
#include <cmath>
#include <iostream>
#include <string>
#include <utility>
#include <vector>

#include <Eigen/Core>

#ifdef USE_TBB
#include <tbb/concurrent_hash_map.h>
#include <tbb/parallel_for.h>
#include <tbb/task_scheduler_init.h>
#else
#include <map>
#endif

#include <limbo/experimental/acqui/ucb_imgpo.hpp>
#include <limbo/experimental/bayes_opt/imgpo.hpp>
#include <limbo/init/no_init.hpp>
#include <limbo/mean/constant.hpp>
#include <limbo/tools/macros.hpp>
#include <limbo/tools/parallel.hpp>

using namespace limbo;

static constexpr int nb_replicates = 4;

namespace colors {
    static const char* red = "\33[31m";
    static const char* green = "\33[32m";
    static const char* yellow = "\33[33m";
    static const char* reset = "\33[0m";
    static const char* bold = "\33[1m";
}

// support functions
inline double sign(double x)
{
    if (x < 0)
        return -1;
    if (x > 0)
        return 1;
    return 0;
}

inline double hat(double x)
{
    if (x != 0)
        return log(fabs(x));
    return 0;
}

inline double c1(double x)
{
    if (x > 0)
        return 10;
    return 5.5;
}

inline double c2(double x)
{
    if (x > 0)
        return 7.9;
    return 3.1;
}

inline Eigen::VectorXd t_osz(const Eigen::VectorXd& x)
{
    Eigen::VectorXd r = x;
    for (int i = 0; i < x.size(); i++)
        r(i) = sign(x(i)) * exp(hat(x(i)) + 0.049 * sin(c1(x(i)) * hat(x(i))) + sin(c2(x(i)) * hat(x(i))));
    return r;
}

struct Sphere {
    BO_PARAM(size_t, dim_in, 2);
    BO_PARAM(size_t, dim_out, 1);

    Eigen::VectorXd operator()(const Eigen::VectorXd& x) const
    {
        Eigen::Vector2d opt(0.5, 0.5);
        return tools::make_vector(-(x - opt).squaredNorm());
    }
};

struct Ellipsoid {
    BO_PARAM(size_t, dim_in, 2);
    BO_PARAM(size_t, dim_out, 1);

    Eigen::VectorXd operator()(const Eigen::VectorXd& x) const
    {
        Eigen::Vector2d opt(0.5, 0.5);
        Eigen::Vector2d z = t_osz(x - opt);
        double r = 0;
        for (size_t i = 0; i < dim_in(); ++i)
            r += std::pow(10, ((double)i) / (dim_in() - 1.0)) * z(i) * z(i) + 1;
        return tools::make_vector(-r);
    }
};

struct Rastrigin {
    BO_PARAM(size_t, dim_in, 4);
    BO_PARAM(size_t, dim_out, 1);

    Eigen::VectorXd operator()(const Eigen::VectorXd& x) const
    {
        double f = 10 * x.size();
        for (int i = 0; i < x.size(); ++i)
            f += x(i) * x(i) - 10 * cos(2 * M_PI * x(i));
        return tools::make_vector(-f);
    }
};

// see : http://www.sfu.ca/~ssurjano/hart3.html
struct Hartman3 {
    BO_PARAM(size_t, dim_in, 3);
    BO_PARAM(size_t, dim_out, 1);

    Eigen::VectorXd operator()(const Eigen::VectorXd& x) const
    {
        Eigen::Matrix<double, 4, 3> a, p;
        a << 3.0, 10, 30, 0.1, 10, 35, 3.0, 10, 30, 0.1, 10, 36;
        p << 0.3689, 0.1170, 0.2673, 0.4699, 0.4387, 0.7470, 0.1091, 0.8732, 0.5547,
            0.0382, 0.5743, 0.8828;
        Eigen::Vector4d alpha;
        alpha << 1.0, 1.2, 3.0, 3.2;

        double res = 0;
        for (int i = 0; i < 4; i++) {
            double s = 0.0f;
            for (size_t j = 0; j < 3; j++) {
                s += a(i, j) * (x(j) - p(i, j)) * (x(j) - p(i, j));
            }
            res += alpha(i) * exp(-s);
        }
        return tools::make_vector(res);
    }
};

// see : http://www.sfu.ca/~ssurjano/hart6.html
struct Hartman6 {
    BO_PARAM(size_t, dim_in, 6);
    BO_PARAM(size_t, dim_out, 1);

    Eigen::VectorXd operator()(const Eigen::VectorXd& x) const
    {
        Eigen::Matrix<double, 4, 6> a, p;
        a << 10, 3, 17, 3.5, 1.7, 8, 0.05, 10, 17, 0.1, 8, 14, 3, 3.5, 1.7, 10, 17,
            8, 17, 8, 0.05, 10, 0.1, 14;
        p << 0.1312, 0.1696, 0.5569, 0.0124, 0.8283, 0.5886, 0.2329, 0.4135, 0.8307,
            0.3736, 0.1004, 0.9991, 0.2348, 0.1451, 0.3522, 0.2883, 0.3047, 0.665,
            0.4047, 0.8828, 0.8732, 0.5743, 0.1091, 0.0381;

        Eigen::Vector4d alpha;
        alpha << 1.0, 1.2, 3.0, 3.2;

        double res = 0;
        for (int i = 0; i < 4; i++) {
            double s = 0.0f;
            for (size_t j = 0; j < 6; j++) {
                s += a(i, j) * (x(j) - p(i, j)) * (x(j) - p(i, j));
            }
            res += alpha(i) * exp(-s);
        }
        return tools::make_vector(res);
    }
};

// see : http://www.sfu.ca/~ssurjano/goldpr.html
// (with ln, as suggested in Jones et al.)
struct GoldenPrice {
    BO_PARAM(size_t, dim_in, 2);
    BO_PARAM(size_t, dim_out, 1);

    Eigen::VectorXd operator()(const Eigen::VectorXd& xx) const
    {
        Eigen::VectorXd x = (4.0 * xx).array() - 2.0;
        double r = (1 + (x(0) + x(1) + 1) * (x(0) + x(1) + 1) * (19 - 14 * x(0) + 3 * x(0) * x(0) - 14 * x(1) + 6 * x(0) * x(1) + 3 * x(1) * x(1))) * (30 + (2 * x(0) - 3 * x(1)) * (2 * x(0) - 3 * x(1)) * (18 - 32 * x(0) + 12 * x(0) * x(0) + 48 * x(1) - 36 * x(0) * x(1) + 27 * x(1) * x(1)));

        return tools::make_vector(-log(r) + 5);
    }
};

struct Params {
    struct bayes_opt_bobase : public defaults::bayes_opt_bobase {
        BO_PARAM(bool, stats_enabled, false);
    };

    struct bayes_opt_imgpo : public defaults::bayes_opt_imgpo {
    };

    struct kernel : public defaults::kernel {
        BO_PARAM(double, noise, 1e-10);
    };

    struct kernel_exp : public defaults::kernel_exp {
    };

    struct stop_maxiterations {
        BO_PARAM(int, iterations, 100);
    };

    struct acqui_ucb_imgpo : public defaults::acqui_ucb_imgpo {
    };

    struct mean_constant {
        BO_PARAM(double, constant, 0);
    };
};

template <typename T>
void print_res(const T& r)
{
    std::cout << "====== RESULTS ======" << std::endl;
    for (auto x : r) {
        for (auto y : x.second) {
            std::cout << x.first << "\t =>"
                      << " found :" << y.second << " expected " << y.first
                      << std::endl;
        }
        std::vector<std::pair<double, double>>& v = x.second;
        std::sort(v.begin(), v.end(),
            [](const std::pair<double, double>& x1,
                      const std::pair<double, double>& x2) {
                // clang-format off
                return x1.second < x2.second;
                // clang-format on
            });
        double med = v[v.size() / 2].second;
        if (fabs(v[0].first - med) < 0.05)
            std::cout << "[" << colors::green << "OK" << colors::reset << "] ";
        else
            std::cout << "[" << colors::red << "ERROR" << colors::reset << "] ";
        std::cout << colors::yellow << colors::bold << " -- " << x.first
                  << colors::reset << " ";
        std::cout << "Median: " << med << " error :" << fabs(v[0].first - med)
                  << std::endl;
    }
}

bool is_in_argv(int argc, char** argv, const char* needle)
{
    auto it = std::find_if(argv, argv + argc,
        [=](const char* s) {
            // clang-format off
            return strcmp(needle, s) == 0;
            // clang-format on
        });
    return !(it == argv + argc);
}

template <typename T1, typename T2>
void add_to_results(const char* key, T1& map, const T2& p)
{
#ifdef USE_TBB
    typename T1::accessor a;
    if (!map.find(a, key))
        map.insert(a, key);
#else
    typename T1::iterator a;
    a = map.find(key);
    if (a == map.end())
        map[key] = std::vector<std::pair<double, double>>();
#endif
    a->second.push_back(p);
}

int main(int argc, char** argv)
{
    tools::par::init();

#ifdef USE_TBB
    using res_t = tbb::concurrent_hash_map<std::string, std::vector<std::pair<double, double>>>;
#else
    using res_t = std::map<std::string, std::vector<std::pair<double, double>>>;
#endif
    res_t results;

    using kf_t = kernel::Exp<Params>;
    using mean_t = mean::Constant<Params>;
    using model_t = model::GP<Params, kf_t, mean_t>;
    using init_t = init::NoInit<Params>;
    using acqui_t = acqui::experimental::UCB_IMGPO<Params, model_t>;

    using Opt_t = bayes_opt::experimental::IMGPO<Params, modelfun<model_t>, initfun<init_t>, acquifun<acqui_t>>;

    if (!is_in_argv(argc, argv, "--only") || is_in_argv(argc, argv, "sphere"))
        tools::par::replicate(nb_replicates, [&]() {
            // clang-format off
                Opt_t opt;
                opt.optimize(Sphere());
                Eigen::Vector2d s_val(0.5, 0.5);
                double x_opt = FirstElem()(Sphere()(s_val));
                add_to_results("Sphere", results, std::make_pair(x_opt, opt.best_observation()(0)));
            // clang-format on
        });

    if (!is_in_argv(argc, argv, "--only") || is_in_argv(argc, argv, "ellipsoid"))
        tools::par::replicate(nb_replicates, [&]() {
            // clang-format off
                Opt_t opt;
                opt.optimize(Ellipsoid());
                Eigen::Vector2d s_val(0.5, 0.5);
                double x_opt = FirstElem()(Ellipsoid()(s_val));
                add_to_results("Ellipsoid", results, std::make_pair(x_opt, opt.best_observation()(0)));
            // clang-format on
        });

    if (!is_in_argv(argc, argv, "--only") || is_in_argv(argc, argv, "rastrigin"))
        tools::par::replicate(nb_replicates, [&]() {
            // clang-format off
                Opt_t opt;
                opt.optimize(Rastrigin());
                Eigen::Vector4d s_val(0, 0, 0, 0);
                double x_opt = FirstElem()(Rastrigin()(s_val));
                add_to_results("Rastrigin", results, std::make_pair(x_opt, opt.best_observation()(0)));
            // clang-format on
        });

    if (!is_in_argv(argc, argv, "--only") || is_in_argv(argc, argv, "hartman3"))
        tools::par::replicate(nb_replicates, [&]() {
            // clang-format off
                Opt_t opt;
                opt.optimize(Hartman3());
                // double s_max = 3.86278;
                Eigen::Vector3d s_val(0.114614, 0.555549, 0.852547);
                double x_opt = FirstElem()(Hartman3()(s_val));
                add_to_results("Hartman 3", results, std::make_pair(x_opt, opt.best_observation()(0)));
            // clang-format on
        });

    if (!is_in_argv(argc, argv, "--only") || is_in_argv(argc, argv, "hartman6"))
        tools::par::replicate(nb_replicates, [&]() {
            // clang-format off
                Opt_t opt;
                opt.optimize(Hartman6());
                Eigen::Matrix<double, 6, 1> s_val;
                s_val << 0.20169, 0.150011, 0.476874, 0.275332, 0.311652, 0.6573;
                //double s_max = 3.32237;
                double x_opt = FirstElem()(Hartman6()(s_val));
                add_to_results("Hartman 6", results, std::make_pair(x_opt, opt.best_observation()(0)));
            // clang-format on
        });

    if (!is_in_argv(argc, argv, "--only") || is_in_argv(argc, argv, "golden_price"))
        tools::par::replicate(nb_replicates, [&]() {
            // clang-format off
                Opt_t opt;
                opt.optimize(GoldenPrice());
                //    double s_max = -log(3);
                Eigen::Vector2d s_val(0.5, 0.25);
                double x_opt = FirstElem()(GoldenPrice()(s_val));
                add_to_results("Golden Price", results, std::make_pair(x_opt, opt.best_observation()(0)));
            // clang-format on
        });

    std::cout << "Benchmark finished." << std::endl;

    print_res(results);
    return 0;
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_OFFSET_MULTIPLIER_FREE_HPP
#define STAN_MATH_PRIM_SCAL_FUN_OFFSET_MULTIPLIER_FREE_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/fun/identity_free.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <boost/math/tools/promotion.hpp>
#include <cmath>
#include <limits>

namespace stan {
namespace math {

/**
 * Return the unconstrained scalar that transforms to the
 * specified offset and multiplier constrained scalar given the specified
 * offset and multiplier.
 *
 *
The transfrom in locmultiplier_constrain(T, double, double),
 * is reversed by the reverse affine transformation,
 *
 *
\f$f^{-1}(y) = \frac{y - L}{S}\f$
 *
 * where \f$L\f$ and \f$S\f$ are the offset and multiplier.
 *
 *
If the offset is zero and multiplier is one,
 * this function reduces to  identity_free(y).
 *
 * @tparam T type of scalar
 * @tparam L type of offset
 * @tparam S type of multiplier
 * @param y constrained value
 * @param[in] mu offset of constrained output
 * @param[in] sigma multiplier of constrained output
 * @return the free scalar that transforms to the input scalar
 *   given the offset and multiplier
 * @throw std::domain_error if sigma <= 0
 * @throw std::domain_error if mu is not finite
 */
template <typename T, typename L, typename S>
inline return_type_t<T, L, S> offset_multiplier_free(const T& y, const L& mu,
                                                     const S& sigma) {
  check_finite("offset_multiplier_free", "offset", mu);
  if (sigma == 1) {
    if (mu == 0)
      return identity_free(y);
    return y - mu;
  }
  check_positive_finite("offset_multiplier_free", "multiplier", sigma);
  return (y - mu) / sigma;
}

}  // namespace math
}  // namespace stan
#endif


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

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

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

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

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

#include "utils_bounding_box.h"

// eigen
#include <Eigen/Core>

// utils
#include "utils_filtering.h"
#include "camera.h"
#include "utils_common.h"

namespace SUN {
    namespace utils {
        namespace bbox {

            Eigen::Vector4d Intersection2d(const Eigen::Vector4d &rect1, const Eigen::Vector4d &rect2) {
                const double rect1_x = rect1[0];
                const double rect1_y = rect1[1];
                const double rect1_w = rect1[2];
                const double rect1_h = rect1[3];

                const double rect2_x = rect2[0];
                const double rect2_y = rect2[1];
                const double rect2_w = rect2[2];
                const double rect2_h = rect2[3];

                const double left = rect1_x > rect2_x ? rect1_x : rect2_x;
                const double top = rect1_y > rect2_y ? rect1_y : rect2_y;
                double lhs = rect1_x + rect1_w;
                double rhs = rect2_x + rect2_w;
                const double right = lhs < rhs ? lhs : rhs;
                lhs = rect1_y + rect1_h;
                rhs = rect2_y + rect2_h;
                const double bottom = lhs < rhs ? lhs : rhs;

                Eigen::Vector4d rect_intersection;
                rect_intersection[0] = right < left ? 0 : left;
                rect_intersection[1] = bottom < top ? 0 : top;
                rect_intersection[2] = right < left ? 0 : right - left;
                rect_intersection[3] = bottom < top ? 0 : bottom - top;

                return rect_intersection;
            }

            double IntersectionOverUnion2d(const Eigen::Vector4d &rect1, const Eigen::Vector4d &rect2) {
                Eigen::Vector4d rect_intersection = Intersection2d(rect1, rect2);
                const double intersection_area = rect_intersection[2] * rect_intersection[3]; // Surface of the intersection of the rects
                const double union_of_rects = rect1[2] * rect1[3] + rect2[2] * rect2[3] - intersection_area; // Union of the area of the rects
                return intersection_area / union_of_rects; // Intersection over union
            }

            Eigen::Vector4d BoundingBox2d(pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr scene_cloud,
                                          const std::vector<int> &indices, double percentage) {

                pcl::PointIndices filtered_indices = SUN::utils::filter::FilterPointCloudBasedOnRadius(scene_cloud, indices, percentage);

                int bbx_min = static_cast<int>(1e10);
                int bbx_max = static_cast<int>(-1e10);
                int bby_min = static_cast<int>(1e10);
                int bby_max = static_cast<int>(-1e10);

                for (auto ind:filtered_indices.indices) {
                    int x = -1, y = -1;
                    UnravelIndex(ind, scene_cloud->width, &x, &y);
                    if (x < bbx_min)
                        bbx_min = x;
                    if (x > bbx_max)
                        bbx_max = x;
                    if (y < bby_min)
                        bby_min = y;
                    if (y > bby_max)
                        bby_max = y;
                }

                // [min_x min_y w h]
                Eigen::Vector4d bb2d_out;
                bb2d_out[0] = static_cast<double>(bbx_min);
                bb2d_out[1] = static_cast<double>(bby_min);
                bb2d_out[2] = static_cast<double>(bbx_max - bbx_min);
                bb2d_out[3] = static_cast<double>(bby_max - bby_min);

                return bb2d_out;
            }

            Eigen::Vector4d BoundingBox2d(pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr object_cloud, const SUN::utils::Camera &cam, double percentage) {
                auto filtered_cloud = SUN::utils::filter::FilterPointCloudBasedOnRadius(object_cloud,  percentage);

                int bbx_min = static_cast<int>(1e10);
                int bbx_max = static_cast<int>(-1e10);
                int bby_min = static_cast<int>(1e10);
                int bby_max = static_cast<int>(-1e10);

                for (const auto &pt:filtered_cloud->points) {
                    int x, y;

                    auto proj_pt = cam.CameraToImage(pt.getVector4fMap().cast<double>());
                    x = proj_pt[0];
                    y = proj_pt[1];

                    if (x < bbx_min)
                        bbx_min = x;
                    if (x > bbx_max)
                        bbx_max = x;
                    if (y < bby_min)
                        bby_min = y;
                    if (y > bby_max)
                        bby_max = y;
                }

                // [min_x min_y w h]
                Eigen::Vector4d bb2d_out;
                bb2d_out[0] = static_cast<double>(bbx_min);
                bb2d_out[1] = static_cast<double>(bby_min);
                bb2d_out[2] = static_cast<double>(bbx_max - bbx_min);
                bb2d_out[3] = static_cast<double>(bby_max - bby_min);

                return bb2d_out;
            }

            Eigen::VectorXd BoundingBox3d(pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud_in, double percentage) {
                // Remove some points (radius-based cleaning)
                auto cloud_to_process = SUN::utils::filter::FilterPointCloudBasedOnRadius(cloud_in, percentage);

                // Compute mean, covariance matrix 3d
                Eigen::Matrix3d cov_mat3d;
                Eigen::Vector4d mean3d;
                pcl::computeMeanAndCovarianceMatrix(*cloud_to_process, cov_mat3d, mean3d);

                // Let's restrict ourselves to 2D ground-plane projection. More robust.
                Eigen::Matrix2d cov_mat2d;
                cov_mat2d(0, 0) = cov_mat3d(0, 0);
                cov_mat2d(1, 1) = cov_mat3d(2, 2);
                cov_mat2d(0, 1) = cov_mat3d(0, 2);
                cov_mat2d(1, 0) = cov_mat3d(2, 0);

                // Compute Eigen vectors, values.
                // Here, we get 2 eigenvectors, corresponding to dominant axes of 2D proj. of object (to the ground plane).
                Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigen_solver(cov_mat2d, Eigen::ComputeEigenvectors);
                Eigen::Matrix2d eigen_vectors = eigen_solver.eigenvectors();

                // Def. local coord. sys.
                Eigen::Matrix3d p2w(Eigen::Matrix3d::Identity());
                p2w.block<2, 2>(0, 0) = eigen_vectors.transpose();
                Eigen::Vector2d c_2d(mean3d[0], mean3d[2]); // Mean on gp. proj.
                p2w.block<2, 1>(0, 2) = -1.f * (p2w.block<2, 2>(0, 0) * c_2d); //centroid.head<2>());

                // Find bbox extent (width, depth)
                float bbx_min = static_cast<float>(1e4);
                float bbx_max = static_cast<float>(-1e4);
                float bbz_min = static_cast<float>(1e4);
                float bbz_max = static_cast<float>(-1e4);

                for (int i = 0; i <cloud_to_process->size(); i++) {

                    const auto &p_ref = cloud_to_process->at(i);
                    Eigen::Vector3d p_eig(p_ref.x, p_ref.z, 1.0);

                    if (std::isnan(p_ref.x))
                        continue;

                    p_eig = p2w * p_eig;

                    const float tmp_x = p_eig[0];
                    const float tmp_z = p_eig[1];

                    if (tmp_x < bbx_min)
                        bbx_min = tmp_x;
                    if (tmp_x > bbx_max)
                        bbx_max = tmp_x;
                    if (tmp_z < bbz_min)
                        bbz_min = tmp_z;
                    if (tmp_z > bbz_max)
                        bbz_max = tmp_z;
                }

                // Find out orientation
                Eigen::Matrix3d R_mat;
                R_mat.setIdentity();
                R_mat(0, 0) = eigen_vectors(0, 0);
                R_mat(0, 2) = eigen_vectors(0, 1);
                R_mat(2, 0) = eigen_vectors(1, 0);
                R_mat(2, 2) = eigen_vectors(1, 1);
                const Eigen::Vector2d mean_diag = 0.5f * (Eigen::Vector2d(bbx_min + bbx_max, bbz_min + bbz_max));
                const Eigen::Vector2d tfinal = eigen_vectors * mean_diag + Eigen::Vector2d(mean3d[0], mean3d[2]);

                // Final transform
                Eigen::Quaterniond qfinal(R_mat);
                double bb1 = bbx_max - bbx_min;
                double bb3 = bbz_max - bbz_min;

                // Get height
                Eigen::Vector4f cloud_min, cloud_max;
                pcl::getMinMax3D(*cloud_to_process, cloud_min, cloud_max);
                double min_y = cloud_min[1];
                double max_y = cloud_max[1];
                double cloud_height = std::abs(max_y - min_y); // Difference between Y-coords.

                // Resulting data structure
                Eigen::VectorXd bb3d_out;
                bb3d_out.setZero(10, 1); // center_x, center_y, center_z, width, height, depth, quaternion

                // X-Z center from 2d-gp-PCA, Y compute from 3D points
                bb3d_out(0) = tfinal[0];
                bb3d_out(1) = min_y + (max_y - min_y) / 2.0;
                bb3d_out(2) = tfinal[1];

                // Width, height, depth
                bb3d_out(3) = std::abs(bb1);
                bb3d_out(4) = std::abs(cloud_height);
                bb3d_out(5) = std::abs(bb3);

                // Quaternion, representing orientation
                //qfinal.setIdentity();
                bb3d_out(6) = qfinal.w();
                bb3d_out(7) = qfinal.x();
                bb3d_out(8) = qfinal.y();
                bb3d_out(9) = qfinal.z();

                return bb3d_out;
            }

            Eigen::VectorXd BoundingBox3d(pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr scene_cloud,
                                          const std::vector<int> &indices, double percentage) {
                // Remove some points (radius-based cleaning)
                auto filtered_indices = SUN::utils::filter::FilterPointCloudBasedOnRadius(scene_cloud, indices, percentage);

                // Compute mean, covariance matrix 3d
                Eigen::Matrix3d cov_mat3d;
                Eigen::Vector4d mean3d;
                pcl::computeMeanAndCovarianceMatrix(*scene_cloud, indices, cov_mat3d, mean3d);

                // Let's restrict ourselves to 2D ground-plane projection. More robust.
                Eigen::Matrix2d cov_mat2d;
                cov_mat2d(0, 0) = cov_mat3d(0, 0);
                cov_mat2d(1, 1) = cov_mat3d(2, 2);
                cov_mat2d(0, 1) = cov_mat2d(1, 0) = cov_mat3d(0, 2);

                // Compute Eigen vectors, values..
                // Here, we get 2 eigenvectors, corresponding to dominant axes of 2D proj. of object (to the ground plane).
                Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigen_solver(cov_mat2d, Eigen::ComputeEigenvectors);
                Eigen::Matrix2d eigen_vectors = eigen_solver.eigenvectors();

                // Def. local coord. sys.
                Eigen::Matrix3d p2w(Eigen::Matrix3d::Identity());
                p2w.block<2, 2>(0, 0) = eigen_vectors.transpose();
                Eigen::Vector2d c_2d(mean3d[0], mean3d[2]); // Mean on gp. proj.
                p2w.block<2, 1>(0, 2) = -1.f * (p2w.block<2, 2>(0, 0) * c_2d); //centroid.head<2>());

                // Find bbox extent (width, depth)
                float bbx_min = 1e10;
                float bbx_max = -1e10;
                float bbz_min = 1e10;
                float bbz_max = -1e10;

                for (int i = 0; i < filtered_indices.indices.size(); i++) {
                    int ind = filtered_indices.indices.at(i);
                    const auto &p_ref = scene_cloud->at(ind);

                    if (std::isnan(p_ref.x))
                        continue;

                    Eigen::Vector3d p_eig(p_ref.x, p_ref.z, 1.0);
                    p_eig = p2w * p_eig;

                    const double tmp_x = p_eig[0];
                    const double tmp_z = p_eig[1];

                    if (tmp_x < bbx_min)
                        bbx_min = tmp_x;
                    if (tmp_x > bbx_max)
                        bbx_max = tmp_x;
                    if (tmp_z < bbz_min)
                        bbz_min = tmp_z;
                    if (tmp_z > bbz_max)
                        bbz_max = tmp_z;
                }

                // Find out orientation
                Eigen::Matrix3d R_mat;
                R_mat.setIdentity();
                R_mat(0, 0) = eigen_vectors(0, 0);
                R_mat(0, 2) = eigen_vectors(0, 1);
                R_mat(2, 0) = eigen_vectors(1, 0);
                R_mat(2, 2) = eigen_vectors(1, 1);
                const Eigen::Vector2d mean_diag = 0.5f * (Eigen::Vector2d(bbx_min + bbx_max, bbz_min + bbz_max));
                const Eigen::Vector2d tfinal = eigen_vectors * mean_diag + Eigen::Vector2d(mean3d[0], mean3d[2]);

                // Final transform
                const Eigen::Quaterniond qfinal(R_mat);
                double bb1 = bbx_max - bbx_min;
                double bb3 = bbz_max - bbz_min;

                // Get height
                Eigen::Vector4f cloud_min, cloud_max;
                pcl::getMinMax3D(*scene_cloud, filtered_indices, cloud_min, cloud_max);
                double min_y = cloud_min[1];
                double max_y = cloud_max[1];
                double cloud_height = std::abs(max_y - min_y); // Difference between Y-coords.

                // Resulting data structure
                Eigen::VectorXd bb3d_out;
                bb3d_out.setZero(10, 1); // center_x, center_y, center_z, width, height, depth, quaternion

                // X-Z center from 2d-gp-PCA, Y compute from 3D points
                bb3d_out(0) = tfinal[0];
                bb3d_out(1) = min_y + (max_y - min_y) / 2.0;
                bb3d_out(2) = tfinal[1];

                // Width, height, depth
                bb3d_out(3) = std::abs(bb1);
                bb3d_out(4) = std::abs(cloud_height);
                bb3d_out(5) = std::abs(bb3);

                // Quaternion, representing orientation
                bb3d_out(6) = qfinal.w();
                bb3d_out(7) = qfinal.x();
                bb3d_out(8) = qfinal.y();
                bb3d_out(9) = qfinal.z();

                return bb3d_out;
            }

            Eigen::Vector4d ReparametrizeBoundingBoxCenterMidToLeftTopPoint(const Eigen::Vector4d &bounding_box_2d) {
                auto cx = bounding_box_2d[0];
                auto cy = bounding_box_2d[1];
                auto w = bounding_box_2d[2];
                auto h = bounding_box_2d[3];
                return Eigen::Vector4d(cx - (w / 2.0), cy - (h / 2.0), w, h);
            }

            Eigen::Vector4d ReparametrizeBoundingBoxCenterTopToLeftTopPoint(const Eigen::Vector4d &bounding_box_2d) {
                auto cx = bounding_box_2d[0];
                auto cy = bounding_box_2d[1];
                auto w = bounding_box_2d[2];
                auto h = bounding_box_2d[3];
                return Eigen::Vector4d(cx - (w / 2.0), cy, w, h);
            }
        }
    }
}


/*
 * WaveEquationAdjoint.cpp
 *
 *  Created on: 17.07.2017
 *      Author: thies
 */

/*
 * based on WaveEquation.cpp
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/thread_management.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/types.h>
#include <deal.II/base/utilities.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/identity_matrix.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/solver_control.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>

#include <forward/WaveEquationAdjoint.h>
#include <forward/WaveEquation.h>

#include <stddef.h>
#include <iostream>
#include <map>
#include <string>

namespace wavepi {
namespace forward {
using namespace dealii;
using namespace wavepi::base;

template<int dim>
WaveEquationAdjoint<dim>::WaveEquationAdjoint(std::shared_ptr<SpaceTimeMesh<dim>> mesh)
      : AbstractEquationAdjoint<dim>(mesh), zero(std::make_shared<Functions::ZeroFunction<dim>>(1)) {
}

template<int dim>
WaveEquationAdjoint<dim>::WaveEquationAdjoint(const WaveEquation<dim> &wave)
      : AbstractEquationAdjoint<dim>(wave.get_mesh()), zero(std::make_shared<Functions::ZeroFunction<dim>>(1)) {

   this->set_param_c(wave.get_param_c());
   this->set_param_nu(wave.get_param_nu());
   this->set_param_rho(wave.get_param_rho());
   this->set_param_q(wave.get_param_q());
   this->set_rho_time_dependent(wave.is_rho_time_dependent());

   this->set_theta(wave.get_theta());
   this->set_solver_tolerance(wave.get_solver_tolerance());
   this->set_solver_max_iter(wave.get_solver_max_iter());
}

template<int dim>
void WaveEquationAdjoint<dim>::assemble_matrices(size_t time_idx) {
   LogStream::Prefix p("assemble_matrices");

   this->fill_matrices(mesh, time_idx, matrix_A, matrix_B, matrix_C);
}

template<int dim>
void WaveEquationAdjoint<dim>::apply_boundary_conditions_u(double time __attribute ((unused))) {
   std::map<types::global_dof_index, double> boundary_values;
   VectorTools::interpolate_boundary_values(*dof_handler, 0, *this->zero, boundary_values);
   MatrixTools::apply_boundary_values(boundary_values, system_matrix, solution_u, system_rhs_u);
}

template<int dim>
void WaveEquationAdjoint<dim>::apply_boundary_conditions_v(double time __attribute ((unused))) {
   std::map<types::global_dof_index, double> boundary_values;
   VectorTools::interpolate_boundary_values(*dof_handler, 0, *this->zero, boundary_values);
   MatrixTools::apply_boundary_values(boundary_values, system_matrix, solution_v, system_rhs_v);
}

template class WaveEquationAdjoint<1> ;
template class WaveEquationAdjoint<2> ;
template class WaveEquationAdjoint<3> ;

} /* namespace forward */
} /* namespace wavepi */


#include "Formatings.h"

#include <stdio.h>
#include <stdlib.h>
#include <datas/sensor_data.hpp>
#include <vector>
#include <cmath>
#include "Geometrie.h"
#include <Eigen/Core>
#include <Eigen/Eigenvalues>

float compute_value(laser_4m_record m1, int i, double limit){
  if(0.02<m1.data[i].dist<limit){
    return m1.data[i].dist;
  }
  if (0.02<m1.data[i-1].dist<limit && 0.02<m1.data[i+1].dist<limit){
    return (m1.data[i-1].dist+m1.data[i+1].dist)/2;
  }
  //si on arrive ici (m1[1] >= limite && (m1[i+1]>=limite || m1[i-1]>=limite)
  //Sois les trois mesures sont a la limite sois une seul ne l'est pas (elle deviendra la "mesure reel")
  if(m1.data[i-1].dist<m1.data[i+1].dist && m1.data[i-1].dist>0.02){
    return m1.data[i-1].dist;
  }
  return m1.data[i+1].dist>0.02?m1.data[i+1].dist:m1.data[i].dist;
}

std::vector<struct polar_coordinate> format_Telemetre(polar_coordinate Values[], int length, float offsetX, float offsetY, float offsetTheta){
  double Xstart,Xend,Ystart,Yend;
  polar_coordinate tmpres={{0,0,0},0};
  std::vector<struct polar_coordinate> Res;
  for(int i=0;i<length;i++){
    Xstart=Values[i].dist*cos(Values[i].dir.theta);
    Ystart=Values[i].dist*sin(Values[i].dir.theta);
    Xend=cos(offsetTheta)*Xstart+sin(offsetTheta)*Ystart+offsetX;
    Yend=-1*sin(offsetTheta)*Xstart+cos(offsetTheta)*Ystart+offsetY;
    tmpres.dist=sqrt(Xend*Xend+Yend*Yend);
    tmpres.dir.theta=atan2(Yend,Xend);
    Res.push_back(tmpres);
  }
  return Res;
}

std::vector<struct polar_coordinate> format_US(double US[]){
  std::vector<struct polar_coordinate> Res;
  double Xstart,Xend,Ystart,Yend;
  eucclid_coordinate Relevee={0,0,0};
  polar_coordinate tmpres={{0,0,0},0};
  double Pas=(20*2*M_PI)/360;
  double Tright=-1*M_PI;
  double Tleft=M_PI;
  double Thetas_Deg_US[]={90,50,30,10,-10,-30,-50,-90,-90,-130,-150,-170,170,150,130,90};
//  double Thetas_US[16]={Tright,Tright+Pas,Tright+2*Pas,Tright+3*Pas,Tright+4*Pas,Tright+5*Pas,Tright+6*Pas,Tleft,Tleft,Tleft+Pas,Tleft+2*Pas,Tleft+3*Pas,Tleft+4*Pas,Tleft+5*Pas,Tleft+6*Pas,Tleft+7*Pas,Tright};
  for(int i=0;i<16;i++){
    tmpres.dist=US[i]+0.1;//Aproximation
    tmpres.dir.theta=(Thetas_Deg_US[i]/180)*M_PI;
    Relevee=ConvPolToEuclide(tmpres);
    if(i<8){//US Front
      //LES US se "rejoignes" environ 8CM en avant des roues.
      Relevee.x=Relevee.x+0.08;
    }else{//US rear
      //LES US se "rejoignes" environ 16CM en arrière des roues.
      Relevee.x=Relevee.x-0.16;
    }
    tmpres=ConvEuclideToPol(Relevee);
    Res.push_back(tmpres);
  }
  return Res;
}

std::vector<pfoa::waypoint> format_Path(std::vector<struct eucclid_coordinate> Points){
  double dist=0;
  std::vector<pfoa::waypoint> Path;
  eucclid_coordinate lastPoint=Points[0];
  for(int i=0;i<Points.size();i++){
    dist=dist+sqrt(pow(lastPoint.x-Points[i].x,2)+pow(lastPoint.y-Points[i].y,2));
    Path.push_back({Points[i].x,Points[i].y,dist});
    lastPoint=Points[i];
  }
}

void Recomput_Weeldist(euclid_position Oldstate,euclid_position R_state, double &ssg, double &ssd){
  double L=ENTRE_ROUE;
  float dx=R_state.pos.x-Oldstate.pos.x;
  float dy=R_state.pos.y-Oldstate.pos.y;
  float dt=R_state.dir.theta-Oldstate.dir.theta;
  float ds=sqrt(pow(dx,2)+pow(dy,2));
  ssg=(ds-(dt*L)/2);
  ssd=(dt*L)+ssg;
}

void SimuOdo(euclid_position Oldstate,euclid_position &R_state, double DistRG, double DistRD){
  double L=ENTRE_ROUE;
  float ds = (DistRG + DistRD)/2;
  float dt = (DistRD-DistRG)/L;
  R_state.pos.x=Oldstate.pos.x+ds*cos(Oldstate.dir.theta+dt/2);
  R_state.pos.y=Oldstate.pos.y+ds*sin(Oldstate.dir.theta+dt/2);
  R_state.dir.theta=Oldstate.dir.theta+dt;
}

void compute_Fenetre_Error_Odo(euclid_position R_state, double distLeft, double distRight, Eigen::MatrixXd &P, double &x1, double &x2, double &y1, double &y2, double &t1, double &t2){
  Eigen::MatrixXd OdoCov(2,2);
  OdoCov<< 0.001780754260856,0,0,0.001691065861758;
  Eigen::MatrixXd OdoCovtmp(2,2);
  Eigen::MatrixXd Dist(2,2);
  Dist<< 1.0,0.0,0.0,1.0;
  Eigen::MatrixXd Jacobrob(3,3);
  Jacobrob << 1,0,0,0,1,0,0,0,1;
  Eigen::MatrixXd Jacobdist(3,2);
  Eigen::MatrixXd Pstart(3,3);
  Pstart=P;

  double Lodo=ENTRE_ROUE;
  double ds,dt;
  double demicos,demisin;
  int iex, iey, iet;
  double Ex, Ey, OrientEy, ExSim, EySim, OrientEySim;
  double ext1, ext2, eyt1, eyt2;
  euclid_position OdoEstime;

  //Compute odometries errors.
  Dist(0,0)=distRight;
  Dist(1,1)=distLeft;
  OdoCovtmp = OdoCov*Dist;
  ds=(distRight+distLeft)/2;
  dt=(distRight-distLeft)/Lodo;
  Jacobrob(0,2)=-1*ds*sin(R_state.dir.theta+dt/2);
  Jacobrob(1,2)=ds*cos(R_state.dir.theta+dt/2);

  demicos=(cos(R_state.dir.theta+dt/2))/2;
  demisin=(sin(R_state.dir.theta+dt/2))/2;

  Jacobdist(0,0)=demicos-(ds/Lodo)*demisin;
  Jacobdist(0,1)=demicos+(ds/Lodo)*demisin;
  Jacobdist(1,0)=demisin+(ds/Lodo)*demicos;
  Jacobdist(1,1)=demisin-(ds/Lodo)*demicos;
  Jacobdist(2,0)=1/Lodo;
  Jacobdist(2,1)=-1/Lodo;

  P=Jacobrob*Pstart*Jacobrob.transpose()+Jacobdist*OdoCovtmp*Jacobdist.transpose();

  Eigen::EigenSolver<Eigen::MatrixXd> VecpropSolvP(P);
  Eigen::MatrixXd ValsProps=VecpropSolvP.pseudoEigenvalueMatrix();
  Eigen::MatrixXd VecsProps=VecpropSolvP.pseudoEigenvectors();
  //Ey = val prop la plus grande puis Ex puis Et

  if(ValsProps(0,0)>ValsProps(1,1)){//0 sup a 1
    if(ValsProps(2,2)>ValsProps(0,0)){//2-0-1
      iex = 0;
      iey = 2;
      iet = 1;
    }
    else if(ValsProps(2,2)>ValsProps(1,1)){//0-2-1
      iex = 2;
      iey = 0;
      iet = 1;
    }else{//0-1-2
      iex = 1;
      iey = 0;
      iet = 2;
    }
  }else{//1 sup a 0
    if(ValsProps(2,2)>ValsProps(1,1)){//2-1-0
      iex = 1;
      iey = 2;
      iet = 0;
    }
    else if(ValsProps(2,2)>ValsProps(0,0)){//1-2-0
      iex = 2;
      iey = 1;
      iet = 0;
    }else{//1-0-2
      iex = 0;
      iey = 1;
      iet = 2;
    }
  }
  Ex = pow(ValsProps(iex,iex),(1/3.0));
  Ey = pow(ValsProps(iey,iey),(1/3.0));
  OrientEy = atan2(VecsProps(1,iey),VecsProps(2,iey));
  SimuOdo(R_state,OdoEstime,distLeft,distRight);
  ext1=abs(Ex*cos(R_state.dir.theta+OrientEy-M_PI/2));
  eyt1=abs(Ex*sin(R_state.dir.theta+OrientEy-M_PI/2));
  ext2=abs(Ey*cos(R_state.dir.theta+OrientEy));
  eyt2=abs(Ey*sin(R_state.dir.theta+OrientEy));
//  ext1=max(ext1,ext2);
  if(ext2>ext1){
    ext1=ext2;
  }
//  eyt1=max(eyt1,eyt2);
  if(eyt2>eyt1){
    eyt1=eyt2;
  }
  x1=OdoEstime.pos.x+ext1;
  x2=OdoEstime.pos.x-ext1;
  y1=OdoEstime.pos.y+eyt1;
  y2=OdoEstime.pos.y-eyt1;
  t2=R_state.dir.theta+dt-pow(fabs(ValsProps(iet,iet)),(1/3.0));
  t1=R_state.dir.theta+dt+pow(fabs(ValsProps(iet,iet)),(1/3.0));

}

void compute_Fenetre_Error_Odo_Rob(euclid_position R_state, double distLeft, double distRight, Eigen::MatrixXd &P, eucclid_coordinate &PEShort,eucclid_coordinate &PELong){
  /*Renvois les coordonees des points des axes representant l'elipse d'incertitude.*/
  Eigen::MatrixXd OdoCov(2,2);
  OdoCov<< 0.001780754260856,0,0,0.001691065861758;
  Eigen::MatrixXd OdoCovtmp(2,2);
  Eigen::MatrixXd Dist(2,2);
  Dist<< 1.0,0.0,0.0,1.0;
  Eigen::MatrixXd Jacobrob(3,3);
  Jacobrob << 1,0,0,0,1,0,0,0,1;
  Eigen::MatrixXd Jacobdist(3,2);
  Eigen::MatrixXd Pstart(3,3);
  Pstart=P;

  double Lodo=ENTRE_ROUE;
  double ds,dt;
  double demicos,demisin;
  int iex, iey, iet;
  double Ex, Ey, OrientEy, ExSim, EySim, OrientEySim;
  double ext1, ext2, eyt1, eyt2;
  euclid_position OdoEstime;

  //Compute odometries errors.
  Dist(0,0)=distRight;
  Dist(1,1)=distLeft;
  OdoCovtmp = OdoCov*Dist;
  ds=(distRight+distLeft)/2;
  dt=(distRight-distLeft)/Lodo;
  Jacobrob(0,2)=-1*ds*sin(R_state.dir.theta+dt/2);
  Jacobrob(1,2)=ds*cos(R_state.dir.theta+dt/2);

  demicos=(cos(R_state.dir.theta+dt/2))/2;
  demisin=(sin(R_state.dir.theta+dt/2))/2;

  Jacobdist(0,0)=demicos-(ds/Lodo)*demisin;
  Jacobdist(0,1)=demicos+(ds/Lodo)*demisin;
  Jacobdist(1,0)=demisin+(ds/Lodo)*demicos;
  Jacobdist(1,1)=demisin-(ds/Lodo)*demicos;
  Jacobdist(2,0)=1/Lodo;
  Jacobdist(2,1)=-1/Lodo;

  P=Jacobrob*Pstart*Jacobrob.transpose()+Jacobdist*OdoCovtmp*Jacobdist.transpose();

  Eigen::EigenSolver<Eigen::MatrixXd> VecpropSolvP(P);
  Eigen::MatrixXd ValsProps=VecpropSolvP.pseudoEigenvalueMatrix();
  Eigen::MatrixXd VecsProps=VecpropSolvP.pseudoEigenvectors();
  //Ey = val prop la plus grande puis Ex puis Et

  if(ValsProps(0,0)>ValsProps(1,1)){//0 sup a 1
    if(ValsProps(2,2)>ValsProps(0,0)){//2-0-1
      iex = 0;
      iey = 2;
      iet = 1;
    }
    else if(ValsProps(2,2)>ValsProps(1,1)){//0-2-1
      iex = 2;
      iey = 0;
      iet = 1;
    }else{//0-1-2
      iex = 1;
      iey = 0;
      iet = 2;
    }
  }else{//1 sup a 0
    if(ValsProps(2,2)>ValsProps(1,1)){//2-1-0
      iex = 1;
      iey = 2;
      iet = 0;
    }
    else if(ValsProps(2,2)>ValsProps(0,0)){//1-2-0
      iex = 2;
      iey = 1;
      iet = 0;
    }else{//1-0-2
      iex = 0;
      iey = 1;
      iet = 2;
    }
  }
  Ex = pow(ValsProps(iex,iex),(1/3.0));
  Ey = pow(ValsProps(iey,iey),(1/3.0));
  OrientEy = atan2(VecsProps(1,iey),VecsProps(2,iey));
//  SimuOdo(R_state,OdoEstime,distLeft,distRight);
  ext1=abs(Ex*cos(OrientEy-M_PI/2));
  eyt1=abs(Ex*sin(OrientEy-M_PI/2));
  ext2=abs(Ey*cos(OrientEy));
  eyt2=abs(Ey*sin(OrientEy));

  PEShort.x=ext1;
  PEShort.y=ext1;
//  PEShort.theta=R_state.dir.theta+dt-pow(ValsProps(iet,iet),(1/3.0));
  PELong.x=ext2;
  PELong.y=ext2;
//  PELong.theta=R_state.dir.theta+dt+pow(ValsProps(iet,iet),(1/3.0));

}

double Compute_ThetaCompas(std::vector<polar_coordinate> Telemetrie, euclid_position Odom){
  std::vector<polar_coordinate> Tmp_telemetries;
  std::vector<int> iRans;
  std::vector<double> As, difAs;
  std::vector<double> WAs, WdifAs;

  double Coridor_orient[][5]={{4.88,26.38,-1,2.5,0},{23,30,2.88,50.88,M_PI/2}};
  int Coridor_Num=2;
  int RoboDir, PresentCoridor;
  int ifind;
  double awall, bwall, Angletmp;
  double Angle_Compas;
  struct polar_coordinate P1,P2;
  iRans.clear();
  As.clear();
  WAs.clear();
  awall=2000;
  Angle_Compas=999;

  //Retrais des points pouvant être trop eloignee
  for(int i=0; i<Telemetrie.size(); i++){
    if(Telemetrie[i].dist<3.5 && Telemetrie[i].dist>0.05){
      Tmp_telemetries.push_back(Telemetrie[i]);
    }
  }
  //Recuperation de l'orientation principale a cet endroit dans le reper monde.
  PresentCoridor=-1;
  RoboDir=-1;
  for(int i=0; i<Coridor_Num; i++){
    //Si l'odometrie se pense dans une zone d'un couloir
    if(Odom.pos.x>Coridor_orient[i][0] && Odom.pos.x<Coridor_orient[i][1] && Odom.pos.y>Coridor_orient[i][2] && Odom.pos.y<Coridor_orient[i][3]){
      PresentCoridor=i;
      break;
    }
  }
/*
  std::cout << "Present Coridor = "<< PresentCoridor  << '\n';
  std::cout << "Info Coridor = "<< Coridor_orient[PresentCoridor][4] << " - Odo theta = "<< Odom.dir.theta << '\n';
  std::cout << " - Fabs observe => "<< fabs(Coridor_orient[PresentCoridor][4]-Odom.dir.theta) << '\n';/**/
  //On determine le sens de deplacement du robot.
  if(PresentCoridor!=-1){
    if(fabs(Coridor_orient[PresentCoridor][4]-Odom.dir.theta)<4*M_PI/12){//Retour
      RoboDir=1;
    }
    if(fabs(Coridor_orient[PresentCoridor][4]-Odom.dir.theta)>8*M_PI/12){//Aller
      RoboDir=2;
    }
  }
//  std::cout << "ROBODIR = "<< RoboDir << '\n';
  if(Telemetrie.size()!=0 && RoboDir!=-1){
    while(As.size()<5 && Tmp_telemetries.size()>Telemetrie.size()*0.1){
      for(int i=0; i<iRans.size(); i++){
        ifind=iRans.size()-1-i;
        Tmp_telemetries.erase(Tmp_telemetries.begin()+iRans[ifind]);
      }
      iRans=ransaclignefrompole(Tmp_telemetries, awall, bwall, 0.01, P1, P2);
      As.push_back(awall);
      WAs.push_back(iRans.size());
//      std::cout << "New A = "<< awall << " - Poid = "<< iRans.size() << '\n';
    }
    difAs.clear();
    WdifAs.clear();
    //On met la plus grosse ligne en premier parmis les possibilités.
    difAs.push_back(As[0]);
    WdifAs.push_back(WAs[0]);
    for(int i=1; i<As.size(); i++){
      awall=0;
      for(int j=0; i<difAs.size(); j++){
        Angletmp=fabs(atan2(difAs[j],1)-atan2(As[i],1));
        while(Angletmp>=M_PI){
          Angletmp=Angletmp-M_PI;
        }
        if(Angletmp<M_PI/6 && Angletmp>5*M_PI/6){//Le mur est paralèle a +/-pi/4
          WdifAs[j]=WdifAs[j]+WAs[i];
          awall=1;
          break;
        }
      }
      if(awall!=1){//Pas de droite "paralèle" trouvé. on cré un nouveau groupe. avec la droite ayant le plus de "membres"
        difAs.push_back(As[i]);
        WdifAs.push_back(WAs[i]);
      }
    }
    //Recuperation de l'orientation dont le poid des paralèle est le plus important
    bwall=0;
    for(int i=0; i<WdifAs.size(); i++){
      if(WdifAs[i]>bwall){
        bwall=WdifAs[i];
        awall=difAs[i];
      }
    }
//    std::cout << "awall = "<< awall<< " - Atan awall = "<< atan2(awall,1) << '\n';
    if(RoboDir==1){ //retour
      Angle_Compas=-1*(atan2(awall,1))+Coridor_orient[PresentCoridor][4];
    }
    if(RoboDir==2){ //aller
      Angle_Compas=-1*(atan2(awall,1))+(Coridor_orient[PresentCoridor][4]-M_PI);
    }
  }
  return Angle_Compas;
}

std::vector<double> Iadd(std::vector<double> ia, std::vector<double>ib){
  std::vector<double> res;
  if (IisEmpty(ia)){
    return res;
  }
  if (IisEmpty(ib)){
    return res;
  }
  double mina, maxa, minb, maxb;
  mina=*min_element(ia.begin(), ia.end());
  maxa=*max_element(ia.begin(), ia.end());
  minb=*min_element(ib.begin(), ib.end());
  maxb=*max_element(ib.begin(), ib.end());
  res.reserve(2);
  res.push_back(mina+minb);
  res.push_back(maxa+maxb);
  return res;
}
std::vector<double> Isub(std::vector<double> ia, std::vector<double>ib){
    std::vector<double> res;
    if (IisEmpty(ia)){
      return res;
    }
    if (IisEmpty(ib)){
      return res;
    }
    double mina, maxa, minb, maxb;
    mina=*min_element(ia.begin(), ia.end());
    maxa=*max_element(ia.begin(), ia.end());
    minb=*min_element(ib.begin(), ib.end());
    maxb=*max_element(ib.begin(), ib.end());
    res.reserve(2);
    res.push_back(mina-maxb);
    res.push_back(maxa-minb);
    return res;
}
std::vector<double> Imul(std::vector<double> ia, std::vector<double>ib){
  std::vector<double> res;
  if (IisEmpty(ia)){
    return res;
  }
  if (IisEmpty(ib)){
    return res;
  }
  std::vector<double> tmp(4);
  double mina, maxa, minb, maxb;
  mina=*min_element(ia.begin(), ia.end());
  maxa=*max_element(ia.begin(), ia.end());
  minb=*min_element(ib.begin(), ib.end());
  maxb=*max_element(ib.begin(), ib.end());
  tmp[0]=mina*minb;
  tmp[1]=mina*maxb;
  tmp[2]=maxa*minb;
  tmp[3]=maxa*maxb;
  res.reserve(2);
  res.push_back(*min_element(tmp.begin(), tmp.end()));
  res.push_back(*max_element(tmp.begin(), tmp.end()));
  return res;
}
std::vector<double> Icos(std::vector<double> ia){
  std::vector<double> res, tmp(2);
  if (IisEmpty(ia)){
    return res;
  }
  double mina, maxa, minb, maxb;
  tmp[0]=*min_element(ia.begin(), ia.end())+M_PI/2;
  tmp[1]=*max_element(ia.begin(), ia.end())+M_PI/2;
  res=Isin(tmp);
  return res;
}
std::vector<double> Isin(std::vector<double> ia){
  std::vector<double> res,tmp(2),rtmp,Snum(1);
  if (IisEmpty(ia)){
    return res;
  }
  double mina, maxa, minb, maxb;
  tmp[0]=*min_element(ia.begin(), ia.end());
  tmp[1]=*max_element(ia.begin(), ia.end());
  if(Ilength(tmp)>2*M_PI){
    res.reserve(2);
    res.push_back(-1);
    res.push_back(1);
    return res;
  }
  while(tmp[1]>2*M_PI){
    tmp[0]=tmp[0]-2*M_PI;
    tmp[1]=tmp[1]-2*M_PI;
  }
  while(tmp[0]<0){
    tmp[0]=tmp[0]+2*M_PI;
    tmp[1]=tmp[1]+2*M_PI;
  }
  rtmp.push_back(sin(tmp[0]));
  rtmp.push_back(sin(tmp[1]));
  Snum[0]=M_PI/2;
  if(Iin(Snum,tmp)==true){
    rtmp.push_back(1);
  }
  Snum[0]=3*M_PI/2;
  if(Iin(Snum,tmp)==true){
    rtmp.push_back(-1);
  }
  res.reserve(2);
  res.push_back(*min_element(rtmp.begin(), rtmp.end()));
  res.push_back(*max_element(rtmp.begin(), rtmp.end()));
  return res;
}

double Imid(std::vector<double> ia){
    double res;
    double mina, maxa;
    mina=*min_element(ia.begin(), ia.end());
    maxa=*max_element(ia.begin(), ia.end());
    res=mina+(maxa-mina)/2;
    return res;
}

double Ilength(std::vector<double> ia){
  double res;
  double mina, maxa;
  if (IisEmpty(ia)){
    return -1;
  }
  mina=*min_element(ia.begin(), ia.end());
  maxa=*max_element(ia.begin(), ia.end());
  res=maxa-mina;
  return res;
}

bool Iin(std::vector<double> ia,std::vector<double> ib){
  if (IisEmpty(ia)||IisEmpty(ib)){
    return false;
  }
  double mina, maxa, minb, maxb;
  mina=*min_element(ia.begin(), ia.end());
  maxa=*max_element(ia.begin(), ia.end());
  minb=*min_element(ib.begin(), ib.end());
  maxb=*max_element(ib.begin(), ib.end());
  return ((mina>=minb)&&(maxa<=maxb));
}

void IVals(std::vector<double> ia,double &min, double &max){
  if (IisEmpty(ia)){
    return;
  }
  min=*min_element(ia.begin(), ia.end());
  max=*max_element(ia.begin(), ia.end());
  return;
}

std::vector<double> initInterval(double a,double b){
  std::vector<double> res={a,b};
  return res;
}

std::vector<double> Icentering(std::vector<double> ia){
  std::vector<double> res;
  if (IisEmpty(ia)){
    return res;
  }
//  double mina, maxa, minb, maxb;
  res.reserve(2);
  res.push_back(*min_element(ia.begin(), ia.end())-Imid(ia));
  res.push_back(*max_element(ia.begin(), ia.end())-Imid(ia));
  return res;
}

std::vector<double> Iintersec(std::vector<double> ia, std::vector<double>ib){
  std::vector<double> res;
  if (IisEmpty(ia)){
    return res;
  }
  if (IisEmpty(ib)){
    return res;
  }
  double mina, maxa, minb, maxb;
  mina=*min_element(ia.begin(), ia.end());
  maxa=*max_element(ia.begin(), ia.end());
  minb=*min_element(ib.begin(), ib.end());
  maxb=*max_element(ib.begin(), ib.end());
  if(!(maxa < minb || mina > maxb)){
    res.reserve(2);
    res.push_back(mina*(mina>minb)+minb*!(mina>minb));
    res.push_back(maxa*(maxa<maxb)+maxb*!(maxa<maxb));
  }
  return res;
}
std::vector<double> Iunion(std::vector<double> ia, std::vector<double>ib){
  std::vector<double> res;
  double mina, maxa, minb, maxb;
  if (IisEmpty(ia)){
    return ib;
  }
  if (IisEmpty(ib)){
    return ia;
  }
  mina=*min_element(ia.begin(), ia.end());
  maxa=*max_element(ia.begin(), ia.end());
  minb=*min_element(ib.begin(), ib.end());
  maxb=*max_element(ib.begin(), ib.end());
  res.reserve(2);
  res.push_back(mina*(mina<minb)+minb*!(mina<minb));
  res.push_back(maxa*(maxa>maxb)+maxb*!(maxa>maxb));
  return res;
}

bool IisEmpty(std::vector<double> ia){
  return ia.size()==0;
}


// Compile with:
// emcc -I${BOOST} -I${QUANTLIB} -s BINARYEN_TRAP_MODE=clamp -o billiontrader-bootstrapping.js billiontrader-bootstrapping.cpp ${QUANTLIB}/ql/.libs/libQuantLib.a

#include <ql/quantlib.hpp>

#ifdef BOOST_MSVC
/* Uncomment the following lines to unmask floating-point
exceptions. Warning: unpredictable results can arise...

See http://www.wilmott.com/messageview.cfm?catid=10&threadid=9481
Is there anyone with a definitive word about this?
*/
// #include <float.h>
// namespace { unsigned int u = _controlfp(_EM_INEXACT, _MCW_EM); }
#endif

#include <ql/quantlib.hpp>
//#include <boost/timer.hpp>
#include <iostream>
#include <iomanip>
#include <fstream>

using namespace std;
using namespace QuantLib;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib
{

Integer sessionId() { return 0; }

} // namespace QuantLib
#endif

using namespace QuantLib;

int main(int argc, char *argv[])
{
    Calendar calendar = JointCalendar(UnitedKingdom(UnitedKingdom::Exchange), UnitedStates(UnitedStates::Settlement), JoinHolidays);

    Date settlementDate(18, February, 2015);
    settlementDate = calendar.adjust(settlementDate);
    // Due to Release 1.20 - October 26th, 2020: Improved calculation of spot date for vanilla swap around holidays (thanks to Paul Giltinan),
    // the example no longer works with 2 spot days
    Integer fixingDays = 0;
    Date todaysDate = calendar.advance(settlementDate, -fixingDays, Days);
    Settings::instance().evaluationDate() = todaysDate;
    DayCounter depositDayCounter = Actual360();

    Rate d1wQuote = 0.001375;
    Rate d1mQuote = 0.001717;
    Rate d2mQuote = 0.002112;
    Rate d3mQuote = 0.002581;
    boost::shared_ptr<Quote> d1wRate(new SimpleQuote(d1wQuote));
    boost::shared_ptr<Quote> d1mRate(new SimpleQuote(d1mQuote));
    boost::shared_ptr<Quote> d2mRate(new SimpleQuote(d2mQuote));
    boost::shared_ptr<Quote> d3mRate(new SimpleQuote(d3mQuote));
    boost::shared_ptr<RateHelper> d1w(new DepositRateHelper(Handle<Quote>(d1wRate), 7 * Days, fixingDays, calendar, ModifiedFollowing, true, depositDayCounter));
    boost::shared_ptr<RateHelper> d1m(new DepositRateHelper(Handle<Quote>(d1mRate), 4 * Weeks, fixingDays, calendar, ModifiedFollowing, true, depositDayCounter));
    boost::shared_ptr<RateHelper> d2m(new DepositRateHelper(Handle<Quote>(d2mRate), 2 * Months, fixingDays, calendar, ModifiedFollowing, true, depositDayCounter));
    boost::shared_ptr<RateHelper> d3m(new DepositRateHelper(Handle<Quote>(d3mRate), 3 * Months, fixingDays, calendar, ModifiedFollowing, true, depositDayCounter));

    DayCounter FutDayCounter = Actual360();
    Real fut1Quote = 99.725; // 0.2750
    Real fut2Quote = 99.585; // 0.4150
    Real fut3Quote = 99.385; //0.6150
    Real fut4Quote = 99.16;  // 0.84
    Real fut5Quote = 98.93;  // 1.07
    Real fut6Quote = 98.715; // 1.285
    boost::shared_ptr<Quote> fut1Price(new SimpleQuote(fut1Quote));
    boost::shared_ptr<Quote> fut2Price(new SimpleQuote(fut2Quote));
    boost::shared_ptr<Quote> fut3Price(new SimpleQuote(fut3Quote));
    boost::shared_ptr<Quote> fut4Price(new SimpleQuote(fut4Quote));
    boost::shared_ptr<Quote> fut5Price(new SimpleQuote(fut5Quote));
    boost::shared_ptr<Quote> fut6Price(new SimpleQuote(fut6Quote));
    Integer futMonths = 3;

    Date imm = IMM::nextDate(settlementDate);
    boost::shared_ptr<RateHelper> fut1(new FuturesRateHelper(Handle<Quote>(fut1Price), imm, futMonths, calendar, ModifiedFollowing, true, depositDayCounter));
    imm = IMM::nextDate(imm + 1);
    boost::shared_ptr<RateHelper> fut2(new FuturesRateHelper(Handle<Quote>(fut2Price), imm, futMonths, calendar, ModifiedFollowing, true, depositDayCounter));
    imm = IMM::nextDate(imm + 1);
    boost::shared_ptr<RateHelper> fut3(new FuturesRateHelper(Handle<Quote>(fut3Price), imm, futMonths, calendar, ModifiedFollowing, true, depositDayCounter));
    imm = IMM::nextDate(imm + 1);
    boost::shared_ptr<RateHelper> fut4(new FuturesRateHelper(Handle<Quote>(fut4Price), imm, futMonths, calendar, ModifiedFollowing, true, depositDayCounter));

    imm = IMM::nextDate(imm + 1);
    boost::shared_ptr<RateHelper> fut5(new FuturesRateHelper(Handle<Quote>(fut5Price), imm, futMonths, calendar, ModifiedFollowing, true, depositDayCounter));
    imm = IMM::nextDate(imm + 1);
    boost::shared_ptr<RateHelper> fut6(new FuturesRateHelper(Handle<Quote>(fut6Price), imm, futMonths, calendar, ModifiedFollowing, true, depositDayCounter));

    Rate s2yQuote = 0.0089268;
    Rate s3yQuote = 0.0123343;
    Rate s4yQuote = 0.0147985;
    Rate s5yQuote = 0.0165843;
    Rate s6yQuote = 0.0179191;
    boost::shared_ptr<Quote> s2yRate(new SimpleQuote(s2yQuote));
    boost::shared_ptr<Quote> s3yRate(new SimpleQuote(s3yQuote));
    boost::shared_ptr<Quote> s4yRate(new SimpleQuote(s4yQuote));
    boost::shared_ptr<Quote> s5yRate(new SimpleQuote(s5yQuote));
    boost::shared_ptr<Quote> s6yRate(new SimpleQuote(s6yQuote));

    Frequency swFixedLegFrequency = Annual;
    BusinessDayConvention swFixedLegConvention = Unadjusted;
    DayCounter swFixedLegDayCounter = Actual360();
    boost::shared_ptr<IborIndex> swFloatingLegIndex(new USDLibor(Period(3, Months)));

    boost::shared_ptr<RateHelper> s2y(new SwapRateHelper(
        Handle<Quote>(s2yRate), 2 * Years,
        calendar, swFixedLegFrequency,
        swFixedLegConvention, swFixedLegDayCounter,
        swFloatingLegIndex));
    boost::shared_ptr<RateHelper> s3y(new SwapRateHelper(
        Handle<Quote>(s3yRate), 3 * Years,
        calendar, swFixedLegFrequency,
        swFixedLegConvention, swFixedLegDayCounter,
        swFloatingLegIndex));
    boost::shared_ptr<RateHelper> s4y(new SwapRateHelper(
        Handle<Quote>(s4yRate), 4 * Years,
        calendar, swFixedLegFrequency,
        swFixedLegConvention, swFixedLegDayCounter,
        swFloatingLegIndex));
    boost::shared_ptr<RateHelper> s5y(new SwapRateHelper(
        Handle<Quote>(s5yRate), 5 * Years,
        calendar, swFixedLegFrequency,
        swFixedLegConvention, swFixedLegDayCounter,
        swFloatingLegIndex));
    boost::shared_ptr<RateHelper> s6y(new SwapRateHelper(
        Handle<Quote>(s6yRate), 6 * Years,
        calendar, swFixedLegFrequency,
        swFixedLegConvention, swFixedLegDayCounter,
        swFloatingLegIndex));

    typedef boost::shared_ptr<RateHelper> SharedPtrRateHelper;
    vector<SharedPtrRateHelper> depoFutSwapInstruments;
    depoFutSwapInstruments.push_back(d1w);
    depoFutSwapInstruments.push_back(d1m);
    depoFutSwapInstruments.push_back(d2m);
    depoFutSwapInstruments.push_back(d3m);
    depoFutSwapInstruments.push_back(fut1);
    depoFutSwapInstruments.push_back(fut2);
    depoFutSwapInstruments.push_back(fut3);
    depoFutSwapInstruments.push_back(fut4);
    depoFutSwapInstruments.push_back(fut5);
    depoFutSwapInstruments.push_back(fut6);
    depoFutSwapInstruments.push_back(s2y);
    depoFutSwapInstruments.push_back(s3y);
    depoFutSwapInstruments.push_back(s4y);
    depoFutSwapInstruments.push_back(s5y);
    depoFutSwapInstruments.push_back(s6y);

    DayCounter termStructureDayCounter = Actual360();
    boost::shared_ptr<YieldTermStructure> depoFutSwapTermStructure(new PiecewiseYieldCurve<Discount,
                                                                                Linear>(settlementDate, depoFutSwapInstruments, termStructureDayCounter));
    Date matDate1(25, February, 2015);
    Date matDate2(18, March, 2015);
    Date matDate3(20, April, 2015);
    Date matDate4(18, May, 2015);

    Date matDate5(17, June, 2015);
    Date matDate6(16, September, 2015);
    Date matDate7(16, December, 2015);
    Date matDate8(16, March, 2016);
    Date matDate9(15, June, 2016);

    Date matDate10(21, September, 2016);
    Date matDate11(21, February, 2017);
    Date matDate12(20, February, 2018);
    Date matDate13(19, February, 2019);
    Date matDate14(18, February, 2020);

    // Round output to 4 decimals
    std::cout.precision(4);

    std::cout << "0.1375: " << depoFutSwapTermStructure->zeroRate(matDate1, depositDayCounter, Simple) << std::endl;
    std::cout << "0.1717: " << depoFutSwapTermStructure->zeroRate(matDate2, depositDayCounter, Simple) << std::endl;
    std::cout << "0.2112: " << depoFutSwapTermStructure->zeroRate(matDate3, depositDayCounter, Simple) << std::endl;
    std::cout << "0.2581: " << depoFutSwapTermStructure->zeroRate(matDate4, depositDayCounter, Simple) << std::endl;

    std::cout << "0.2511: " << depoFutSwapTermStructure->zeroRate(matDate5, FutDayCounter, Simple) << std::endl;
    std::cout << "0.3223: " << depoFutSwapTermStructure->zeroRate(matDate6, FutDayCounter, Simple) << std::endl;
    std::cout << "0.4111: " << depoFutSwapTermStructure->zeroRate(matDate7, FutDayCounter, Simple) << std::endl;
    std::cout << "0.5113: " << depoFutSwapTermStructure->zeroRate(matDate8, FutDayCounter, Simple) << std::endl;
    std::cout << "0.6177: " << depoFutSwapTermStructure->zeroRate(matDate9, FutDayCounter, Simple) << std::endl;

    std::cout << "0.7325: " << depoFutSwapTermStructure->zeroRate(matDate10, FutDayCounter, Compounded, Annual) << std::endl;
    std::cout << "0.8911: " << depoFutSwapTermStructure->zeroRate(matDate11, FutDayCounter, Compounded, Annual) << std::endl;
    std::cout << "1.2373: " << depoFutSwapTermStructure->zeroRate(matDate12, FutDayCounter, Compounded, Annual) << std::endl;
    std::cout << "1.4884: " << depoFutSwapTermStructure->zeroRate(matDate13, FutDayCounter, Compounded, Annual) << std::endl;
    std::cout << "1.6719: " << depoFutSwapTermStructure->zeroRate(matDate14, FutDayCounter, Compounded, Annual) << std::endl;
    std::cout << "Discount Rate : " << depoFutSwapTermStructure->discount(matDate14) << std::endl;
    std::cout << "Forward Rate : " << depoFutSwapTermStructure->forwardRate(matDate13, matDate14, FutDayCounter, Simple) << std::endl;

    return 0;
}