// Jacobi theta functions
// Copyright Evan Miller 2020
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0. (See accompanying file
// LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Four main theta functions with various flavors of parameterization,
// floating-point policies, and bonus "minus 1" versions of functions 3 and 4
// designed to preserve accuracy for small q. Twenty-four C++ functions are
// provided in all.
//
// The functions take a real argument z and a parameter known as q, or its close
// relative tau.
//
// The mathematical functions are best understood in terms of their Fourier
// series. Using the q parameterization, and summing from n = 0 to INF:
//
// theta_1(z,q) = 2 SUM (-1)^n * q^(n+1/2)^2 * sin((2n+1)z)
// theta_2(z,q) = 2 SUM q^(n+1/2)^2 * cos((2n+1)z)
// theta_3(z,q) = 1 + 2 SUM q^n^2 * cos(2nz)
// theta_4(z,q) = 1 + 2 SUM (-1)^n * q^n^2 * cos(2nz)
//
// Appropriately multiplied and divided, these four theta functions can be used
// to implement the famous Jacabi elliptic functions - but this is not really
// recommended, as the existing Boost implementations are likely faster and
// more accurate.  More saliently, setting z = 0 on the fourth theta function
// will produce the limiting CDF of the Kolmogorov-Smirnov distribution, which
// is this particular implementation's raison d'etre.
//
// Separate C++ functions are provided for q and for tau. The main q functions are:
//
// template <class T> inline T jacobi_theta1(T z, T q);
// template <class T> inline T jacobi_theta2(T z, T q);
// template <class T> inline T jacobi_theta3(T z, T q);
// template <class T> inline T jacobi_theta4(T z, T q);
//
// The parameter q, also known as the nome, is restricted to the domain (0, 1),
// and will throw a domain error otherwise.
//
// The equivalent functions that use tau instead of q are:
//
// template <class T> inline T jacobi_theta1tau(T z, T tau);
// template <class T> inline T jacobi_theta2tau(T z, T tau);
// template <class T> inline T jacobi_theta3tau(T z, T tau);
// template <class T> inline T jacobi_theta4tau(T z, T tau);
//
// Mathematically, q and tau are related by:
//
// q = exp(i PI*Tau)
//
// However, the tau in the equation above is *not* identical to the tau in the function
// signature. Instead, `tau` is the imaginary component of tau. Mathematically, tau can
// be complex - but practically, most applications call for a purely imaginary tau.
// Rather than provide a full complex-number API, the author decided to treat the
// parameter `tau` as an imaginary number. So in computational terms, the
// relationship between `q` and `tau` is given by:
//
// q = exp(-constants::pi<T>() * tau)
//
// The tau versions are provided for the sake of accuracy, as well as conformance
// with common notation. If your q is an exponential, you are better off using
// the tau versions, e.g.
//
// jacobi_theta1(z, exp(-a)); // rather poor accuracy
// jacobi_theta1tau(z, a / constants::pi<T>()); // better accuracy
//
// Similarly, if you have a precise (small positive) value for the complement
// of q, you can obtain a more precise answer overall by passing the result of
// `log1p` to the tau parameter:
//
// jacobi_theta1(z, 1-q_complement); // precision lost in subtraction
// jacobi_theta1tau(z, -log1p(-q_complement) / constants::pi<T>()); // better!
//
// A third quartet of functions are provided for improving accuracy in cases
// where q is small, specifically |q| < exp(-PI) = 0.04 (or, equivalently, tau
// greater than unity). In this domain of q values, the third and fourth theta
// functions always return values close to 1. So the following "m1" functions
// are provided, similar in spirit to `expm1`, which return one less than their
// regular counterparts:
//
// template <class T> inline T jacobi_theta3m1(T z, T q);
// template <class T> inline T jacobi_theta4m1(T z, T q);
// template <class T> inline T jacobi_theta3m1tau(T z, T tau);
// template <class T> inline T jacobi_theta4m1tau(T z, T tau);
//
// Note that "m1" versions of the first and second theta would not be useful,
// as their ranges are not confined to a neighborhood around 1 (see the Fourier
// transform representations above).
//
// Finally, the twelve functions above are each available with a third Policy
// argument, which can be used to define a custom epsilon value. These Policy
// versions bring the total number of functions provided by jacobi_theta.hpp
// to twenty-four.
//
// See:
// https://mathworld.wolfram.com/JacobiThetaFunctions.html
// https://dlmf.nist.gov/20

#ifndef BOOST_MATH_JACOBI_THETA_HPP
#define BOOST_MATH_JACOBI_THETA_HPP

#include <boost/math/tools/complex.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/math/tools/promotion.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/constants/constants.hpp>

namespace boost{ namespace math{

// Simple functions - parameterized by q
template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta1(T z, U q);
template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta2(T z, U q);
template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta3(T z, U q);
template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta4(T z, U q);

// Simple functions - parameterized by tau (assumed imaginary)
// q = exp(i*PI*TAU)
// tau = -log(q)/PI
template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta1tau(T z, U tau);
template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta2tau(T z, U tau);
template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta3tau(T z, U tau);
template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta4tau(T z, U tau);

// Minus one versions for small q / large tau
template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta3m1(T z, U q);
template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta4m1(T z, U q);
template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta3m1tau(T z, U tau);
template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta4m1tau(T z, U tau);

// Policied versions - parameterized by q
template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta1(T z, U q, const Policy& pol);
template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta2(T z, U q, const Policy& pol);
template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta3(T z, U q, const Policy& pol);
template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta4(T z, U q, const Policy& pol);

// Policied versions - parameterized by tau
template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta1tau(T z, U tau, const Policy& pol);
template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta2tau(T z, U tau, const Policy& pol);
template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta3tau(T z, U tau, const Policy& pol);
template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta4tau(T z, U tau, const Policy& pol);

// Policied m1 functions
template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta3m1(T z, U q, const Policy& pol);
template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta4m1(T z, U q, const Policy& pol);
template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta3m1tau(T z, U tau, const Policy& pol);
template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta4m1tau(T z, U tau, const Policy& pol);

// Compare the non-oscillating component of the delta to the previous delta.
// Both are assumed to be non-negative.
template <class RealType>
inline bool
_jacobi_theta_converged(RealType last_delta, RealType delta, RealType eps) {
    return delta == 0.0 || delta < eps*last_delta;
}

template <class RealType>
inline RealType
_jacobi_theta_sum(RealType tau, RealType z_n, RealType z_increment, RealType eps) {
    BOOST_MATH_STD_USING
    RealType delta = 0, partial_result = 0;
    RealType last_delta = 0;

    do {
        last_delta = delta;
        delta = exp(-tau*z_n*z_n/constants::pi<RealType>());
        partial_result += delta;
        z_n += z_increment;
    } while (!_jacobi_theta_converged(last_delta, delta, eps));

    return partial_result;
}

// The following _IMAGINARY theta functions assume imaginary z and are for
// internal use only. They are designed to increase accuracy and reduce the
// number of iterations required for convergence for large |q|. The z argument
// is scaled by tau, and the summations are rewritten to be double-sided
// following DLMF 20.13.4 and 20.13.5. The return values are scaled by
// exp(-tau*z^2/Pi)/sqrt(tau).
//
// These functions are triggered when tau < 1, i.e. |q| > exp(-Pi) = 0.043
//
// Note that jacobi_theta4 uses the imaginary version of jacobi_theta2 (and
// vice-versa). jacobi_theta1 and jacobi_theta3 use the imaginary versions of
// themselves, following DLMF 20.7.30 - 20.7.33.
template <class RealType, class Policy>
inline RealType
_IMAGINARY_jacobi_theta1tau(RealType z, RealType tau, const Policy&) {
    BOOST_MATH_STD_USING
    RealType eps = policies::get_epsilon<RealType, Policy>();
    RealType result = RealType(0);

    // n>=0 even
    result -= _jacobi_theta_sum(tau, RealType(z + constants::half_pi<RealType>()), constants::two_pi<RealType>(), eps);
    // n>0 odd
    result += _jacobi_theta_sum(tau, RealType(z + constants::half_pi<RealType>() + constants::pi<RealType>()), constants::two_pi<RealType>(), eps);
    // n<0 odd
    result += _jacobi_theta_sum(tau, RealType(z - constants::half_pi<RealType>()), RealType (-constants::two_pi<RealType>()), eps);
    // n<0 even
    result -= _jacobi_theta_sum(tau, RealType(z - constants::half_pi<RealType>() - constants::pi<RealType>()), RealType (-constants::two_pi<RealType>()), eps);

    return result * sqrt(tau);
}

template <class RealType, class Policy>
inline RealType
_IMAGINARY_jacobi_theta2tau(RealType z, RealType tau, const Policy&) {
    BOOST_MATH_STD_USING
    RealType eps = policies::get_epsilon<RealType, Policy>();
    RealType result = RealType(0);

    // n>=0
    result += _jacobi_theta_sum(tau, RealType(z + constants::half_pi<RealType>()), constants::pi<RealType>(), eps);
    // n<0
    result += _jacobi_theta_sum(tau, RealType(z - constants::half_pi<RealType>()), RealType (-constants::pi<RealType>()), eps);

    return result * sqrt(tau);
}

template <class RealType, class Policy>
inline RealType
_IMAGINARY_jacobi_theta3tau(RealType z, RealType tau, const Policy&) {
    BOOST_MATH_STD_USING
    RealType eps = policies::get_epsilon<RealType, Policy>();
    RealType result = 0;

    // n=0
    result += exp(-z*z*tau/constants::pi<RealType>());
    // n>0
    result += _jacobi_theta_sum(tau, RealType(z + constants::pi<RealType>()), constants::pi<RealType>(), eps);
    // n<0
    result += _jacobi_theta_sum(tau, RealType(z - constants::pi<RealType>()), RealType(-constants::pi<RealType>()), eps);

    return result * sqrt(tau);
}

template <class RealType, class Policy>
inline RealType
_IMAGINARY_jacobi_theta4tau(RealType z, RealType tau, const Policy&) {
    BOOST_MATH_STD_USING
    RealType eps = policies::get_epsilon<RealType, Policy>();
    RealType result = 0;

    // n = 0
    result += exp(-z*z*tau/constants::pi<RealType>());

    // n > 0 odd
    result -= _jacobi_theta_sum(tau, RealType(z + constants::pi<RealType>()), constants::two_pi<RealType>(), eps);
    // n < 0 odd
    result -= _jacobi_theta_sum(tau, RealType(z - constants::pi<RealType>()), RealType (-constants::two_pi<RealType>()), eps);
    // n > 0 even
    result += _jacobi_theta_sum(tau, RealType(z + constants::two_pi<RealType>()), constants::two_pi<RealType>(), eps);
    // n < 0 even
    result += _jacobi_theta_sum(tau, RealType(z - constants::two_pi<RealType>()), RealType (-constants::two_pi<RealType>()), eps);

    return result * sqrt(tau);
}

// First Jacobi theta function (Parameterized by tau - assumed imaginary)
// = 2 * SUM (-1)^n * exp(i*Pi*Tau*(n+1/2)^2) * sin((2n+1)z)
template <class RealType, class Policy>
inline RealType
jacobi_theta1tau_imp(RealType z, RealType tau, const Policy& pol, const char *function)
{
    BOOST_MATH_STD_USING
    unsigned n = 0;
    RealType eps = policies::get_epsilon<RealType, Policy>();
    RealType q_n = 0, last_q_n, delta, result = 0;

    if (tau <= 0.0)
        return policies::raise_domain_error<RealType>(function,
                "tau must be greater than 0 but got %1%.", tau, pol);

    if (abs(z) == 0.0)
        return result;

    if (tau < 1.0) {
        z = fmod(z, constants::two_pi<RealType>());
        while (z > constants::pi<RealType>()) {
            z -= constants::two_pi<RealType>();
        }
        while (z < -constants::pi<RealType>()) {
            z += constants::two_pi<RealType>();
        }

        return _IMAGINARY_jacobi_theta1tau(z, RealType(1/tau), pol);
    }

    do {
        last_q_n = q_n;
        q_n = exp(-tau * constants::pi<RealType>() * RealType(n + 0.5)*RealType(n + 0.5) );
        delta = q_n * sin(RealType(2*n+1)*z);
        if (n%2)
            delta = -delta;

        result += delta + delta;
        n++;
    } while (!_jacobi_theta_converged(last_q_n, q_n, eps));

    return result;
}

// First Jacobi theta function (Parameterized by q)
// = 2 * SUM (-1)^n * q^(n+1/2)^2 * sin((2n+1)z)
template <class RealType, class Policy>
inline RealType
jacobi_theta1_imp(RealType z, RealType q, const Policy& pol, const char *function) {
    BOOST_MATH_STD_USING
    if (q <= 0.0 || q >= 1.0) {
        return policies::raise_domain_error<RealType>(function,
                "q must be greater than 0 and less than 1 but got %1%.", q, pol);
    }
    return jacobi_theta1tau_imp(z, RealType (-log(q)/constants::pi<RealType>()), pol, function);
}

// Second Jacobi theta function (Parameterized by tau - assumed imaginary)
// = 2 * SUM exp(i*Pi*Tau*(n+1/2)^2) * cos((2n+1)z)
template <class RealType, class Policy>
inline RealType
jacobi_theta2tau_imp(RealType z, RealType tau, const Policy& pol, const char *function)
{
    BOOST_MATH_STD_USING
    unsigned n = 0;
    RealType eps = policies::get_epsilon<RealType, Policy>();
    RealType q_n = 0, last_q_n, delta, result = 0;

    if (tau <= 0.0) {
        return policies::raise_domain_error<RealType>(function,
                "tau must be greater than 0 but got %1%.", tau, pol);
    } else if (tau < 1.0 && abs(z) == 0.0) {
        return jacobi_theta4tau(z, 1/tau, pol) / sqrt(tau);
    } else if (tau < 1.0) { // DLMF 20.7.31
        z = fmod(z, constants::two_pi<RealType>());
        while (z > constants::pi<RealType>()) {
            z -= constants::two_pi<RealType>();
        }
        while (z < -constants::pi<RealType>()) {
            z += constants::two_pi<RealType>();
        }

        return _IMAGINARY_jacobi_theta4tau(z, RealType(1/tau), pol);
    }

    do {
        last_q_n = q_n;
        q_n = exp(-tau * constants::pi<RealType>() * RealType(n + 0.5)*RealType(n + 0.5));
        delta = q_n * cos(RealType(2*n+1)*z);
        result += delta + delta;
        n++;
    } while (!_jacobi_theta_converged(last_q_n, q_n, eps));

    return result;
}

// Second Jacobi theta function, parameterized by q
// = 2 * SUM q^(n+1/2)^2 * cos((2n+1)z)
template <class RealType, class Policy>
inline RealType
jacobi_theta2_imp(RealType z, RealType q, const Policy& pol, const char *function) {
    BOOST_MATH_STD_USING
    if (q <= 0.0 || q >= 1.0) {
        return policies::raise_domain_error<RealType>(function,
                "q must be greater than 0 and less than 1 but got %1%.", q, pol);
    }
    return jacobi_theta2tau_imp(z, RealType (-log(q)/constants::pi<RealType>()), pol, function);
}

// Third Jacobi theta function, minus one (Parameterized by tau - assumed imaginary)
// This function preserves accuracy for small values of q (i.e. |q| < exp(-Pi) = 0.043)
// For larger values of q, the minus one version usually won't help.
// = 2 * SUM exp(i*Pi*Tau*(n)^2) * cos(2nz)
template <class RealType, class Policy>
inline RealType
jacobi_theta3m1tau_imp(RealType z, RealType tau, const Policy& pol)
{
    BOOST_MATH_STD_USING

    RealType eps = policies::get_epsilon<RealType, Policy>();
    RealType q_n = 0, last_q_n, delta, result = 0;
    unsigned n = 1;

    if (tau < 1.0)
        return jacobi_theta3tau(z, tau, pol) - RealType(1);

    do {
        last_q_n = q_n;
        q_n = exp(-tau * constants::pi<RealType>() * RealType(n)*RealType(n));
        delta = q_n * cos(RealType(2*n)*z);
        result += delta + delta;
        n++;
    } while (!_jacobi_theta_converged(last_q_n, q_n, eps));

    return result;
}

// Third Jacobi theta function, parameterized by tau
// = 1 + 2 * SUM exp(i*Pi*Tau*(n)^2) * cos(2nz)
template <class RealType, class Policy>
inline RealType
jacobi_theta3tau_imp(RealType z, RealType tau, const Policy& pol, const char *function)
{
    BOOST_MATH_STD_USING
    if (tau <= 0.0) {
        return policies::raise_domain_error<RealType>(function,
                "tau must be greater than 0 but got %1%.", tau, pol);
    } else if (tau < 1.0 && abs(z) == 0.0) {
        return jacobi_theta3tau(z, RealType(1/tau), pol) / sqrt(tau);
    } else if (tau < 1.0) { // DLMF 20.7.32
        z = fmod(z, constants::pi<RealType>());
        while (z > constants::half_pi<RealType>()) {
            z -= constants::pi<RealType>();
        }
        while (z < -constants::half_pi<RealType>()) {
            z += constants::pi<RealType>();
        }
        return _IMAGINARY_jacobi_theta3tau(z, RealType(1/tau), pol);
    }
    return RealType(1) + jacobi_theta3m1tau_imp(z, tau, pol);
}

// Third Jacobi theta function, minus one (parameterized by q)
// = 2 * SUM q^n^2 * cos(2nz)
template <class RealType, class Policy>
inline RealType
jacobi_theta3m1_imp(RealType z, RealType q, const Policy& pol, const char *function) {
    BOOST_MATH_STD_USING
    if (q <= 0.0 || q >= 1.0) {
        return policies::raise_domain_error<RealType>(function,
                "q must be greater than 0 and less than 1 but got %1%.", q, pol);
    }
    return jacobi_theta3m1tau_imp(z, RealType (-log(q)/constants::pi<RealType>()), pol);
}

// Third Jacobi theta function (parameterized by q)
// = 1 + 2 * SUM q^n^2 * cos(2nz)
template <class RealType, class Policy>
inline RealType
jacobi_theta3_imp(RealType z, RealType q, const Policy& pol, const char *function) {
    BOOST_MATH_STD_USING
    if (q <= 0.0 || q >= 1.0) {
        return policies::raise_domain_error<RealType>(function,
                "q must be greater than 0 and less than 1 but got %1%.", q, pol);
    }
    return jacobi_theta3tau_imp(z, RealType (-log(q)/constants::pi<RealType>()), pol, function);
}

// Fourth Jacobi theta function, minus one (Parameterized by tau)
// This function preserves accuracy for small values of q (i.e. tau > 1)
// = 2 * SUM (-1)^n exp(i*Pi*Tau*(n)^2) * cos(2nz)
template <class RealType, class Policy>
inline RealType
jacobi_theta4m1tau_imp(RealType z, RealType tau, const Policy& pol)
{
    BOOST_MATH_STD_USING

    RealType eps = policies::get_epsilon<RealType, Policy>();
    RealType q_n = 0, last_q_n, delta, result = 0;
    unsigned n = 1;

    if (tau < 1.0)
        return jacobi_theta4tau(z, tau, pol) - RealType(1);

    do {
        last_q_n = q_n;
        q_n = exp(-tau * constants::pi<RealType>() * RealType(n)*RealType(n));
        delta = q_n * cos(RealType(2*n)*z);
        if (n%2)
            delta = -delta;

        result += delta + delta;
        n++;
    } while (!_jacobi_theta_converged(last_q_n, q_n, eps));

    return result;
}

// Fourth Jacobi theta function (Parameterized by tau)
// = 1 + 2 * SUM (-1)^n exp(i*Pi*Tau*(n)^2) * cos(2nz)
template <class RealType, class Policy>
inline RealType
jacobi_theta4tau_imp(RealType z, RealType tau, const Policy& pol, const char *function)
{
    BOOST_MATH_STD_USING
    if (tau <= 0.0) {
        return policies::raise_domain_error<RealType>(function,
                "tau must be greater than 0 but got %1%.", tau, pol);
    } else if (tau < 1.0 && abs(z) == 0.0) {
        return jacobi_theta2tau(z, 1/tau, pol) / sqrt(tau);
    } else if (tau < 1.0) { // DLMF 20.7.33
        z = fmod(z, constants::pi<RealType>());
        while (z > constants::half_pi<RealType>()) {
            z -= constants::pi<RealType>();
        }
        while (z < -constants::half_pi<RealType>()) {
            z += constants::pi<RealType>();
        }
        return _IMAGINARY_jacobi_theta2tau(z, RealType(1/tau), pol);
    }

    return RealType(1) + jacobi_theta4m1tau_imp(z, tau, pol);
}

// Fourth Jacobi theta function, minus one (Parameterized by q)
// This function preserves accuracy for small values of q
// = 2 * SUM q^n^2 * cos(2nz)
template <class RealType, class Policy>
inline RealType
jacobi_theta4m1_imp(RealType z, RealType q, const Policy& pol, const char *function) {
    BOOST_MATH_STD_USING
    if (q <= 0.0 || q >= 1.0) {
        return policies::raise_domain_error<RealType>(function,
                "q must be greater than 0 and less than 1 but got %1%.", q, pol);
    }
    return jacobi_theta4m1tau_imp(z, RealType (-log(q)/constants::pi<RealType>()), pol);
}

// Fourth Jacobi theta function, parameterized by q
// = 1 + 2 * SUM q^n^2 * cos(2nz)
template <class RealType, class Policy>
inline RealType
jacobi_theta4_imp(RealType z, RealType q, const Policy& pol, const char *function) {
    BOOST_MATH_STD_USING
    if (q <= 0.0 || q >= 1.0) {
        return policies::raise_domain_error<RealType>(function,
            "|q| must be greater than zero and less than 1, but got %1%.", q, pol);
    }
    return jacobi_theta4tau_imp(z, RealType(-log(q)/constants::pi<RealType>()), pol, function);
}

// Begin public API

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta1tau(T z, U tau, const Policy&) {
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T, U>::type result_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::jacobi_theta1tau<%1%>(%1%)";

   return policies::checked_narrowing_cast<result_type, Policy>(
           jacobi_theta1tau_imp(static_cast<result_type>(z), static_cast<result_type>(tau),
               forwarding_policy(), function), function);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta1tau(T z, U tau) {
    return jacobi_theta1tau(z, tau, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta1(T z, U q, const Policy&) {
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T, U>::type result_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::jacobi_theta1<%1%>(%1%)";

   return policies::checked_narrowing_cast<result_type, Policy>(
           jacobi_theta1_imp(static_cast<result_type>(z), static_cast<result_type>(q),
               forwarding_policy(), function), function);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta1(T z, U q) {
    return jacobi_theta1(z, q, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta2tau(T z, U tau, const Policy&) {
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T, U>::type result_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::jacobi_theta2tau<%1%>(%1%)";

   return policies::checked_narrowing_cast<result_type, Policy>(
           jacobi_theta2tau_imp(static_cast<result_type>(z), static_cast<result_type>(tau),
               forwarding_policy(), function), function);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta2tau(T z, U tau) {
    return jacobi_theta2tau(z, tau, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta2(T z, U q, const Policy&) {
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T, U>::type result_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::jacobi_theta2<%1%>(%1%)";

   return policies::checked_narrowing_cast<result_type, Policy>(
           jacobi_theta2_imp(static_cast<result_type>(z), static_cast<result_type>(q),
               forwarding_policy(), function), function);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta2(T z, U q) {
    return jacobi_theta2(z, q, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta3m1tau(T z, U tau, const Policy&) {
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T, U>::type result_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::jacobi_theta3m1tau<%1%>(%1%)";

   return policies::checked_narrowing_cast<result_type, Policy>(
           jacobi_theta3m1tau_imp(static_cast<result_type>(z), static_cast<result_type>(tau),
               forwarding_policy()), function);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta3m1tau(T z, U tau) {
    return jacobi_theta3m1tau(z, tau, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta3tau(T z, U tau, const Policy&) {
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T, U>::type result_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::jacobi_theta3tau<%1%>(%1%)";

   return policies::checked_narrowing_cast<result_type, Policy>(
           jacobi_theta3tau_imp(static_cast<result_type>(z), static_cast<result_type>(tau),
               forwarding_policy(), function), function);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta3tau(T z, U tau) {
    return jacobi_theta3tau(z, tau, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta3m1(T z, U q, const Policy&) {
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T, U>::type result_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::jacobi_theta3m1<%1%>(%1%)";

   return policies::checked_narrowing_cast<result_type, Policy>(
           jacobi_theta3m1_imp(static_cast<result_type>(z), static_cast<result_type>(q),
               forwarding_policy(), function), function);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta3m1(T z, U q) {
    return jacobi_theta3m1(z, q, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta3(T z, U q, const Policy&) {
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T, U>::type result_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::jacobi_theta3<%1%>(%1%)";

   return policies::checked_narrowing_cast<result_type, Policy>(
           jacobi_theta3_imp(static_cast<result_type>(z), static_cast<result_type>(q),
               forwarding_policy(), function), function);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta3(T z, U q) {
    return jacobi_theta3(z, q, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta4m1tau(T z, U tau, const Policy&) {
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T, U>::type result_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::jacobi_theta4m1tau<%1%>(%1%)";

   return policies::checked_narrowing_cast<result_type, Policy>(
           jacobi_theta4m1tau_imp(static_cast<result_type>(z), static_cast<result_type>(tau),
               forwarding_policy()), function);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta4m1tau(T z, U tau) {
    return jacobi_theta4m1tau(z, tau, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta4tau(T z, U tau, const Policy&) {
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T, U>::type result_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::jacobi_theta4tau<%1%>(%1%)";

   return policies::checked_narrowing_cast<result_type, Policy>(
           jacobi_theta4tau_imp(static_cast<result_type>(z), static_cast<result_type>(tau),
               forwarding_policy(), function), function);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta4tau(T z, U tau) {
    return jacobi_theta4tau(z, tau, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta4m1(T z, U q, const Policy&) {
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T, U>::type result_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::jacobi_theta4m1<%1%>(%1%)";

   return policies::checked_narrowing_cast<result_type, Policy>(
           jacobi_theta4m1_imp(static_cast<result_type>(z), static_cast<result_type>(q),
               forwarding_policy(), function), function);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta4m1(T z, U q) {
    return jacobi_theta4m1(z, q, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_theta4(T z, U q, const Policy&) {
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T, U>::type result_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::jacobi_theta4<%1%>(%1%)";

   return policies::checked_narrowing_cast<result_type, Policy>(
           jacobi_theta4_imp(static_cast<result_type>(z), static_cast<result_type>(q),
               forwarding_policy(), function), function);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_theta4(T z, U q) {
    return jacobi_theta4(z, q, policies::policy<>());
}

}}

#endif


#include <tiny_math_types.h>
#include <tiny_matrix_functions.h>
#include <tiny_eigen3x3.h>

#define BOOST_AUTO_TEST_MAIN
#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 <cmath>                       // for fabs

template<typename vector3_type,typename matrix3x3_type>
inline void eigen_value_decomposition_test(vector3_type d,matrix3x3_type R)
{
  using std::fabs;

  typedef typename vector3_type::real_type     real_type;
  typedef typename vector3_type::value_traits  value_traits;

  real_type const tol = value_traits::numeric_cast( 0.0001 );
  real_type const d0  = d(0);
  real_type const d1  = d(1);
  real_type const d2  = d(2);

  matrix3x3_type D = matrix3x3_type::make_diag(d);
  matrix3x3_type A = R*D*tiny::trans(R);
  BOOST_CHECK( tiny::is_symmetric(A,tol) );

  matrix3x3_type V;
  tiny::eigen(A, V, d);

  real_type determinant = tiny::det(V);
  BOOST_CHECK_CLOSE( fabs( determinant ), value_traits::one(), tol );

  real_type epsilon = 10e-7;

  matrix3x3_type Itest = tiny::trans(V)*V;

  BOOST_CHECK_CLOSE( Itest(0,0), value_traits::one() , tol );
  BOOST_CHECK_CLOSE( Itest(1,1), value_traits::one() , tol );
  BOOST_CHECK_CLOSE( Itest(2,2), value_traits::one() , tol );
  //--- The check close version behaves strange when rhs is exactly zero?, so we use the check-version instead...
  BOOST_CHECK( fabs(Itest(0,1))<epsilon );
  BOOST_CHECK( fabs(Itest(0,2))<epsilon );
  BOOST_CHECK( fabs(Itest(1,0))<epsilon );
  BOOST_CHECK( fabs(Itest(1,2))<epsilon );
  BOOST_CHECK( fabs(Itest(2,0))<epsilon );
  BOOST_CHECK( fabs(Itest(2,1))<epsilon );

  matrix3x3_type Atest = A - V*matrix3x3_type::make_diag(d)*tiny::trans(V);

  BOOST_CHECK( fabs(Atest(0,0))<epsilon );
  BOOST_CHECK( fabs(Atest(0,1))<epsilon );
  BOOST_CHECK( fabs(Atest(0,2))<epsilon );
  BOOST_CHECK( fabs(Atest(1,0))<epsilon );
  BOOST_CHECK( fabs(Atest(1,1))<epsilon );
  BOOST_CHECK( fabs(Atest(1,2))<epsilon );
  BOOST_CHECK( fabs(Atest(2,0))<epsilon );
  BOOST_CHECK( fabs(Atest(2,1))<epsilon );
  BOOST_CHECK( fabs(Atest(2,2))<epsilon );

  bool match1 = fabs( d0 - d(0) ) < epsilon &&
                fabs( d1 - d(1) ) < epsilon &&
                fabs( d2 - d(2) ) < epsilon ;

  bool match2 = fabs( d0 - d(0) )< epsilon &&
                fabs( d1 - d(2) )< epsilon &&
                fabs( d2 - d(1) )< epsilon ;

  bool match3 = fabs( d0 - d(1) )< epsilon &&
                fabs( d1 - d(0) )< epsilon &&
                fabs( d2 - d(2) )< epsilon ;

  bool match4 = fabs( d0 - d(1) )< epsilon &&
                fabs( d1 - d(2) )< epsilon &&
                fabs( d2 - d(0) )< epsilon ;

  bool match5 = fabs( d0 - d(2) )< epsilon &&
                fabs( d1 - d(0) )< epsilon &&
                fabs( d2 - d(1) )< epsilon ;

  bool match6 = fabs( d0 - d(2) )< epsilon &&
                fabs( d1 - d(1) )< epsilon &&
                fabs( d2 - d(0) )< epsilon ;

  BOOST_CHECK( match1 || match2 || match3 || match4 || match5 || match6 );
}

BOOST_AUTO_TEST_SUITE(tiny_eigen);

  BOOST_AUTO_TEST_CASE(random_testing)
  {
    typedef tiny::MathTypes<double>                    math_types;
    typedef math_types::vector3_type                  vector3_type;
    typedef math_types::matrix3x3_type                matrix3x3_type;

    matrix3x3_type R;
    vector3_type d;

    for(int i= 0;i<100;++i)
    {
      //--- non-negative eigen-values
      d = vector3_type::random(0,1);
      R = matrix3x3_type::random();
      R = tiny::ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- non-positive eigen-values
      d = vector3_type::random(-1,0);
      R = matrix3x3_type::random();
      R = tiny::ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- one zero eigen-values
      d = vector3_type::random(0,1);
      d(0) = 0;
      R = matrix3x3_type::random();
      R = tiny::ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- one zero eigen-values
      d = vector3_type::random(0,1);
      d(1) = 0;
      R = matrix3x3_type::random();
      R = tiny::ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- one zero eigen-values
      d = vector3_type::random(0,1);
      d(2) = 0;
      R = matrix3x3_type::random();
      R = tiny::ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- two zero eigen-values
      d = vector3_type::random(0,1);
      d(0) = 0;
      d(1) = 0;
      R = matrix3x3_type::random();
      R = tiny::ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- two zero eigen-values
      d = vector3_type::random(0,1);
      d(0) = 0;
      d(2) = 0;
      R = matrix3x3_type::random();
      R = tiny::ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- two zero eigen-values
      d = vector3_type::random(0,1);
      d(1) = 0;
      d(2) = 0;
      R = matrix3x3_type::random();
      R = tiny::ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- three zero eigen-values
      d.clear();
      R = matrix3x3_type::random();
      R = tiny::ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- multiplicity of 3
      d = vector3_type::random(0,1);
      d(1) = d(0);
      d(2) = d(0);
      R = matrix3x3_type::random();
      R = tiny::ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- multiplicity of 2
      d = vector3_type::random(0,1);
      d(1) = d(0);
      R = matrix3x3_type::random();
      R = tiny::ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- multiplicity of 2
      d = vector3_type::random(0,1);
      d(2) = d(0);
      R = matrix3x3_type::random();
      R = tiny::ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- multiplicity of 2
      d = vector3_type::random(0,1);
      d(2) = d(1);
      R = matrix3x3_type::random();
      R = tiny::ortonormalize(R);
      eigen_value_decomposition_test(d,R);
    }
  }

BOOST_AUTO_TEST_CASE(detailed_testing)
{
  typedef tiny::MathTypes<double>           MT;
  typedef MT::vector3_type                  V;
  typedef MT::matrix3x3_type                M;
  typedef MT::value_traits                  VT;

  M A;

  // This test case is known to result in a R-matrix with det(R)=-1
  A(0,0) =  10.0;  A(0,1) = -1.0;  A(0,2) = -1.0;
  A(1,0) =  -1.0;  A(1,1) = 10.0;  A(1,2) = -1.0;
  A(2,0) =  -1.0;  A(2,1) = -1.0;  A(2,2) = 10.0;

  M R;
  V d;
  tiny::eigen(A, R, d);

  double determinant = tiny::det(R);
  BOOST_CHECK_CLOSE( determinant, -VT::one(), 0.01 );

}

BOOST_AUTO_TEST_SUITE_END();


#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;

MatrixXcd bl_bicgstab(const MatrixXcd& A, const MatrixXcd& B, const double& tol, const int& itermax)
{
// Block BiCGSTAB [Tadano etal 2009 JSIAM letters]
  double Bnorm= B.norm();
  MatrixXcd X= MatrixXcd::Zero(B.rows(),B.cols()); // Initial guess of X (zeros)
  MatrixXcd R= B-A*X;
  MatrixXcd P= R;
  MatrixXcd R0til= R; //MatrixXcd::Random(B.rows(),B.cols());
  MatrixXcd R0til_H= R0til.adjoint();
  for(int k= 0; k < itermax; ++k){
      MatrixXcd V= A*P;
      FullPivLU<MatrixXcd> lu(R0til_H*V);
      MatrixXcd alfa= lu.solve(R0til_H*R);
      MatrixXcd T= R-V*alfa;
      MatrixXcd Z= A*T;
      complex<double> qsi= (Z.adjoint()*T).trace()/(Z.adjoint()*Z).trace();
      X= X+P*alfa+qsi*T;
      R= T-qsi*Z;
      double err= R.norm()/Bnorm;
      cout << "bl_bicgstab: " << "iter= " << k << " relative err= " << err << endl;
      if(err < tol) break;
      MatrixXcd beta= lu.solve(-R0til_H*Z);
      P= R+(P-qsi*V)*beta;
  }
  if((A*X-B).norm()/Bnorm > 10*tol){
      cerr << "bl_bicgstab did not converge to solution within error tolerance !" << endl;
     // exit(EXIT_FAILURE);
  }

  return X;
}


#include <glog/logging.h>
#include <math.h> /* sin */
#include <omp.h>
#include <pcl/common/angles.h>
#include <v4r/common/color_comparison.h>
#include <boost/algorithm/string.hpp>

namespace v4r {

std::istream &operator>>(std::istream &in, ColorComparisonMethod &cm) {
  std::string token;
  in >> token;
  boost::to_upper(token);
  if (token == "CIE76")
    cm = ColorComparisonMethod::CIE76;
  else if (token == "CIE94")
    cm = ColorComparisonMethod::CIE94;
  else if (token == "CIEDE2000")
    cm = ColorComparisonMethod::CIEDE2000;
  else if (token == "CUSTOM")
    cm = ColorComparisonMethod::CUSTOM;
  else
    in.setstate(std::ios_base::failbit);
  return in;
}

std::ostream &operator<<(std::ostream &out, const ColorComparisonMethod &cm) {
  switch (cm) {
    case ColorComparisonMethod::CIE76:
      out << "CIE76";
      break;
    case ColorComparisonMethod::CIE94:
      out << "CIE94";
      break;
    case ColorComparisonMethod::CIEDE2000:
      out << "CIEDE2000";
      break;
    case ColorComparisonMethod::CUSTOM:
      out << "CUSTOM";
      break;
    default:
      out.setstate(std::ios_base::failbit);
  }
  return out;
}

float computeCIE76(const Eigen::Vector3f &a, const Eigen::Vector3f &b) {
  return (a - b).norm();
}

float computeCIE94_DEFAULT(const Eigen::Vector3f &a, const Eigen::Vector3f &b) {
  return computeCIE94(a, b, 1.f, .045f, .015f);
}

float computeCIE94(const Eigen::Vector3f &a, const Eigen::Vector3f &b, float K1, float K2, float Kl) {
  float deltaL = a(0) - b(0);
  float deltaA = a(1) - b(1);
  float deltaB = a(2) - b(2);

  float c1 = sqrt(a(1) * a(1) + a(2) * a(2));
  float c2 = sqrt(b(1) * b(1) + b(2) * b(2));
  float deltaC = c1 - c2;

  float deltaH = deltaA * deltaA + deltaB * deltaB - deltaC * deltaC;
  deltaH = deltaH < 0 ? 0 : sqrt(deltaH);

  const double sl = 1.0;
  const double kc = 1.0;
  const double kh = 1.0;

  float sc = 1.0f + K1 * c1;
  float sh = 1.0f + K2 * c1;

  float deltaLKlsl = deltaL / (Kl * sl);
  float deltaCkcsc = deltaC / (kc * sc);
  float deltaHkhsh = deltaH / (kh * sh);
  float i = deltaLKlsl * deltaLKlsl + deltaCkcsc * deltaCkcsc + deltaHkhsh * deltaHkhsh;
  return i < 0 ? 0 : sqrt(i);
}

float computeCIEDE2000(const Eigen::Vector3f &a, const Eigen::Vector3f &b) {
  // Set weighting factors to 1
  double k_L = 1.0;
  double k_C = 1.0;
  double k_H = 1.0;

  // Calculate Cprime1, Cprime2, Cabbar
  double c_star_1_ab = sqrt(a(1) * a(1) + a(2) * a(2));
  double c_star_2_ab = sqrt(b(1) * b(1) + b(2) * b(2));
  double c_star_average_ab = (c_star_1_ab + c_star_2_ab) / 2;

  double c_star_average_ab_pot7 = c_star_average_ab * c_star_average_ab * c_star_average_ab;
  c_star_average_ab_pot7 *= c_star_average_ab_pot7 * c_star_average_ab;

  double G = 0.5 * (1 - sqrt(c_star_average_ab_pot7 / (c_star_average_ab_pot7 + 6103515625)));  // 25^7
  double a1_prime = (1. + G) * a(1);
  double a2_prime = (1. + G) * b(1);

  double C_prime_1 = sqrt(a1_prime * a1_prime + a(2) * a(2));
  double C_prime_2 = sqrt(a2_prime * a2_prime + b(2) * b(2));
  // Angles in Degree.
  double h_prime_1 = fmod(((atan2(a(2), a1_prime) * 180. / M_PI) + 360.), 360.);
  double h_prime_2 = fmod(((atan2(b(2), a2_prime) * 180. / M_PI) + 360.), 360.);

  double delta_L_prime = b(0) - a(0);
  double delta_C_prime = C_prime_2 - C_prime_1;

  double h_bar = std::abs(h_prime_1 - h_prime_2);
  double delta_h_prime;
  if (C_prime_1 * C_prime_2 == 0)
    delta_h_prime = 0;
  else {
    if (h_bar <= 180.) {
      delta_h_prime = h_prime_2 - h_prime_1;
    } else if (h_bar > 180. && h_prime_2 <= h_prime_1) {
      delta_h_prime = h_prime_2 - h_prime_1 + 360.;
    } else {
      delta_h_prime = h_prime_2 - h_prime_1 - 360.;
    }
  }
  double delta_H_prime = 2 * sqrt(C_prime_1 * C_prime_2) * sin(delta_h_prime * M_PI / 360.);

  // Calculate CIEDE2000
  double L_prime_average = (a(0) + b(0)) / 2.0;
  double C_prime_average = (C_prime_1 + C_prime_2) / 2.0;

  // Calculate h_prime_average

  double h_prime_average;
  if (C_prime_1 * C_prime_2 == 0)
    h_prime_average = 0;
  else {
    if (h_bar <= 180) {
      h_prime_average = (h_prime_1 + h_prime_2) / 2;
    } else if (h_bar > 180. && (h_prime_1 + h_prime_2) < 360) {
      h_prime_average = (h_prime_1 + h_prime_2 + 360) / 2;
    } else {
      h_prime_average = (h_prime_1 + h_prime_2 - 360) / 2;
    }
  }
  double L_prime_average_minus_50_square = (L_prime_average - 50);
  L_prime_average_minus_50_square *= L_prime_average_minus_50_square;

  double S_L = 1 + ((.015 * L_prime_average_minus_50_square) / sqrt(20. + L_prime_average_minus_50_square));
  double S_C = 1 + .045 * C_prime_average;
  double T = 1 - .17 * cos(pcl::deg2rad(h_prime_average - 30)) + .24 * cos(pcl::deg2rad(h_prime_average * 2)) +
             .32 * cos(pcl::deg2rad(h_prime_average * 3 + 6)) - .2 * cos(pcl::deg2rad(h_prime_average * 4 - 63));
  double S_H = 1 + .015 * T * C_prime_average;
  double h_prime_average_minus_275_div_25_square = (h_prime_average - 275) / (25);
  h_prime_average_minus_275_div_25_square *= h_prime_average_minus_275_div_25_square;
  double delta_theta = 30 * std::exp(-h_prime_average_minus_275_div_25_square);

  double C_prime_average_pot_7 = C_prime_average * C_prime_average * C_prime_average;
  C_prime_average_pot_7 *= C_prime_average_pot_7 * C_prime_average;
  double R_C = 2 * sqrt(C_prime_average_pot_7 / (C_prime_average_pot_7 + 6103515625));

  double R_T = -sin(pcl::deg2rad(2 * delta_theta)) * R_C;

  double delta_L_prime_div_k_L_S_L = delta_L_prime / (S_L * k_L);
  double delta_C_prime_div_k_C_S_C = delta_C_prime / (S_C * k_C);
  double delta_H_prime_div_k_H_S_H = delta_H_prime / (S_H * k_H);

  double CIEDE2000 = sqrt(delta_L_prime_div_k_L_S_L * delta_L_prime_div_k_L_S_L +
                          delta_C_prime_div_k_C_S_C * delta_C_prime_div_k_C_S_C +
                          delta_H_prime_div_k_H_S_H * delta_H_prime_div_k_H_S_H +
                          R_T * delta_C_prime_div_k_C_S_C * delta_H_prime_div_k_H_S_H);

  return CIEDE2000;
}

// Eigen::VectorXf computeCIE76(const Eigen::MatrixXf &a, const Eigen::MatrixXf &b)
//{
//    CHECK(a.rows() == b.rows() && a.cols() == b.cols());

//    Eigen::VectorXf diff(a.rows());

//#pragma omp parallel for schedule(dynamic)

//    for(size_t i=0; i<a.rows(); i++)
//        diff(i) = computeCIE76(a.row(i), b.row(i));

//    return diff;
//}
}  // namespace v4r


#include <iostream>
#include <ctime>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Dense>

#define MATRIX_SIZE 50

int main( int argc, char** argv)
{
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零

    matrix_33 = Eigen::Matrix3d::Random();
    cout << matrix_33 << endl << endl;

    cout << matrix_33.transpose() << endl;
    cout << matrix_33.sum() << endl;
    cout << matrix_33.trace() << endl;
    cout << 10*matrix_33 << endl;
    cout << 10*matrix_33.inverse() << endl;
    cout << matrix_33.determinant() << endl << endl;

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_33.transpose()*matrix_33);
    cout << "Eigen Value = " << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors = " << eigen_solver.eigenvectors() << endl;

}


#include <Eigen/LU>
#include <mathtoolbox/matrix-inversion.hpp>
#include <memory>
#include <timer.hpp>

using Eigen::MatrixXd;

inline bool CheckInvertibleness(const MatrixXd& matrix)
{
    const Eigen::FullPivLU<MatrixXd> lu(matrix);
    return lu.isInvertible();
}

int main(int argc, char** argv)
{
    constexpr int size = 500;

    std::shared_ptr<timer::Timer> t;

#if 1
    // Generate a test matrix
    const MatrixXd random_matrix = [](const int size) {
        while (true)
        {
            const MatrixXd candidate = MatrixXd::Random(size, size);

            if (CheckInvertibleness(candidate))
            {
                return candidate;
            }
        }
    }(size);

    constexpr int block_size = 495;

    std::cout << "Matrix size: " << size << std::endl;
    std::cout << "Block size: " << block_size << std::endl;

    // Prepare the inverse of the upper left block
    const Eigen::MatrixXd block_inv = random_matrix.block(0, 0, block_size, block_size).inverse();

    // Perform matrix inversion in the naive direct approach
    t = std::make_shared<timer::Timer>("Naive approach");

    const MatrixXd naive_result = random_matrix.inverse();

    // Perform matrix inversion in the block matrix approach
    t = std::make_shared<timer::Timer>("Block approach");

    const MatrixXd block_result = mathtoolbox::GetInverseUsingUpperLeftBlockInverse(random_matrix, block_inv);

    // Stop the timer
    t = nullptr;

    // Error check
    if (!naive_result.isApprox(block_result, 1e-06))
    {
        throw std::runtime_error("The results are not consistent.");
    }
#else
    const int start_size = 100;

    MatrixXd test_matrix     = MatrixXd::Random(start_size, start_size);
    MatrixXd test_matrix_inv = test_matrix.inverse();
    for (int i = test_matrix.rows(); i < size; ++i)
    {
        const MatrixXd new_test_matrix = [&]() {
            MatrixXd mat(i + 1, i + 1);

            mat.block(0, 0, i, i) = test_matrix; // A
            while (true)
            {
                mat.block(0, i, i, 1) = MatrixXd::Random(i, 1); // B
                mat.block(i, 0, 1, i) = MatrixXd::Random(1, i); // C
                mat.block(i, i, 1, 1) = MatrixXd::Random(1, 1); // D

                const MatrixXd sub_matrix =
                    mat.block(i, i, 1, 1) - mat.block(i, 0, 1, i) * test_matrix_inv * mat.block(0, i, i, 1);

                if (CheckInvertibleness(sub_matrix))
                {
                    break;
                }
            }

            return mat;
        }();

        std::cout << "Matrix size: " << std::to_string(i + 1) << std::endl;

        // Perform matrix inversion in the naive direct approach
        t = std::make_shared<timer::Timer>("Naive approach");

        const MatrixXd naive_result = new_test_matrix.inverse();

        // Perform matrix inversion in the block matrix approach
        t = std::make_shared<timer::Timer>("Block approach");

        const MatrixXd block_result =
            mathtoolbox::GetInverseUsingUpperLeftBlockInverse(new_test_matrix, test_matrix_inv);

        // Stop the timer
        t = nullptr;

        // Error check
        if (!naive_result.isApprox(block_result, 1e-06))
        {
            throw std::runtime_error("The results are not consistent.");
        }

        // Store the results for the next step
        test_matrix     = new_test_matrix;
        test_matrix_inv = naive_result;
    }
#endif

    return 0;
}


#ifndef SKYLARK_SPECTRAL_HPP
#define SKYLARK_SPECTRAL_HPP

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

#include <El.hpp>

namespace skylark { namespace nla {

/**
 * Chebyshev points of the second kind.
 *
 * Returns the N Chebyshev points of the second kind, rescaled to [a,b].
 * That is, x_j = (cos(j* pi / N) + a + 1) * (b - a) / 2 for j=0,..N-1.
 */
template<typename T>
void ChebyshevPoints(int N, El::Matrix<T>& X, double a = -1, double b = 1) {
    const double s = (b - a) / 2.0;
    const double pi = boost::math::constants::pi<double>();

    N = N - 1;
    X.Resize(N+1, 1);
    for(int j = 0; j <= N; j++)
        X.Set(j, 0,
            (std::cos(j * pi / N) + a + 1) * s);

    if (N % 2 == 0)
        X.Set(N / 2, 0, 0.0);
}

/**
 * Differentation matrix associated with with interpolation on N Chebyshev
 * points of the second kind.
 *
 * Returns a NxN matrix with the following property:
 *
 * Suppose a vector p representes a polynomial p(x) of degree N-1 by keeping its
 * value at the N points x_j = (cos(j* pi / N) + a + 1) * (b - a) / 2
 * for j=0,..,N-1. That is p_i = p(x_j-1) for i = 1,..,N-1. There is a unique
 * degree N-1 polynomial interpolating those points, and that is p(x).
 *
 * ([a,b] is the range of values of x we are interested in.)
 *
 * The dervitative of p(x), p'(x), is a degree N polynomial as well, and it
 * can be represented in the same manner by a vector p'. D is built such that
 *                    p' = D * p
 *
 * \param N degree of differentation matrix (i.e. degree of polynomials + 1).
 * \param D matrix to be filled.
 * \param X matrix containing the Chebyshev points used.
 * \param a,b  range of the parameter we are interested in.
 */
template<typename T>
void ChebyshevDiffMatrix(int N, El::Matrix<T>& D, El::Matrix<T> &X,
    double a = -1, double b = 1) {

    ChebyshevPoints(N, X);
    N = N - 1;
    double *x = X.Buffer();

    D.Resize(N+1, N+1);
    for(int j = 0; j <= N; j++)
        for(int i = 0; i <= N; i++) {
            int d = i - j;
            double v = 2.0 / (b - a);

            if (i == 0 && j == 0)
                v *= (2.0 * N * N + 1.0) / 6.0;
            else if (i == N && j == N)
                v *= -(2.0 * N * N + 1.0) / 6.0;
            else {
                if (i == 0 || i == N)
                    v *= 2.0;
                if (j ==0 || j == N)
                    v /= 2.0;

                if (d == 0)
                    v *= -x[j] / (2.0 * (1 - x[j] * x[j]));
                else if (d % 2 == 0)
                    v *= 1.0 / (x[i] - x[j]);
                else
                    v *= -1.0 / (x[i] - x[j]);
            }

            D.Set(i, j, v);
        }

    // Rescale points from [-1, 1] to [a, b]
    if (a != -1 && b != 1)
        for(int i = 0; i <= N; i++)
            x[i] = a + (x[i] + 1.0) * (b - a) / 2.0;
}

} }

#endif


#pragma once

#include <Eigen/Dense>

namespace filter_bay
{
/*!
A linear state space model for transitioning between states with explicit noise.
The equation ist:
\f[
  x_{k+1} = F x_k + B u_k + G w_k
\f]
Where \f$x_k\f$ is the state, \f$u_k\f$ the input and \f$w_k\f$ the process
noise at step k.
The noise is not actually used in the state prediction but in the state
covariance prediction:
\f[
  P_{k+1} = F P_k F^T + G Q_k G^T
\f]
The noise *\f$w_k\f$ is described via its covariance *\f$Q_k\f$.

See https://en.wikipedia.org/wiki/Kalman_filter#Example_application,_technical
for an example how \f$G\f% might be introduced into the state equation.
*/
template <size_t state_size, size_t input_size, size_t noise_size>
struct LinearTransitionModel
{
  /*! \f$F\f$ */
  using TransitionMatrix = Eigen::Matrix<double, state_size, state_size>;
  /*! \f$B\f$ */
  using InputMatrix = Eigen::Matrix<double, state_size, input_size>;
  /*! \f$G\f$ */
  using NoiseMatrix = Eigen::Matrix<double, state_size, noise_size>;
  /*! \f$x_k\f$ */
  using State = Eigen::Matrix<double, state_size, 1>;
  /*! \f$U_k\f$ */
  using Input = Eigen::Matrix<double, input_size, 1>;
  /*! \f$P_k\f$ */
  using StateCovariance = Eigen::Matrix<double, state_size, state_size>;
  /*! \f$Q_k\f$ */
  using NoiseCovariance = Eigen::Matrix<double, noise_size, noise_size>;

  /*! Transition matrix of the model. */
  TransitionMatrix F;
  /*! Control input matrix of the model. */
  InputMatrix B;
  /*! Explicit noise input matrix of the model. */
  NoiseMatrix G;

  LinearTransitionModel() {}

  LinearTransitionModel(TransitionMatrix f, InputMatrix b,
                        NoiseMatrix g) : F(std::move(f)), B(std::move(b)),
                                         G(std::move(g)) {}

  /*!
  Predict the new state via this model.
  \param x the last state
  \param u the last control input
  */
  State predict_state(const State &x, const Input &u) const
  {
    return F * x + B * u;
  }

  /*!
  Predict the new covariance of the state via this model.
  \param x the last state
  \param u the last control input
  */
  StateCovariance predict_covariance(const StateCovariance &P,
                                     const NoiseCovariance &Q) const
  {
    return F * P * F.transpose() + G * Q * G.transpose();
  }
};
} // namespace filter_bay


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   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 <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Algorithms/Math/ComputeSVD.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Eigen/SVD>

#include <Core/Algorithms/Base/AlgorithmVariableNames.h>

using namespace SCIRun;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Algorithms::Math;

void ComputeSVDAlgo::run(MatrixHandle input, DenseMatrixHandle& LeftSingMat, DenseMatrixHandle& SingVals, DenseMatrixHandle& RightSingMat) const
{
  if (input->nrows() == 0 || input->ncols() == 0){

    THROW_ALGORITHM_INPUT_ERROR("Input has a zero dimension.");
}
  if (matrixIs::dense(input))
  {
    auto denseInput = castMatrix::toDense(input);

    Eigen::JacobiSVD<DenseMatrix::EigenBase> svd_mat(*denseInput, Eigen::ComputeFullU | Eigen::ComputeFullV);

    LeftSingMat = boost::make_shared<DenseMatrix>(svd_mat.matrixU());

    SingVals = boost::make_shared<DenseMatrix>(svd_mat.singularValues());

    RightSingMat = boost::make_shared<DenseMatrix>(svd_mat.matrixV());
  }
  else
  {
    THROW_ALGORITHM_INPUT_ERROR("ComputeSVD works for dense matrix input only.");
  }
}

AlgorithmOutput ComputeSVDAlgo::run(const AlgorithmInput& input) const
{
	auto input_matrix = input.get<Matrix>(Variables::InputMatrix);

	DenseMatrixHandle LeftSingMat;
	DenseMatrixHandle RightSingMat;
	DenseMatrixHandle SingVals;

	run(input_matrix, LeftSingMat, SingVals, RightSingMat);

	AlgorithmOutput output;

	output[LeftSingularMatrix] = LeftSingMat;
	output[SingularValues] = SingVals;
	output[RightSingularMatrix] = RightSingMat;

	return output;
}

AlgorithmOutputName ComputeSVDAlgo::LeftSingularMatrix("LeftSingularMatrix");
AlgorithmOutputName ComputeSVDAlgo::SingularValues("SingularValues");
AlgorithmOutputName ComputeSVDAlgo::RightSingularMatrix("RightSingularMatrix");


//-----------------------------------------------------------------------------
// Copyright (c) 2016 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
//-----------------------------------------------------------------------------
#ifndef _mitrax__uBLAS__convolution__hpp_INCLUDED_
#define _mitrax__uBLAS__convolution__hpp_INCLUDED_

#include <boost/numeric/ublas/matrix.hpp>

namespace uBLAS{

	namespace ublas = boost::numeric::ublas;

	template < typename M1, typename M2 >
	inline auto convolution(M1 const& m, M2 const& k){
		int kc = k.size1();
		int kr = k.size2();

		int res_c = m.size1() - kc + 1;
		int res_r = m.size2() - kr + 1;

		using value_type = typename M2::value_type;
		ublas::matrix< value_type > res(res_c, res_r);

		for(int my = 0; my < res_r; ++my){
			for(int mx = 0; mx < res_c; ++mx){
				value_type b = 0;

				for(int ky = 0; ky < kr; ++ky){
					for(int kx = 0; kx < kc; ++kx){
						b += m(mx + kx, my + ky) * k(kx, ky);
					}
				}

				res(mx, my) = b;
			}
		}

		return res;
	}

	template < typename TM, typename V1, typename V2 >
	inline auto convolution(
		ublas::matrix< TM > const& m,
		V1 const& vc,
		V2 const& vr
	){
		return convolution(convolution(m, vr), vc);
	}

}

#endif


#include <Core/Core.h>
#include <Functions4U/Functions4U.h>
#include <Eigen/Eigen.h>
#include "Utility.h"

namespace Upp {

using namespace Eigen;

double R2(const VectorXd &serie, const VectorXd &serie0, double mean) {
	if (IsNull(mean))
		mean = serie.mean();
	double sse = 0, sst = 0;
	for (Eigen::Index i = 0; i < serie.size(); ++i) {
		double y = serie(i);
		double err = y - serie0(i);
		sse += err*err;
		double d = y - mean;
		sst += d*d;
	}
	if (sst < 1E-50 || sse > sst)
		return 0;
	return 1 - sse/sst;
}

}


#include <Eigen/LU>
#include <mathtoolbox/matrix-inversion.hpp>

using Eigen::MatrixXd;

MatrixXd mathtoolbox::GetInverseUsingUpperLeftBlockInverse(const MatrixXd& matrix,
                                                           const MatrixXd& upper_left_block_inverse)
{
    const int size       = matrix.rows();
    const int block_size = upper_left_block_inverse.rows();
    const int rest_size  = size - block_size;

    assert(block_size > 0);
    assert(size > block_size);

    assert(matrix.cols() == size);
    assert(upper_left_block_inverse.cols() == block_size);

    const Eigen::MatrixXd& A_inv = upper_left_block_inverse;
    const Eigen::MatrixXd& B     = matrix.block(0, block_size, block_size, rest_size);
    const Eigen::MatrixXd& C     = matrix.block(block_size, 0, rest_size, block_size);
    const Eigen::MatrixXd& D     = matrix.block(block_size, block_size, rest_size, rest_size);

    const Eigen::MatrixXd E     = D - C * A_inv * B;
    const Eigen::MatrixXd E_inv = E.inverse();

    assert((E * E_inv - MatrixXd::Identity(E.rows(), E.cols())).cwiseAbs().maxCoeff() < 1e-10);

    Eigen::MatrixXd result(size, size);

    result.block(0, 0, block_size, block_size)                 = A_inv + (A_inv * B) * E_inv * (C * A_inv);
    result.block(0, block_size, block_size, rest_size)         = -A_inv * B * E_inv;
    result.block(block_size, 0, rest_size, block_size)         = -E_inv * C * A_inv;
    result.block(block_size, block_size, rest_size, rest_size) = E_inv;

    assert((matrix * result - MatrixXd::Identity(size, size)).cwiseAbs().maxCoeff() < 1e-10);

    return result;
}


#pragma once
#include <cmath>
#include <Eigen/Dense>

inline double r2d(double rad)
{
    return rad*180.0/M_PI;
}

inline double d2r(double degree)
{
    return degree*M_PI/180.0;
}

inline double get_angle(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
{
    return r2d(atan2(v1.cross(v2).norm(), v1.transpose() * v2));
}

inline double get_angle2(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
{
    double x = (v1.transpose()*v2);
    return  r2d(acos( x / ( v1.norm()*v2.norm() ) ));
}

inline double get_distance(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2)
{
    return sqrt( pow(p1.x()-p2.x(), 2.0)
               + pow(p1.y()-p2.y(), 2.0)
               + pow(p1.z()-p2.z(), 2.0));
}


#include <iostream>
#include <cmath>
#include <xtensor/xarray.hpp>
#include <xtensor/xmath.hpp>
#include <xtensor/xview.hpp>
#include "../src/distance/distance.cpp"
#define BOOST_TEST_MODULE "Distance test"
#include <boost/test/unit_test.hpp>

using namespace std;
namespace utf = boost::unit_test;

BOOST_AUTO_TEST_CASE(TestKLDivergence, * utf::tolerance(0.00001))
{
	xt::xarray<double> p = {0.5, 0.5};
	xt::xarray<double> q = {(double) 9 / 10, (double) 1 / 10};

	double v = kl_divergence(p, q);

	BOOST_TEST(v == 0.7369656);

	double v2 = kl_divergence(q, p);

	BOOST_TEST(v2 == 0.5310044);

	xt::xarray<double> p1 = {
		{0.5, 0.5},
		{(double) 9 / 10, (double) 1 / 10}
	};
	xt::xarray<double> q1  = {
		{(double) 9 / 10, (double) 1 / 10},
		{0.5, 0.5}
	};

	xt::xarray<double> res1 = kl_divergence(p1, q1, 1);

	BOOST_TEST(res1[0] == 0.7369656);
	BOOST_TEST(res1[1] == 0.5310044);

	xt::xarray<double> p0 = {
		{0.5, (double) 9 / 10},
		{0.5, (double) 1 / 10}
	};
	xt::xarray<double> q0  = {
		{(double) 9 / 10, 0.5},
		{(double) 1 / 10, 0.5}
	};

	xt::xarray<double> res0 = kl_divergence(p0, q0, 0);

	BOOST_TEST(res0[0] == 0.7369656);
	BOOST_TEST(res0[1] == 0.5310044);

	xt::xarray<double> a = {1.0, 0.0};
	xt::xarray<double> b = {0.5, 0.5};

	BOOST_TEST(kl_divergence(a, b) == 1.0);

	// Throw: values in q are not allowed to be zero
	BOOST_CHECK_THROW(kl_divergence(b, a), runtime_error);

	// Negative number: kl_divergence should throw an exception.
	xt::xarray<double> d = {1.0, -0.00001};
	xt::xarray<double> e = {0.5, 0.5};

	BOOST_CHECK_THROW(kl_divergence(d, e), runtime_error);

	xt::xarray<double> y = {
		{-0.001, (double) 9 / 10},
		{0.5, (double) 1 / 10}
	};
	xt::xarray<double> z  = {
		{(double) 9 / 10, 0.5},
		{(double) 1 / 10, 0.5}
	};

	BOOST_CHECK_THROW(kl_divergence(y, z, 0);, runtime_error);

	// All values are zero: throw
	xt::xarray<double> pzero = {0.0, 0.0};

	BOOST_CHECK_THROW(kl_divergence(pzero, q), runtime_error);
	BOOST_CHECK_THROW(kl_divergence(q, pzero), runtime_error);

	xt::xarray<double> p1zero = {{0.0, 0.0}, {0.5, 0.5}};

	BOOST_CHECK_THROW(kl_divergence(p1zero, q1, 1), runtime_error);

	// Values do not sum to 1: throw
	xt::xarray<double> p_not_one = {0.33, 0.1};

	BOOST_CHECK_THROW(kl_divergence(p_not_one, q), runtime_error);
	BOOST_CHECK_THROW(kl_divergence(q, p_not_one), runtime_error);

	xt::xarray<double> p_not_one_1 = {{0.33, 0.1}, {0.5, 0.5}};

	BOOST_CHECK_THROW(kl_divergence(p_not_one_1, q1, 1), runtime_error);

	// Shape mismatch exception
	BOOST_CHECK_THROW(kl_divergence(p, q0);, runtime_error);
	BOOST_CHECK_THROW(kl_divergence(p, q0, 0);, runtime_error);
	BOOST_CHECK_THROW(kl_divergence(p, q0, 1);, runtime_error);
}

BOOST_AUTO_TEST_CASE(TestJensenShannonDistance, * utf::tolerance(0.00001)) {
	xt::xarray<double> p = {0.5, 0.5};
	xt::xarray<double> q = {(double) 9 / 10, (double) 1 / 10};

	double v = jensen_shannon_distance(p, q);

	BOOST_TEST(v == 0.3831359);

	// distance is symmetric
	double v_sym = jensen_shannon_distance(q, p);

	BOOST_TEST(v_sym == 0.3831359);

	xt::xarray<double> p1 = {
		{0.5, 0.5},
		{(double) 9 / 10, (double) 1 / 10}
	};
	xt::xarray<double> q1  = {
		{(double) 9 / 10, (double) 1 / 10},
		{0.5, 0.5}
	};

	xt::xarray<double> res1 = jensen_shannon_distance(p1, q1, 1);

	BOOST_TEST(res1[0] == 0.3831359);
	BOOST_TEST(res1[1] == 0.3831359);

	xt::xarray<double> res1_sym = jensen_shannon_distance(q1, p1, 1);

	BOOST_TEST(res1_sym[0] == 0.3831359);
	BOOST_TEST(res1_sym[1] == 0.3831359);

	xt::xarray<double> a = {1.0, 0.0};
	xt::xarray<double> b = {0.5, 0.5};

	BOOST_TEST(jensen_shannon_distance(a, b) == 0.5579230);
	BOOST_TEST(jensen_shannon_distance(b, a) == 0.5579230);

	xt::xarray<double> aa = {1.0, 0.0};
	xt::xarray<double> bb = {0.0, 1.0};

	BOOST_TEST(jensen_shannon_distance(aa, bb) == 1.0);

	// All values are zero: throw
	xt::xarray<double> pzero = {0.0, 0.0};

	BOOST_CHECK_THROW(jensen_shannon_distance(pzero, q), runtime_error);
	BOOST_CHECK_THROW(jensen_shannon_distance(q, pzero), runtime_error);

	xt::xarray<double> p1zero = {{0.0, 0.0}, {0.5, 0.5}};

	BOOST_CHECK_THROW(jensen_shannon_distance(p1zero, q1, 1), runtime_error);

	// Values do not sum to 1: throw
	xt::xarray<double> p_not_one = {0.33, 0.1};

	BOOST_CHECK_THROW(jensen_shannon_distance(p_not_one, q), runtime_error);
	BOOST_CHECK_THROW(jensen_shannon_distance(q, p_not_one), runtime_error);

	xt::xarray<double> p_not_one_1 = {{0.33, 0.1}, {0.5, 0.5}};

	BOOST_CHECK_THROW(jensen_shannon_distance(p_not_one_1, q1, 1), runtime_error);

	// Negative number: jensen_shannon_distance should throw an exception.
	xt::xarray<double> d = {1.0, -0.00001};
	xt::xarray<double> e = {0.5, 0.5};

	BOOST_CHECK_THROW(jensen_shannon_distance(d, e), runtime_error);

	// Shape mismatch exception
	BOOST_CHECK_THROW(jensen_shannon_distance(p, q1);, runtime_error);
	BOOST_CHECK_THROW(jensen_shannon_distance(p, q1, 0);, runtime_error);
	BOOST_CHECK_THROW(jensen_shannon_distance(p, q1, 1);, runtime_error);
}

BOOST_AUTO_TEST_CASE(TestDuplicateRow, * utf::tolerance(0.00001)) {
	xt::xarray<double> a = {0.5, 0.5};

	auto b = duplicate_rows(a, 10);

	BOOST_TEST(b.shape()[0] == 10);
	BOOST_TEST(xt::row(b, 0)[0] == a[0]);
	BOOST_TEST(xt::row(b, 0)[1] == a[1]);
	BOOST_TEST(xt::row(b, 5)[0] == a[0]);
	BOOST_TEST(xt::row(b, 5)[1] == a[1]);
}


#pragma once

#include <Eigen/Dense>
#include <cmath>

#include "Generator.hpp"

//A class to generate normal distribution values of type T
//with expected value 0.0 and variation 1.0
template<class T>
class NormalGenerator {
private:
    int hasValue = 0;
    T value;
public:
    T operator()(){
        if(hasValue){
            hasValue = 0;
            return value;
        } else {
            T x, y;
            T s;
            do{
                x = 2.0 * (T) rand()/RAND_MAX - 1.0;
                y = 2.0 * (T) rand()/RAND_MAX - 1.0;
                s = x*x + y*y;
            } while(s>1.0);
            T tmp = sqrt(-2*log(s)/s);
            value = x*tmp;
            hasValue = 1;
            return y*tmp;
        }
    }
};

//A class to generate N-dimensional multivariate normal distributions of floating type Float
template<class Float, int N>
class Generator<Gaussian<Float, N> > {
private:
public:
    typedef Eigen::Matrix<Float, N, 1> Vec;
    typedef Eigen::Matrix<Float, N, N> Mat;
    NormalGenerator<Float> ng;
    Vec mean;
    Mat transform;
public:
    Generator(const Gaussian<Float, N>& gaussian) :
        Generator(gaussian.invCovariance.inverse(), gaussian.mean){
    }
    Generator(const Mat& covar, const Vec& _mean) :
        mean{_mean}
    {
        Eigen::SelfAdjointEigenSolver<Mat> eigenSolver(covar);
        transform = eigenSolver.eigenvectors()
                  * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();
    }

    Vec operator()(){
        Vec normal(mean.size());
        return mean + transform * normal.unaryExpr([&](auto){ return ng(); });
    }

};

template<class Float, int N>
class Generator<IndependentGaussian<Float, N> > {
private:
    typedef Eigen::Matrix<Float, N, 1> Vec;
    typedef Eigen::Matrix<Float, N, N> Mat;
    Generator<Gaussian<Float, N> > gg;
public:
    Generator(const IndependentGaussian<Float, N>& gaussian) :
        gg(gaussian.deviation.cwiseAbs2().asDiagonal(), gaussian.mean){
    }
    Generator(const Vec& deviation, const Vec& mean) :
        gg(deviation.cwiseAbs2().asDiagonal(), mean){
    }
    Vec operator()(){
        return gg();
    }
};


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Helpers/NumericalAlgorithms/Spectral/SwshTestHelpers.hpp"

#include <boost/math/special_functions/binomial.hpp>
#include <cmath>
#include <complex>

#include "NumericalAlgorithms/Spectral/SwshTags.hpp"  // IWYU pragma: keep

namespace Spectral {
namespace Swsh {
namespace TestHelpers {

double factorial(const size_t arg) {
  if (arg <= 1) {
    return 1.0;
  }
  double factorial_result = 1.0;
  for (size_t product_term = 1; product_term <= arg; ++product_term) {
    factorial_result *= static_cast<double>(product_term);
  }
  return factorial_result;
}

std::complex<double> spin_weighted_spherical_harmonic(const int s, const int l,
                                                      const int m,
                                                      const double theta,
                                                      const double phi) {
  if (l + s < 0 or l - s < 0) {
    return 0.0;
  }
  std::complex<double> swshval = 0.0;
  for (int r = 0; r <= l - s; ++r) {
    if (r + s - m >= 0 and l - r + m >= 0) {
      swshval +=
          boost::math::binomial_coefficient<double>(
              static_cast<unsigned>(l - s), static_cast<unsigned>(r)) *
          boost::math::binomial_coefficient<double>(
              static_cast<unsigned>(l + s), static_cast<unsigned>(r + s - m)) *
          ((l - r - s) % 2 == 0 ? 1.0 : -1.0) *
          pow(cos(theta / 2.0) / sin(theta / 2.0), 2.0 * r + s - m);
    }
  }
  swshval *= (m % 2 == 0 ? 1.0 : -1.0) *
             sqrt(factorial(static_cast<size_t>(l) + static_cast<size_t>(m)) *
                  factorial(static_cast<size_t>(l - m)) * (2.0 * l + 1) /
                  (4.0 * M_PI *
                   factorial(static_cast<size_t>(l) + static_cast<size_t>(s)) *
                   factorial(static_cast<size_t>(l - s)))) *
             (std::complex<double>(cos(m * phi), sin(m * phi))) *
             pow(sin(theta / 2.0), 2.0 * l);
  return swshval;
}

template <>
std::complex<double> derivative_of_spin_weighted_spherical_harmonic<Tags::Eth>(
    const int s, const int l, const int m, const double theta,
    const double phi) {
  return sqrt(static_cast<std::complex<double>>((l - s) * (l + s + 1))) *
         spin_weighted_spherical_harmonic(s + 1, l, m, theta, phi);
}

template <>
std::complex<double>
derivative_of_spin_weighted_spherical_harmonic<Tags::Ethbar>(const int s,
                                                             const int l,
                                                             const int m,
                                                             const double theta,
                                                             const double phi) {
  return -sqrt(static_cast<std::complex<double>>((l + s) * (l - s + 1))) *
         spin_weighted_spherical_harmonic(s - 1, l, m, theta, phi);
}

template <>
std::complex<double>
derivative_of_spin_weighted_spherical_harmonic<Tags::EthEth>(const int s,
                                                             const int l,
                                                             const int m,
                                                             const double theta,
                                                             const double phi) {
  return sqrt(static_cast<std::complex<double>>((l - s) * (l + s + 1))) *
         derivative_of_spin_weighted_spherical_harmonic<Tags::Eth>(s + 1, l, m,
                                                                   theta, phi);
}

template <>
std::complex<double>
derivative_of_spin_weighted_spherical_harmonic<Tags::EthbarEth>(
    const int s, const int l, const int m, const double theta,
    const double phi) {
  return sqrt(static_cast<std::complex<double>>((l - s) * (l + s + 1))) *
         derivative_of_spin_weighted_spherical_harmonic<Tags::Ethbar>(
             s + 1, l, m, theta, phi);
}

template <>
std::complex<double>
derivative_of_spin_weighted_spherical_harmonic<Tags::EthEthbar>(
    const int s, const int l, const int m, const double theta,
    const double phi) {
  return -sqrt(static_cast<std::complex<double>>((l + s) * (l - s + 1))) *
         derivative_of_spin_weighted_spherical_harmonic<Tags::Eth>(s - 1, l, m,
                                                                   theta, phi);
}

template <>
std::complex<double>
derivative_of_spin_weighted_spherical_harmonic<Tags::EthbarEthbar>(
    const int s, const int l, const int m, const double theta,
    const double phi) {
  return -sqrt(static_cast<std::complex<double>>((l + s) * (l - s + 1))) *
         derivative_of_spin_weighted_spherical_harmonic<Tags::Ethbar>(
             s - 1, l, m, theta, phi);
}

template <>
std::complex<double>
derivative_of_spin_weighted_spherical_harmonic<Tags::InverseEth>(
    const int s, const int l, const int m, const double theta,
    const double phi) {
  return (l - s + 1) * (l + s) == 0
             ? 0.0
             : spin_weighted_spherical_harmonic(s - 1, l, m, theta, phi) /
                   sqrt(static_cast<std::complex<double>>((l - s + 1) *
                                                          (l + s)));
}

template <>
std::complex<double>
derivative_of_spin_weighted_spherical_harmonic<Tags::InverseEthbar>(
    const int s, const int l, const int m, const double theta,
    const double phi) {
  return (l + s + 1) * (l - s) == 0
             ? 0.0
             : spin_weighted_spherical_harmonic(s + 1, l, m, theta, phi) /
                   -sqrt(static_cast<std::complex<double>>((l + s + 1) *
                                                           (l - s)));
  ;
}

}  // namespace TestHelpers
}  // namespace Swsh
}  // namespace Spectral


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/math/special_functions/erf.hpp>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Mathematics/Statistics/continuousProbabilityDistributions.h"

namespace tudat
{

namespace statistics
{

//! Function to evaluate pdf of Gaussian distribution.
double evaluateGaussianPdf( const double independentVariable, const double mean, const double standardDeviation )
{
    double offsetFromMean = independentVariable - mean;
    return 1.0 / ( std::sqrt( 2.0 * mathematical_constants::PI ) * standardDeviation ) *
            std::exp( -( offsetFromMean * offsetFromMean ) / ( 2.0 * standardDeviation * standardDeviation ) );
}

//! Function to evaluate cdf of Gaussian distribution.
double calculateGaussianCdf( const double independentVariable, const double mean, const double standardDeviation )
{
    return 0.5 * ( 1.0 + boost::math::erf( ( independentVariable - mean ) / ( std::sqrt( 2.0 ) * ( standardDeviation ) ) ) );
}

}

}