#define BOOST_TEST_MODULE "test_worm_like_chain_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/forcefield/local/WormLikeChainPotential.hpp>

BOOST_AUTO_TEST_CASE(WormLikeChain_double)
{
    mjolnir::LoggerManager::set_default_logger("test_worm_like_chain_potential.log");

    using real_type = double;
    constexpr std::size_t N = 900;
    constexpr real_type   h = 1e-6;
    const real_type       p = 3.9;
    const real_type      lc = 19.0;

    mjolnir::WormLikeChainPotential<real_type> wormlikechain(p, lc);

    const real_type x_min = lc * 0.0;
    const real_type x_max = lc * 0.9;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=1; i<N; ++i)
    {
        const real_type x = x_min + dx * i;
        const real_type pot1 = wormlikechain.potential(x + h);
        const real_type pot2 = wormlikechain.potential(x - h);
        const real_type dpot = (pot1 - pot2) / (2.0 * h);
        const real_type deri = wormlikechain.derivative(x);

        if(std::abs(dpot) > h && std::abs(deri) > h)
        {
            BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
        }
    }
}

BOOST_AUTO_TEST_CASE(WormLikeChain_float)
{
    using real_type = float;
    constexpr std::size_t N = 900;
    constexpr real_type   h = 1e-3;
    const real_type       p = 3.9;
    const real_type      lc = 19.0;

    mjolnir::WormLikeChainPotential<real_type> wormlikechain(p, lc);

    const real_type x_min = 0.0;
    const real_type x_max = lc * 0.9;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=1; i<N; ++i)
    {
        const real_type x = x_min + dx * i;
        const real_type pot1 = wormlikechain.potential(x + h);
        const real_type pot2 = wormlikechain.potential(x - h);
        const real_type dpot = (pot1 - pot2) / (2.0 * h);
        const real_type deri = wormlikechain.derivative(x);

        if(std::abs(dpot) > h && std::abs(deri) > h)
        {
            BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
        }
    }
}


#include <vector>
#include <boost/math/distributions/beta.hpp>
#include "beta_dist.h"

stochastic::BetaDistribution::BetaDistribution(double alpha, double beta)
  : Distribution(),
    alpha_{alpha},
    beta_{beta},
    distribution_{alpha, beta_}
{}

std::vector<double> stochastic::BetaDistribution::cumulative_dist_func(
    const std::vector<double>& locations) const {
  std::vector<double> evaluations(locations.size());

  for (unsigned int i = 0; i < locations.size(); ++i) {
    evaluations[i] = cdf(distribution_, locations[i]);
  }

  return evaluations;
}

std::vector<double> stochastic::BetaDistribution::inv_cumulative_dist_func(
    const std::vector<double>& probabilities) const {
  std::vector<double> evaluations(probabilities.size());

  for (unsigned int i = 0; i < probabilities.size(); ++i) {
    evaluations[i] = quantile(distribution_, probabilities[i]);
  }

  return evaluations;
}


/* This file is part of the Tomographer project, which is distributed under the
 * terms of the MIT license.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 ETH Zurich, Institute for Theoretical Physics, Philippe Faist
 * Copyright (c) 2017 Caltech, Institute for Quantum Information and Matter, Philippe Faist
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <cmath>

#include <string>
#include <iostream>
#include <random>

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

// definitions for Tomographer test framework -- this must be included before any
// <Eigen/...> or <tomographer/...> header
#include "test_tomographer.h"

#include <tomographer/mathtools/pos_semidef_util.h>
#include <tomographer/mathtools/random_unitary.h>

// -----------------------------------------------------------------------------
// fixture(s)

// -----------------------------------------------------------------------------
// test suites

BOOST_AUTO_TEST_SUITE(test_mathtools_pos_semidef_util)

BOOST_AUTO_TEST_CASE(force_pos_semidef)
{
  Eigen::Matrix4cd rho;
  rho <<
    -0.1, 0, 0, 0,
    0, 0.05, 0, 0,
    0, 0, 0.55, 0,
    0, 0, 0, 0.5;

  BOOST_CHECK_CLOSE(rho.trace().real(), 1.0, tol_percent);

  Eigen::Matrix4cd rhopos;

  rhopos = Tomographer::MathTools::forcePosSemiDef<Eigen::Matrix4cd>(rho, 0.1); // high tolerance, check our algo

  BOOST_CHECK_CLOSE(rhopos.trace().real(), 1.0, tol_percent);

  Eigen::Matrix4cd rhopos_ref_withtol;
  rhopos_ref_withtol <<
    0.1, 0, 0, 0,
    0, 0.1, 0, 0,
    0, 0, 0.425, 0,
    0, 0, 0, 0.375;
  // the 0.25 "excess trace" is evenly subtracted from all good eigenvalues

  MY_BOOST_CHECK_EIGEN_EQUAL(rhopos, rhopos_ref_withtol, tol);

  // should get the same behavior if we apply some Unitary

  // get some nontrivial unitary (fixed by deterministic seeded rng)
  Eigen::Matrix4cd Unitary;
  std::mt19937 rng(1); // seeded, deterministic random number generator
  Tomographer::MathTools::randomUnitary(Unitary, rng);

  BOOST_MESSAGE("Chose Unitary = \n" << Unitary) ;

  Eigen::Matrix4cd rhoposU;
  rhoposU = Tomographer::MathTools::forcePosSemiDef<Eigen::Matrix4cd>(Unitary*rho*Unitary.adjoint(), 0.1);

  BOOST_CHECK_CLOSE(rhoposU.trace().real(), 1.0, tol_percent);
  MY_BOOST_CHECK_EIGEN_EQUAL(rhoposU, Unitary*rhopos_ref_withtol*Unitary.adjoint(), tol);
}

BOOST_AUTO_TEST_CASE(safe_ops1)
{
  Eigen::Matrix3cd A;
  A <<
    0, 0, 0,
    0, 0, 0,
    0, 0, 1;

  MY_BOOST_CHECK_EIGEN_EQUAL(Tomographer::MathTools::safeOperatorSqrt<Eigen::Matrix3cd>(A, 1e-18), A, 2e-9);
  MY_BOOST_CHECK_EIGEN_EQUAL(Tomographer::MathTools::safeOperatorInvSqrt<Eigen::Matrix3cd>(A, 1e-12), A, 2e-6);
}

BOOST_AUTO_TEST_CASE(safe_ops2)
{
  Eigen::Matrix3cd U;
  std::mt19937 rng(3982);
  Tomographer::MathTools::randomUnitary(U, rng);

  Eigen::Matrix3cd A;
  A <<
    0, 0, 0,
    0, 0, 0,
    0, 0, 1;

  MY_BOOST_CHECK_EIGEN_EQUAL(Tomographer::MathTools::safeOperatorSqrt<Eigen::Matrix3cd>(2*U*A*U.adjoint(), 1e-18),
                             U*A*U.adjoint()*boost::math::constants::root_two<double>(), 1e-7);
  MY_BOOST_CHECK_EIGEN_EQUAL(Tomographer::MathTools::safeOperatorInvSqrt<Eigen::Matrix3cd>(2*U*A*U.adjoint(), 1e-12),
                             U*A*U.adjoint()*boost::math::constants::half_root_two<double>(), 1e-5);
}

BOOST_AUTO_TEST_SUITE_END();


// Boost.Geometry

// Copyright (c) 2016 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP
#define BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP

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

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/algorithms/detail/flattening.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief The intersection of two geodesics as proposed by Sjoberg.
\author See
    - [Sjoberg02] Lars E. Sjoberg, Intersections on the sphere and ellipsoid, 2002
      http://link.springer.com/article/10.1007/s00190-001-0230-9
    - [Sjoberg07] Lars E. Sjoberg, Geodetic intersection on the ellipsoid, 2007
      http://link.springer.com/article/10.1007/s00190-007-0204-7
*/
template
<
    typename CT,
    template <typename, bool, bool, bool, bool, bool> class Inverse,
    unsigned int Order = 4
>
class sjoberg_intersection
{
    typedef Inverse<CT, false, true, false, false, false> inverse_type;
    typedef typename inverse_type::result_type inverse_result;

public:
    template <typename T1, typename T2, typename Spheroid>
    static inline bool apply(T1 const& lona1, T1 const& lata1,
                             T1 const& lona2, T1 const& lata2,
                             T2 const& lonb1, T2 const& latb1,
                             T2 const& lonb2, T2 const& latb2,
                             CT & lon, CT & lat,
                             Spheroid const& spheroid)
    {
        CT const lon_a1 = lona1;
        CT const lat_a1 = lata1;
        CT const lon_a2 = lona2;
        CT const lat_a2 = lata2;
        CT const lon_b1 = lonb1;
        CT const lat_b1 = latb1;
        CT const lon_b2 = lonb2;
        CT const lat_b2 = latb2;

        CT const alpha1 = inverse_type::apply(lon_a1, lat_a1, lon_a2, lat_a2, spheroid).azimuth;
        CT const alpha2 = inverse_type::apply(lon_b1, lat_b1, lon_b2, lat_b2, spheroid).azimuth;

        return apply(lon_a1, lat_a1, alpha1, lon_b1, lat_b1, alpha2, lon, lat, spheroid);
    }

    template <typename Spheroid>
    static inline bool apply(CT const& lon1, CT const& lat1, CT const& alpha1,
                             CT const& lon2, CT const& lat2, CT const& alpha2,
                             CT & lon, CT & lat,
                             Spheroid const& spheroid)
    {
        // coordinates in radians

        // TODO - handle special cases like degenerated segments, equator, poles, etc.

        CT const c0 = 0;
        CT const c1 = 1;
        CT const c2 = 2;

        CT const pi = math::pi<CT>();
        CT const pi_half = pi / c2;
        CT const f = detail::flattening<CT>(spheroid);
        CT const one_minus_f = c1 - f;
        CT const e_sqr = f * (c2 - f);

        CT const sin_alpha1 = sin(alpha1);
        CT const sin_alpha2 = sin(alpha2);

        CT const tan_beta1 = one_minus_f * tan(lat1);
        CT const tan_beta2 = one_minus_f * tan(lat2);
        CT const beta1 = atan(tan_beta1);
        CT const beta2 = atan(tan_beta2);
        CT const cos_beta1 = cos(beta1);
        CT const cos_beta2 = cos(beta2);
        CT const sin_beta1 = sin(beta1);
        CT const sin_beta2 = sin(beta2);

        // Clairaut constants (lower-case in the paper)
        int const sign_C1 = math::abs(alpha1) <= pi_half ? 1 : -1;
        int const sign_C2 = math::abs(alpha2) <= pi_half ? 1 : -1;
        // Cj = 1 if on equator
        CT const C1 = sign_C1 * cos_beta1 * sin_alpha1;
        CT const C2 = sign_C2 * cos_beta2 * sin_alpha2;

        CT const sqrt_1_C1_sqr = math::sqrt(c1 - math::sqr(C1));
        CT const sqrt_1_C2_sqr = math::sqrt(c1 - math::sqr(C2));

        // handle special case: segments on the equator
        bool const on_equator1 = math::equals(sqrt_1_C1_sqr, c0);
        bool const on_equator2 = math::equals(sqrt_1_C2_sqr, c0);
        if (on_equator1 && on_equator2)
        {
            return false;
        }
        else if (on_equator1)
        {
            CT const dL2 = d_lambda_e_sqr(sin_beta2, c0, C2, sqrt_1_C2_sqr, e_sqr);
            CT const asin_t2_t02 = asin(C2 * tan_beta2 / sqrt_1_C2_sqr);
            lat = c0;
            lon = lon2 - asin_t2_t02 + dL2;
            return true;
        }
        else if (on_equator2)
        {
            CT const dL1 = d_lambda_e_sqr(sin_beta1, c0, C1, sqrt_1_C1_sqr, e_sqr);
            CT const asin_t1_t01 = asin(C1 * tan_beta1 / sqrt_1_C1_sqr);
            lat = c0;
            lon = lon1 - asin_t1_t01 + dL1;
            return true;
        }

        CT const t01 = sqrt_1_C1_sqr / C1;
        CT const t02 = sqrt_1_C2_sqr / C2;

        CT const asin_t1_t01 = asin(tan_beta1 / t01);
        CT const asin_t2_t02 = asin(tan_beta2 / t02);
        CT const t01_t02 = t01 * t02;
        CT const t01_t02_2 = c2 * t01_t02;
        CT const sqr_t01_sqr_t02 = math::sqr(t01) + math::sqr(t02);

        CT t = tan_beta1;
        int t_id = 0;

        // find the initial t using simplified spherical solution
        // though not entirely since the reduced latitudes and azimuths are spheroidal
        // [Sjoberg07]
        CT const k_base = lon1 - lon2 + asin_t2_t02 - asin_t1_t01;

        {
            CT const K = sin(k_base);
            CT const d1 = sqr_t01_sqr_t02;
            //CT const d2 = t01_t02_2 * math::sqrt(c1 - math::sqr(K));
            CT const d2 = t01_t02_2 * cos(k_base);
            CT const D1 = math::sqrt(d1 - d2);
            CT const D2 = math::sqrt(d1 + d2);
            CT const K_t01_t02 = K * t01_t02;

            CT const T1 = K_t01_t02 / D1;
            CT const T2 = K_t01_t02 / D2;
            CT asin_T1_t01 = 0;
            CT asin_T1_t02 = 0;
            CT asin_T2_t01 = 0;
            CT asin_T2_t02 = 0;

            // test 4 possible results
            CT l1 = 0, l2 = 0, dl = 0;
            bool found = check_t<0>( T1,
                                    lon1,  asin_T1_t01 = asin(T1 / t01), asin_t1_t01,
                                    lon2,  asin_T1_t02 = asin(T1 / t02), asin_t2_t02,
                                    t, l1, l2, dl, t_id)
                      || check_t<1>(-T1,
                                    lon1, -asin_T1_t01                 , asin_t1_t01,
                                    lon2, -asin_T1_t02                 , asin_t2_t02,
                                    t, l1, l2, dl, t_id)
                      || check_t<2>( T2,
                                    lon1,  asin_T2_t01 = asin(T2 / t01), asin_t1_t01,
                                    lon2,  asin_T2_t02 = asin(T2 / t02), asin_t2_t02,
                                    t, l1, l2, dl, t_id)
                      || check_t<3>(-T2,
                                    lon1, -asin_T2_t01                 , asin_t1_t01,
                                    lon2, -asin_T2_t02                 , asin_t2_t02,
                                    t, l1, l2, dl, t_id);

            boost::ignore_unused(found);
        }

        // [Sjoberg07]
        //int const d2_sign = t_id < 2 ? -1 : 1;
        int const t_sign = (t_id % 2) ? -1 : 1;
        // [Sjoberg02]
        CT const C1_sqr = math::sqr(C1);
        CT const C2_sqr = math::sqr(C2);

        CT beta = atan(t);
        CT dL1 = 0, dL2 = 0;
        CT asin_t_t01 = 0;
        CT asin_t_t02 = 0;

        for (int i = 0; i < 10; ++i)
        {
            CT const sin_beta = sin(beta);

            // integrals approximation
            dL1 = d_lambda_e_sqr(sin_beta1, sin_beta, C1, sqrt_1_C1_sqr, e_sqr);
            dL2 = d_lambda_e_sqr(sin_beta2, sin_beta, C2, sqrt_1_C2_sqr, e_sqr);

            // [Sjoberg07]
            /*CT const k = k_base + dL1 - dL2;
            CT const K = sin(k);
            CT const d1 = sqr_t01_sqr_t02;
            //CT const d2 = t01_t02_2 * math::sqrt(c1 - math::sqr(K));
            CT const d2 = t01_t02_2 * cos(k);
            CT const D = math::sqrt(d1 + d2_sign * d2);
            CT const t_new = t_sign * K * t01_t02 / D;
            CT const dt = math::abs(t_new - t);
            t = t_new;
            CT const new_beta = atan(t);
            CT const dbeta = math::abs(new_beta - beta);
            beta = new_beta;*/

            // [Sjoberg02] - it converges faster
            // Newton�Raphson method
            asin_t_t01 = asin(t / t01);
            asin_t_t02 = asin(t / t02);
            CT const R1 = asin_t_t01 + dL1;
            CT const R2 = asin_t_t02 + dL2;
            CT const cos_beta = cos(beta);
            CT const cos_beta_sqr = math::sqr(cos_beta);
            CT const G = c1 - e_sqr * cos_beta_sqr;
            CT const f1 = C1 / cos_beta * math::sqrt(G / (cos_beta_sqr - C1_sqr));
            CT const f2 = C2 / cos_beta * math::sqrt(G / (cos_beta_sqr - C2_sqr));
            CT const abs_f1 = math::abs(f1);
            CT const abs_f2 = math::abs(f2);
            CT const dbeta = t_sign * (k_base - R2 + R1) / (abs_f1 + abs_f2);

            if (math::equals(dbeta, CT(0)))
            {
                break;
            }

            beta = beta - dbeta;
            t = tan(beta);
        }

        // t = tan(beta) = (1-f)tan(lat)
        lat = atan(t / one_minus_f);

        CT const l1 = lon1 + asin_t_t01 - asin_t1_t01 + dL1;
        //CT const l2 = lon2 + asin_t_t02 - asin_t2_t02 + dL2;
        lon = l1;

        return true;
    }

private:
    /*! Approximation of dLambda_j [Sjoberg07], expanded into taylor series in e^2
        Maxima script:
        dLI_j(c_j, sinB_j, sinB) := integrate(1 / (sqrt(1 - c_j ^ 2 - x ^ 2)*(1 + sqrt(1 - e2*(1 - x ^ 2)))), x, sinB_j, sinB);
        dL_j(c_j, B_j, B) := -e2 * c_j * dLI_j(c_j, B_j, B);
        S: taylor(dLI_j(c_j, sinB_j, sinB), e2, 0, 3);
        assume(c_j < 1);
        assume(c_j > 0);
        L1: factor(integrate(sqrt(-x ^ 2 - c_j ^ 2 + 1) / (x ^ 2 + c_j ^ 2 - 1), x));
        L2: factor(integrate(((x ^ 2 - 1)*sqrt(-x ^ 2 - c_j ^ 2 + 1)) / (x ^ 2 + c_j ^ 2 - 1), x));
        L3: factor(integrate(((x ^ 4 - 2 * x ^ 2 + 1)*sqrt(-x ^ 2 - c_j ^ 2 + 1)) / (x ^ 2 + c_j ^ 2 - 1), x));
        L4: factor(integrate(((x ^ 6 - 3 * x ^ 4 + 3 * x ^ 2 - 1)*sqrt(-x ^ 2 - c_j ^ 2 + 1)) / (x ^ 2 + c_j ^ 2 - 1), x));
    */
    static inline CT d_lambda_e_sqr(CT const& sin_betaj, CT const& sin_beta,
                                    CT const& Cj, CT const& sqrt_1_Cj_sqr,
                                    CT const& e_sqr)
    {
        if (Order == 0)
        {
            return 0;
        }

        CT const c2 = 2;

        CT const asin_B = asin(sin_beta / sqrt_1_Cj_sqr);
        CT const asin_Bj = asin(sin_betaj / sqrt_1_Cj_sqr);
        CT const L0 = (asin_B - asin_Bj) / c2;

        if (Order == 1)
        {
            return -Cj * e_sqr * L0;
        }

        CT const c1 = 1;
        CT const c16 = 16;

        CT const X = sin_beta;
        CT const Xj = sin_betaj;
        CT const Cj_sqr = math::sqr(Cj);
        CT const Cj_sqr_plus_one = Cj_sqr + c1;
        CT const one_minus_Cj_sqr = c1 - Cj_sqr;
        CT const sqrt_Y = math::sqrt(-math::sqr(X) + one_minus_Cj_sqr);
        CT const sqrt_Yj = math::sqrt(-math::sqr(Xj) + one_minus_Cj_sqr);
        CT const L1 = (Cj_sqr_plus_one * (asin_B - asin_Bj) + X * sqrt_Y - Xj * sqrt_Yj) / c16;

        if (Order == 2)
        {
            return -Cj * e_sqr * (L0 + e_sqr * L1);
        }

        CT const c3 = 3;
        CT const c5 = 5;
        CT const c128 = 128;

        CT const E = Cj_sqr * (c3 * Cj_sqr + c2) + c3;
        CT const X_sqr = math::sqr(X);
        CT const Xj_sqr = math::sqr(Xj);
        CT const F = X * (-c2 * X_sqr + c3 * Cj_sqr + c5);
        CT const Fj = Xj * (-c2 * Xj_sqr + c3 * Cj_sqr + c5);
        CT const L2 = (E * (asin_B - asin_Bj) + F * sqrt_Y - Fj * sqrt_Yj) / c128;

        if (Order == 3)
        {
            return -Cj * e_sqr * (L0 + e_sqr * (L1 + e_sqr * L2));
        }

        CT const c8 = 8;
        CT const c9 = 9;
        CT const c10 = 10;
        CT const c15 = 15;
        CT const c24 = 24;
        CT const c26 = 26;
        CT const c33 = 33;
        CT const c6144 = 6144;

        CT const G = Cj_sqr * (Cj_sqr * (Cj_sqr * c15 + c9) + c9) + c15;
        CT const H = -c10 * Cj_sqr - c26;
        CT const I = Cj_sqr * (Cj_sqr * c15 + c24) + c33;
        CT const J = X_sqr * (X * (c8 * X_sqr + H)) + X * I;
        CT const Jj = Xj_sqr * (Xj * (c8 * Xj_sqr + H)) + Xj * I;
        CT const L3 = (G * (asin_B - asin_Bj) + J * sqrt_Y - Jj * sqrt_Yj) / c6144;

        // Order 4 and higher
        return -Cj * e_sqr * (L0 + e_sqr * (L1 + e_sqr * (L2 + e_sqr * L3)));
    }

    static inline CT fj(CT const& cos_beta, CT const& cos2_beta, CT const& Cj, CT const& e_sqr)
    {
        CT const c1 = 1;
        CT const Cj_sqr = math::sqr(Cj);
        return Cj / cos_beta * math::sqrt((c1 - e_sqr * cos2_beta) / (cos2_beta - Cj_sqr));
    }

    template <int TId>
    static inline bool check_t(CT const& t,
                               CT const& lon_a1, CT const& asin_t_t01, CT const& asin_t1_t01,
                               CT const& lon_b1, CT const& asin_t_t02, CT const& asin_t2_t02,
                               CT & current_t, CT & current_lon1, CT & current_lon2, CT & current_dlon,
                               int & t_id)
    {
        CT const lon1 = lon_a1 + asin_t_t01 - asin_t1_t01;
        CT const lon2 = lon_b1 + asin_t_t02 - asin_t2_t02;

        // TODO - true angle difference
        CT const dlon = math::abs(lon2 - lon1);

        bool are_equal = math::equals(dlon, CT(0));

        if ((TId == 0) || are_equal || dlon < current_dlon)
        {
            current_t = t;
            current_lon1 = lon1;
            current_lon2 = lon2;
            current_dlon = dlon;
            t_id = TId;
        }

        return are_equal;
    }
};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP


#include <boost/math/distributions/lognormal.hpp>


/*
*  @file 		ex7.cpp
*  @details  	This file is the solution to exercise 7.
*  @author    	Alexander Rettkowski
*  @date      	28.06.2017
*/
#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/fusion/include/io.hpp>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/property_map/property_map.hpp>

#include <boost/timer/timer.hpp>
#include <boost/chrono.hpp>

#include <iostream>
#include <string>
#include <complex>
#include <fstream>
#include <queue>

using namespace boost;

namespace exercise5
{
	namespace qi = boost::spirit::qi;
	namespace ascii = boost::spirit::ascii;
	struct edge
	{
		int startNode;
		int endNode;
		int length;
	};
}

BOOST_FUSION_ADAPT_STRUCT(
	exercise5::edge,
	(int, startNode)
	(int, endNode)
	(int, length)
)

namespace exercise5
{
	template <typename Iterator>
	struct line_parser : qi::grammar<Iterator, edge()>
	{
		line_parser() : line_parser::base_type(start)
		{
			using qi::int_;
			start %= int_ >> ' ' >> int_ >> ' ' >> int_;
		}

		qi::rule<Iterator, edge()> start;
	};
}

/**
* The function that calculates the longest shortest path from node 0.
* @param numberOfNodes Number of nodes in the graph.
* @param graph The graph represented as a vector auf edge-vectors.
*/
std::pair<int, int> dijkstra( int numberOfNodes, std::vector< std::vector< std::pair<int, int> > > graph)
{
	std::vector<int> distanceTo(numberOfNodes, INT_MAX);
	std::priority_queue< std::pair<int, int>, std::vector< std::pair<int, int> >, std::greater< std::pair<int, int> > > queue;
	queue.push(std::pair<int, int>(0, 0));
	distanceTo[0] = 0;

	int currentNode, compareNode, compareNodeDistance, currentNodeDistance;

	while (!queue.empty()) {
		currentNode = queue.top().first;
		currentNodeDistance = queue.top().second;
		queue.pop();

		if (distanceTo[currentNode] < currentNodeDistance) continue;

		for (int i = 0; i < graph[currentNode].size(); i++) {
			compareNode = graph[currentNode][i].first;
			compareNodeDistance = graph[currentNode][i].second;
			if (distanceTo[compareNode] > distanceTo[currentNode] + compareNodeDistance) {
				distanceTo[compareNode] = distanceTo[currentNode] + compareNodeDistance;
				queue.push(std::pair<int, int>(compareNode, distanceTo[compareNode]));
			}
		}
	}

	std::pair<int, int> solution(-1, -1);
	for (int i = 0; i < numberOfNodes; i++)
	{
		if (distanceTo[i] > solution.second)
		{
			solution.second = distanceTo[i];
			solution.first = i;
		}
		if ((distanceTo[i] == solution.second) && (i > solution.first))
		{
			solution.first = i;
		}
	}

	return solution;
}

/**
* The main function that reads in a file and processes it.
* @param argc Number of command line arguments.
* @param *argv a pointer to the array of command line arguments.
*/
int main(int argc, char *argv[])
{
	timer::cpu_timer boostTimer;

	using boost::spirit::ascii::space;
	typedef std::string::const_iterator iterator_type;
	typedef exercise5::line_parser<iterator_type> line_parser;
	line_parser parser;
	std::string currentLine;
	std::ifstream file(argv[1]);

	// get number of nodes
	char delimiter = ' ';
	getline(file, currentLine, delimiter);
	const int numberOfNodes = std::stoi(currentLine);
	getline(file, currentLine);
	int constSub = 1;

	std::vector<std::vector<std::pair<int, int>>> edges(numberOfNodes);
	while (getline(file, currentLine))
	{
		exercise5::edge parsedLine;
		std::string::const_iterator currentPosition = currentLine.begin();
		std::string::const_iterator lineEnd = currentLine.end();
		bool parsingSucceeded = phrase_parse(currentPosition, lineEnd, parser, space, parsedLine);

		if (parsingSucceeded && currentPosition == lineEnd)
		{
			std::pair<int, int> *tempEdge = new std::pair<int, int>();
			tempEdge->first = parsedLine.endNode - constSub;
			tempEdge->second = parsedLine.length;
			edges[parsedLine.startNode - constSub].push_back(*tempEdge);

			std::pair<int, int> *reverseEdge = new std::pair<int, int>();
			reverseEdge->first = parsedLine.startNode - constSub;
			reverseEdge->second = parsedLine.length;
			edges[parsedLine.endNode - constSub].push_back(*reverseEdge);
		}
	}

	file.close();

	std::pair<int, int> solution = dijkstra(numberOfNodes, edges);
	int vertex = solution.first, distance = solution.second;

	std::cout << "RESULT VERTEX " << vertex << std::endl;
	std::cout << "RESULT DIST " << distance << std::endl;

	timer::cpu_times time = boostTimer.elapsed();
	std::cout << "CPU TIME: " << (time.user + time.system) / 1e9 << "s\n";
	std::cout << "WALL CLOCK TIME: " << time.wall / 1e9 << "s\n";

	return 0;
}


#ifndef _SIAR_ARM_HPP_
#define _SIAR_ARM_HPP_

#include "math.h"
#include <functions/linear_interpolator.hpp>
#include <string>
#include <queue>
#include <vector>
#include <boost/concept_check.hpp>

#define L1 0.0475
#define L2 0.215
#define L3 0.155
#define L4 0.080
#include "siar_functions.hpp"

class SiarArm{

  public:

    // Linear interpolators data
  int n_motors;
  std::vector<functions::LinearInterpolator *> pos_mot_interpol_, mot_pos_interpol_;
  std::vector<double> length;

  SiarArm(const std::string &mot_arm_file, const std::string &pos_arm_file) {
    load_data(mot_arm_file, pos_arm_file);
  }

  bool load_data(const std::string &mot_arm_file, const std::string &pos_arm_file) {
    n_motors = 5;
    for (int i = 0; i < n_motors; i++) {
      std::ostringstream mot_arm, pos_arm;
      mot_arm << mot_arm_file << i;
      pos_arm << pos_arm_file << i;
      pos_mot_interpol_.push_back(new functions::LinearInterpolator(mot_arm.str(), pos_arm.str()));
    }
    length.push_back(0.035); // TODO: read it from file?
    length.push_back(0.186);
    length.push_back(0.140);
    length.push_back(0.0651);
    length.push_back(0.04297);

  }

  ~SiarArm() {
    for(int i = 0; i < pos_mot_interpol_; i++) {
      delete pos_mot_interpol_[i];
    }
    for(int i = 0; i < mot_pos_interpol_; i++) {
      delete mot_pos_interpol_[i];
    }
  }

  bool inverseKinematics(double x, double y, double z, std::vector <double> &result)
  {
    bool coordenadas_correctas = true;

    // a1 = atan2(Z,X)

// a3 = acos(Z^2+X^2+(Y-L1)^2-L2^2-L3^2)

// d = sqrt(L2^2 + L3^2 -2*L2*L3*cos(a3))

// a2 = acos((y-L1)/d)

// donde L1 = 35.06; L2 = 186; L3 =140.05

// a1,2,3 son los ángulos de la primera, segunda y tercera articulación.

    result[0] = atan2(z, x);
    result[2] = acos(z*z+ x*x + pow(y-length[0],2.0) - length[2]*length[2] - length[3]*length[3]);
    double d = sqrt(length[1]*length[1] + length[2]*length[2] - 2.0 * length[1] * length[2] * cos(result[2]);
    result[1] = acos((y - length[0])/d;
    //ROS_INFO("x: %f; y: %f;z: %f",x,y,z);
    //ROS_INFO("q1: %f, q2: %f, q3: %f, q4: %f, q5:%f",q1,q2,q3,q4,q5);
  }

  void rad2motor(const std::vector<double> &angles, std::vector<int> &commands) {
    commands.resize(n_motors);
    for (int i = 0; i < n_motors; i++) {
      functions::LinearInterpolator &interpol = *pos_mot_interpol_[i];
      commands[i] = interpol(angles[i]);
    }
  }

  void motor2rad(const std::vector<int> &commands, std::vector<double> &angles) {
    angles.resize(n_motors);
    for (int i = 0; i < n_motors; i++) {
      functions::LinearInterpolator &interpol = *mot_pos_interpol_[i];
      angles[i] = interpol(commands[i]);
    }
  }

  void forwardKinematics(const std::vector  <int> &joint_values, double &x, double &y, double &z)
  {

//       X = cos(a1) *(L2*sin(a2)+L3*sin(a2+a3))
// Y = L1 + (L2*cos(a2)+L3*cos(a2+a3))
// Z = -1 * sin(a1) *(L2*sin(a2)+L3*sin(a2+a3))
// Donde Y es positiva desde la base en dirección a la primera articulación, X es ortogonal  a Y en el plano proyectado por el dibujo y Z va desde el plano hacia dentro.

    std::vector<double> angles;
    motor2rad(joint_values, angles);
    double a1 = joint_values[0];
    double a2 = joint_values[1];
    double a3 = joint_values[2];
    double L1 = length[0];
    double L2 = length[1];
    double L3 = length[2];
    x = cos(a1) * (L2*sin(a2) + L3*sin(a2 + a3));
    y = L1 + L2*cos(a2) + L3*cos(a2 + a3);
    z = -sin(a1) * (L2*sin(a2) + L3*sin(a2 + a3));
  }

  std::vector<std::vector<int> > straightInterpol(double x, double y, double z, uint8_t n_points, const std::vector<int> &curr_pos)
  {
    doble a_x, a_y, a_z;
    forwardKinematics(curr_pos, a_x, a_y, a_z);
    doble i_x, i_y, i_z;
    i_x = (x - a_x)/(n_points+1);
    i_y = (y - a_y)/(n_points+1);
    i_z = (z - a_z)/(n_points+1);

    std::vector<std::vector<int> > ret;

    std::vector<double> angles;
    std::vector<int> commands;
    for( int i = 0; i < n_points+1; i++)
    {
      a_x += i_x;
      a_y += i_y;
      a_z += i_z;

      inverseKinematics(a_x, a_y, a_z, angles);
      rad2motor(angles, commands);
      ret.push_back(commands);
    }

  }

  bool checkJointLimits(const boost::array<int16_t, 5> joint_values)
  {
    bool ret_val = true;

    for (int i = 0; i < n_motors && ret_val; i++) {
      double max, min;
      functions::LinearInterpolator &curr_inter = *mot_pos_interpol_[i];
      min = curr_inter.upper_bound(0);
      max = curr_inter.lower_bound(2000); // Usually the commands are in the [0, 2000] range

      ret_val &= joint_values[i] > min && joint_values[i] < max;

    }

    return ret_val;
  }

  bool checkTemperatureAndStatus(const boost::array<uint8_t,5> &herculex_temperature, const boost::array<uint8_t,5> &herculex_status) {
    bool ret_val = true;
    for(int i = 0; i < 5; i++)
    {
      if (herculex_temperature[i]<0 && herculex_temperature[i]>50)
      {
        ROS_ERROR("TEMPERATURE OF THE %d LINK IS OUT OF RANGE: %d", i, herculex_temperature[i]);
        ret_val = false;
      }
//       if (herculex_status[i]!=1)    // TODO: Check this!!
//       {
//         ROS_ERROR("%d LINK STATUS: %d", i, herculex_status[i]);
//         ret_val = false;
//       }
    }
    return ret_val;
  }

};
#endif /* _SIAR_ARM_H_ */


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE QuaternionTest
#define BOOST_TEST_NO_OLD_TOOLS
#include <boost/test/unit_test.hpp>
#include <Quaternion.h>
#include <iostream>

void checkEquals(const Quaternion &q1, const Quaternion &q2) {
    BOOST_CHECK_SMALL(q1.a - q2.a, .0001);
    BOOST_CHECK_SMALL(q1.b - q2.b, .0001);
    BOOST_CHECK_SMALL(q1.c - q2.c, .0001);
    BOOST_CHECK_SMALL(q1.d - q2.d, .0001);
}

BOOST_AUTO_TEST_CASE(rotateX) {
    auto rotateX = Quaternion::from_euler_rotation(M_PI_2, 0, 0);
    BOOST_CHECK_CLOSE(sqrt(2)/2.0, rotateX.a, .00001);
    BOOST_CHECK_CLOSE(sqrt(2)/2.0, rotateX.b, .00001);
    auto q = rotateX.rotate(Quaternion(0, 1, 0));
    BOOST_CHECK_CLOSE(1.0, q.d, .00001);
    checkEquals(q, Quaternion(0, 0, 1));
}

BOOST_AUTO_TEST_CASE(rotateY) {
    auto rotate = Quaternion::from_euler_rotation(0, M_PI_2, 0);
    BOOST_CHECK_CLOSE(sqrt(2)/2.0, rotate.a, .00001);
    BOOST_CHECK_CLOSE(sqrt(2)/2.0, rotate.c, .00001);
    auto q = rotate.rotate(Quaternion(0, 0, 1));
    BOOST_CHECK_CLOSE(1.0, q.b, .00001);
}

BOOST_AUTO_TEST_CASE(rotateZ) {
    auto rotate = Quaternion::from_euler_rotation(0, 0, M_PI_2);
    BOOST_CHECK_CLOSE(sqrt(2)/2.0, rotate.a, .00001);
    BOOST_CHECK_CLOSE(sqrt(2)/2.0, rotate.d, .00001);
    auto q = rotate.rotate(Quaternion(1, 0, 0));
    BOOST_CHECK_CLOSE(1.0, q.c, .00001);
}

BOOST_AUTO_TEST_CASE(rotateTwice) {
    auto rotate = Quaternion::from_euler_rotation(0, 0, M_PI_2) *
            Quaternion::from_euler_rotation(M_PI_2, 0, 0);
    rotate.normalize();
    BOOST_CHECK_CLOSE(.5, rotate.a, .0001);
    BOOST_CHECK_CLOSE(.5, rotate.b, .0001);
    BOOST_CHECK_CLOSE(.5, rotate.c, .0001);
    BOOST_CHECK_CLOSE(.5, rotate.d, .0001);

    auto q = rotate.rotate(Quaternion(0, 1, 0));
    BOOST_CHECK_CLOSE(1.0, q.d, .0001);
}

BOOST_AUTO_TEST_CASE(nonCommutative) {
    auto q1 = Quaternion::from_euler_rotation(0, 0, M_PI_2);
    auto q2 = Quaternion::from_euler_rotation(M_PI_2, 0, 0);
    auto rotate1 = (q1 * q2).conj();
    auto rotate2 = q2.conj() * q1.conj();
    checkEquals(rotate1, rotate2);
}

BOOST_AUTO_TEST_CASE(rotateRelative) {
    auto yaw = Quaternion::from_euler_rotation(0, 0, M_PI_2);
    auto pitch = Quaternion::from_euler_rotation(0, M_PI_2, 0);
    auto roll = Quaternion::from_euler_rotation(M_PI_2, 0, 0);
    auto composite = yaw * pitch * roll;
    auto rotated = composite.rotate(Quaternion(0, 0, 1));
    checkEquals(rotated, Quaternion(1, 0, 0));
}


#pragma once

// deal.II includes ------------------------------------------------------------
#include <deal.II/base/numbers.h>
#include <deal.II/base/tensor.h>
//#include <deal.II/base/quadrature_lib.h>
// system includes -------------------------------------------------------------
#include <map>
// own includes ----------------------------------------------------------------
#include <quadrature/trig_int.hpp>

namespace boltzmann {

namespace local_ {
// ----------------------------------------------------------------------
inline bool
is_zero(const dealii::Tensor<2, 2, double>& t2)
{
  const double tol = 1e-16;
  return std::abs(t2[0][0]) < tol && std::abs(t2[0][1]) < tol && std::abs(t2[1][0]) < tol &&
         std::abs(t2[1][1]) < tol;
}

// ----------------------------------------------------------------------
inline bool
is_zero(const dealii::Tensor<1, 2, double>& t1)
{
  const double tol = 1e-16;
  return std::abs(t1[0]) < tol && std::abs(t1[1]) < tol;
}

}  // end namespace local_

template <int DIM>
class VelocityAngularIntegrator;

/**
 * @brief helper class for system matrix assembly
 *
 */
template <>
class VelocityAngularIntegrator<2>
{
 public:
  typedef double numeric_t;
  typedef dealii::Tensor<2, 2, numeric_t> T2_t;
  typedef dealii::Tensor<1, 2, numeric_t> T1_t;
  typedef std::pair<unsigned int, unsigned int> key_t;

 private:
  typedef std::map<key_t, numeric_t> map_S0_t;
  typedef std::map<key_t, T1_t> map_T1_t;
  typedef std::map<key_t, T2_t> map_T2_t;

 public:
  VelocityAngularIntegrator() { /* empty */}
  template <typename BASIS>
  void init(const BASIS& angular_basis);

  map_S0_t::const_iterator begin_s0() const;
  map_S0_t::const_iterator end_s0() const;
  map_T1_t::const_iterator begin_t1() const;
  map_T1_t::const_iterator end_t1() const;
  map_T2_t::const_iterator begin_t2() const;
  map_T2_t::const_iterator end_t2() const;

  const map_S0_t& get_s0() const { return ms0; }
  const map_T1_t& get_s1() const { return mt1; }
  const map_T1_t& get_t1() const { return mt1; }
  const map_T2_t& get_t2() const { return mt2; }

  //  void write_to_file(std::string fname) const;
 private:
  map_S0_t ms0;
  map_T1_t mt1;
  map_T2_t mt2;
};

// ---------------------------------------------------------------------------
template <typename BASIS>
void
VelocityAngularIntegrator<2>::init(const BASIS& angular_basis)
{
  auto make_t1 = [](int l1, int t1, int l2, int t2) {
    T1_t m1;
    m1[0] = trig_int(COS, 1, {(TRIG)t1, (TRIG)t2}, {l1, l2});
    m1[1] = trig_int(SIN, 1, {(TRIG)t1, (TRIG)t2}, {l1, l2});
    return m1;
  };

  auto make_t2 = [](int l1, int t1, int l2, int t2) {
    T2_t m2;
    for (int tp = 0; tp < 2; ++tp) {
      for (int t = 0; t < 2; ++t) {
        m2[tp][t] = trig_int((TRIG)tp, 1, {(TRIG)t, (TRIG)t1, (TRIG)t2}, {1, l1, l2});
      }
    }
    return m2;
  };

  for (auto it1 = angular_basis.begin(); it1 != angular_basis.end(); ++it1) {
    int l1 = it1->get_id().l;
    int t1 = it1->get_id().t;
    unsigned int ix1 = it1 - angular_basis.begin();
    for (auto it2 = angular_basis.begin(); it2 != angular_basis.end(); ++it2) {
      int l2 = it2->get_id().l;
      int t2 = it2->get_id().t;
      unsigned int ix2 = it2 - angular_basis.begin();
      // S0
      double s0 = trig_int((TRIG)t1, l1, {(TRIG)t2}, {l2});
      if (std::abs(s0) > 1e-16) ms0[std::make_pair(ix1, ix2)] = s0;
      // T1
      T1_t m1 = make_t1(l1, t1, l2, t2);
      if (!local_::is_zero(m1)) mt1[std::make_pair(ix1, ix2)] = m1;
      // T2
      T2_t m2 = make_t2(l1, t1, l2, t2);
      if (!local_::is_zero(m2)) mt2[std::make_pair(ix1, ix2)] = m2;
    }
  }
}

}  // end namespace boltzmann


/**
 * @file   main_advection_dir.cpp
 * @author Simon Pintarelli <simon@thinkpadX1>
 * @date   Wed Mar 30 18:30:47 2016
 *
 * @brief  solve advection equation separately for each direction (quad points)
 *
 *
 */

// system includes -----------------------------------------------
#include <boost/math/constants/constants.hpp>
#include <boost/program_options.hpp>
#include <iostream>
// own includes --------------------------------------------------
#include <base/eigen2hdf.hpp>
#include <base/init.hpp>
#include <base/timer.hpp>
#include <fft/fft2.hpp>
#include <fft/fft2_r2c.hpp>
#include <ridgelet/init_fftw.hpp>
#include <ridgelet/ridgelet_cell_array.hpp>
#include <ridgelet/ridgelet_frame.hpp>
#include <ridgelet/rt.hpp>

#include <omp.h>
#include <operators/operators.hpp>
#include <solver/cg.hpp>
#include <solver/ridgelet_solver.hpp>
#include <spectral/quadrature/gauss_hermite_roots.hpp>

using namespace std;
typedef FFTr2c<PlannerR2C> fft_t;
// TODO: check if RT coeffs are real valued in this case
typedef RT<double, RidgeletFrame, fft_t> RT_t;
typedef RT_t::array_t array_t;
typedef RT_t::complex_array_t complex_array_t;
typedef RT_t::rt_coeff_t rt_coeff_t;

int main(int argc, char* argv[])
{
  SOURCE_INFO();

  namespace po = boost::program_options;

  unsigned int Jx, Jy, rho_x, rho_y;
  double dt;
  unsigned int f;
  int K;

  unsigned int cg_maxit;
  double cg_reltol;

  po::options_description options("options");
  options.add_options()("help", "produce help message")
      ("Jx,i", po::value<unsigned int>(&Jx)->default_value(3), "Jx")
      ("Jy,j", po::value<unsigned int>(&Jy)->default_value(3), "Jy")
      ("rx,x", po::value<unsigned int>(&rho_x)->default_value(1), "rho_x")
      ("ry,y", po::value<unsigned int>(&rho_y)->default_value(1), "rho_x")
      ("dt,t", po::value<double>(&dt)->default_value(0.1), "dt")
      ("deg,K", po::value<int>(&K)->default_value(10), "poly. deg.")
      ("f", po::value<unsigned int>(&f)->default_value(2), "grid out factor")
      ("maxiter", po::value<unsigned int>(&cg_maxit)->default_value(40), "cg::maxiter")
      ("reltol", po::value<double>(&cg_reltol)->default_value(1e-4), "cg::reltol");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << options << "\n";
    return 0;
  }

  cout << "CMD::";
  for (int i = 0; i < argc; ++i) {
    cout << argv[i] << " ";
  }
  cout << "\n";

  cout << setw(20) << "Jx"
       << ": " << Jx << "\n"
       << setw(20) << "Jy"
       << ": " << Jy << "\n"
       << setw(20) << "rho_x"
       << ": " << rho_x << "\n"
       << setw(20) << "rho_y"
       << ": " << rho_y << "\n"
       << setw(20) << "K"
       << ": " << K << "\n"
       << setw(20) << "dt"
       << ": " << dt << "\n";

  RidgeletFrame rf(Jx, Jy, rho_x, rho_y);
  const unsigned int Nx = rf.Nx();  // #cols
  const unsigned int Ny = rf.Ny();  // #rows
  cout << "Nx: " << Nx << "\n";
  cout << "Ny: " << Ny << "\n";

  fft_t fft;
  init_fftw(fft, FFTW_MEASURE, rf);
  fft.get_plan().create_and_get_plan(f * Ny, f * Nx, PlannerR2C::INV);
  fft.get_plan().create_and_get_plan(f * Ny, f * Nx, PlannerR2C::FWD);

  std::vector<double> vi(K);
  boltzmann::gauss_hermite_roots(vi, K);

  Eigen::ArrayXd xi = Eigen::ArrayXd::LinSpaced(Nx + 1, 0, 1).segment(0, Nx);
  Eigen::ArrayXd yi = Eigen::ArrayXd::LinSpaced(Ny + 1, 0, 1).segment(0, Ny);

  array_t F =
      (xi.transpose().replicate(Ny, 1)).binaryExpr(yi.replicate(1, Nx), [](double x, double y) {
        return std::exp(-300 * (std::pow(x - 0.5, 2) + std::pow(y - 0.5, 2)));
      });

  RT_t rt(rf);

  complex_array_t Fh(Ny, Nx);
  fft.ft(Fh, F, false);
  typedef RidgeletCellArray<rt_coeff_t> rca_t;
  rca_t rt_cell_array(rf);
  auto& rt_coeffs = rt_cell_array.coeffs();
  rt.rt(rt_coeffs, Fh);

  hid_t file = H5Fcreate("advection_dir.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  cout << "write results to `advection_dir.h5`"
       << "\n";
  cout << "max thread: " << omp_get_max_threads() << "\n";
  cout << "RT_SOLVER::tol  : " << cg_reltol << "\n";
  cout << "RT_SOLVER::maxit: " << cg_maxit << "\n";

  eigen2hdf::save(file, "F", F);
  eigen2hdf::save(file, "Fh", Fh);
  cout << "dt:" << dt << "\n";
  const double Lx = 1;
  const double Ly = 1;

  Eigen::MatrixXi ITER_MAT(K, K);
  Eigen::MatrixXd RELRES_MAT(K, K);  // rel. residual
  Eigen::MatrixXd TIME_MAT(K, K);    // [GCycle]

#pragma omp parallel for
  for (int qx = 0; qx < K; ++qx) {
    for (int qy = 0; qy < K; ++qy) {
      const double vx = vi[qx];
      const double vy = vi[qy];
      AhAOp AhA(vx, vy, Lx, Ly, Nx, Ny, dt);
      // preconditioned operator
      RDTSCTimer timer;

      PTransportOp<RT_t> A(rt, AhA, vx, vy);
      TransportOperator T(vx, vy, Lx, Ly, Nx, Ny, dt);  // required for rhs
      // T'*Fh
      complex_array_t Bh(Ny, Nx);
      T.apply(Bh, Fh, true /* hermitian transpose */);
      rca_t b(rf);
      rt.rt(b.coeffs(), Bh);
      RidgeletSolver<rt_coeff_t> rt_solver(rf, vx, vy);
      rca_t x(rf);
      x.resize(rt_cell_array);
      x = rt_cell_array;
      timer.start();
      rt_solver.solve(x, A, b, cg_reltol, cg_maxit);
      auto nc_solve = timer.stop();

      RELRES_MAT(qx, qy) = rt_solver.relres();
      ITER_MAT(qx, qy) = rt_solver.iter();
      TIME_MAT(qx, qy) = nc_solve / 1e9;

      // #pragma omp critical
      //       {
      //         cout << "vx: " << vx << "\n";
      //         cout << "vy: " << vy << "\n";
      //         cout << "RidgeletSolver took: " << nc_solve / 1e9 << " Gcycles\n";
      //         cout << "cg::relres: " << rt_solver.relres() << "\n";
      //         cout << "cg::iter: " << rt_solver.iter() << "\n\n";
      //       }
      // write arrays to hdf5
      complex_array_t tmp(Ny, Nx);
      rt.irt(tmp, x.coeffs());
      array_t sol(f * Ny, f * Nx);
      complex_array_t solh(f * Ny, f * Nx);
      solh.setZero();
      ftcut(solh, Ny / 2, Nx / 2) = ftcut(tmp, Ny / 2, Nx / 2);
      // ftcut(solh, Ny, Nx) = tmp;
      fft.ift(sol, solh);
      char buf[256];
      std::sprintf(buf, "%d_%d", qx, qy);
#pragma omp critical
      {
        hid_t group = H5Gcreate(file, buf, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
        eigen2hdf::save(group, "sol", sol);
        eigen2hdf::save(group, "solh_ftcut", solh);
        eigen2hdf::save(group, "solh", tmp);
        H5Gclose(group);
      }
    }
  }

  eigen2hdf::save(file, "cg_relres", RELRES_MAT);
  eigen2hdf::save(file, "cg_iter", ITER_MAT);
  eigen2hdf::save(file, "cg_time", TIME_MAT);

  H5Fclose(file);

  return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IDIVCEIL_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IDIVCEIL_HPP_INCLUDED

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

 /*!

    @ingroup group-arithmetic
    Function object implementing idivceil capabilities

    Computes the integer conversion of the ceil of the division of its parameters.

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = idivceil(x, y);
    @endcode

    The code is similar to:

    @code
    as_integer_t<T> r = toints(ceil(x/y));
    @endcode

    If y is @ref Zero, it returns @ref Valmax (resp. @ref Valmin)
    if x is positive (resp. negative) and @ref Zero if x is @ref Zero.

    @see toints, ceil

  **/
  const boost::dispatch::functor<tag::idivceil_> idivceil = {};
} }
#endif

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

#endif


#include <gtest/gtest.h>
#include <string>
#include <stdexcept>
#include <Eigen/Dense>

// testing following API
#include "estimation/IEstimator.h"
#include "estimation/KalmanFilter.h"
#include "estimation/Input.h"
#include "estimation/InputValue.h"
#include "estimation/Output.h"
#include "estimation/OutputValue.h"

using namespace std;
using namespace estimation;

namespace KalmanFilterTest
{
  // -----------------------------------------
  // tests
  // -----------------------------------------
  TEST(KalmanFilterTest, initializationAndValidation)
  {
    KalmanFilter kf;

    // check setting required params
    EXPECT_THROW(kf.validate(), IEstimator::estimator_error);	// "STM missing"
    MatrixXd A_f(2,1); A_f << 1, 0;
    kf.setStateTransitionModel(A_f);
    EXPECT_THROW(kf.validate(), IEstimator::estimator_error);	// "OM missing"
    MatrixXd H(1,2); H << 1,0;
    kf.setObservationModel(H);
    EXPECT_THROW(kf.validate(), IEstimator::estimator_error);	// "PNC missing"
    MatrixXd Q(2,2); Q << 0.1,0,0,0.1;
    kf.setProcessNoiseCovariance(Q);
    EXPECT_THROW(kf.validate(), IEstimator::estimator_error);	// "MNC missing"
    MatrixXd R(1,1); R << 10;
    kf.setMeasurementNoiseCovariance(R);

    EXPECT_THROW(kf.validate(), IEstimator::estimator_error);	// "STM invalid size"
    MatrixXd A(2,2); A << 1,0,0,1;
    kf.setStateTransitionModel(A);
    EXPECT_NO_THROW(kf.validate());			// all required params given

    MatrixXd R_f(2,1); R_f << 10,1;
    kf.setMeasurementNoiseCovariance(R_f);
    EXPECT_THROW(kf.validate(), IEstimator::estimator_error);	// "MNC invalid size"
    kf.setMeasurementNoiseCovariance(R);
    MatrixXd H_f(3,1); H_f << 1,0,2;
    kf.setObservationModel(H_f);
    EXPECT_THROW(kf.validate(), IEstimator::estimator_error);	// "OM invalid size"
    kf.setObservationModel(H);

    // check setting optional params
    MatrixXd B_f(1,1); B_f << 0;
    kf.setControlInputModel(B_f);
    EXPECT_THROW(kf.validate(), IEstimator::estimator_error);	// "CIM invalid size"
    MatrixXd B(2,1); B << 0,0;
    kf.setControlInputModel(B);
    EXPECT_NO_THROW(kf.validate());

    VectorXd x(2); x << 0,1;
    kf.setInitialState(x);
    EXPECT_NO_THROW(kf.validate());

    // check initialization of output
    Output out = kf.getLastEstimate();
    OutputValue defaultOutVal;
    EXPECT_GT(out.size(), 0);
    EXPECT_DOUBLE_EQ(out.getValue(), defaultOutVal.getValue());
  }

  TEST(KalmanFilterTest, validationEffect)
  {
    KalmanFilter kf;

    MatrixXd A(1,1); A << 1;
    kf.setStateTransitionModel(A);
    MatrixXd H(1,1); H << 1;
    kf.setObservationModel(H);
    MatrixXd Q(1,1); Q << 0.1;
    kf.setProcessNoiseCovariance(Q);
    MatrixXd R(1,1); R << 10;
    kf.setMeasurementNoiseCovariance(R);

    // validate has an effect?
    InputValue measurement(1);
    Input in(measurement);
    Output out;

    EXPECT_THROW(out = kf.estimate(in), IEstimator::estimator_error);	// "not yet validated"

    EXPECT_NO_THROW(kf.validate());

    // changing a parameter -> KF must be re-validated
    kf.setProcessNoiseCovariance(Q);
    EXPECT_THROW(out = kf.estimate(in), IEstimator::estimator_error);	// "not yet validated"
    EXPECT_NO_THROW(kf.validate());

    // KF should now be released and return an estimate (should not be
    // the default)
    EXPECT_NO_THROW(out = kf.estimate(in));
    EXPECT_GT(out.size(), 0);
    EXPECT_EQ(kf.getState().size(), out.size());
  }

  TEST(KalmanFilterTest, functionality)
  {
    KalmanFilter kf;

    MatrixXd A(1,1); A << 1;
    kf.setStateTransitionModel(A);
    MatrixXd H(1,1); H << 1;
    kf.setObservationModel(H);
    MatrixXd Q(1,1); Q << 0.1;
    kf.setProcessNoiseCovariance(Q);
    MatrixXd R(1,1); R << 10;
    kf.setMeasurementNoiseCovariance(R);

    kf.validate();

    // check if the calculation of an estimate is correct
    InputValue measurement(1);
    Input in(measurement);
    Output out = kf.estimate(in);

    EXPECT_EQ(out.size(), 1);
    EXPECT_NEAR(out[0].getValue(), 0.0099, 0.0001);
    EXPECT_NEAR(out[0].getVariance(), 0.0990, 0.0001);

    in[0].setValue(5);
    EXPECT_NO_THROW(out = kf.estimate(in));

    EXPECT_NEAR(out[0].getValue(), 0.1073, 0.0001);
    EXPECT_NEAR(out[0].getVariance(), 0.1951, 0.0001);

    // missing measurement
    InputValue missingValue;
    Input inMissing(missingValue);
    EXPECT_NO_THROW(out = kf.estimate(inMissing));

    EXPECT_NEAR(out[0].getValue(), 0.1073, 0.0001);
    EXPECT_NEAR(out[0].getVariance(), 0.2866, 0.0001);

    // another example ---------------------------------
    KalmanFilter kf2;
    MatrixXd A2(2,2); A2 << 1,0.1,0,1;
    kf2.setStateTransitionModel(A2);
    MatrixXd H2(1,2); H2 << 0,1;
    kf2.setObservationModel(H2);
    MatrixXd Q2(2,2); Q2 << 0.1,0,0,0.1;
    kf2.setProcessNoiseCovariance(Q2);
    MatrixXd R2(1,1); R2 << 10;
    kf2.setMeasurementNoiseCovariance(R2);

    kf2.validate();

    in[0].setValue(1);
    EXPECT_NO_THROW(out = kf2.estimate(in));
    EXPECT_EQ(out.size(), 2);

    EXPECT_NEAR(out[0].getValue(), 0.0, 0.0001);
    EXPECT_NEAR(out[0].getVariance(), 0.1, 0.0001);
    EXPECT_NEAR(out[1].getValue(), 0.0099, 0.0001);
    EXPECT_NEAR(out[1].getVariance(), 0.0990, 0.0001);

    // add control input
    MatrixXd B2(2,1); B2 << 0,1;
    kf2.setControlInputModel(B2);	// needs to be validated
    InputValue ctrl(0.5);
    Input in_ctrl(ctrl);
    kf2.setControlInput(in_ctrl);

    in[0].setValue(5);
    EXPECT_NO_THROW(kf2.validate());
    EXPECT_NO_THROW(out = kf2.estimate(in));

    EXPECT_NEAR(out[0].getValue(), 0.0053, 0.0001);
    EXPECT_NEAR(out[0].getVariance(), 0.20098, 0.0001);
    EXPECT_NEAR(out[1].getValue(), 0.5975, 0.0001);
    EXPECT_NEAR(out[1].getVariance(), 0.1951, 0.0001);

    // pass control input with invalid size
    in_ctrl.add(ctrl);	// -> u.size = 2

    // setting invalid control input throws exception
    EXPECT_THROW(kf2.setControlInput(in_ctrl), length_error);

    // changing control input model works
    MatrixXd B3(2,2); B3 << 0,0,0,0;
    EXPECT_NO_THROW(kf2.setControlInputModel(B3));
    EXPECT_ANY_THROW(out = kf2.estimate(in));		// not validated
    EXPECT_NO_THROW(kf2.validate());

    // missing measurement
    EXPECT_NO_THROW(out = kf2.estimate(inMissing));

    EXPECT_NEAR(out[0].getValue(), 0.0651, 0.0001);
    EXPECT_NEAR(out[0].getVariance(), 0.3048, 0.0001);
    EXPECT_NEAR(out[1].getValue(), 0.5975, 0.0001);
    EXPECT_NEAR(out[1].getVariance(), 0.2867, 0.0001);
  }
}


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

/*!
 Copyright (C) 2005, 2006, 2007, 2009 StatPro Italia srl

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

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

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

// the only header you need to use QuantLib
#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 <boost/timer.hpp>
#include <iostream>
#include <iomanip>

using namespace QuantLib;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {

    Integer sessionId() { return 0; }

}
#endif

int main(int, char* []) {

    try {

        boost::timer timer;
        std::cout << std::endl;

        // set up dates
        Calendar calendar = TARGET();
        Date todaysDate(15, May, 1998);
        Date settlementDate(17, May, 1998);
        Settings::instance().evaluationDate() = todaysDate;

        // our options
        Option::Type type(Option::Put);
        Real underlying = 36;
        Real strike = 40;
        Spread dividendYield = 0.00;
        Rate riskFreeRate = 0.06;
        Volatility volatility = 0.20;
        Date maturity(17, May, 1999);
        DayCounter dayCounter = Actual365Fixed();

        std::cout << "Option type = "  << type << std::endl;
        std::cout << "Maturity = "        << maturity << std::endl;
        std::cout << "Underlying price = "        << underlying << std::endl;
        std::cout << "Strike = "                  << strike << std::endl;
        std::cout << "Risk-free interest rate = " << io::rate(riskFreeRate)
                  << std::endl;
        std::cout << "Dividend yield = " << io::rate(dividendYield)
                  << std::endl;
        std::cout << "Volatility = " << io::volatility(volatility)
                  << std::endl;
        std::cout << std::endl;
        std::string method;
        std::cout << std::endl ;

        // write column headings
        Size widths[] = { 35, 14, 14, 14 };
        std::cout << std::setw(widths[0]) << std::left << "Method"
                  << std::setw(widths[1]) << std::left << "European"
                  << std::setw(widths[2]) << std::left << "Bermudan"
                  << std::setw(widths[3]) << std::left << "American"
                  << std::endl;

        std::vector<Date> exerciseDates;
        for (Integer i=1; i<=4; i++)
            exerciseDates.push_back(settlementDate + 3*i*Months);

        boost::shared_ptr<Exercise> europeanExercise(
                                         new EuropeanExercise(maturity));

        boost::shared_ptr<Exercise> bermudanExercise(
                                         new BermudanExercise(exerciseDates));

        boost::shared_ptr<Exercise> americanExercise(
                                         new AmericanExercise(settlementDate,
                                                              maturity));

        Handle<Quote> underlyingH(
            boost::shared_ptr<Quote>(new SimpleQuote(underlying)));

        // bootstrap the yield/dividend/vol curves
        Handle<YieldTermStructure> flatTermStructure(
            boost::shared_ptr<YieldTermStructure>(
                new FlatForward(settlementDate, riskFreeRate, dayCounter)));
        Handle<YieldTermStructure> flatDividendTS(
            boost::shared_ptr<YieldTermStructure>(
                new FlatForward(settlementDate, dividendYield, dayCounter)));
        Handle<BlackVolTermStructure> flatVolTS(
            boost::shared_ptr<BlackVolTermStructure>(
                new BlackConstantVol(settlementDate, calendar, volatility,
                                     dayCounter)));
        boost::shared_ptr<StrikedTypePayoff> payoff(
                                        new PlainVanillaPayoff(type, strike));
        boost::shared_ptr<BlackScholesMertonProcess> bsmProcess(
                 new BlackScholesMertonProcess(underlyingH, flatDividendTS,
                                               flatTermStructure, flatVolTS));

        // options
        VanillaOption europeanOption(payoff, europeanExercise);
        VanillaOption bermudanOption(payoff, bermudanExercise);
        VanillaOption americanOption(payoff, americanExercise);

        // Analytic formulas:

        // Black-Scholes for European
        method = "Black-Scholes";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                                     new AnalyticEuropeanEngine(bsmProcess)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << "N/A"
                  << std::endl;

        // semi-analytic Heston for European
        method = "Heston semi-analytic";
        boost::shared_ptr<HestonProcess> hestonProcess(
            new HestonProcess(flatTermStructure, flatDividendTS,
                              underlyingH, volatility*volatility,
                              1.0, volatility*volatility, 0.001, 0.0));
        boost::shared_ptr<HestonModel> hestonModel(
                                              new HestonModel(hestonProcess));
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                                     new AnalyticHestonEngine(hestonModel)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << "N/A"
                  << std::endl;

        // semi-analytic Bates for European
        method = "Bates semi-analytic";
        boost::shared_ptr<BatesProcess> batesProcess(
            new BatesProcess(flatTermStructure, flatDividendTS,
                             underlyingH, volatility*volatility,
                             1.0, volatility*volatility, 0.001, 0.0,
                             1e-14, 1e-14, 1e-14));
        boost::shared_ptr<BatesModel> batesModel(new BatesModel(batesProcess));
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                                                new BatesEngine(batesModel)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << "N/A"
                  << std::endl;

        // Barone-Adesi and Whaley approximation for American
        method = "Barone-Adesi/Whaley";
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                       new BaroneAdesiWhaleyApproximationEngine(bsmProcess)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << "N/A"
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Bjerksund and Stensland approximation for American
        method = "Bjerksund/Stensland";
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BjerksundStenslandApproximationEngine(bsmProcess)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << "N/A"
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Integral
        method = "Integral";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                                             new IntegralEngine(bsmProcess)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << "N/A"
                  << std::endl;

        // Finite differences
        Size timeSteps = 801;
        method = "Finite differences";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                 new FDEuropeanEngine<CrankNicolson>(bsmProcess,
                                                     timeSteps,timeSteps-1)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                 new FDBermudanEngine<CrankNicolson>(bsmProcess,
                                                     timeSteps,timeSteps-1)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                 new FDAmericanEngine<CrankNicolson>(bsmProcess,
                                                     timeSteps,timeSteps-1)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Binomial method: Jarrow-Rudd
        method = "Binomial Jarrow-Rudd";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<JarrowRudd>(bsmProcess,timeSteps)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<JarrowRudd>(bsmProcess,timeSteps)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<JarrowRudd>(bsmProcess,timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;
        method = "Binomial Cox-Ross-Rubinstein";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BinomialVanillaEngine<CoxRossRubinstein>(bsmProcess,
                                                                   timeSteps)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BinomialVanillaEngine<CoxRossRubinstein>(bsmProcess,
                                                                   timeSteps)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BinomialVanillaEngine<CoxRossRubinstein>(bsmProcess,
                                                                   timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Binomial method: Additive equiprobabilities
        method = "Additive equiprobabilities";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<AdditiveEQPBinomialTree>(bsmProcess,
                                                                   timeSteps)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<AdditiveEQPBinomialTree>(bsmProcess,
                                                                   timeSteps)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<AdditiveEQPBinomialTree>(bsmProcess,
                                                                   timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Binomial method: Binomial Trigeorgis
        method = "Binomial Trigeorgis";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<Trigeorgis>(bsmProcess,timeSteps)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<Trigeorgis>(bsmProcess,timeSteps)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<Trigeorgis>(bsmProcess,timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Binomial method: Binomial Tian
        method = "Binomial Tian";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BinomialVanillaEngine<Tian>(bsmProcess,timeSteps)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BinomialVanillaEngine<Tian>(bsmProcess,timeSteps)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BinomialVanillaEngine<Tian>(bsmProcess,timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Binomial method: Binomial Leisen-Reimer
        method = "Binomial Leisen-Reimer";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
              new BinomialVanillaEngine<LeisenReimer>(bsmProcess,timeSteps)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
              new BinomialVanillaEngine<LeisenReimer>(bsmProcess,timeSteps)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
              new BinomialVanillaEngine<LeisenReimer>(bsmProcess,timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Binomial method: Binomial Joshi
        method = "Binomial Joshi";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                    new BinomialVanillaEngine<Joshi4>(bsmProcess,timeSteps)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                    new BinomialVanillaEngine<Joshi4>(bsmProcess,timeSteps)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                    new BinomialVanillaEngine<Joshi4>(bsmProcess,timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Monte Carlo Method: MC (crude)
        timeSteps = 1;
        method = "MC (crude)";
        Size mcSeed = 42;
        boost::shared_ptr<PricingEngine> mcengine1;
        mcengine1 = MakeMCEuropeanEngine<PseudoRandom>(bsmProcess)
            .withSteps(timeSteps)
            .withAbsoluteTolerance(0.02)
            .withSeed(mcSeed);
        europeanOption.setPricingEngine(mcengine1);
        // Real errorEstimate = europeanOption.errorEstimate();
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << "N/A"
                  << std::endl;

        // Monte Carlo Method: QMC (Sobol)
        method = "QMC (Sobol)";
        Size nSamples = 32768;  // 2^15

        boost::shared_ptr<PricingEngine> mcengine2;
        mcengine2 = MakeMCEuropeanEngine<LowDiscrepancy>(bsmProcess)
            .withSteps(timeSteps)
            .withSamples(nSamples);
        europeanOption.setPricingEngine(mcengine2);
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << "N/A"
                  << std::endl;

        // Monte Carlo Method: MC (Longstaff Schwartz)
        method = "MC (Longstaff Schwartz)";
        boost::shared_ptr<PricingEngine> mcengine3;
        mcengine3 = MakeMCAmericanEngine<PseudoRandom>(bsmProcess)
            .withSteps(100)
            .withAntitheticVariate()
            .withCalibrationSamples(4096)
            .withAbsoluteTolerance(0.02)
            .withSeed(mcSeed);
        americanOption.setPricingEngine(mcengine3);
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << "N/A"
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // End test
        double seconds = timer.elapsed();
        Integer hours = int(seconds/3600);
        seconds -= hours * 3600;
        Integer minutes = int(seconds/60);
        seconds -= minutes * 60;
        std::cout << " \nRun completed in ";
        if (hours > 0)
            std::cout << hours << " h ";
        if (hours > 0 || minutes > 0)
            std::cout << minutes << " m ";
        std::cout << std::fixed << std::setprecision(0)
                  << seconds << " s\n" << std::endl;
        return 0;

    } catch (std::exception& e) {
        std::cerr << e.what() << std::endl;
        return 1;
    } catch (...) {
        std::cerr << "unknown error" << std::endl;
        return 1;
    }
}


/** MIT License

Copyright (c) 2018 Benjamin Bercovici and Jay McMahon

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

/**
 * @file   RigidBodyKinematics.hpp
 * @Author Benjamin Bercovici (bebe0705@colorado.edu)
 * @date   July, 2017
 * @brief  Header of the RigidBodyKinematics libary
 *
 * Rigid Body Kinematics library implementating a handful of useful rigid body routines
 */

#ifndef RIGIDBODYKINEMATICS_HPP
#define RIGIDBODYKINEMATICS_HPP

#include <armadillo>

namespace RBK {

/**
Converts MRP to DCM
@param sigma MRP vector
@return dcm DCM matrix
*/
	arma::mat::fixed<3,3> mrp_to_dcm(const arma::vec::fixed<3> & mrp);

/**
Converts DCM to MRP.
@param dcm DCM
@param short_rot True if short rotation is desired (default), false otherwise
@return mrp MRP set
*/
	arma::vec::fixed<3> dcm_to_mrp(const arma::mat::fixed<3,3> & dcm, const bool short_rot = true);

/**
Converts Quaternions to MRP.
@param Q Unit Quaternion
@param short_rot True if short rotation is desired (default), false otherwise
@return mrp MRP set
*/
	arma::vec::fixed<3> quat_to_mrp(const arma::vec::fixed<4> & Q , const bool short_rot = true);

/**
Converts a set of 321 Euler angles to DCM
@param euler_angles 321 sequence of Euler angles (rad)
@return dcm DCM
*/
	arma::mat::fixed<3,3> euler321_to_dcm(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of (longitude,latitude) angles to DCM
@param (longitude,latitude) angles (rad)
@return dcm DCM
*/
	arma::mat::fixed<3,3> longitude_latitude_to_dcm(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 321 Euler angles to mrp
@param euler_angles 321 sequence of Euler angles (rad)
@return mrp MRP set
*/
	arma::vec::fixed<3> euler321_to_mrp(const arma::vec::fixed<3> & euler_angles);

/**
Computes the time derivative of the attitude set
assuming torque free rotational dynamics and MRP as attitude coordinate
@param[in] t Current time
@param[in] attitude_set mrp + angular velocities
@param[in] inertia inertia matrix
@param[in] L external torque (defaults to (0,0,0))
@return time derivative of the input attitude set
*/
	arma::vec::fixed<6> dXattitudedt(double t, const arma::vec::fixed<6> & attitude_set, const arma::mat::fixed<3,3> & inertia,
		const arma::vec::fixed<3> & L = arma::zeros<arma::vec>(3)) ;

/**
Computes the time derivative of the angular velocity set
@param[in] t Current time
@param[in] attitude_set mrp + angular velocities
@param[in] inertia inertia matrix
@param[in] L external torque (defaults to (0,0,0))
@return time derivative of the input angular velocity
*/
	arma::vec::fixed<3> domegadt(double t,
		const arma::vec::fixed<6> & attitude_set,
		const arma::mat::fixed<3,3> & inertia,
		const arma::vec::fixed<3> & L = arma::zeros<arma::vec>(3)) ;

/**
Computes the time derivative of a mrp set given
a corresponding angular velocity
@param t Current time
@param attitude_set mrp + angular velocities
@return time derivative of the input mrp
*/
	arma::vec::fixed<3> dmrpdt(double t, const arma::vec::fixed<6> & attitude_set );

/**
Returns the shadow set of the input mrp if crossing
surface is reached
@param mrp MRP set
@param force_switch if true, will force the switching of the MRP to its shadow without checking its norm
@return mrp or its shadow set

*/
	arma::vec::fixed<3> shadow_mrp(const arma::vec::fixed<3> & mrp, bool force_switch = false) ;

/**
Converts MRP to quaternions
@param sigma MRP vector
@return quat Unit quaternion
*/
	arma::vec::fixed<4> mrp_to_quat(const arma::vec::fixed<3> & mrp);

/**
Converts a set of 321 Euler angles to DCM
@param euler_angles 321 sequence of Euler angles (deg)
@return dcm DCM
*/
	arma::mat::fixed<3,3> euler321d_to_dcm(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 321 Euler angles to mrp
@param euler_angles 321 sequence of Euler angles (deg)
@return mrp MRP set
*/
	arma::vec::fixed<3> euler321d_to_mrp(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 313 Euler angles to DCM
@param euler_angles 313 sequence of Euler angles (deg)
@return dcm DCM
*/
	arma::mat::fixed<3,3> euler313d_to_dcm(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 313 Euler angles to mrp
@param euler_angles 313 sequence of Euler angles (deg)
@return mrp MRP set
*/
	arma::vec::fixed<3> euler313d_to_mrp(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 321 Euler angles expressed in degrees to DCM
@param euler_angles 321 sequence of Euler angles (deg)
@return dcm DCM
*/
	arma::mat::fixed<3,3> euler321d_to_dcm(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 321 Euler angles expressed in degrees to mrp
@param euler_angles 321 sequence of Euler angles (deg)
@return mrp MRP set
*/
	arma::vec::fixed<3> euler321d_to_mrp(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 313 Euler angles to mrp
@param euler_angles 313 sequence of Euler angles (rad)
@return mrp MRP set
*/
	arma::vec::fixed<3> euler313_to_mrp(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 313 Euler angles to DCM
@param euler_angles 313 sequence of Euler angles (rad)
@return dcm DCM
*/
	arma::mat::fixed<3,3> euler313_to_dcm(const arma::vec::fixed<3> & euler_angles);

/**
Returns the matrix tilde[x] of the linear operator v|---> cross(x,v)
@param vec 3-by-1 vector
@return M Skew-symmetric matrix of the said linear operator
*/
	arma::mat::fixed<3,3> tilde(const arma::vec::fixed<3> & vec);

/**
Matrix of the elemental rotation about the first axis of the current frame
@param angle Rotation angle (rad)
@return M Elemental rotation matrix
*/
	arma::mat::fixed<3,3> M1(const double angle);

/**
Matrix of the elemental rotation about the second axis of the current frame
@param angle Rotation angle (rad)
@return M Elemental rotation matrix
*/
	arma::mat::fixed<3,3> M2(const double angle);

/**
Matrix of the elemental rotation about the third axis of the current frame
@param angle Rotation angle (rad)
@return M Elemental rotation matrix
*/
	arma::mat::fixed<3,3> M3(const double angle);

/**
Converts a DCM to the corresponding set of Euler angles
@param m DCM
@return angles Sequence of 321 Euler angles angles [yaw,pitch,roll]
*/
	arma::vec::fixed<3> dcm_to_euler321(const arma::mat::fixed<3,3> & dcm);

/**
Converts a DCM to the corresponding set of Euler angles
@param m DCM
@return angles Sequence of 313 Euler angles angles [right ascension,inclination,longitude]
*/
	arma::vec::fixed<3> dcm_to_euler313(const arma::mat::fixed<3,3> & dcm);

/**
Converts a MRP to the corresponding set of Euler angles
@param sigma MRP vector
@return angles Sequence of 313 Euler angles angles [right ascension,inclination,longitude]
*/
	arma::vec::fixed<3> mrp_to_euler313(const arma::vec::fixed<3> & mrp);

/**
Converts a MRP to the corresponding set of Euler angles
@param sigma MRP vector
@return angles Sequence of 321 Euler angles angles [right ascension,inclination,longitude]
*/
	arma::vec::fixed<3> mrp_to_euler321(const arma::vec::fixed<3> & mrp);

/**
Converts a DCM to the corresponding set of Euler angles in degrees
@param m DCM
@return angles Sequence of 321 Euler angles angles [yaw,pitch,roll]
*/
	arma::vec::fixed<3> dcm_to_euler321d(const arma::mat::fixed<3,3> & dcm);

/**
Converts a DCM to the corresponding set of Euler angles in degrees
@param m DCM
@return angles Sequence of 313 Euler angles angles [right ascension,inclination,longitude]
*/
	arma::vec::fixed<3> dcm_to_euler313d(const arma::mat::fixed<3,3> & dcm);

/**
Converts a MRP to the corresponding set of Euler angles in degrees
@param sigma MRP vector
@return angles Sequence of 313 Euler angles angles [right ascension,inclination,longitude]
*/
	arma::vec::fixed<3> mrp_to_euler313d(const arma::vec::fixed<3> & mrp);

/**
Converts a MRP to the corresponding set of Euler angles in degrees
@param sigma MRP vector
@return angles Sequence of 321 Euler angles angles [right ascension,inclination,longitude]
*/
	arma::vec::fixed<3> mrp_to_euler321d(const arma::vec::fixed<3> & mrp);

/**
Converts a DCM to a quaternion corresponding to the short-path rotation
@param dcm DCM
@return Q Unit quaternion
*/
	arma::vec::fixed<4> dcm_to_quat(const arma::mat::fixed<3,3> & dcm) ;

/**
Converts a dcm to the principal rotation vector
@param dcm DCM
@return prv Principal rotation vector
*/
	arma::vec::fixed<3> dcm_to_prv(const arma::mat::fixed<3,3> & dcm) ;

/**
Converts a PRV to the corresponding DCM
@param prv Principal rotation vector
@return DCM
*/

	arma::mat::fixed<3,3> prv_to_dcm(const arma::vec::fixed<3> & prv);

/**
Converts a PRV to a well-behaved MRP set
@param prv Principal rotation vector
@return MRP set
*/

	arma::vec::fixed<3> prv_to_mrp(const arma::vec::fixed<3> & prv);

/**
Returns the B matrix in the evaluation of the MRP's time derivative (sigma_dot = 1/4 * Bmat(sigma) * omega)
@param mrp MRP set
@return instantiated B mtrix
*/
	arma::mat::fixed<3,3> Bmat(const arma::vec::fixed<3> & mrp);

/**
Returns the partial derivative of mrp_dot with respect to the mrp
@param attitude_set attitude set comprised of the mrp set and its associated angular velocity
@return partial derivative of mrp_dot with respect to the mrp
*/
	arma::mat::fixed<3,3> partial_mrp_dot_partial_mrp(const arma::vec::fixed<6> & attitude_set);

}

#endif


#ifndef MOCHIMOCHI_AROW_HPP_
#define MOCHIMOCHI_AROW_HPP_

#include <Eigen/Dense>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <fstream>
#include "../../functions/enumerate.hpp"

class AROW {
private :
  const std::size_t kDim;
  const double kR;

private :
  Eigen::VectorXd _covariances;
  Eigen::VectorXd _means;

public :
  AROW(const std::size_t dim, const double r)
    : kDim(dim),
      kR(r),
      _covariances(Eigen::VectorXd::Ones(kDim)),
      _means(Eigen::VectorXd::Zero(kDim)) {

    static_assert(std::numeric_limits<decltype(dim)>::max() > 0, "Dimension Error. (Dimension > 0)");
    static_assert(std::numeric_limits<decltype(r)>::max() > 0, "Hyper Parameter Error. (r > 0)");
    assert(dim > 0);
    assert(r > 0);

  }

  virtual ~AROW() { }

private :

  double suffer_loss(const double margin, const int label) const {
    return margin * label;
  }

  double compute_margin(const Eigen::VectorXd& x) const {
    return _means.dot(x);
  }

  double compute_confidence(const Eigen::VectorXd& feature) const {
    auto confidence = 0.0;
    functions::enumerate(feature.data(), feature.data() + feature.size(), 0,
                         [&](const int index, const double value) {
                           confidence += _covariances[index] * value * value;
                         });
    return confidence;
  }

public :

  bool update(const Eigen::VectorXd& feature, const int label) {
    const auto margin = compute_margin(feature);

    if (suffer_loss(margin, label) >= 1.0) { return false; }

    const auto confidence = compute_confidence(feature);
    const auto beta = 1.0 / (confidence + kR);
    const auto alpha = std::max(0.0, 1.0 - label * margin) * beta;

    functions::enumerate(feature.data(), feature.data() + feature.size(), 0,
                         [&](const int index, const double value) {
                           const auto v = _covariances[index] * value;
                           _means[index] += alpha * label * v;
                           _covariances[index] -= beta * v * v;
                         });
    return true;
  }

  int predict(const Eigen::VectorXd& x) const {
    return compute_margin(x) > 0.0 ? 1 : -1;
  }

  Eigen::VectorXd get_means(void) const {
    return _means;
  }

  void save(const std::string& filename) {
    std::ofstream ofs(filename);
    assert(ofs);
    boost::archive::text_oarchive oa(ofs);
    oa << *this;
    ofs.close();
  }

  void load(const std::string& filename) {
    std::ifstream ifs(filename);
    assert(ifs);
    boost::archive::text_iarchive ia(ifs);
    ia >> *this;
    ifs.close();
  }

private :
  friend class boost::serialization::access;
  BOOST_SERIALIZATION_SPLIT_MEMBER();
  template <class Archive>
  void save(Archive& ar, const unsigned int version) const {
    std::vector<double> covariances_vector(_covariances.data(), _covariances.data() + _covariances.size());
    std::vector<double> means_vector(_means.data(), _means.data() + _means.size());
    ar & boost::serialization::make_nvp("covariances", covariances_vector);
    ar & boost::serialization::make_nvp("means", means_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
    ar & boost::serialization::make_nvp("r", const_cast<double&>(kR));
  }

  template <class Archive>
  void load(Archive& ar, const unsigned int version) {
    std::vector<double> covariances_vector;
    std::vector<double> means_vector;
    ar & boost::serialization::make_nvp("covariances", covariances_vector);
    ar & boost::serialization::make_nvp("means", means_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
    ar & boost::serialization::make_nvp("r", const_cast<double&>(kR));
    _covariances = Eigen::Map<Eigen::VectorXd>(&covariances_vector[0], covariances_vector.size());
    _means = Eigen::Map<Eigen::VectorXd>(&means_vector[0], means_vector.size());
  }
};

#endif //MOCHIMOCHI_AROW_HPP_


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_random.h>
#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/optimization/optimization_compute_index_reordering.h>
#include <OpenTissue/core/math/optimization/non_smooth_newton/optimization_compute_partitioned_jacobian.h>
#include <OpenTissue/core/math/optimization/optimization_make_mbd_bounds.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_math_big_non_smooth_newton_compute_partitioned_jacobian);

BOOST_AUTO_TEST_CASE(test_case)
{
  typedef ublas::vector<size_t>   idx_vector_type;
  typedef ublas::vector<double>   vector_type;
  typedef ublas::compressed_matrix<double>   matrix_type;
  typedef OpenTissue::math::ValueTraits<double> value_traits;
  typedef double real_type;
  typedef size_t size_type;

  matrix_type A;
  vector_type mu,lo,hi;
  A.resize(10,10,false);
  mu.resize(10,false);
  lo.resize(10,false);
  hi.resize(10,false);

  OpenTissue::math::Random<double> value(0.0,1.0);
  for(size_t i=0;i<A.size1();++i)
  {
    mu(i) = value();
    lo(i) = value_traits::zero();
    hi(i) = value_traits::infinity();
    for(size_t j=0;j<A.size2();++j)
      A(i,j) = value();
  }

  idx_vector_type bitmask;

  bitmask.resize(10,false);

  static size_t const in_lower  = 1;
  static size_t const in_upper  = 2;
  static size_t const in_active = 4;

  bitmask(0) = in_upper;   // pi = inf
  bitmask(1) = in_lower;   // pi = 0
  bitmask(2) = in_active;  // pi = 0
  bitmask(3) = in_lower;   // pi = inf
  bitmask(4) = in_active;  // pi = 3
  bitmask(5) = in_upper;   // pi = 3
  bitmask(6) = in_active;  // pi = inf
  bitmask(7) = in_upper;   // pi = 6
  bitmask(8) = in_lower;   // pi = 6
  bitmask(9) = in_active;  // pi = inf

  idx_vector_type old2new;
  idx_vector_type new2old;

  OpenTissue::math::optimization::compute_index_reordering( bitmask, old2new, new2old );

  idx_vector_type pi;
  pi.resize(10,false);
  size_type nodep = OpenTissue::math::detail::highest<size_t>();
  pi(0) = nodep;
  pi(1) = 0;
  pi(2) = 0;
  pi(3) = nodep;
  pi(4) = 3;
  pi(5) = 3;
  pi(6) = nodep;
  pi(7) = 6;
  pi(8) = 6;
  pi(9) = nodep;

  matrix_type A_aa;
  matrix_type A_ab;
  matrix_type C;
  matrix_type D;

  OpenTissue::math::optimization::detail::compute_partitioned_jacobian(
    A
    , OpenTissue::math::optimization::make_lower_mbd_bounds( pi, mu, lo )
    , OpenTissue::math::optimization::make_upper_mbd_bounds( pi, mu, hi )
    ,  bitmask
    , old2new
    ,  4
    ,  6
    ,  A_aa
    ,  A_ab
    ,  C
    ,  D
    );

  double tol = 0.01;

  // old : new   set      old : new
  //  2 <-> 0    active
  //  4 <-> 1    active
  //  6 <-> 2    active
  //  9 <-> 3    active
  //  1 <-> 4    lower      0 -> 7         (4,4) (4,7)     ->   D(0,0) D(0,3)
  //  3 <-> 5    lower      -              (5,5)           ->   D(1,1)
  //  8 <-> 6    lower      6 -> 2         (6,6) (6,2)     ->   D(2,2) C(2,2)
  //  0 <-> 7    upper      -              (7,7)           ->   D(3,3)
  //  5 <-> 8    upper      3 -> 5         (8,8) (8,5)     ->   D(4,4) D(4,1)
  //  7 <-> 9    upper      6 -> 2         (9,9) (9,2)     ->   D(5,5) C(5,2)

  BOOST_CHECK( A_aa.size1()==4 );
  BOOST_CHECK( A_aa.size2()==4 );
  for(size_type i = 0;i<4;++i)
  {
    for(size_type j = 0;j<4;++j)
    {
      BOOST_CHECK_CLOSE( double( A_aa(i,j) ), double( A(new2old(i),new2old(j)) ), tol );
    }
  }

  BOOST_CHECK( A_ab.size1()==4 );
  BOOST_CHECK( A_ab.size2()==6 );
  for(size_type i = 0;i<4;++i)
  {
    for(size_type j = 0;j<6;++j)
    {
      BOOST_CHECK_CLOSE( double( A_ab(i,j) ), double( A(new2old(i),new2old(j+4)) ), tol );
    }
  }

  BOOST_CHECK( C.size1()==6 );
  BOOST_CHECK( C.size2()==4 );
  BOOST_CHECK_CLOSE( double( C(0,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(0,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(0,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(0,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(1,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(1,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(1,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(1,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(2,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(2,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(2,2) ), double( mu(8) ), tol );
  BOOST_CHECK_CLOSE( double( C(2,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(4,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(4,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(4,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(4,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(5,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(5,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( C(5,2) ), double( -mu(7) ), tol );
  BOOST_CHECK_CLOSE( double( C(5,3) ), double( 0.0 ), tol );

  BOOST_CHECK( D.size1()==6 );
  BOOST_CHECK( D.size2()==6 );

  BOOST_CHECK_CLOSE( double( D(0,0) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(0,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(0,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(0,3) ), double( mu(1) ), tol );
  BOOST_CHECK_CLOSE( double( D(0,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(0,5) ), double( 0.0 ), tol );

  BOOST_CHECK_CLOSE( double( D(1,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(1,1) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(1,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(1,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(1,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(1,5) ), double( 0.0 ), tol );

  BOOST_CHECK_CLOSE( double( D(2,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(2,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(2,2) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(2,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(2,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(2,5) ), double( 0.0 ), tol );

  BOOST_CHECK_CLOSE( double( D(3,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(3,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(3,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(3,3) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(3,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(3,5) ), double( 0.0 ), tol );

  BOOST_CHECK_CLOSE( double( D(4,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(4,1) ), double( -mu(5) ), tol );
  BOOST_CHECK_CLOSE( double( D(4,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(4,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(4,4) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(4,5) ), double( 0.0 ), tol );

  BOOST_CHECK_CLOSE( double( D(5,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(5,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(5,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(5,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(5,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( D(5,5) ), double( 1.0 ), tol );

}

BOOST_AUTO_TEST_SUITE_END();


#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>
#include <sstream>
#include <vector>
#include <cstddef>
#include <iostream>
#include <map>
#include <string>
#include <utility>

std::vector<float> fibonacci() {
    std::vector<float> ret(8);
    ret[0] = 0;
    ret[1] = 1;

    for (std::size_t s{2}; s < ret.size(); ++s)
        ret[s] = ret[s - 1] + ret[s - 2];

    return ret;
}

BOOST_DATA_TEST_CASE(
    test1,
    boost::unit_test::data::make(fibonacci()),
    array_element)
{
    std::cout << "test 1: " << array_element << std::endl;
    BOOST_TEST(array_element <= 13);
}

std::map<std::string, float> vect_2_str(std::vector<float> v) {
    std::map<std::string, float> out{};
    for (std::size_t s{0}; s < v.size(); ++s) {
        std::ostringstream o{};
        o << v[s];
        out[o.str()] = v[s];
    }
    return out;
}

typedef std::pair<const std::string, float> pair_map_t;
BOOST_TEST_DONT_PRINT_LOG_VALUE(pair_map_t)

BOOST_DATA_TEST_CASE(
    test2,
    boost::unit_test::data::make(vect_2_str(fibonacci())),
    array_element)
{
    std::cout << "test 2: \""
              << array_element.first << "\", " << array_element.second
              << std::endl;
    BOOST_TEST(array_element.second <= 13);
}