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

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include "toplevelfixture.hpp"
#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <ql/currencies/all.hpp>
#include <ql/indexes/indexmanager.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/actualactual.hpp>
#include <qle/cashflows/equitycoupon.hpp>
#include <qle/cashflows/equitycouponpricer.hpp>
#include <qle/cashflows/fxlinkedcashflow.hpp>

using namespace QuantLib;
using namespace QuantExt;
using namespace boost::unit_test_framework;
using namespace std;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(CashFlowTest)

BOOST_AUTO_TEST_CASE(testFXLinkedCashFlow) {

    BOOST_TEST_MESSAGE("Testing FX Linked CashFlow");

    // Test today = 5 Jan 2016
    Settings::instance().evaluationDate() = Date(5, Jan, 2016);
    Date today = Settings::instance().evaluationDate();

    Date cfDate1(5, Jan, 2015); // historical
    Date cfDate2(5, Jan, 2016); // today
    Date cfDate3(5, Jan, 2017); // future

    Real foreignAmount = 1000000; // 1M
    boost::shared_ptr<SimpleQuote> sq = boost::make_shared<SimpleQuote>(123.45);
    Handle<Quote> spot(sq);
    DayCounter dc = ActualActual();
    Calendar cal = TARGET();
    Handle<YieldTermStructure> domYTS(boost::shared_ptr<YieldTermStructure>(new FlatForward(0, cal, 0.005, dc))); // JPY
    Handle<YieldTermStructure> forYTS(boost::shared_ptr<YieldTermStructure>(new FlatForward(0, cal, 0.03, dc)));  // USD
    // TODO foreign/domestic vs source/target
    boost::shared_ptr<FxIndex> fxIndex =
        boost::make_shared<FxIndex>("FX::USDJPY", 0, USDCurrency(), JPYCurrency(), TARGET(), spot, domYTS, forYTS);

    FXLinkedCashFlow fxlcf1(cfDate1, cfDate1, foreignAmount, fxIndex);
    FXLinkedCashFlow fxlcf2(cfDate2, cfDate2, foreignAmount, fxIndex);
    FXLinkedCashFlow fxlcf3(cfDate3, cfDate3, foreignAmount, fxIndex);

    // Add historical and todays fixing
    fxIndex->addFixing(cfDate1, 112.0);
    fxIndex->addFixing(cfDate2, sq->value());

    BOOST_TEST_MESSAGE("Check historical flow is correct");
    BOOST_CHECK_CLOSE(fxlcf1.amount(), 112000000.0, 1e-10);

    BOOST_TEST_MESSAGE("Check todays flow is correct");
    BOOST_CHECK_CLOSE(fxlcf2.amount(), 123450000.0, 1e-10);

    BOOST_TEST_MESSAGE("Check future (expected) flow is correct");
    Real fwd = sq->value() * domYTS->discount(cfDate3) / forYTS->discount(cfDate3);
    BOOST_CHECK_CLOSE(fxlcf3.amount(), foreignAmount * fwd, 1e-10);

    // Now move forward in time, check historical value is still correct
    Settings::instance().evaluationDate() = Date(1, Feb, 2016);
    sq->setValue(150.0);
    domYTS->update();
    forYTS->update();
    BOOST_CHECK_CLOSE(fxlcf1.amount(), 112000000.0, 1e-10);

    // check foward quote is still valid
    fwd = sq->value() * domYTS->discount(cfDate3) / forYTS->discount(cfDate3);
    BOOST_CHECK_CLOSE(fxlcf3.amount(), foreignAmount * fwd, 1e-10);

    // reset
    Settings::instance().evaluationDate() = today;
}

BOOST_AUTO_TEST_CASE(testEquityCoupon) {

    BOOST_TEST_MESSAGE("Testing Equity Coupon");

    // Test today = 5 Jan 2016
    Settings::instance().evaluationDate() = Date(5, Jan, 2016);
    Date today = Settings::instance().evaluationDate();

    Date cfDate1(4, Dec, 2015);
    Date cfDate2(5, Apr, 2016); // future
    Date fixingDate1(31, Dec, 2015);
    Date fixingDate2(1, Apr, 2016);

    Real nominal = 1000000; // 1M
    string eqName = "SP5";
    boost::shared_ptr<SimpleQuote> sq = boost::make_shared<SimpleQuote>(2100);
    Handle<Quote> spot(sq);
    DayCounter dc = ActualActual();
    Calendar cal = TARGET();
    Natural fixingLag = 2;
    Real divFactor = 1.0;
    Handle<YieldTermStructure> dividend(
        boost::shared_ptr<YieldTermStructure>(new FlatForward(0, cal, 0.01, dc))); // Dividend Curve
    Handle<YieldTermStructure> equityforecast(
        boost::shared_ptr<YieldTermStructure>(new FlatForward(0, cal, 0.02, dc))); // Equity Forecast Curve

    boost::shared_ptr<EquityIndex> eqIndex =
        boost::make_shared<EquityIndex>(eqName, cal, spot, equityforecast, dividend);

    eqIndex->addFixing(cfDate1, 2000);
    eqIndex->addFixing(fixingDate1, 1980);

    // Price Return coupon
    EquityCoupon eq1(cfDate2, 1000000, today, cfDate2, 0, eqIndex, dc);
    // Total Return Coupon
    EquityCoupon eq2(cfDate2, 1000000, today, cfDate2, 0, eqIndex, dc, true, divFactor);
    // historical starting coupon
    EquityCoupon eq3(cfDate2, 1000000, cfDate1, cfDate2, 0, eqIndex, dc);
    // Total Return Coupon with fixing lag
    EquityCoupon eq4(cfDate2, 1000000, today, cfDate2, fixingLag, eqIndex, dc, true);

    boost::shared_ptr<EquityCouponPricer> pricer1(new EquityCouponPricer());
    boost::shared_ptr<EquityCouponPricer> pricer2(new EquityCouponPricer());
    boost::shared_ptr<EquityCouponPricer> pricer3(new EquityCouponPricer());
    boost::shared_ptr<EquityCouponPricer> pricer4(new EquityCouponPricer());
    eq1.setPricer(pricer1);
    eq2.setPricer(pricer2);
    eq3.setPricer(pricer3);
    eq4.setPricer(pricer4);

    // Price Return coupon
    Time dt = dc.yearFraction(today, cfDate2);
    Real forward = spot->value() * std::exp((0.02 - 0.01) * dt);
    Real expectedAmount = nominal * (forward - spot->value()) / spot->value();
    BOOST_TEST_MESSAGE("Check Price Return is correct.");
    BOOST_CHECK_CLOSE(eq1.amount(), expectedAmount, 1e-10);

    // Total Return Coupon
    forward = spot->value() * std::exp((0.02 - 0.01) * dt);
    Real div = spot->value() * std::exp((0.02) * dt) - forward;
    expectedAmount = nominal * (forward + divFactor * div - spot->value()) / spot->value();
    BOOST_TEST_MESSAGE("Check Total Return is correct");
    BOOST_CHECK_CLOSE(eq2.amount(), expectedAmount, 1e-10);

    // Historical starting Price Return coupon
    forward = spot->value() * std::exp((0.02 - 0.01) * dt);
    expectedAmount = nominal * (forward - eqIndex->fixing(cfDate1)) / eqIndex->fixing(cfDate1);
    BOOST_TEST_MESSAGE("Check Historical starting Price Return is correct.");
    BOOST_CHECK_CLOSE(eq3.amount(), expectedAmount, 1e-10);

    // Total Return Coupon with fixing lag
    dt = dc.yearFraction(today, fixingDate2);
    forward = spot->value() * std::exp(0.02 * dt);
    expectedAmount = nominal * (forward - eqIndex->fixing(fixingDate1)) / eqIndex->fixing(fixingDate1);
    BOOST_TEST_MESSAGE("Check Total Return fixing lag handling is correct.");
    BOOST_CHECK_CLOSE(eq4.amount(), expectedAmount, 1e-10);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


// Copyright 2015-2022 The ALMA Project Developers
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
// implied. See the License for the specific language governing
// permissions and limitations under the License.

#pragma once

/// @file
///
/// Code related to the phonon density fo states (DOS).

#include <qpoint_grid.hpp>
#include <boost/math/distributions/normal.hpp>

namespace alma {
/// Objects of this class handle the contribution of a mode to the
/// phonon DOS. We use an adaptive Gaussian smearing algorithm
/// to broaden the isolated modes.
class Gaussian_for_DOS {
public:
    /// Average energy.
    const double mu;
    /// Standard deviation.
    const double sigma;
    /// True if the usual lower bound would be negative.
    const bool truncated;
    /// Lower bound to the values that can be considered compatible
    /// with the average energy.
    const double lbound;
    /// Upper bound to the values that can be considered compatible
    /// with the average energy.
    const double ubound;
    /// Constructor.
    ///
    /// @param[in] grid - phonon spectrum on a regular grid
    /// @param[in] iq - q point index
    /// @param[in] im - branch index
    /// @param[in] scalebroad - prefactor for the standard
    /// deviation.
    Gaussian_for_DOS(const Gamma_grid& grid,
                     std::size_t iq,
                     std::size_t im,
                     double scalebroad)
        : mu(grid.get_spectrum_at_q(iq).omega(im)),
          sigma(scalebroad *
                grid.base_sigma(grid.get_spectrum_at_q(iq).vg.col(im))),
          truncated(mu - constants::nsigma * sigma < 0.),
          lbound(truncated ? 0. : mu - constants::nsigma * sigma),
          ubound(mu + constants::nsigma * sigma) {
        if ((this->mu == 0.) || (this->sigma == 0.))
            this->dist = nullptr;
        else
            this->dist = std::make_unique<boost::math::normal>(mu, sigma);
    }

    /// Get the amplitude of this contribution at a given frequency.
    ///
    /// @param[in] omega - an angular frequency in rad / ps
    /// @return the value of the Gaussian, normalized to 1 with
    /// respect to integration over omega.
    double get_contribution(double omega) const {
        if (this->dist)
            return boost::math::pdf(*(this->dist), omega);
        else
            return 0.;
    }

private:
    /// Underlying Gaussian distribution object.
    std::unique_ptr<boost::math::normal> dist;
};
} // namespace alma


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2010
//  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)

#include <iostream>
using std::cout; using std::endl;
#include <cerrno> // for ::errno

//[policy_eg_2

#include <boost/math/special_functions/gamma.hpp>
using boost::math::tgamma;

int main()
{
   // using namespace boost::math::policies; // or
   using boost::math::policies::errno_on_error;
   using boost::math::policies::make_policy;
   using boost::math::policies::pole_error;
   using boost::math::policies::domain_error;
   using boost::math::policies::overflow_error;
   using boost::math::policies::evaluation_error;

   errno = 0;
   std::cout << "Result of tgamma(30000) is: "
      << boost::math::tgamma(
         30000,
         make_policy(
            domain_error<errno_on_error>(),
            pole_error<errno_on_error>(),
            overflow_error<errno_on_error>(),
            evaluation_error<errno_on_error>()
         )
      ) << std::endl;
   // Check errno was set:
   std::cout << "errno = " << errno << std::endl;
   // and again with evaluation at a pole:
   std::cout << "Result of tgamma(-10) is: "
      << boost::math::tgamma(
         -10,
         make_policy(
            domain_error<errno_on_error>(),
            pole_error<errno_on_error>(),
            overflow_error<errno_on_error>(),
            evaluation_error<errno_on_error>()
         )
      ) << std::endl;
   // Check errno was set:
   std::cout << "errno = " << errno << std::endl;
}

//] //[/policy_eg_2]

/*

Output:

  Result of tgamma(30000) is: 1.#INF
  errno = 34
  Result of tgamma(-10) is: 1.#QNAN
  errno = 33
*/


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef CBR_CONTROL__MPC__CLTV_OCP_LIE_HPP_
#define CBR_CONTROL__MPC__CLTV_OCP_LIE_HPP_

#include <Eigen/Dense>

#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>

#include <cbr_math/lie/common.hpp>
#include <cbr_utils/utils.hpp>

#include <limits>
#include <utility>

#include "ocp_common.hpp"

namespace cbr
{

/* ---------------------------------------------------------------------------------------------- */
/*                          Lie group Optimal Control Problem Linearizer                          */
/* ---------------------------------------------------------------------------------------------- */

/**
 * @brief Linearize a nonlinear control problem defined on a Lie group to a continuous linear time-varying problem
 * @tparam lie_pb_t nonlinear problem on Lie group satisfying interface conditions
 * The difference from linearized trajectory is the Rn state
 *  a(t) = log( Xl(t)^{-1} * X(t) )
 *             <=>
 *  X(t) = Xl(t) * exp (a(t))
 *
 * We have that
 *  \dot a = [d^r exp_a]^{-1} * f(Xl * exp(a), ul + ue) - [d^l exp_a]^{-1} d^r Xl_t
 * which is a nonlinear system on R^{DoF}. Thus the linearized system is
 *  \dot a = A(t) a + B(t) u + E(t) for
 *
 *  A(t) = (d/da) [d^r exp_a]^{-1} * f(Xl * exp(a), ul + ue)   at a = 0, ue = 0
 *  B(t) = (d/du) [d^r exp_a]^{-1} * f(Xl * exp(a), ul + ue)   at a = 0, ue = 0
 *  E(t) = [d^r exp_a]^{-1} f(Xl * exp(a), ul + ue) - [d^l exp_a]^{-1} d^r Xl_t  at a = 0, ue = 0
 *
 * Setting to zero:
 *
 *  A(t) = (d/da) [d^r exp_a]^{-1} * f(Xl * exp(a), ul)  at a = 0
 *  B(t) = (d/u) f(Xl, ul)                               at u = ul
 *  E(t) = f(Xl, ul) - d^r Xl_t
 */
template<typename lie_pb_t>
class CltvOcpLie
{
public:
  using lie_t = typename lie_pb_t::state_t;
  using input_t = typename lie_pb_t::input_t;
  using state_t = typename lie_pb_t::deriv_t;     // linearized problem is defined in tangent space

  static constexpr std::size_t nx = state_t::SizeAtCompileTime;
  static constexpr std::size_t nu = input_t::SizeAtCompileTime;

  using A_t = Eigen::Matrix<double, nx, nx>;
  using B_t = Eigen::Matrix<double, nx, nu>;
  using E_t = Eigen::Matrix<double, nx, 1>;
  using Q_t = Eigen::Matrix<double, nx, nx>;
  using R_t = Eigen::Matrix<double, nu, nu>;

  // Get return type of problem functions
  using T_t = std::result_of_t<decltype(&lie_pb_t::get_T)(lie_pb_t)>;
  using x0_t = std::result_of_t<decltype(&lie_pb_t::get_x0)(lie_pb_t)>;
  using xlr_t = std::result_of_t<decltype(&lie_pb_t::get_xl)(lie_pb_t, double)>;
  using ulr_t = std::result_of_t<decltype(&lie_pb_t::get_ul)(lie_pb_t, double)>;
  using xdr_t = std::result_of_t<decltype(&lie_pb_t::get_xd)(lie_pb_t, double)>;
  using udr_t = std::result_of_t<decltype(&lie_pb_t::get_ud)(lie_pb_t, double)>;
  using Qr_t = std::result_of_t<decltype(&lie_pb_t::get_Q)(lie_pb_t, double)>;
  using QTr_t = std::result_of_t<decltype(&lie_pb_t::get_QT)(lie_pb_t)>;
  using Rr_t = std::result_of_t<decltype(&lie_pb_t::get_R)(lie_pb_t, double)>;

  // Check return type of problem functions
  static_assert(
    std::is_same_v<std::decay_t<T_t>, double>,
    "The get_xl method of the problem must return a double.");
  static_assert(
    std::is_same_v<std::decay_t<x0_t>, lie_t>,
    "The get_x0 method of the problem must return the group type (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<xlr_t>, lie_t>,
    "The get_x0 method of the problem must return the group type (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<ulr_t>, input_t>,
    "The get_ul method of the problem must return an nu*1 Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<xdr_t>, lie_t>,
    "The get_x0 method of the problem must return the group type (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<udr_t>, input_t>,
    "The get_ud method of the problem must return an nu*1 Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<Qr_t>, Q_t>,
    "The get_Q method of the problem must return an nx*nx Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<Rr_t>, R_t>,
    "The get_R method of the problem must return an nu*nu Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<QTr_t>, Q_t>,
    "The get_QT method of the problem must return an nx*nx Eigen::Matrix (or a reference to one).");

  // Check problem dimensions
  static_assert(nx > 0, "Number of states must be > 0.");
  static_assert(nu > 0, "Number of inputs must be > 0.");

public:
  CltvOcpLie() = delete;
  CltvOcpLie(const CltvOcpLie &) = default;
  CltvOcpLie(CltvOcpLie &&) = default;
  CltvOcpLie & operator=(const CltvOcpLie &) = default;
  CltvOcpLie & operator=(CltvOcpLie &&) = default;

  explicit CltvOcpLie(const lie_pb_t & pb)
  : nl_pb_(pb)
  {}

  explicit CltvOcpLie(lie_pb_t && pb)
  : nl_pb_(std::move(pb))
  {}

  template<typename T1>
  CltvOcpLie(T1 && pb)
  : nl_pb_(std::forward<T1>(pb))
  {}

  void get_x0(Eigen::Ref<state_t> x0) const
  {
    x0 = (nl_pb_.get_xl(0.).inverse() * nl_pb_.get_x0()).log();
  }

  void get_T(double & T) const
  {
    T = nl_pb_.get_T();
  }

  void get_state_lb(double, Eigen::Ref<state_t> state_lb) const
  {
    // state bounds not supported for now
    state_lb.setConstant(-std::numeric_limits<double>::infinity());
  }

  void get_state_ub(double, Eigen::Ref<state_t> state_ub) const
  {
    // state bounds not supported for now
    state_ub.setConstant(std::numeric_limits<double>::infinity());
  }

  void get_input_lb(double t, Eigen::Ref<input_t> input_lb) const
  {
    nl_pb_.get_input_lb(t, input_lb);
    input_lb -= nl_pb_.get_ul(t);
  }

  void get_input_ub(double t, Eigen::Ref<input_t> input_ub) const
  {
    nl_pb_.get_input_ub(t, input_ub);
    input_ub -= nl_pb_.get_ul(t);
  }

  A_t get_A(double t) const
  {
    using lie_ad_t = lie::detail::change_scalar_t<lie_t, autodiff::dual>;
    using tangent_ad_t = Eigen::Matrix<autodiff::dual, nx, 1>;

    const lie_ad_t xlin = nl_pb_.get_xl(t).template cast<autodiff::dual>();
    const input_t ulin = nl_pb_.get_ul(t);

    auto fx = [&](const tangent_ad_t & a) -> tangent_ad_t {
        return lie::dr_expinv<lie_ad_t>(a) * nl_pb_.get_f(xlin * lie_ad_t::exp(a), ulin);
      };

    tangent_ad_t a = tangent_ad_t::Zero();
    return autodiff::forward::jacobian(fx, autodiff::wrt(a), autodiff::forward::at(a));
  }

  B_t get_B(double t) const
  {
    using input_ad_t = Eigen::Matrix<autodiff::dual, nu, 1>;
    using tangent_ad_t = Eigen::Matrix<autodiff::dual, nx, 1>;

    const lie_t xlin = nl_pb_.get_xl(t);

    auto fu = [&](const input_ad_t & u) -> tangent_ad_t {
        return nl_pb_.get_f(xlin, u);
      };

    input_ad_t ulin = nl_pb_.get_ul(t);
    return autodiff::forward::jacobian(fu, autodiff::wrt(ulin), autodiff::forward::at(ulin));
  }

  E_t get_E(double t) const
  {
    const lie_t xlin = nl_pb_.get_xl(t);
    const input_t ulin = nl_pb_.get_ul(t);
    const typename lie_t::Tangent xlDot = nl_pb_.get_xldot(t);

    return nl_pb_.get_f(xlin, ulin) - xlDot;
  }

  Qr_t get_Q(double t) const
  {
    return nl_pb_.get_Q(t);
  }

  QTr_t get_QT() const
  {
    return nl_pb_.get_QT();
  }

  Rr_t get_R(double t) const
  {
    return nl_pb_.get_R(t);
  }

  state_t get_q(double t) const
  {
    const xlr_t xl = nl_pb_.get_xl(t);
    const xdr_t xd = nl_pb_.get_xd(t);
    const Qr_t Q = nl_pb_.get_Q(t);
    return (xd.inverse() * xl).log().transpose() * Q;
  }

  state_t get_qT() const
  {
    double T = nl_pb_.get_T();
    const xlr_t xl = nl_pb_.get_xl(T);
    const xdr_t xd = nl_pb_.get_xd(T);
    const QTr_t QT = nl_pb_.get_QT();
    return (xd.inverse() * xl).log().transpose() * QT;
  }

  input_t get_r(double t) const
  {
    const ulr_t ul = nl_pb_.get_ul(t);
    const udr_t ud = nl_pb_.get_ud(t);
    const Rr_t R = nl_pb_.get_R(t);
    return (ul - ud).transpose() * R;
  }

  lie_pb_t & problem()
  {
    return nl_pb_;
  }

protected:
  lie_pb_t nl_pb_{};
};

// Class template argument deduction guides
template<typename T>
CltvOcpLie(T)->CltvOcpLie<T>;

template<typename T1, typename T2>
CltvOcpLie(T1, T2)->CltvOcpLie<T1>;

}  // namespace cbr

#endif  // CBR_CONTROL__MPC__CLTV_OCP_LIE_HPP_


//
//  Copyright Toon Knapen, Karl Meerbergen
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include "random.hpp"

#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#include <boost/numeric/bindings/blas/blas1.hpp>

#include <vector>
#include <complex>
#include <iostream>
#include <limits>
#include <cmath>

// Randomize a vector (using functions from random.hpp)
template <typename V>
void randomize(V& v) {
   for (typename V::size_type i=0; i<v.size(); ++i)
      v[i] = random_value< typename V::value_type >() ;
} // randomize()

float abs_sum_value( float const& f ) {
  using namespace std ;
  return abs(f) ;
}

double abs_sum_value( double const& f ) {
  using namespace std ;
  return abs(f) ;
}

float abs_sum_value( std::complex< float > const& f ) {
  using namespace std ;
  return abs(f.real()) + abs(f.imag()) ;
}

double abs_sum_value( std::complex< double > const& f ) {
  using namespace std ;
  return abs(f.real()) + abs(f.imag()) ;
}

template <typename V>
typename boost::numeric::bindings::traits::type_traits<typename V::value_type>::real_type abs_sum( V const& v) {
  typedef typename boost::numeric::bindings::traits::type_traits<typename V::value_type>::real_type real_type ;

  real_type sum( 0.0 ) ;
  for ( typename V::size_type i=0; i<v.size(); ++i ) {
    sum += abs_sum_value( v[i] ) ;
  }
  return sum ;
}

// Blas operations using one vector.
template <typename T>
struct OneVector {
  boost::numeric::ublas::vector<T> v_ref_ ;

  // Initialize : set reference vector (ublas)
  OneVector()
  : v_ref_( 10 )
  {
     randomize(v_ref_);
  }

  template <typename V>
  int operator()(V& v) const {
     using namespace boost::numeric::bindings::blas ;

     typedef typename V::value_type                                                        value_type ;
     typedef typename boost::numeric::bindings::traits::type_traits<value_type>::real_type real_type ;

     // Copy vector from reference
     for (typename V::size_type i=0; i<v_ref_.size(); ++i)
        v[i] = v_ref_(i);

     // Test blas routines and compare with reference
     real_type nrm = nrm2( v );
     if ( std::abs(nrm - norm_2(v_ref_)) > std::numeric_limits< real_type >::epsilon() * norm_2(v_ref_)) {
       std::cout << "nrm2 : " << std::abs(nrm - norm_2(v_ref_)) << " > " << std::numeric_limits< real_type >::epsilon() * norm_2(v_ref_) << std::endl ;
       return 255 ;
     }

     nrm = asum( v );
     if ( std::abs(nrm - abs_sum(v_ref_)) > std::numeric_limits< real_type >::epsilon() * abs_sum(v_ref_)) {
       std::cout << "asum : " << std::abs(nrm - abs_sum(v_ref_)) << " > " << std::numeric_limits< real_type >::epsilon() * abs_sum(v_ref_) << std::endl ;
       return 255 ;
     }

     scal( value_type(2.0), v );
     for (typename V::size_type i=0; i<v_ref_.size(); ++i)
        if (std::abs( v[i] - real_type(2.0)*v_ref_(i) ) > real_type(2.0)*std::abs(v_ref_(i))) return 255 ;

     return 0;
  }

  // Return the size of a vector.
  size_t size() const {return v_ref_.size();}
};

// Operations with two vectors.
template <typename T, typename V>
struct BaseTwoVectorOperations {
  typedef T                                                                             value_type ;
  typedef typename boost::numeric::bindings::traits::type_traits<value_type>::real_type real_type ;
  typedef boost::numeric::ublas::vector<T>                                              ref_vector_type ;

  // Initialize: select the first vector and set the reference vectors (ublas)
  BaseTwoVectorOperations(V& v, const ref_vector_type& v1_ref, const ref_vector_type& v2_ref)
  : v_( v )
  , v1_ref_( v1_ref )
  , v2_ref_( v2_ref )
  {}

  // Copy the 2nd reference vector into w.
  template <typename W>
  void copy_vector(W& w) const {
     for (size_t i=0; i<size(); ++i) {
        w[i] = v2_ref_(i);
     }
  } // copy_vector()

  // Get the size of a vector.
  size_t size() const {return v_.size();}

  // Data members.
  V&                     v_ ;
  const ref_vector_type& v1_ref_, v2_ref_ ;
};

template <typename T, typename V>
struct TwoVectorOperations { } ;

template <typename V>
struct TwoVectorOperations< float, V>
: BaseTwoVectorOperations<float,V> {
  typedef typename V::value_type                                                        value_type ;
  typedef typename boost::numeric::bindings::traits::type_traits<value_type>::real_type real_type ;
  typedef typename BaseTwoVectorOperations<float,V>::ref_vector_type                    ref_vector_type ;

  TwoVectorOperations(V& v, const ref_vector_type& v1_ref, const ref_vector_type& v2_ref)
  : BaseTwoVectorOperations<float,V>( v, v1_ref, v2_ref )
  {}

  // Perform the tests of blas functions and compare with reference
  template <typename W>
  int operator()(W& w) const {
     using namespace boost::numeric::bindings::blas ;
     real_type safety_factor (1.5);

     copy_vector(w);

     // Test blas routines
     value_type prod = dot( this->v_, w );
     if ( std::abs(prod - inner_prod( this->v1_ref_, this->v2_ref_ ))
          > safety_factor*std::numeric_limits< real_type >::epsilon() * std::abs(prod)) return 255 ;

     axpy( value_type(2.0), this->v_, w );
     for (size_t i=0; i<this->size(); ++i)
        if ( std::abs(w[i] - (this->v2_ref_(i) + value_type(2.0)*this->v1_ref_(i)))
          > safety_factor*std::numeric_limits< real_type >::epsilon() * std::abs(w[i])) return 255 ;

     scal( value_type(0.0), w ) ;
     copy( this->v_, w ) ;
     for (size_t i=0; i<this->size(); ++i) {
        if ( std::abs( w[i] - this->v_[i] ) != 0.0 ) return 255 ;
     }

     return 0;
  }
};

template <typename V>
struct TwoVectorOperations< double, V>
: BaseTwoVectorOperations<double,V> {
  typedef typename V::value_type                                                        value_type ;
  typedef typename boost::numeric::bindings::traits::type_traits<value_type>::real_type real_type ;
  typedef typename BaseTwoVectorOperations<double,V>::ref_vector_type                   ref_vector_type ;

  TwoVectorOperations(V& v, const ref_vector_type& v1_ref, const ref_vector_type& v2_ref)
  : BaseTwoVectorOperations<double,V>( v, v1_ref, v2_ref )
  {}

  // Perform the tests of blas functions and compare with reference
  template <typename W>
  int operator()(W& w) const {
     using namespace boost::numeric::bindings::blas ;

     copy_vector( w );

     // Test blas routines
     value_type prod = dot( this->v_, w );
     if ( std::abs(prod - inner_prod( this->v1_ref_, this->v2_ref_ ))
          > std::numeric_limits< real_type >::epsilon() * std::abs(prod)) return 255 ;

     axpy( value_type(2.0), this->v_, w );
     for (size_t i=0; i<this->size(); ++i)
        if ( std::abs(w[i] - (this->v2_ref_(i) + value_type(2.0)*this->v1_ref_(i)))
          > std::numeric_limits< real_type >::epsilon() * std::abs(w[i])) return 255 ;

     copy_vector( w ) ;
     scal( value_type(-1.0), w ) ;
     ::boost::numeric::bindings::blas::copy( this->v_, w ) ;
     for (size_t i=0; i<this->size(); ++i) {
        if ( w[i] != this->v_[i] ) return 255 ;
     }

     return 0;
  }
};

template <typename V>
struct TwoVectorOperations< std::complex<float>, V>
: BaseTwoVectorOperations< std::complex<float>, V>
{
  typedef typename V::value_type                                                        value_type ;
  typedef typename boost::numeric::bindings::traits::type_traits<value_type>::real_type real_type ;
  typedef typename BaseTwoVectorOperations<std::complex<float>,V>::ref_vector_type      ref_vector_type ;

  TwoVectorOperations(V& v, const ref_vector_type& v1_ref, const ref_vector_type& v2_ref)
  : BaseTwoVectorOperations< std::complex<float>, V>( v, v1_ref, v2_ref )
  {}

  // Perform the tests of blas functions and compare with reference
  template <typename W>
  int operator()(W& w) const {
     using namespace boost::numeric::bindings::blas ;
     real_type safety_factor (1.5);

     copy_vector( w );

     // Test blas routines
     value_type prod = dotc( this->v_, w );
     if ( std::abs(prod - inner_prod( conj(this->v1_ref_), this->v2_ref_ ))
          > safety_factor*std::numeric_limits< real_type >::epsilon() * std::abs(prod)) return 255 ;

     prod = dotu( this->v_, w );
     if ( std::abs(prod - inner_prod( this->v1_ref_, this->v2_ref_ ))
          > safety_factor*std::numeric_limits< real_type >::epsilon() * std::abs(prod)) return 255 ;

     axpy( value_type(2.0), this->v_, w );
     for (size_t i=0; i<this->size(); ++i)
        if ( std::abs(w[i] - (this->v2_ref_(i) + value_type(2.0)*this->v1_ref_(i)))
          > safety_factor*std::numeric_limits< real_type >::epsilon() * std::abs(w[i])) return 255 ;

     scal( value_type(0.0), w ) ;
     copy( this->v_, w ) ;
     for (size_t i=0; i<this->size(); ++i) {
        if ( std::abs( w[i] - this->v_[i] ) != 0.0 ) return 255 ;
     }

     return 0;
  }
};

template <typename V>
struct TwoVectorOperations< std::complex<double>, V>
: BaseTwoVectorOperations< std::complex<double>, V>
{
  typedef typename V::value_type                                                        value_type ;
  typedef typename boost::numeric::bindings::traits::type_traits<value_type>::real_type real_type ;
  typedef typename BaseTwoVectorOperations<std::complex<double>,V>::ref_vector_type     ref_vector_type ;

  TwoVectorOperations(V& v, const ref_vector_type& v1_ref, const ref_vector_type& v2_ref)
  : BaseTwoVectorOperations< std::complex<double>, V>( v, v1_ref, v2_ref )
  {}

  // Perform the tests of blas functions and compare with reference
  template <typename W>
  int operator()(W& w) const {
     using namespace boost::numeric::bindings::blas ;
     real_type safety_factor (1.5);

     copy_vector( w );

     // Test blas routines
     value_type prod = dotc( this->v_, w );
     if ( std::abs(prod - inner_prod( conj(this->v1_ref_), this->v2_ref_ ))
          > safety_factor*std::numeric_limits< real_type >::epsilon() * std::abs(prod)) return 255 ;

     prod = dotu( this->v_, w );
     if ( std::abs(prod - inner_prod( this->v1_ref_, this->v2_ref_ ))
          > safety_factor*std::numeric_limits< real_type >::epsilon() * std::abs(prod)) return 255 ;

     axpy( value_type(2.0), this->v_, w );
     for (size_t i=0; i<this->size(); ++i)
        if ( std::abs(w[i] - (this->v2_ref_(i) + value_type(2.0)*this->v1_ref_(i)))
          > safety_factor*std::numeric_limits< real_type >::epsilon() * std::abs(w[i])) return 255 ;

     scal( value_type(0.0), w ) ;
     copy( this->v_, w ) ;
     for (size_t i=0; i<this->size(); ++i) {
        if ( std::abs( w[i] - this->v_[i] ) != 0.0 ) return 255 ;
     }

     return 0;
  }
};

// Run the tests for different types of vectors.
template <typename T, typename F>
int different_vectors(const F& f) {
   // Do test for different types of vectors
   {
      std::cout << "  ublas::vector\n" ;
      boost::numeric::ublas::vector< T > v(f.size());
      if (f( v )) return 255 ;
   }
   {
      std::cout << "  std::vector\n" ;
      std::vector<T> v_ref(f.size());
      if (f( v_ref )) return 255 ;
   }
   {
      std::cout << "  ublas::vector_range\n" ;
      typedef boost::numeric::ublas::vector< T > vector_type ;
      vector_type v(f.size()*2);
      boost::numeric::ublas::vector_range< vector_type > vr(v, boost::numeric::ublas::range(1,1+f.size()));
      if (f( vr )) return 255 ;
   }
   {
      typedef boost::numeric::ublas::matrix< T, boost::numeric::ublas::column_major >  matrix_type ;
      matrix_type  m(f.size(),f.size()) ;

      std::cout << "  ublas::matrix_column\n" ;
      boost::numeric::ublas::matrix_column< matrix_type > m_c( m, 2 );
      if (f( m_c )) return 255 ;

      std::cout << "  ublas::matrix_row\n" ;
      boost::numeric::ublas::matrix_row< matrix_type > m_r( m, 1 );
      if (f( m_r )) return 255 ;
   }
   return 0;
} // different_vectors()

// This is the functor that selects the first vector of the tests that use two vectors.
template <typename T>
struct TwoVector {
   TwoVector()
   : v1_ref_( 10 )
   , v2_ref_( 10 )
   {}

   template <typename V>
   int operator() (V& v) const {
      for (size_t i=0; i<size(); ++i) v[i] = v1_ref_(i) ;
      return different_vectors<T,TwoVectorOperations<T,V> >( TwoVectorOperations<T,V>(v, v1_ref_, v2_ref_) ) ;
   }

   size_t size() const {
      return v1_ref_.size() ;
   }

   boost::numeric::ublas::vector<T> v1_ref_ ;
   boost::numeric::ublas::vector<T> v2_ref_ ;
}; // TwoVector

// Run the test for a specific value_type T.
template <typename T>
int do_value_type() {
   // Tests for functions with one vector argument.
   std::cout << " one argument\n";
   if (different_vectors<T,OneVector<T> >(OneVector<T> ())) return 255 ;

   // Tests for functions with two vector arguments.
   std::cout << " two arguments\n";
   if (different_vectors<T,TwoVector<T> >(TwoVector<T>())) return 255;
   return 0;
} // do_value_type()

int main() {
  // Run regression for Real/Complex
  std::cout << "float\n"; if (do_value_type<float>() ) return 255 ;
  std::cout << "double\n"; if (do_value_type<double>() ) return 255 ;
  std::cout << "complex<float>\n"; if (do_value_type<std::complex<float> >() ) return 255 ;
  std::cout << "complex<double>\n"; if (do_value_type<std::complex<double> >() ) return 255 ;

  std::cout << "Regression test successful\n" ;

  return 0 ;
}


/*
 This program is free software; you can redistribute it and/or modify it under
 the terms of the European Union Public Licence - EUPL v.1.1 as published by
 the European Commission.

 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 European Union Public Licence - EUPL v.1.1
 for more details.

 You should have received a copy of the European Union Public Licence - EUPL v.1.1
 along with this program.

 Further information about the European Union Public Licence - EUPL v.1.1 can
 also be found on the world wide web at http://ec.europa.eu/idabc/eupl

*/

/*
 ------ Copyright (C) 2011 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/
/*
------------------ Author: Guillermo Ortega  ----------------------------------------
 July 2011

 */

#include "serviceAngleRateUnit.h"

#include <Eigen/Core>

using namespace Eigen;

#include "QDebug"

DialogServiceAngleRateUnitFrame::DialogServiceAngleRateUnitFrame( QWidget * parent, Qt::WindowFlags f) : QFrame(parent,f)
{
    setupUi(this);
    angleRateUnitWidget = DialogServiceAngleRateUnitFrame::comboBoxAngleRateUnitsChoice;
    myPastUnits = 0;
    comboBoxAngleRateUnitsChoice->setCurrentIndex(myPastUnits);
}

DialogServiceAngleRateUnitFrame::~DialogServiceAngleRateUnitFrame()
{
}

// Index meaning is as follows:
// index = 0  is Degree
// index = 1  is radians

// Matrix coefficients as follows
// Deg/s->Deg/s  Rad/s->Deg/s
// Deg/s->Rad/s  Rad/s->Rad/s

static double angleRateConversionMatrixCoeffs[4] =
{1.0,            57.295779513,
 0.0174532925,   1.0};

static const Matrix<double, 2, 2> angleRateConversionMatrix(angleRateConversionMatrixCoeffs);

double DialogServiceAngleRateUnitFrame::convertAngleRate(int fromAngleRateUnit, int toAngleRateUnit, double angleRate)
{
    double finalAngleRate = angleRate * angleRateConversionMatrix(fromAngleRateUnit, toAngleRateUnit);
    return finalAngleRate;
}

//// Sets the input distance, the output distance and the current index inside the method
void DialogServiceAngleRateUnitFrame::setInputAngleRate(double niceInputAngleRate)
{
    myPastAngleRate = niceInputAngleRate;
}

// Index meaning is as follows:
// index = 0  is Kilometers
// index = 1  is meters
// index = 2  is centi-meters
// index = 3  is mili-meters
// index = 4 is Astronomical Units
void DialogServiceAngleRateUnitFrame::on_comboBoxAngleRateUnitsChoice_currentIndexChanged(int myIndex)
{
    myFutureUnits = myIndex;
    myFutureAngleRate = convertAngleRate(myPastUnits, myFutureUnits, myPastAngleRate);
    myPastAngleRate = myFutureAngleRate;
    myPastUnits = myFutureUnits;
    myRealAngleRateForXMLSchema = convertAngleRate (myPastUnits, 0, myFutureAngleRate);
}


#include <UnitTest++/UnitTest++.h>
#include <stdexcept>
#include <iostream>
#include <cmath>
#include <limits>
#include <boost/filesystem.hpp>
#include <fstream>

#include "coela_utility/src/string_utils.h"
#include "coela_core/src/ccd_image.h"
#include "coela_analysis/src/psf_generation.h"
#include "coela_analysis/src/psf_fitting_recipes.h"
#include "coela_random/src/random.h"

using namespace coela;
using namespace std;

SUITE(PsfFitting_recipes)
{

    string test_suite_output_dir= string(UnitTestSuite::GetSuiteName()) + "_tests/";
    TEST(Run_Standard_Suite_Setup) {
        cout << "*** \""<< UnitTestSuite::GetSuiteName() <<"\" unit tests running ***" <<endl;
        boost::filesystem::create_directories(test_suite_output_dir);
    }

    TEST(Gaussian_fitting_recipe) {

        bool output_to_screen=false;
        PixelRange img_size(1,1,50,50);

        psf_models::GaussianPsfModel g_model(5.0, 1.2);

        vector<unsigned long> seed;
        seed.push_back(111);
        seed.push_back(122);
        seed.push_back(133);
        seed.push_back(144);
        seed.push_back(155);
        seed.push_back(166);

        unuran::StreamWrapper::
        set_unuran_package_seed(seed, false);
        unuran::StreamWrapper rns;
        unuran::UniformRandomVariate urv(10.,40., rns);

        CcdPosition g_model_true_centre(urv(), urv());

        cerr<<"CCD centre Position: " <<g_model_true_centre<<endl;

        psf_models::ReferencePsf test_psf_ref_img =
            psf_models::generate_psf(g_model, img_size,
                                     CcdPosition(0.0,0.0),
                                     g_model_true_centre,
                                     1.0,
                                     2,
                                     true, 10.0);

        string filename = test_suite_output_dir +"gauss_model.fits";
        test_psf_ref_img.psf_image.write_to_file(filename);

        PixelIndex peak_pixel = test_psf_ref_img.psf_image.pix.max_PixelIndex();
        CcdPosition initial_centre_estimate =
            test_psf_ref_img.psf_image.CCD_grid.corresponding_grid_Position(
                PixelPosition::centre_of_pixel(peak_pixel));

        psf_fitting::least_squares_weighting lsq_weight;

        psf_fitting::Mn2_models::PsfFit<psf_models::GaussianPsfModel>
        fit_result =
            psf_fitting::fit_gaussian_model(test_psf_ref_img.psf_image,
                                            initial_centre_estimate,
                                            lsq_weight,
                                            1.8,
                                            4,
                                            output_to_screen);

        CHECK_CLOSE(g_model.peak_val, fit_result.model.peak_val, g_model.peak_val * 0.02);
        CHECK_CLOSE(g_model.sigma_in_CCD_pix, fit_result.model.sigma_in_CCD_pix,
                    g_model.sigma_in_CCD_pix * 0.025);
        CHECK_CLOSE(g_model_true_centre.x, fit_result.Position.centre.x, 0.05);
        CHECK_CLOSE(g_model_true_centre.y, fit_result.Position.centre.y, 0.05);

    }

}


/**********************************************************************************************************************
This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
Licensed under the BSD-2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#pragma once

#include <ct/core/core.h>
#include <memory>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/info_parser.hpp>
#include <boost/algorithm/string.hpp>

#include "CostFunctionQuadratic.hpp"
#include "utility/utilities.hpp"

#include "term/TermLoadMacros.hpp"

namespace ct {
namespace optcon {

/**
 * \ingroup CostFunction
 *
 * \brief Cost Function with Auto-Diff support
 *
 * This cost function can work with both, analytical terms as well as
 * auto-diff terms. For analytical terms it will use provided derivatives
 * and for auto-diff terms derivatives will be computed using auto-diff.
 *
 * Unit test \ref ADTest.cpp illustrates the use of a CostFunctionAD.
 */
template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
class CostFunctionAD : public CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef core::DerivativesCppadJIT<STATE_DIM + CONTROL_DIM + 1, 1> JacCG;
    typedef typename JacCG::CG_SCALAR CGScalar;
    typedef Eigen::Matrix<CGScalar, 1, 1> MatrixCg;

    typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
    typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
    typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;

    typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
    typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
    typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
    typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;

    /**
	 * \brief Basic constructor
	 */
    CostFunctionAD();

    /**
	 * \brief Constructor loading function from file
	 * @param filename config file location
	 * @param verbose flag enabling printouts
	 */
    CostFunctionAD(const std::string& filename, bool verbose = false);

    /**
	 * Deep-cloning of cost function
	 * @return base pointer to clone
	 */
    CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const;

    /**
	 * \brief Copy constructor
	 * @param arg cost function to copy
	 */
    CostFunctionAD(const CostFunctionAD& arg);

    /**
	 * \brief Destructor
	 */
    virtual ~CostFunctionAD();

    /**
	 * @brief      Initializes the AD costfunction, generates and compiles
	 *             source code
	 */
    virtual void initialize() override;

    /**
	 * \brief Add an intermediate, auto-differentiable term
	 *
	 * Use this function to add an auto-differentiable, intermediate term to the cost function.
	 *
	 * @param term The term to be added
	 * @param verbose Flag enabling printouts
	 * @return
	 */
    void addIntermediateADTerm(std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> term,
        bool verbose = false) override;

    /**
	 * \brief Add a final, auto-differentiable term
	 *
	 * Use this function to add an auto-differentiable, final term to the cost function.
	 *
	 * @param term The term to be added
	 * @param verbose Flag enabling printouts
	 * @return
	 */
    void addFinalADTerm(std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> term,
        bool verbose = false) override;

    void setCurrentStateAndControl(const state_vector_t& x, const control_vector_t& u, const SCALAR& t = 0.0) override;

    void loadFromConfigFile(const std::string& filename, bool verbose = false) override;

    SCALAR evaluateIntermediate() override;
    SCALAR evaluateTerminal() override;

    state_vector_t stateDerivativeIntermediate() override;
    state_vector_t stateDerivativeTerminal() override;

    control_vector_t controlDerivativeIntermediate() override;
    control_vector_t controlDerivativeTerminal() override;

    state_matrix_t stateSecondDerivativeIntermediate() override;
    state_matrix_t stateSecondDerivativeTerminal() override;

    control_matrix_t controlSecondDerivativeIntermediate() override;
    control_matrix_t controlSecondDerivativeTerminal() override;

    control_state_matrix_t stateControlDerivativeIntermediate() override;
    control_state_matrix_t stateControlDerivativeTerminal() override;

    std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> getIntermediateADTermById(const size_t id);

    std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> getFinalADTermById(const size_t id);

    std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> getIntermediateADTermByName(
        const std::string& name);

    std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> getFinalADTermByName(const std::string& name);

private:
    MatrixCg evaluateIntermediateCg(const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime);
    MatrixCg evaluateTerminalCg(const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime);

    //! combined state, control and time vector
    Eigen::Matrix<SCALAR, STATE_DIM + CONTROL_DIM + 1, 1> stateControlTime_;

    //! intermediate AD terms
    std::vector<std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>>> intermediateTerms_;
    //! final AD terms
    std::vector<std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>>> finalTerms_;

    //! generated jacobians
    std::shared_ptr<JacCG> intermediateCostCodegen_;
    std::shared_ptr<JacCG> finalCostCodegen_;

    //! cppad functions
    typename JacCG::FUN_TYPE_CG intermediateFun_;
    typename JacCG::FUN_TYPE_CG finalFun_;
};

}  // namespace optcon
}  // namespace ct


/*
   Copyright (c) Marshall Clow 2013.

   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)

    For more information, see http://www.boost.org
*/

#include <boost/config.hpp>
#include <boost/algorithm/cxx17/transform_reduce.hpp>

#include "iterator_test.hpp"

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

namespace ba = boost::algorithm;

template <class _Tp>
struct identity
{
    const _Tp& operator()(const _Tp& __x) const { return __x;}
};

template <class _Tp>
struct twice
{
  	const _Tp operator()(const _Tp& __x) const { return 2 * __x; }
};

template <class Iter1, class T, class BOp, class UOp>
void
test_init_bop_uop(Iter1 first1, Iter1 last1, T init, BOp bOp, UOp uOp, T x)
{
    BOOST_CHECK(ba::transform_reduce(first1, last1, init, bOp, uOp) == x);
}

template <class Iter>
void
test_init_bop_uop()
{
    int ia[]          = {1, 2, 3, 4, 5, 6};
    unsigned sa = sizeof(ia) / sizeof(ia[0]);

    test_init_bop_uop(Iter(ia), Iter(ia),    0, std::plus<int>(),       identity<int>(),       0);
    test_init_bop_uop(Iter(ia), Iter(ia),    1, std::multiplies<int>(), identity<int>(),       1);
    test_init_bop_uop(Iter(ia), Iter(ia+1),  0, std::multiplies<int>(), identity<int>(),       0);
    test_init_bop_uop(Iter(ia), Iter(ia+1),  2, std::plus<int>(),       identity<int>(),       3);
    test_init_bop_uop(Iter(ia), Iter(ia+2),  0, std::plus<int>(),       identity<int>(),       3);
    test_init_bop_uop(Iter(ia), Iter(ia+2),  3, std::multiplies<int>(), identity<int>(),       6);
    test_init_bop_uop(Iter(ia), Iter(ia+sa), 4, std::multiplies<int>(), identity<int>(),    2880);
    test_init_bop_uop(Iter(ia), Iter(ia+sa), 4, std::plus<int>(),       identity<int>(),      25);

    test_init_bop_uop(Iter(ia), Iter(ia),    0, std::plus<int>(),       twice<int>(),       0);
    test_init_bop_uop(Iter(ia), Iter(ia),    1, std::multiplies<int>(), twice<int>(),       1);
    test_init_bop_uop(Iter(ia), Iter(ia+1),  0, std::multiplies<int>(), twice<int>(),       0);
    test_init_bop_uop(Iter(ia), Iter(ia+1),  2, std::plus<int>(),       twice<int>(),       4);
    test_init_bop_uop(Iter(ia), Iter(ia+2),  0, std::plus<int>(),       twice<int>(),       6);
    test_init_bop_uop(Iter(ia), Iter(ia+2),  3, std::multiplies<int>(), twice<int>(),      24);
    test_init_bop_uop(Iter(ia), Iter(ia+sa), 4, std::multiplies<int>(), twice<int>(),  184320); // 64 * 2880
    test_init_bop_uop(Iter(ia), Iter(ia+sa), 4, std::plus<int>(),       twice<int>(),      46);
}

void test_transform_reduce_init_bop_uop()
{
	BOOST_CHECK ( true );
}

template <class Iter1, class Iter2, class T, class Op1, class Op2>
void
test_init_bop_bop(Iter1 first1, Iter1 last1, Iter2 first2, T init, Op1 op1, Op2 op2, T x)
{
    BOOST_CHECK(ba::transform_reduce(first1, last1, first2, init, op1, op2) == x);
}

template <class SIter, class UIter>
void
test_init_bop_bop()
{
    int ia[]          = {1, 2, 3, 4, 5, 6};
    unsigned int ua[] = {2, 4, 6, 8, 10,12};
    unsigned sa = sizeof(ia) / sizeof(ia[0]);
    BOOST_CHECK(sa == sizeof(ua) / sizeof(ua[0]));       // just to be sure

    test_init_bop_bop(SIter(ia), SIter(ia),    UIter(ua), 0, std::plus<int>(), std::multiplies<int>(),       0);
    test_init_bop_bop(UIter(ua), UIter(ua),    SIter(ia), 1, std::multiplies<int>(), std::plus<int>(),       1);
    test_init_bop_bop(SIter(ia), SIter(ia+1),  UIter(ua), 0, std::multiplies<int>(), std::plus<int>(),       0);
    test_init_bop_bop(UIter(ua), UIter(ua+1),  SIter(ia), 2, std::plus<int>(), std::multiplies<int>(),       4);
    test_init_bop_bop(SIter(ia), SIter(ia+2),  UIter(ua), 0, std::plus<int>(), std::multiplies<int>(),      10);
    test_init_bop_bop(UIter(ua), UIter(ua+2),  SIter(ia), 3, std::multiplies<int>(), std::plus<int>(),      54);
    test_init_bop_bop(SIter(ia), SIter(ia+sa), UIter(ua), 4, std::multiplies<int>(), std::plus<int>(), 2099520);
    test_init_bop_bop(UIter(ua), UIter(ua+sa), SIter(ia), 4, std::plus<int>(), std::multiplies<int>(),     186);
}

void test_transform_reduce_init_bop_bop()
{
//  All the iterator categories
    test_init_bop_bop<input_iterator        <const int*>, input_iterator        <const unsigned int*> >();
    test_init_bop_bop<input_iterator        <const int*>, forward_iterator      <const unsigned int*> >();
    test_init_bop_bop<input_iterator        <const int*>, bidirectional_iterator<const unsigned int*> >();
    test_init_bop_bop<input_iterator        <const int*>, random_access_iterator<const unsigned int*> >();

    test_init_bop_bop<forward_iterator      <const int*>, input_iterator        <const unsigned int*> >();
    test_init_bop_bop<forward_iterator      <const int*>, forward_iterator      <const unsigned int*> >();
    test_init_bop_bop<forward_iterator      <const int*>, bidirectional_iterator<const unsigned int*> >();
    test_init_bop_bop<forward_iterator      <const int*>, random_access_iterator<const unsigned int*> >();

    test_init_bop_bop<bidirectional_iterator<const int*>, input_iterator        <const unsigned int*> >();
    test_init_bop_bop<bidirectional_iterator<const int*>, forward_iterator      <const unsigned int*> >();
    test_init_bop_bop<bidirectional_iterator<const int*>, bidirectional_iterator<const unsigned int*> >();
    test_init_bop_bop<bidirectional_iterator<const int*>, random_access_iterator<const unsigned int*> >();

    test_init_bop_bop<random_access_iterator<const int*>, input_iterator        <const unsigned int*> >();
    test_init_bop_bop<random_access_iterator<const int*>, forward_iterator      <const unsigned int*> >();
    test_init_bop_bop<random_access_iterator<const int*>, bidirectional_iterator<const unsigned int*> >();
    test_init_bop_bop<random_access_iterator<const int*>, random_access_iterator<const unsigned int*> >();

//  just plain pointers (const vs. non-const, too)
    test_init_bop_bop<const int*, const unsigned int *>();
    test_init_bop_bop<const int*,       unsigned int *>();
    test_init_bop_bop<      int*, const unsigned int *>();
    test_init_bop_bop<      int*,       unsigned int *>();
}

template <class Iter1, class Iter2, class T>
void
test_init(Iter1 first1, Iter1 last1, Iter2 first2, T init, T x)
{
    BOOST_CHECK(ba::transform_reduce(first1, last1, first2, init) == x);
}

template <class SIter, class UIter>
void
test_init()
{
    int ia[]          = {1, 2, 3, 4, 5, 6};
    unsigned int ua[] = {2, 4, 6, 8, 10,12};
    unsigned sa = sizeof(ia) / sizeof(ia[0]);
    BOOST_CHECK(sa == sizeof(ua) / sizeof(ua[0]));       // just to be sure

    test_init(SIter(ia), SIter(ia),    UIter(ua), 0,   0);
    test_init(UIter(ua), UIter(ua),    SIter(ia), 1,   1);
    test_init(SIter(ia), SIter(ia+1),  UIter(ua), 0,   2);
    test_init(UIter(ua), UIter(ua+1),  SIter(ia), 2,   4);
    test_init(SIter(ia), SIter(ia+2),  UIter(ua), 0,  10);
    test_init(UIter(ua), UIter(ua+2),  SIter(ia), 3,  13);
    test_init(SIter(ia), SIter(ia+sa), UIter(ua), 0, 182);
    test_init(UIter(ua), UIter(ua+sa), SIter(ia), 4, 186);
}

void test_transform_reduce_init()
{
//  All the iterator categories
    test_init<input_iterator        <const int*>, input_iterator        <const unsigned int*> >();
    test_init<input_iterator        <const int*>, forward_iterator      <const unsigned int*> >();
    test_init<input_iterator        <const int*>, bidirectional_iterator<const unsigned int*> >();
    test_init<input_iterator        <const int*>, random_access_iterator<const unsigned int*> >();

    test_init<forward_iterator      <const int*>, input_iterator        <const unsigned int*> >();
    test_init<forward_iterator      <const int*>, forward_iterator      <const unsigned int*> >();
    test_init<forward_iterator      <const int*>, bidirectional_iterator<const unsigned int*> >();
    test_init<forward_iterator      <const int*>, random_access_iterator<const unsigned int*> >();

    test_init<bidirectional_iterator<const int*>, input_iterator        <const unsigned int*> >();
    test_init<bidirectional_iterator<const int*>, forward_iterator      <const unsigned int*> >();
    test_init<bidirectional_iterator<const int*>, bidirectional_iterator<const unsigned int*> >();
    test_init<bidirectional_iterator<const int*>, random_access_iterator<const unsigned int*> >();

    test_init<random_access_iterator<const int*>, input_iterator        <const unsigned int*> >();
    test_init<random_access_iterator<const int*>, forward_iterator      <const unsigned int*> >();
    test_init<random_access_iterator<const int*>, bidirectional_iterator<const unsigned int*> >();
    test_init<random_access_iterator<const int*>, random_access_iterator<const unsigned int*> >();

//  just plain pointers (const vs. non-const, too)
    test_init<const int*, const unsigned int *>();
    test_init<const int*,       unsigned int *>();
    test_init<      int*, const unsigned int *>();
    test_init<      int*,       unsigned int *>();
}

BOOST_AUTO_TEST_CASE( test_main )
{
  test_transform_reduce_init();
  test_transform_reduce_init_bop_uop();
  test_transform_reduce_init_bop_bop();
}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/test/arithmetic_opts.cpp
 *
 * \brief Test suite for matrix/vector arithmetic operators.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2012, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublasx/detail/debug.hpp>
#include <boost/numeric/ublasx/operation/arithmetic_ops.hpp>
#include "libs/numeric/ublasx/test/utils.hpp"

namespace ublas = ::boost::numeric::ublas;

static const double tol(1e-5);

BOOST_UBLASX_TEST_DEF( scalar_div_real_vector )
{
    BOOST_UBLASX_DEBUG_TRACE("Test Case: Scalar divides Real Vector");

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;

    const ::std::size_t n(4);

    value_type c(2);
    vector_type v(n);

    vector_type res;
    vector_type expect(n);

    v(0) = 1;
    v(1) = 2;
    v(2) = 3;
    v(3) = 4;

    for (::std::size_t i = 0; i < n; ++i)
    {
        expect(i) = c/v(i);
    }

    res = c / v;

    BOOST_UBLASX_DEBUG_TRACE("c=" << c);
    BOOST_UBLASX_DEBUG_TRACE("v=" << v);
    BOOST_UBLASX_DEBUG_TRACE("c / v? " << res);
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect, n, tol );
}

BOOST_UBLASX_TEST_DEF( scalar_div_real_matrix )
{
    BOOST_UBLASX_DEBUG_TRACE("Test Case: Scalar divides Real Matrix");

    typedef double value_type;
    typedef ublas::matrix<value_type> matrix_type;

    const ::std::size_t nr(3);
    const ::std::size_t nc(4);

    value_type c(2);
    matrix_type A(nr,nc);

    matrix_type res;
    matrix_type expect(nr,nc);

    A(0,0) = 1; A(0,1) = 4; A(0,2) = 7; A(0,3) = 10;
    A(1,0) = 2; A(1,1) = 5; A(1,2) = 8; A(1,3) = 11;
    A(2,0) = 3; A(2,1) = 6; A(2,2) = 9; A(2,3) = 12;

    for (::std::size_t i = 0; i < nr; ++i)
    {
        for (::std::size_t j = 0; j < nc; ++j)
        {
            expect(i,j) = c/A(i,j);
        }
    }

    res = c / A;

    BOOST_UBLASX_DEBUG_TRACE("c=" << c);
    BOOST_UBLASX_DEBUG_TRACE("A=" << A);
    BOOST_UBLASX_DEBUG_TRACE("c / A? " << res);
    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( res, expect, nr, nc, tol );
}

int main()
{
    BOOST_UBLASX_TEST_BEGIN();

    BOOST_UBLASX_TEST_DO( scalar_div_real_vector );
    BOOST_UBLASX_TEST_DO( scalar_div_real_matrix );

    BOOST_UBLASX_TEST_END();
}


/*
 * TypeDefs.hpp
 *
 *  Created on: March 18, 2014
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, ANYbotics
 */

// Eigen
#pragma once

#include <Eigen/Core>

namespace grid_map {

  using Matrix = Eigen::MatrixXf;
  using DataType = Matrix::Scalar;
  using Position = Eigen::Vector2d;
  using Vector = Eigen::Vector2d;
  using Position3 = Eigen::Vector3d;
  using Vector3 = Eigen::Vector3d;
  using Index = Eigen::Array2i;
  using Size = Eigen::Array2i;
  using Length = Eigen::Array2d;
  using Time = uint64_t;

  /*
   * Interpolations are ordered in the order
   * of increasing accuracy and computational complexity.
   * INTER_NEAREST - fastest, but least accurate,
   * INTER_CUBIC - slowest, but the most accurate.
   * see:
   * https://en.wikipedia.org/wiki/Bicubic_interpolation
   * https://web.archive.org/web/20051024202307/http://www.geovista.psu.edu/sites/geocomp99/Gc99/082/gc_082.htm
   * for more info. Cubic convolution algorithm is also known as piecewise cubic
   * interpolation and in general does not guarantee continuous
   * first derivatives.
   */
  enum class InterpolationMethods{
      INTER_NEAREST, // nearest neighbor interpolation
      INTER_LINEAR,   // bilinear interpolation
      INTER_CUBIC_CONVOLUTION, //piecewise bicubic interpolation using convolution algorithm
      INTER_CUBIC // standard bicubic interpolation
  };

}  // namespace grid_map


/*
 * Math.cpp
 *
 *  Created on: Nov 3, 2010
 *      Author: dberrios
 */

#include "MathHelper.h"
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/limits.hpp>
#include <cmath>
#include <algorithm>

namespace ccmc
{
	int Math::iminloc1d(int * array, long n, int * mask)
	{
	   long i,loc;
	   int min;
	   min = std::numeric_limits<int>::max( ); //if highest value possible in array is 99999999999
	   for (i=0; i<n; i++)
		   {
				  if( (array[i] < min) && (mask[i] > 0)) {
					 min=array[i];
					 loc=i;
				  }
		   }
	   return loc;
	}

	int Math::imaxloc1d(int * array, long n, int * mask)
	{
	   long i,loc;
	   int max;
	   max = boost::numeric::bounds<int>::lowest(); //if lowest value possible in array is -99999999999
	   for (i=0; i<n; i++)
		   {
				  if( (array[i] > max) && (mask[i] > 0)) {
					 max=array[i];
					 loc=i;
				  }
		   }
	   return loc;
	}

	int Math::fminloc1d(float * array, long n, int * mask)
	{
		long i,loc;
		float min;
		min = std::numeric_limits<float>::max( ); //if highest value possible in array is 99999999999
		for (i=0; i<n; i++)
		{
			if( (array[i] < min) && (mask[i] > 0))
			{
				min=array[i];
				loc=i;
			}
		}
		return loc;
	}

	int Math::fmaxloc1d(float * array, long n, int * mask)
	{
	   long i,loc;
	   float max;
	   max = boost::numeric::bounds<float>::lowest(); //if lowest value possible in array is -99999999999
	   for (i=0; i<n; i++)
		   {
				  if( (array[i] > max) && (mask[i] > 0)) {
					 max=array[i];
					 loc=i;
				  }
		   }
	   return loc;
	}

	int Math::dminloc1d(double * array, long n, int * mask)
	{
	   long i,loc;
	   double min;
	   min = std::numeric_limits<double>::max( ); //if highest value possible in array is 99999999999
	   for (i=0; i<n; i++)
		   {
				  if( (array[i] < min) && (mask[i] > 0)) {
					 min=array[i];
					 loc=i;
				  }
		   }
	   return loc;
	}

	int Math::dmaxloc1d(double * array, long n, int * mask)
	{
	   long i,loc;
	   double max;
	   max = boost::numeric::bounds<double>::lowest(); //if lowest value possible in array is -99999999999
	   for (i=0; i<n; i++)
		   {
				  if( (array[i] > max) && (mask[i] > 0)) {
					 max=array[i];
					 loc=i;
				  }
		   }
	   return loc;
	}

	int Math::ifindmin(int * array, long n)
	{
	   long i;
	   int min;
	   min = std::numeric_limits<int>::max( ); //if highest value possible in array is 99999999999
	   for (i=0; i<n; i++)
		   {
				  if(array[i] < min)
					 min=array[i];
		   }
	   return min;
	}

	int Math::ifindmax(int * array, long n)
	{
	   long i;
	   int max;
	   max = boost::numeric::bounds<int>::lowest(); //if lowest value possible in array is -99999999999
	   for (i=0; i<n; i++)
		   {
				  if(array[i] > max)
					 max=array[i];
		   }
	   return max;
	}

	long Math::lfindmin(long * array, long n)
	{
	   long i;
	   long min;
	   min = std::numeric_limits<long>::max( ); //if highest value possible in array is 99999999999
	   for (i=0; i<n; i++)
		   {
				  if(array[i] < min)
					 min=array[i];
		   }
	   return min;
	}

	long Math::lfindmax(long * array, long n)
	{
	   long i;
	   long max;
	   max = boost::numeric::bounds<long>::lowest(); //if lowest value possible in array is -99999999999
	   for (i=0; i<n; i++)
		   {
				  if(array[i] > max)
					 max=array[i];
		   }
	   return max;
	}

	float Math::ffindmin(float * array, long n)
	{
	   long i;
	   float min = std::numeric_limits<float>::max(); //if highest value possible in array is 1.e13
	   for (i=0; i<n; i++)
	   {
			  min = std::min(array[i], min);

	   }
	   return min;
	}

	float Math::ffindmax(float * array, long n)
	{
	   long i;
	   float max = -1.e20;
	   max = boost::numeric::bounds<float>::lowest(); //if lowest value possible in array is -1.e13
	   for (i=0; i<n; i++)
	   {
		   max = std::max((double)array[i] , (double)max);

	   }
	   return max;
	}

	double Math::dfindmin(double * array, long n)
	{
	   long i;
	   double min;
	   min = 1.e13; //if highest value possible in array is 1.e13
	   for (i=0; i<n; i++)
		   {
				  if(array[i] < min)
					 min=array[i];
		   }
	   return min;
	}

	double Math::dfindmax(double * array, long n)
	{
	   long i;
	   double max;
	   max = -1.e13; //if lowest value possible in array is -1.e13
	   for (i=0; i<n; i++)
		   {
				  if(array[i] > max)
					 max=array[i];
		   }
	   return max;
	}

	void Math::convert_xyz_to_rthetaphi(const float& x, const float& y, const float& z,
			float * r, float * t, float * p)
	{
	/*
	 Converts 3D cartesian coords (x,y,z) into spherical coords (r,theta,phi).
	*/

	      float  piov2,r2,zr,yx;

	      piov2 = std::acos(0.);

	      r2 = x*x+y*y+z*z;
	      if(r2 > 0.) {
	        *r  = std::sqrt(r2);
	        zr=z/ (*r);
	        *t = acos(zr);
	        if(x > 0.) {
	          yx=y/x;
	          *p = std::atan(yx);
	        } else if(x < 0.) {
	          yx=y/std::fabs(x);
	          *p = 2.*piov2 - atan(yx);
	        } else {
	          *p = piov2*sign(y);
	        }
	      } else {
	        *r = 0.;
	        *t = 0.;
	        *p = 0.;
	      }
	      if(*p < 0.) *p=*p+4.*piov2;               /* pmn testing */
	}

	void Math::convert_rthetaphi_to_xyz(const float& r, const float& t, const float& p,
			float * x, float * y, float * z)
	{
	/*
	  Converts spherical coords (r,theta,phi) into 3D cartesian coords (x,y,z)
	*/

	      float  sint,cost,sinp,cosp;

	      sint = std::sin(t);
	      cost = std::cos(t);
	      sinp = std::sin(p);
	      cosp = std::cos(p);

	      *x = r*sint*cosp;
	      *y = r*sint*sinp;
	      *z = r*cost;
	}

	int Math::sign(int v)
	{
	   return v > 0 ? 1 : (v < 0 ? -1 : 0);
	}

}


#ifndef PEOPLE_TRACKER_HPP
#define PEOPLE_TRACKER_HPP

#include <vector>
#include <unordered_map>
#include <Eigen/Dense>
#include <boost/optional.hpp>
#include <opencv2/opencv.hpp>

#include <kkl/alg/data_association.hpp>
#include <kkl/alg/nearest_neighbor_association.hpp>

#include <hdl_people_tracking/Cluster.h>
#include <hdl_people_tracking/kalman_tracker.hpp>

namespace kkl {
  namespace alg {

/**
 * @brief definition of the distance between tracker and observation for data association
 */
template<>
boost::optional<double> distance(const std::shared_ptr<hdl_people_tracking::KalmanTracker>& tracker, const hdl_people_tracking::Cluster& observation) {
  Eigen::Vector3d pos(observation.centroid.x, observation.centroid.y, observation.centroid.z);
  double sq_mahalanobis = tracker->squaredMahalanobisDistance(pos);

  // gating
  if(sq_mahalanobis > pow(3.0, 2) || (tracker->position() - pos).norm() > 1.5) {
    return boost::none;
  }
  return -kkl::math::gaussianProbMul(tracker->position(), tracker->positionCov(), pos);
}
  }
}

namespace hdl_people_tracking {

/**
 * @brief People tracker
 */
class PeopleTracker {
public:
  PeopleTracker(ros::NodeHandle& private_nh) {
    id_gen = 0;
    human_radius = private_nh.param<double>("human_radius", 0.4);
    remove_trace_thresh = private_nh.param<double>("remove_trace_thresh", 1.0);

    data_association.reset(new kkl::alg::NearestNeighborAssociation<KalmanTracker::Ptr, Cluster>());
//    data_association.reset(new kkl::alg::GlobalNearestNeighborAssociation<KalmanTracker::Ptr, VisualDetection>());
  }

  /**
   * @brief predict people states
   * @param time  current time
   */
  void predict(const ros::Time& time) {
    for(auto& person : people) {
      person->predict(time);
    }
  }

  /**
   * @brief correct people states
   * @param time          current time
   * @param detections    detections
   */
  void correct(const ros::Time& time, const std::vector<Cluster>& detections) {
    // data association
    std::vector<bool> associated(detections.size(), false);
    auto associations = data_association->associate(people, detections);
    for(const auto& assoc : associations) {
      associated[assoc.observation] = true;
      const auto& observation = detections[assoc.observation].centroid;
      Eigen::Vector3d observation_pos(observation.x, observation.y, observation.z);
      people[assoc.tracker]->correct(time, observation_pos, detections[assoc.observation]);
    }

    // generate new tracks
    for(int i=0; i<detections.size(); i++) {
      if(!associated[i]) {
        // check if the detection is far from existing tracks
        const auto& observation = detections[i].centroid;
        Eigen::Vector3d observation_pos(observation.x, observation.y, observation.z);

        bool close_to_tracker = false;
        for(const auto& person : people) {

          if((person->position() - observation_pos).norm() < human_radius * 2.0) {
            close_to_tracker = true;
            break;
          }
        }

        if(close_to_tracker) {
          continue;
        }

        // generate a new track
        KalmanTracker::Ptr tracker(new KalmanTracker(id_gen++, time, observation_pos));
        people.push_back(tracker);
      }
    }

    // remove tracks with large covariance
    auto remove_loc = std::partition(people.begin(), people.end(), [&](const KalmanTracker::Ptr& tracker) {
      return tracker->positionCov().trace() < remove_trace_thresh;
    });
    removed_people.clear();
    std::copy(remove_loc, people.end(), std::back_inserter(removed_people));
    people.erase(remove_loc, people.end());
  }

public:
  long id_gen;                  // track ID which will be assigned to the next new track
  double human_radius;          // new tracks must be far from existing tracks than this value
  double remove_trace_thresh;   // tracks with larger covariance trace than this will be removed

  std::vector<KalmanTracker::Ptr> people;
  std::vector<KalmanTracker::Ptr> removed_people;
  std::unique_ptr<kkl::alg::DataAssociation<KalmanTracker::Ptr, Cluster>> data_association;
};

}

#endif // PEOPLE_TRACKER_HPP


#include <string>
#include "Decryptor.h"
#include "RSA.h"
#include <boost/multiprecision/cpp_int.hpp>

using namespace boost::multiprecision;
using namespace Crypto;
using std::string;

Decryptor::Decryptor(const PrivateKey *key) : text(""), private_key(key)
{
};

string Decryptor::decryptString(CryptoString input)
{
  string cypher;

  int256_t tmp;
  int it;
  for (it = 0; it < input.size(); it++)
  {
    tmp = powm(input[it], private_key->s, private_key->q * private_key->p);
    cypher.push_back(tmp.convert_to<char>());
  }

  return cypher;
}

char Decryptor::decryptChar(CryptoChar c) const
{
  CryptoChar res = powm(c, private_key->s, private_key->q * private_key->p);
  return res.convert_to<int>();
}