// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007.

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

// test_dist_overloads.cpp

#include <boost/math/concepts/real_concept.hpp> // for real_concept
#include <boost/math/distributions/normal.hpp>
    using boost::math::normal_distribution;

#include <boost/test/test_exec_monitor.hpp> // Boost.Test
#include <boost/test/floating_point_comparison.hpp>

#include <iostream>
   using std::cout;
   using std::endl;
   using std::setprecision;

template <class RealType>
void test_spots(RealType)
{
   // Basic sanity checks,
   // 2 eps as a percentage:
   RealType tolerance = boost::math::tools::epsilon<RealType>() * 2 * 100;

   cout << "Tolerance for type " << typeid(RealType).name()  << " is " << tolerance << " %" << endl;

   for(int i = -4; i <= 4; ++i)
   {
      BOOST_CHECK_CLOSE(
         ::boost::math::cdf(normal_distribution<RealType>(), i),
         ::boost::math::cdf(normal_distribution<RealType>(), static_cast<RealType>(i)),
         tolerance);
      BOOST_CHECK_CLOSE(
         ::boost::math::pdf(normal_distribution<RealType>(), i),
         ::boost::math::pdf(normal_distribution<RealType>(), static_cast<RealType>(i)),
         tolerance);
      BOOST_CHECK_CLOSE(
         ::boost::math::cdf(complement(normal_distribution<RealType>(), i)),
         ::boost::math::cdf(complement(normal_distribution<RealType>(), static_cast<RealType>(i))),
         tolerance);
      BOOST_CHECK_CLOSE(
         ::boost::math::hazard(normal_distribution<RealType>(), i),
         ::boost::math::hazard(normal_distribution<RealType>(), static_cast<RealType>(i)),
         tolerance);
      BOOST_CHECK_CLOSE(
         ::boost::math::chf(normal_distribution<RealType>(), i),
         ::boost::math::chf(normal_distribution<RealType>(), static_cast<RealType>(i)),
         tolerance);
   }
   for(float f = 0.01f; f < 1; f += 0.01f)
   {
      BOOST_CHECK_CLOSE(
         ::boost::math::quantile(normal_distribution<RealType>(), f),
         ::boost::math::quantile(normal_distribution<RealType>(), static_cast<RealType>(f)),
         tolerance);
      BOOST_CHECK_CLOSE(
         ::boost::math::quantile(complement(normal_distribution<RealType>(), f)),
         ::boost::math::quantile(complement(normal_distribution<RealType>(), static_cast<RealType>(f))),
         tolerance);
   }
} // template <class RealType>void test_spots(RealType)

int test_main(int, char* [])
{
    // Basic sanity-check spot values.
   // (Parameter value, arbitrarily zero, only communicates the floating point type).
  test_spots(0.0F); // Test float. OK at decdigits = 0 tolerance = 0.0001 %
  test_spots(0.0); // Test double. OK at decdigits 7, tolerance = 1e07 %
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
  test_spots(0.0L); // Test long double.
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
  test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::cout;
#endif

   return 0;
} // int test_main(int, char* [])

/*

Output:

Running 1 test case...
Tolerance for type float is 2.38419e-005 %
Tolerance for type double is 4.44089e-014 %
Tolerance for type long double is 4.44089e-014 %
Tolerance for type class boost::math::concepts::real_concept is 4.44089e-014 %
*** No errors detected

*/


// Kolmogorov-Smirnov 1st order asymptotic distribution
// 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)
//
// The Kolmogorov-Smirnov test in statistics compares two empirical distributions,
// or an empirical distribution against any theoretical distribution. It makes
// use of a specific distribution which doesn't have a formal name, but which
// is often called the Kolmogorv-Smirnov distribution for lack of anything
// better. This file implements the limiting form of this distribution, first
// identified by Andrey Kolmogorov in
//
// Kolmogorov, A. (1933) "Sulla Determinazione Empirica di una Legge di
// Distribuzione." Giornale dell' Istituto Italiano degli Attuari
//
// This limiting form of the CDF is a first-order Taylor expansion that is
// easily implemented by the fourth Jacobi Theta function (setting z=0). The
// PDF is then implemented here as a derivative of the Theta function. Note
// that this derivative is with respect to x, which enters into \tau, and not
// with respect to the z argument, which is always zero, and so the derivative
// identities in DLMF 20.4 do not apply here.
//
// A higher order order expansion is possible, and was first outlined by
//
// Pelz W, Good IJ (1976). "Approximating the Lower Tail-Areas of the
// Kolmogorov-Smirnov One-sample Statistic." Journal of the Royal Statistical
// Society B.
//
// The terms in this expansion get fairly complicated, and as far as I know the
// Pelz-Good expansion is not used in any statistics software. Someone could
// consider updating this implementation to use the Pelz-Good expansion in the
// future, but the math gets considerably hairier with each additional term.
//
// A formula for an exact version of the Kolmogorov-Smirnov test is laid out in
// Equation 2.4.4 of
//
// Durbin J (1973). "Distribution Theory for Tests Based on the Sample
// Distribution Func- tion." In SIAM CBMS-NSF Regional Conference Series in
// Applied Mathematics. SIAM, Philadelphia, PA.
//
// which is available in book form from Amazon and others. This exact version
// involves taking powers of large matrices. To do that right you need to
// compute eigenvalues and eigenvectors, which are beyond the scope of Boost.
// (Some recent work indicates the exact form can also be computed via FFT, see
// https://cran.r-project.org/web/packages/KSgeneral/KSgeneral.pdf).
//
// Even if the CDF of the exact distribution could be computed using Boost
// libraries (which would be cumbersome), the PDF would present another
// difficulty. Therefore I am limiting this implementation to the asymptotic
// form, even though the exact form has trivial values for certain specific
// values of x and n. For more on trivial values see
//
// Ruben H, Gambino J (1982). "The Exact Distribution of Kolmogorov's Statistic
// Dn for n <= 10." Annals of the Institute of Statistical Mathematics.
//
// For a good bibliography and overview of the various algorithms, including
// both exact and asymptotic forms, see
// https://www.jstatsoft.org/article/view/v039i11
//
// As for this implementation: the distribution is parameterized by n (number
// of observations) in the spirit of chi-squared's degrees of freedom. It then
// takes a single argument x. In terms of the Kolmogorov-Smirnov statistical
// test, x represents the distribution of D_n, where D_n is the maximum
// difference between the CDFs being compared, that is,
//
//   D_n = sup|F_n(x) - G(x)|
//
// In the exact distribution, x is confined to the support [0, 1], but in this
// limiting approximation, we allow x to exceed unity (similar to how a normal
// approximation always spills over any boundaries).
//
// As mentioned previously, the CDF is implemented using the \tau
// parameterization of the fourth Jacobi Theta function as
//
// CDF=theta_4(0|2*x*x*n/pi)
//
// The PDF is a hand-coded derivative of that function. Actually, there are two
// (independent) derivatives, as separate code paths are used for "small x"
// (2*x*x*n < pi) and "large x", mirroring the separate code paths in the
// Jacobi Theta implementation to achieve fast convergence. Quantiles are
// computed using a Newton-Raphson iteration from an initial guess that I
// arrived at by trial and error.
//
// The mean and variance are implemented using simple closed-form expressions.
// Skewness and kurtosis use slightly more complicated closed-form expressions
// that involve the zeta function. The mode is calculated at run-time by
// maximizing the PDF. If you have an analytical solution for the mode, feel
// free to plop it in.
//
// The CDF and PDF could almost certainly be re-implemented and sped up using a
// polynomial or rational approximation, since the only meaningful argument is
// x * sqrt(n). But that is left as an exercise for the next maintainer.
//
// In the future, the Pelz-Good approximation could be added. I suggest adding
// a second parameter representing the order, e.g.
//
// kolmogorov_smirnov_dist<>(100) // N=100, order=1
// kolmogorov_smirnov_dist<>(100, 1) // N=100, order=1, i.e. Kolmogorov's formula
// kolmogorov_smirnov_dist<>(100, 4) // N=100, order=4, i.e. Pelz-Good formula
//
// The exact distribution could be added to the API with a special order
// parameter (e.g. 0 or infinity), or a separate distribution type altogether
// (e.g. kolmogorov_smirnov_exact_distribution).
//
#ifndef BOOST_MATH_DISTRIBUTIONS_KOLMOGOROV_SMIRNOV_HPP
#define BOOST_MATH_DISTRIBUTIONS_KOLMOGOROV_SMIRNOV_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/special_functions/jacobi_theta.hpp>
#include <boost/math/tools/tuple.hpp>
#include <boost/math/tools/roots.hpp> // Newton-Raphson
#include <boost/math/tools/minima.hpp> // For the mode

namespace boost { namespace math {

namespace detail {
template <class RealType>
inline RealType kolmogorov_smirnov_quantile_guess(RealType p) {
    // Choose a starting point for the Newton-Raphson iteration
    if (p > 0.9)
        return RealType(1.8) - 5 * (1 - p);
    if (p < 0.3)
        return p + RealType(0.45);
    return p + RealType(0.3);
}

// d/dk (theta2(0, 1/(2*k*k/M_PI))/sqrt(2*k*k*M_PI))
template <class RealType, class Policy>
RealType kolmogorov_smirnov_pdf_small_x(RealType x, RealType n, const Policy&) {
    BOOST_MATH_STD_USING
    RealType value = RealType(0), delta = RealType(0), last_delta = RealType(0);
    RealType eps = policies::get_epsilon<RealType, Policy>();
    int i = 0;
    RealType pi2 = constants::pi_sqr<RealType>();
    RealType x2n = x*x*n;
    if (x2n*x2n == 0.0) {
        return static_cast<RealType>(0);
    }
    while (1) {
        delta = exp(-RealType(i+0.5)*RealType(i+0.5)*pi2/(2*x2n)) * (RealType(i+0.5)*RealType(i+0.5)*pi2 - x2n);

        if (delta == 0.0)
            break;

        if (last_delta != 0.0 && fabs(delta/last_delta) < eps)
            break;

        value += delta + delta;
        last_delta = delta;
        i++;
    }

    return value * sqrt(n) * constants::root_half_pi<RealType>() / (x2n*x2n);
}

// d/dx (theta4(0, 2*x*x*n/M_PI))
template <class RealType, class Policy>
inline RealType kolmogorov_smirnov_pdf_large_x(RealType x, RealType n, const Policy&) {
    BOOST_MATH_STD_USING
    RealType value = RealType(0), delta = RealType(0), last_delta = RealType(0);
    RealType eps = policies::get_epsilon<RealType, Policy>();
    int i = 1;
    while (1) {
        delta = 8*x*i*i*exp(-2*i*i*x*x*n);

        if (delta == 0.0)
            break;

        if (last_delta != 0.0 && fabs(delta / last_delta) < eps)
            break;

        if (i%2 == 0)
            delta = -delta;

        value += delta;
        last_delta = delta;
        i++;
    }

    return value * n;
}

}; // detail

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

        // Constructor
    kolmogorov_smirnov_distribution( RealType n ) : n_obs_(n)
    {
        RealType result;
        detail::check_df(
                "boost::math::kolmogorov_smirnov_distribution<%1%>::kolmogorov_smirnov_distribution", n_obs_, &result, Policy());
    }

    RealType number_of_observations()const
    {
        return n_obs_;
    }

    private:

    RealType n_obs_; // positive integer
};

typedef kolmogorov_smirnov_distribution<double> kolmogorov_k; // Convenience typedef for double version.

#ifdef __cpp_deduction_guides
template <class RealType>
kolmogorov_smirnov_distribution(RealType)->kolmogorov_smirnov_distribution<typename boost::math::tools::promote_args<RealType>::type>;
#endif

namespace detail {
template <class RealType, class Policy>
struct kolmogorov_smirnov_quantile_functor
{
  kolmogorov_smirnov_quantile_functor(const boost::math::kolmogorov_smirnov_distribution<RealType, Policy> dist, RealType const& p)
    : distribution(dist), prob(p)
  {
  }

  boost::math::tuple<RealType, RealType> operator()(RealType const& x)
  {
    RealType fx = cdf(distribution, x) - prob;  // Difference cdf - value - to minimize.
    RealType dx = pdf(distribution, x); // pdf is 1st derivative.
    // return both function evaluation difference f(x) and 1st derivative f'(x).
    return boost::math::make_tuple(fx, dx);
  }
private:
  const boost::math::kolmogorov_smirnov_distribution<RealType, Policy> distribution;
  RealType prob;
};

template <class RealType, class Policy>
struct kolmogorov_smirnov_complementary_quantile_functor
{
  kolmogorov_smirnov_complementary_quantile_functor(const boost::math::kolmogorov_smirnov_distribution<RealType, Policy> dist, RealType const& p)
    : distribution(dist), prob(p)
  {
  }

  boost::math::tuple<RealType, RealType> operator()(RealType const& x)
  {
    RealType fx = cdf(complement(distribution, x)) - prob;  // Difference cdf - value - to minimize.
    RealType dx = -pdf(distribution, x); // pdf is the negative of the derivative of (1-CDF)
    // return both function evaluation difference f(x) and 1st derivative f'(x).
    return boost::math::make_tuple(fx, dx);
  }
private:
  const boost::math::kolmogorov_smirnov_distribution<RealType, Policy> distribution;
  RealType prob;
};

template <class RealType, class Policy>
struct kolmogorov_smirnov_negative_pdf_functor
{
    RealType operator()(RealType const& x) {
        if (2*x*x < constants::pi<RealType>()) {
            return -kolmogorov_smirnov_pdf_small_x(x, static_cast<RealType>(1), Policy());
        }
        return -kolmogorov_smirnov_pdf_large_x(x, static_cast<RealType>(1), Policy());
    }
};
} // namespace detail

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

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

template <class RealType, class Policy>
inline RealType pdf(const kolmogorov_smirnov_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_FPU_EXCEPTION_GUARD
   BOOST_MATH_STD_USING  // for ADL of std functions.

   RealType n = dist.number_of_observations();
   RealType error_result;
   static const char* function = "boost::math::pdf(const kolmogorov_smirnov_distribution<%1%>&, %1%)";
   if(false == detail::check_x_not_NaN(function, x, &error_result, Policy()))
      return error_result;

   if(false == detail::check_df(function, n, &error_result, Policy()))
      return error_result;

   if (x < 0 || !(boost::math::isfinite)(x))
   {
      return policies::raise_domain_error<RealType>(
         function, "Kolmogorov-Smirnov parameter was %1%, but must be > 0 !", x, Policy());
   }

   if (2*x*x*n < constants::pi<RealType>()) {
       return detail::kolmogorov_smirnov_pdf_small_x(x, n, Policy());
   }

   return detail::kolmogorov_smirnov_pdf_large_x(x, n, Policy());
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const kolmogorov_smirnov_distribution<RealType, Policy>& dist, const RealType& x)
{
    BOOST_MATH_STD_USING // for ADL of std function exp.
   static const char* function = "boost::math::cdf(const kolmogorov_smirnov_distribution<%1%>&, %1%)";
   RealType error_result;
   RealType n = dist.number_of_observations();
   if(false == detail::check_x_not_NaN(function, x, &error_result, Policy()))
      return error_result;
   if(false == detail::check_df(function, n, &error_result, Policy()))
      return error_result;
   if((x < 0) || !(boost::math::isfinite)(x)) {
      return policies::raise_domain_error<RealType>(
         function, "Random variable parameter was %1%, but must be between > 0 !", x, Policy());
   }

   if (x*x*n == 0)
       return 0;

   return jacobi_theta4tau(RealType(0), 2*x*x*n/constants::pi<RealType>(), Policy());
} // cdf

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<kolmogorov_smirnov_distribution<RealType, Policy>, RealType>& c) {
    BOOST_MATH_STD_USING // for ADL of std function exp.
    RealType x = c.param;
   static const char* function = "boost::math::cdf(const complemented2_type<const kolmogorov_smirnov_distribution<%1%>&, %1%>)";
   RealType error_result;
   kolmogorov_smirnov_distribution<RealType, Policy> const& dist = c.dist;
   RealType n = dist.number_of_observations();

   if(false == detail::check_x_not_NaN(function, x, &error_result, Policy()))
      return error_result;
   if(false == detail::check_df(function, n, &error_result, Policy()))
      return error_result;

   if((x < 0) || !(boost::math::isfinite)(x))
      return policies::raise_domain_error<RealType>(
         function, "Random variable parameter was %1%, but must be between > 0 !", x, Policy());

   if (x*x*n == 0)
       return 1;

   if (2*x*x*n > constants::pi<RealType>())
       return -jacobi_theta4m1tau(RealType(0), 2*x*x*n/constants::pi<RealType>(), Policy());

   return RealType(1) - jacobi_theta4tau(RealType(0), 2*x*x*n/constants::pi<RealType>(), Policy());
} // cdf (complemented)

template <class RealType, class Policy>
inline RealType quantile(const kolmogorov_smirnov_distribution<RealType, Policy>& dist, const RealType& p)
{
    BOOST_MATH_STD_USING
   static const char* function = "boost::math::quantile(const kolmogorov_smirnov_distribution<%1%>&, %1%)";
   // Error check:
   RealType error_result;
   RealType n = dist.number_of_observations();
   if(false == detail::check_probability(function, p, &error_result, Policy()))
      return error_result;
   if(false == detail::check_df(function, n, &error_result, Policy()))
      return error_result;

   RealType k = detail::kolmogorov_smirnov_quantile_guess(p) / sqrt(n);
   const int get_digits = policies::digits<RealType, Policy>();// get digits from policy,
   std::uintmax_t m = policies::get_max_root_iterations<Policy>(); // and max iterations.

   return tools::newton_raphson_iterate(detail::kolmogorov_smirnov_quantile_functor<RealType, Policy>(dist, p),
           k, RealType(0), boost::math::tools::max_value<RealType>(), get_digits, m);
} // quantile

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<kolmogorov_smirnov_distribution<RealType, Policy>, RealType>& c) {
    BOOST_MATH_STD_USING
   static const char* function = "boost::math::quantile(const kolmogorov_smirnov_distribution<%1%>&, %1%)";
   kolmogorov_smirnov_distribution<RealType, Policy> const& dist = c.dist;
   RealType n = dist.number_of_observations();
   // Error check:
   RealType error_result;
   RealType p = c.param;

   if(false == detail::check_probability(function, p, &error_result, Policy()))
      return error_result;
   if(false == detail::check_df(function, n, &error_result, Policy()))
      return error_result;

   RealType k = detail::kolmogorov_smirnov_quantile_guess(RealType(1-p)) / sqrt(n);

   const int get_digits = policies::digits<RealType, Policy>();// get digits from policy,
   std::uintmax_t m = policies::get_max_root_iterations<Policy>(); // and max iterations.

   return tools::newton_raphson_iterate(
           detail::kolmogorov_smirnov_complementary_quantile_functor<RealType, Policy>(dist, p),
           k, RealType(0), boost::math::tools::max_value<RealType>(), get_digits, m);
} // quantile (complemented)

template <class RealType, class Policy>
inline RealType mode(const kolmogorov_smirnov_distribution<RealType, Policy>& dist)
{
    BOOST_MATH_STD_USING
   static const char* function = "boost::math::mode(const kolmogorov_smirnov_distribution<%1%>&)";
   RealType n = dist.number_of_observations();
   RealType error_result;
   if(false == detail::check_df(function, n, &error_result, Policy()))
      return error_result;

    std::pair<RealType, RealType> r = boost::math::tools::brent_find_minima(
            detail::kolmogorov_smirnov_negative_pdf_functor<RealType, Policy>(),
            static_cast<RealType>(0), static_cast<RealType>(1), policies::digits<RealType, Policy>());
    return r.first / sqrt(n);
}

// Mean and variance come directly from
// https://www.jstatsoft.org/article/view/v008i18 Section 3
template <class RealType, class Policy>
inline RealType mean(const kolmogorov_smirnov_distribution<RealType, Policy>& dist)
{
    BOOST_MATH_STD_USING
   static const char* function = "boost::math::mean(const kolmogorov_smirnov_distribution<%1%>&)";
    RealType n = dist.number_of_observations();
    RealType error_result;
    if(false == detail::check_df(function, n, &error_result, Policy()))
        return error_result;
    return constants::root_half_pi<RealType>() * constants::ln_two<RealType>() / sqrt(n);
}

template <class RealType, class Policy>
inline RealType variance(const kolmogorov_smirnov_distribution<RealType, Policy>& dist)
{
   static const char* function = "boost::math::variance(const kolmogorov_smirnov_distribution<%1%>&)";
    RealType n = dist.number_of_observations();
    RealType error_result;
    if(false == detail::check_df(function, n, &error_result, Policy()))
        return error_result;
    return (constants::pi_sqr_div_six<RealType>()
            - constants::pi<RealType>() * constants::ln_two<RealType>() * constants::ln_two<RealType>()) / (2*n);
}

// Skewness and kurtosis come from integrating the PDF
// The alternating series pops out a Dirichlet eta function which is related to the zeta function
template <class RealType, class Policy>
inline RealType skewness(const kolmogorov_smirnov_distribution<RealType, Policy>& dist)
{
    BOOST_MATH_STD_USING
   static const char* function = "boost::math::skewness(const kolmogorov_smirnov_distribution<%1%>&)";
    RealType n = dist.number_of_observations();
    RealType error_result;
    if(false == detail::check_df(function, n, &error_result, Policy()))
        return error_result;
    RealType ex3 = RealType(0.5625) * constants::root_half_pi<RealType>() * constants::zeta_three<RealType>() / n / sqrt(n);
    RealType mean = boost::math::mean(dist);
    RealType var = boost::math::variance(dist);
    return (ex3 - 3 * mean * var - mean * mean * mean) / var / sqrt(var);
}

template <class RealType, class Policy>
inline RealType kurtosis(const kolmogorov_smirnov_distribution<RealType, Policy>& dist)
{
    BOOST_MATH_STD_USING
   static const char* function = "boost::math::kurtosis(const kolmogorov_smirnov_distribution<%1%>&)";
    RealType n = dist.number_of_observations();
    RealType error_result;
    if(false == detail::check_df(function, n, &error_result, Policy()))
        return error_result;
    RealType ex4 = 7 * constants::pi_sqr_div_six<RealType>() * constants::pi_sqr_div_six<RealType>() / 20 / n / n;
    RealType mean = boost::math::mean(dist);
    RealType var = boost::math::variance(dist);
    RealType skew = boost::math::skewness(dist);
    return (ex4 - 4 * mean * skew * var * sqrt(var) - 6 * mean * mean * var - mean * mean * mean * mean) / var / var;
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const kolmogorov_smirnov_distribution<RealType, Policy>& dist)
{
   static const char* function = "boost::math::kurtosis_excess(const kolmogorov_smirnov_distribution<%1%>&)";
    RealType n = dist.number_of_observations();
    RealType error_result;
    if(false == detail::check_df(function, n, &error_result, Policy()))
        return error_result;
    return kurtosis(dist) - 3;
}
}}
#endif


// solving A * X = B
// A hermitian in packed storage
// driver function hesv()

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/lapack/driver/hpsv.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/hermitian.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cin;
using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;
typedef
  ublas::hermitian_matrix<cmplx_t, ublas::lower, ublas::column_major> cherml_t;
typedef
  ublas::hermitian_matrix<cmplx_t, ublas::upper, ublas::column_major> chermu_t;

int main() {

  cherml_t hcal (3, 3);   // hermitian matrix
  chermu_t hcau (3, 3);   // hermitian matrix
  cm_t cx (3, 1);
  cm_t cbl (3, 1), cbu (3, 1);  // RHS

  std::vector<fortran_int_t> ipiv (3);

  hcal (0, 0) = cmplx_t (3, 0);
  hcal (1, 0) = cmplx_t (4, -2);
  hcal (1, 1) = cmplx_t (5, 0);
  hcal (2, 0) = cmplx_t (-7, -5);
  hcal (2, 1) = cmplx_t (0, 3);
  hcal (2, 2) = cmplx_t (2, 0);

  hcau (0, 0) = cmplx_t (3, 0);
  hcau (0, 1) = cmplx_t (4, 2);
  hcau (0, 2) = cmplx_t (-7, 5);
  hcau (1, 1) = cmplx_t (5, 0);
  hcau (1, 2) = cmplx_t (0, -3);
  hcau (2, 2) = cmplx_t (2, 0);

  print_m (hcal, "hcal");
  cout << endl;
  print_m (hcau, "hcau");
  cout << endl;

  for (int i = 0; i < cx.size1(); ++i)
    cx (i, 0) = cmplx_t (1, -1);
  print_m (cx, "cx");
  cout << endl;
  cbl = prod (hcal, cx);
  cbu = prod (hcau, cx);
  print_m (cbl, "cbl");
  cout << endl;
  print_m (cbu, "cbu");
  cout << endl;

//  int ierr = lapack::hpsv (hcal, cbl);
//  no ipiv less version is currently provided, so fall back to using ipiv
  int ierr = lapack::hpsv (hcal, ipiv, cbl);
  if (ierr == 0)
    print_m (cbl, "cxl");
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;

  ierr = lapack::hpsv (hcau, ipiv, cbu);
  if (ierr == 0) {
    print_v (ipiv, "ipiv");
    cout << endl;
    print_m (cbu, "cxu");
  }
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;
}


#ifndef MOCHIMOCHI_ADAGRAD_RDA_HPP_
#define MOCHIMOCHI_ADAGRAD_RDA_HPP_

#include <Eigen/Dense>
#include "../../functions/enumerate.hpp"

class ADAGRAD_RDA {
private :
  const std::size_t kDim;
  const double kEta;
  const double kLambda;

private :
  std::size_t _timestep;
  Eigen::VectorXd _w;
  Eigen::VectorXd _h;
  Eigen::VectorXd _g;

public :
  ADAGRAD_RDA(const std::size_t dim, const double eta, const double lambda)
    : kDim(dim),
      kEta(eta),
      kLambda(lambda),
      _timestep(0),
      _w(Eigen::VectorXd::Zero(kDim)),
      _h(Eigen::VectorXd::Zero(kDim)),
      _g(Eigen::VectorXd::Zero(kDim)) {
    static_assert(std::numeric_limits<decltype(dim)>::max() > 0, "Dimension Error. (Dimension > 0)");
    static_assert(std::numeric_limits<decltype(eta)>::max() > 0, "Hyper Parameter Error. (eta > 0)");
    static_assert(std::numeric_limits<decltype(lambda)>::max() > 0, "Hyper Parameter Error. (lambda > 0)");
    assert(dim > 0);
    assert(eta > 0);
    assert(lambda > 0);
  }

  virtual ~ADAGRAD_RDA() { }

private :

  double calculate_margin(const Eigen::VectorXd& x) const {
    return _w.dot(x);
  }

  double suffer_loss(const Eigen::VectorXd& x, const int y) const {
    return std::max(0.0, 1.0 - y * _w.dot(x));
  }

public :

  bool update(const Eigen::VectorXd& feature, const int label) {
    if (suffer_loss(feature, label) <= 0.0) { return false; }

    _timestep++;
    functions::enumerate(feature.data(), feature.data() + feature.size(), 0,
                       [&](const int index, const double value) {
                         const auto gradiant = -label * value;
                         _g[index] += gradiant;
                         _h[index] += gradiant * gradiant;

                         const auto sign = _g[index] >= 0 ? 1 : -1;
                         const auto eta = kEta / std::sqrt(_h[index]);
                         const auto u = std::abs(_g[index]) / _timestep;

                         _w[index] = (u <= kLambda) ? 0.0 : -sign * eta * _timestep * (u - kLambda);
                       });
    return true;
  }

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

};

#endif //MOCHIMOCHI_ADAGRAD_RDA_HPP_


#include <iostream>
#include <stdio.h>
#include <opencv_workbench/cluster/RTree.h>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>

#include <boost/geometry/index/rtree.hpp>

// to store queries results
#include <vector>
#include <boost/foreach.hpp>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

//
// Test.cpp
//
// This is a direct port of the C version of the RTree test program.
//

using namespace std;

// From: https://github.com/nushoin/RTree

typedef int ValueType;

struct Rect
{
     Rect()  {}

     Rect(int a_minX, int a_minY, int a_maxX, int a_maxY)
          {
               min[0] = a_minX;
               min[1] = a_minY;

               max[0] = a_maxX;
               max[1] = a_maxY;
          }

     int min[2];
     int max[2];
};

struct Rect rects[] =
{
     Rect(0, 0, 2, 2), // xmin, ymin, xmax, ymax (for 2 dimensional RTree)
     Rect(5, 5, 7, 7),
     Rect(8, 5, 9, 6),
     Rect(7, 1, 9, 2),
};

int nrects = sizeof(rects) / sizeof(rects[0]);

Rect search_rect(6, 4, 10, 6); // search will find above rects that this one overlaps

bool MySearchCallback(ValueType id, void* arg)
{
     cout << "Hit data rect " << id << "\n";
     return true; // keep going
}

int main()
{
     ////////////////
     // RTree.h Example
     ////////////////
     typedef RTree<ValueType, int, 2, float> MyTree;
     MyTree tree;

     int i, nhits;
     cout << "nrects = " << nrects << "\n";

     for(i=0; i<nrects; i++) {
          tree.Insert(rects[i].min, rects[i].max, i); // Note, all values including zero are fine in this version
     }

     nhits = tree.Search(search_rect.min, search_rect.max, MySearchCallback, NULL);

     cout << "Search resulted in " << nhits << " hits\n";

     // Iterator test
     int itIndex = 0;
     MyTree::Iterator it;
     for( tree.GetFirst(it);
          !tree.IsNull(it);
          tree.GetNext(it) ) {
          int value = tree.GetAt(it);

          int boundsMin[2] = {0,0};
          int boundsMax[2] = {0,0};
          it.GetBounds(boundsMin, boundsMax);
          cout << "it[" << itIndex++ << "] " << value << " = (" << boundsMin[0] << "," << boundsMin[1] << "," << boundsMax[0] << "," << boundsMax[1] << ")\n";
     }

     // Iterator test, alternate syntax
     itIndex = 0;
     tree.GetFirst(it);
     while( !it.IsNull() )
     {
          int value = *it;
          ++it;
          cout << "it[" << itIndex++ << "] " << value << "\n";
     }

     // Output:
     //
     // nrects = 4
     // Hit data rect 1
     // Hit data rect 2
     // Search resulted in 2 hits
     // it[0] 0 = (0,0,2,2)
     // it[1] 1 = (5,5,7,7)
     // it[2] 2 = (8,5,9,6)
     // it[3] 3 = (7,1,9,2)
     // it[0] 0
     // it[1] 1
     // it[2] 2
     // it[3] 3

     cout << "===================================" << endl;

     ////////////////////
     // Boost RTree
     ////////////////////
     typedef bg::model::point<float, 2, bg::cs::cartesian> point;
     typedef bg::model::box<point> box;
     typedef std::pair<box, unsigned> value;

     // create the rtree using default constructor
     //bgi::rtree< value, bgi::quadratic<16> > rtree; // max elements is 16
     //bgi::rtree< value, bgi::quadratic<16> > rtree; // max elements is 16
     // rstar
     bgi::rtree<value, bgi::dynamic_rstar> rtree(bgi::dynamic_rstar(16));

     // create some values
     for ( unsigned i = 0 ; i < 10 ; ++i ) {
          // create a box
          box b(point(i + 0.0f, i + 0.0f), point(i + 0.5f, i + 0.5f));
          // insert new value
          rtree.insert(std::make_pair(b, i));
     }

     // find values intersecting some area defined by a box
     box query_box(point(0, 0), point(5, 5));
     std::vector<value> result_s;
     rtree.query(bgi::intersects(query_box), std::back_inserter(result_s));

     // find 5 nearest values to a point
     std::vector<value> result_n;
     rtree.query(bgi::nearest(point(0, 0), 5), std::back_inserter(result_n));

     // display results
     std::cout << "spatial query box:" << std::endl;
     std::cout << bg::wkt<box>(query_box) << std::endl;
     std::cout << "spatial query result:" << std::endl;
     BOOST_FOREACH(value const& v, result_s)
          std::cout << bg::wkt<box>(v.first) << " - " << v.second << std::endl;

     std::cout << "knn query point:" << std::endl;
     std::cout << bg::wkt<point>(point(0, 0)) << std::endl;
     std::cout << "knn query result:" << std::endl;
     BOOST_FOREACH(value const& v, result_n)
          std::cout << bg::wkt<box>(v.first) << " - " << v.second << std::endl;

     return 0;
}


// Copyright (c) 2009 libmv authors.
//
// 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 <limits>

#include <Eigen/SVD>
#include <Eigen/Geometry>

#include "libmv/base/vector.h"
#include "libmv/logging/logging.h"
#include "libmv/multiview/euclidean_resection.h"
#include "libmv/multiview/projection.h"

namespace libmv {
namespace euclidean_resection {

bool EuclideanResection(const Mat2X &x_camera,
                        const Mat3X &X_world,
                        Mat3 *R, Vec3 *t,
                        ResectionMethod method) {
  switch (method) {
    case RESECTION_ANSAR_DANIILIDIS:
      EuclideanResectionAnsarDaniilidis(x_camera, X_world, R, t);
      break;
    case RESECTION_EPNP:
      return EuclideanResectionEPnP(x_camera, X_world, R, t);
      break;
    default:
      LOG(FATAL) << "Unknown resection method.";
  }
  return false;
}

bool EuclideanResection(const Mat &x_image,
                        const Mat3X &X_world,
                        const Mat3 &K,
                        Mat3 *R, Vec3 *t,
                        ResectionMethod method) {
  CHECK(x_image.rows() == 2 || x_image.rows() == 3)
    << "Invalid size for x_image: "
    << x_image.rows() << "x" << x_image.cols();

  Mat2X x_camera;
  if (x_image.rows() == 2) {
    EuclideanToNormalizedCamera(x_image, K, &x_camera);
  } else if (x_image.rows() == 3) {
    HomogeneousToNormalizedCamera(x_image, K, &x_camera);
  }
  return EuclideanResection(x_camera, X_world, R, t, method);
}

void AbsoluteOrientation(const Mat3X &X,
                         const Mat3X &Xp,
                         Mat3 *R,
                         Vec3 *t) {
  int num_points = X.cols();
  Vec3 C  = X.rowwise().sum() / num_points;   // Centroid of X.
  Vec3 Cp = Xp.rowwise().sum() / num_points;  // Centroid of Xp.

  // Normalize the two point sets.
  Mat3X Xn(3, num_points), Xpn(3, num_points);
  for( int i = 0; i < num_points; ++i ){
    Xn.col(i)  = X.col(i) - C;
    Xpn.col(i) = Xp.col(i) - Cp;
  }

  // Construct the N matrix (pg. 635).
  double Sxx = Xn.row(0).dot(Xpn.row(0));
  double Syy = Xn.row(1).dot(Xpn.row(1));
  double Szz = Xn.row(2).dot(Xpn.row(2));
  double Sxy = Xn.row(0).dot(Xpn.row(1));
  double Syx = Xn.row(1).dot(Xpn.row(0));
  double Sxz = Xn.row(0).dot(Xpn.row(2));
  double Szx = Xn.row(2).dot(Xpn.row(0));
  double Syz = Xn.row(1).dot(Xpn.row(2));
  double Szy = Xn.row(2).dot(Xpn.row(1));

  Mat4 N;
  N << Sxx + Syy + Szz, Syz - Szy,        Szx - Sxz,        Sxy - Syx,
       Syz - Szy,       Sxx - Syy - Szz,  Sxy + Syx,        Szx + Sxz,
       Szx - Sxz,       Sxy + Syx,       -Sxx + Syy - Szz,  Syz + Szy,
       Sxy - Syx,       Szx + Sxz,        Syz + Szy,       -Sxx - Syy + Szz;

  // Find the unit quaternion q that maximizes qNq. It is the eigenvector
  // corresponding to the lagest eigenvalue.
  Vec4 q = N.jacobiSvd(Eigen::ComputeFullU).matrixU().col(0);

  // Retrieve the 3x3 rotation matrix.
  Vec4 qq = q.array() * q.array();
  double q0q1 = q(0) * q(1);
  double q0q2 = q(0) * q(2);
  double q0q3 = q(0) * q(3);
  double q1q2 = q(1) * q(2);
  double q1q3 = q(1) * q(3);
  double q2q3 = q(2) * q(3);

  (*R) << qq(0) + qq(1) - qq(2) - qq(3),
          2 * (q1q2 - q0q3),
          2 * (q1q3 + q0q2),
          2 * (q1q2+ q0q3),
          qq(0) - qq(1) + qq(2) - qq(3),
          2 * (q2q3 - q0q1),
          2 * (q1q3 - q0q2),
          2 * (q2q3 + q0q1),
          qq(0) - qq(1) - qq(2) + qq(3);

  // Fix the handedness of the R matrix.
  if (R->determinant() < 0) {
    R->row(2) = -R->row(2);
  }
  // Compute the final translation.
  *t = Cp - *R * C;
}

// Convert i and j indices of the original variables into their quadratic
// permutation single index. It follows that t_ij = t_ji.
static int IJToPointIndex(int i, int j, int num_points) {
  // Always make sure that j is bigger than i. This handles t_ij = t_ji.
  if (j < i) {
    std::swap(i, j);
  }
  int idx;
  int num_permutation_rows = num_points * (num_points - 1) / 2;

  // All t_ii's are located at the end of the t vector after all t_ij's.
  if (j == i) {
    idx = num_permutation_rows + i;
  } else {
    int offset = (num_points - i - 1) * (num_points - i) / 2;
    idx = (num_permutation_rows - offset + j - i - 1);
  }
  return idx;
};

// Convert i and j indexes of the solution for lambda to their linear indexes.
static int IJToIndex(int i, int j, int num_lambda) {
  if (j < i) {
    std::swap(i, j);
  }
  int A = num_lambda * (num_lambda + 1) / 2;
  int B = num_lambda - i;
  int C = B * (B + 1) / 2;
  int idx = A - C + j - i;
  return idx;
};

static int Sign(double value) {
  return (value < 0) ? -1 : 1;
};

// Organizes a square matrix into a single row constraint on the elements of
// Lambda to create the constraints in equation (5) in "Linear Pose Estimation
// from Points or Lines", by Ansar, A. and Daniilidis, PAMI 2003. vol. 25, no.
// 5.
static Vec MatrixToConstraint(const Mat &A,
                              int num_k_columns,
                              int num_lambda) {
  Vec C(num_k_columns);
  C.setZero();
  int idx = 0;
  for (int i = 0; i < num_lambda; ++i) {
    for( int j = i; j < num_lambda; ++j) {
      C(idx) = A(i, j);
      if (i != j){
        C(idx) += A(j, i);
      }
      ++ idx;
    }
  }
  return C;
}

// Normalizes the columns of vectors.
static void NormalizeColumnVectors(Mat3X *vectors) {
  int num_columns = vectors->cols();
  for (int i = 0; i < num_columns; ++i){
    vectors->col(i).normalize();
  }
}

void EuclideanResectionAnsarDaniilidis(const Mat2X &x_camera,
                                       const Mat3X &X_world,
                                       Mat3 *R,
                                       Vec3 *t) {
  CHECK(x_camera.cols() == X_world.cols());
  CHECK(x_camera.cols() > 3);

  int num_points = x_camera.cols();

  // Copy the normalized camera coords into 3 vectors and normalize them so
  // that they are unit vectors from the camera center.
  Mat3X x_camera_unit(3, num_points);
  x_camera_unit.block(0, 0, 2, num_points) = x_camera;
  x_camera_unit.row(2).setOnes();
  NormalizeColumnVectors(&x_camera_unit);

  int num_m_rows = num_points * (num_points - 1) / 2;
  int num_tt_variables = num_points * (num_points + 1) / 2;
  int num_m_columns = num_tt_variables + 1;
  Mat M(num_m_columns, num_m_columns);
  M.setZero();
  Matu ij_index(num_tt_variables, 2);

  // Create the constraint equations for the t_ij variables (7) and arrange
  // them into the M matrix (8). Also store the initial (i, j) indices.
  int row=0;
  for (int i = 0; i < num_points; ++i) {
    for (int j = i+1; j < num_points; ++j) {
      M(row, row) = -2 * x_camera_unit.col(i).dot(x_camera_unit.col(j));
      M(row, num_m_rows + i) = x_camera_unit.col(i).dot(x_camera_unit.col(i));
      M(row, num_m_rows + j) = x_camera_unit.col(j).dot(x_camera_unit.col(j));
      Vec3 Xdiff = X_world.col(i) - X_world.col(j);
      double center_to_point_distance = Xdiff.norm();
      M(row, num_m_columns - 1) =
          - center_to_point_distance * center_to_point_distance;
      ij_index(row, 0) = i;
      ij_index(row, 1) = j;
      ++row;
    }
    ij_index(i + num_m_rows, 0) = i;
    ij_index(i + num_m_rows, 1) = i;
  }

  int num_lambda = num_points + 1;  // Dimension of the null space of M.
  Mat V = M.jacobiSvd(Eigen::ComputeFullV).matrixV().block(0,
                                                           num_m_rows,
                                                           num_m_columns,
                                                           num_lambda);

  // TODO(vess): The number of constraint equations in K (num_k_rows) must be
  // (num_points + 1) * (num_points + 2)/2. This creates a performance issue
  // for more than 4 points. It is fine for 4 points at the moment with 18
  // instead of 15 equations.
  int num_k_rows = num_m_rows + num_points *
                   (num_points*(num_points-1)/2 - num_points+1);
  int num_k_columns = num_lambda * (num_lambda + 1) / 2;
  Mat K(num_k_rows, num_k_columns);
  K.setZero();

  // Construct the first part of the K matrix corresponding to (t_ii, t_jk) for
  // i != j.
  int counter_k_row = 0;
  for (int idx1 = num_m_rows; idx1 < num_tt_variables; ++idx1) {
    for (int idx2 = 0; idx2 < num_m_rows; ++idx2) {

      unsigned int i = ij_index(idx1, 0);
      unsigned int j = ij_index(idx2, 0);
      unsigned int k = ij_index(idx2, 1);

      if( i != j && i != k ){
        int idx3 = IJToPointIndex(i, j, num_points);
        int idx4 = IJToPointIndex(i, k, num_points);

        K.row(counter_k_row) =
            MatrixToConstraint(V.row(idx1).transpose() * V.row(idx2)-
                               V.row(idx3).transpose() * V.row(idx4),
                               num_k_columns,
                               num_lambda);
        ++counter_k_row;
      }
    }
  }

  // Construct the second part of the K matrix corresponding to (t_ii,t_jk) for
  // j==k.
  for (int idx1 = num_m_rows; idx1 < num_tt_variables; ++idx1) {
    for (int idx2 = idx1 + 1; idx2 < num_tt_variables; ++idx2) {
      unsigned int i = ij_index(idx1, 0);
      unsigned int j = ij_index(idx2, 0);
      unsigned int k = ij_index(idx2, 1);

      int idx3 = IJToPointIndex(i, j, num_points);
      int idx4 = IJToPointIndex(i, k, num_points);

      K.row(counter_k_row) =
          MatrixToConstraint(V.row(idx1).transpose() * V.row(idx2)-
                             V.row(idx3).transpose() * V.row(idx4),
                             num_k_columns,
                             num_lambda);
      ++counter_k_row;
    }
  }
  Vec L_sq = K.jacobiSvd(Eigen::ComputeFullV).matrixV().col(num_k_columns - 1);

  // Pivot on the largest element for numerical stability. Afterwards recover
  // the sign of the lambda solution.
  double max_L_sq_value = fabs(L_sq(IJToIndex(0, 0, num_lambda)));
  int max_L_sq_index = 1;
  for (int i = 1; i < num_lambda; ++i) {
    double abs_sq_value = fabs(L_sq(IJToIndex(i, i, num_lambda)));
    if (max_L_sq_value < abs_sq_value) {
      max_L_sq_value = abs_sq_value;
      max_L_sq_index = i;
    }
  }
  // Ensure positiveness of the largest value corresponding to lambda_ii.
  L_sq = L_sq * Sign(L_sq(IJToIndex(max_L_sq_index,
                                    max_L_sq_index,
                                    num_lambda)));

  Vec L(num_lambda);
  L(max_L_sq_index) = sqrt(L_sq(IJToIndex(max_L_sq_index,
                                          max_L_sq_index,
                                          num_lambda)));

  for (int i = 0; i < num_lambda; ++i) {
    if (i != max_L_sq_index) {
      L(i) = L_sq(IJToIndex(max_L_sq_index, i, num_lambda)) / L(max_L_sq_index);
    }
  }

  // Correct the scale using the fact that the last constraint is equal to 1.
  L = L / (V.row(num_m_columns - 1).dot(L));
  Vec X = V * L;

  // Recover the distances from the camera center to the 3D points Q.
  Vec d(num_points);
  d.setZero();
  for (int c_point = num_m_rows; c_point < num_tt_variables; ++c_point) {
    d(c_point - num_m_rows) = sqrt(X(c_point));
  }

  // Create the 3D points in the camera system.
  Mat X_cam(3, num_points);
  for (int c_point = 0; c_point < num_points; ++c_point ) {
    X_cam.col(c_point) = d(c_point) * x_camera_unit.col(c_point);
  }
  // Recover the camera translation and rotation.
  AbsoluteOrientation(X_world, X_cam, R, t);
}

// Selects 4 virtual control points using mean and PCA.
void SelectControlPoints(const Mat3X &X_world,
                         Mat *X_centered,
                         Mat34 *X_control_points) {
  size_t num_points = X_world.cols();

  // The first virtual control point, C0, is the centroid.
  Vec mean, variance;
  MeanAndVarianceAlongRows(X_world, &mean, &variance);
  X_control_points->col(0) = mean;

  // Computes PCA
  X_centered->resize (3, num_points);
  for (size_t c = 0; c < num_points; c++) {
    X_centered->col(c) = X_world.col (c) - mean;
  }
  Mat3 X_centered_sq = (*X_centered) * X_centered->transpose();
  Eigen::JacobiSVD<Mat3> X_centered_sq_svd(X_centered_sq, Eigen::ComputeFullU);
  Vec3 w = X_centered_sq_svd.singularValues();
  Mat3 u = X_centered_sq_svd.matrixU();
  for (size_t c = 0; c < 3; c++) {
    double k = sqrt (w (c) / num_points);
    X_control_points->col (c + 1) = mean + k * u.col (c);
  }
}

// Computes the barycentric coordinates for all real points
void ComputeBarycentricCoordinates(const Mat3X &X_world_centered,
                                   const Mat34 &X_control_points,
                                   Mat4X *alphas) {
  size_t num_points = X_world_centered.cols();
  Mat3 C2 ;
  for (size_t c = 1; c < 4; c++) {
    C2.col(c-1) = X_control_points.col(c) - X_control_points.col(0);
  }

  Mat3 C2inv = C2.inverse();
  Mat3X a = C2inv * X_world_centered;

  alphas->resize(4, num_points);
  alphas->setZero();
  alphas->block(1, 0, 3, num_points) = a;
  for (size_t c = 0; c < num_points; c++) {
    (*alphas)(0, c) = 1.0 - alphas->col(c).sum();
  }
}

// Estimates the coordinates of all real points in the camera coordinate frame
void ComputePointsCoordinatesInCameraFrame(
    const Mat4X &alphas,
    const Vec4 &betas,
    const Eigen::Matrix<double, 12, 12> &U,
    Mat3X *X_camera) {
  size_t num_points = alphas.cols();

  // Estimates the control points in the camera reference frame.
  Mat34 C2b; C2b.setZero();
  for (size_t cu = 0; cu < 4; cu++) {
    for (size_t c = 0; c < 4; c++) {
      C2b.col(c) += betas(cu) * U.block(11 - cu, c * 3, 1, 3).transpose();
    }
  }

  // Estimates the 3D points in the camera reference frame
  X_camera->resize(3, num_points);
  for (size_t c = 0; c < num_points; c++) {
    X_camera->col(c) = C2b * alphas.col(c);
  }

  // Check the sign of the z coordinate of the points (should be positive)
  uint num_z_neg = 0;
  for (size_t i = 0; i < X_camera->cols(); ++i) {
    if ((*X_camera)(2,i) < 0) {
      num_z_neg++;
    }
  }

  // If more than 50% of z are negative, we change the signs
  if (num_z_neg > 0.5 * X_camera->cols()) {
    C2b = -C2b;
    *X_camera = -(*X_camera);
  }
}

bool EuclideanResectionEPnP(const Mat2X &x_camera,
                            const Mat3X &X_world,
                            Mat3 *R, Vec3 *t) {
  CHECK(x_camera.cols() == X_world.cols());
  CHECK(x_camera.cols() > 3);
  size_t num_points = X_world.cols();

  // Select the control points.
  Mat34 X_control_points;
  Mat X_centered;
  SelectControlPoints(X_world, &X_centered, &X_control_points);

  // Compute the barycentric coordinates.
  Mat4X alphas(4, num_points);
  ComputeBarycentricCoordinates(X_centered, X_control_points, &alphas);

  // Estimates the M matrix with the barycentric coordinates
  Mat M(2 * num_points, 12);
  Eigen::Matrix<double, 2, 12> sub_M;
  for (size_t c = 0; c < num_points; c++) {
    double a0 = alphas(0, c);
    double a1 = alphas(1, c);
    double a2 = alphas(2, c);
    double a3 = alphas(3, c);
    double ui = x_camera(0, c);
    double vi = x_camera(1, c);
    M.block(2*c, 0, 2, 12) << a0, 0,
                              a0*(-ui), a1, 0,
                              a1*(-ui), a2, 0,
                              a2*(-ui), a3, 0,
                              a3*(-ui), 0,
                              a0, a0*(-vi), 0,
                              a1, a1*(-vi), 0,
                              a2, a2*(-vi), 0,
                              a3, a3*(-vi);
  }

  // TODO(julien): Avoid the transpose by rewriting the u2.block() calls.
  Eigen::JacobiSVD<Mat> MtMsvd(M.transpose()*M, Eigen::ComputeFullU);
  Eigen::Matrix<double, 12, 12> u2 = MtMsvd.matrixU().transpose();

  // Estimate the L matrix.
  Eigen::Matrix<double, 6, 3> dv1;
  Eigen::Matrix<double, 6, 3> dv2;
  Eigen::Matrix<double, 6, 3> dv3;
  Eigen::Matrix<double, 6, 3> dv4;

  dv1.row(0) = u2.block(11, 0, 1, 3) - u2.block(11, 3, 1, 3);
  dv1.row(1) = u2.block(11, 0, 1, 3) - u2.block(11, 6, 1, 3);
  dv1.row(2) = u2.block(11, 0, 1, 3) - u2.block(11, 9, 1, 3);
  dv1.row(3) = u2.block(11, 3, 1, 3) - u2.block(11, 6, 1, 3);
  dv1.row(4) = u2.block(11, 3, 1, 3) - u2.block(11, 9, 1, 3);
  dv1.row(5) = u2.block(11, 6, 1, 3) - u2.block(11, 9, 1, 3);
  dv2.row(0) = u2.block(10, 0, 1, 3) - u2.block(10, 3, 1, 3);
  dv2.row(1) = u2.block(10, 0, 1, 3) - u2.block(10, 6, 1, 3);
  dv2.row(2) = u2.block(10, 0, 1, 3) - u2.block(10, 9, 1, 3);
  dv2.row(3) = u2.block(10, 3, 1, 3) - u2.block(10, 6, 1, 3);
  dv2.row(4) = u2.block(10, 3, 1, 3) - u2.block(10, 9, 1, 3);
  dv2.row(5) = u2.block(10, 6, 1, 3) - u2.block(10, 9, 1, 3);
  dv3.row(0) = u2.block( 9, 0, 1, 3) - u2.block( 9, 3, 1, 3);
  dv3.row(1) = u2.block( 9, 0, 1, 3) - u2.block( 9, 6, 1, 3);
  dv3.row(2) = u2.block( 9, 0, 1, 3) - u2.block( 9, 9, 1, 3);
  dv3.row(3) = u2.block( 9, 3, 1, 3) - u2.block( 9, 6, 1, 3);
  dv3.row(4) = u2.block( 9, 3, 1, 3) - u2.block( 9, 9, 1, 3);
  dv3.row(5) = u2.block( 9, 6, 1, 3) - u2.block( 9, 9, 1, 3);
  dv4.row(0) = u2.block( 8, 0, 1, 3) - u2.block( 8, 3, 1, 3);
  dv4.row(1) = u2.block( 8, 0, 1, 3) - u2.block( 8, 6, 1, 3);
  dv4.row(2) = u2.block( 8, 0, 1, 3) - u2.block( 8, 9, 1, 3);
  dv4.row(3) = u2.block( 8, 3, 1, 3) - u2.block( 8, 6, 1, 3);
  dv4.row(4) = u2.block( 8, 3, 1, 3) - u2.block( 8, 9, 1, 3);
  dv4.row(5) = u2.block( 8, 6, 1, 3) - u2.block( 8, 9, 1, 3);

  Eigen::Matrix<double, 6, 10> L;
  for (size_t r = 0; r < 6; r++) {
    L.row(r) << dv1.row(r).dot(dv1.row(r)),
          2.0 * dv1.row(r).dot(dv2.row(r)),
                dv2.row(r).dot(dv2.row(r)),
          2.0 * dv1.row(r).dot(dv3.row(r)),
          2.0 * dv2.row(r).dot(dv3.row(r)),
                dv3.row(r).dot(dv3.row(r)),
          2.0 * dv1.row(r).dot(dv4.row(r)),
          2.0 * dv2.row(r).dot(dv4.row(r)),
          2.0 * dv3.row(r).dot(dv4.row(r)),
                dv4.row(r).dot(dv4.row(r));
  }
  Vec6 rho;
  rho << (X_control_points.col(0) - X_control_points.col(1)).squaredNorm(),
         (X_control_points.col(0) - X_control_points.col(2)).squaredNorm(),
         (X_control_points.col(0) - X_control_points.col(3)).squaredNorm(),
         (X_control_points.col(1) - X_control_points.col(2)).squaredNorm(),
         (X_control_points.col(1) - X_control_points.col(3)).squaredNorm(),
         (X_control_points.col(2) - X_control_points.col(3)).squaredNorm();

  // There are three possible solutions based on the three approximations of L
  // (betas). Below, each one is solved for then the best one is chosen.
  Mat3X X_camera;
  Mat3 K; K.setIdentity();
  vector<Mat3> Rs(3);
  vector<Vec3> ts(3);
  Vec rmse(3);

  // TODO(julien): Document where the "1e-3" magical constant comes from below.

  // Find the first possible solution for R, t corresponding to:
  // Betas          = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33]
  // Betas_approx_1 = [b00 b01     b02         b03]
  Vec4 betas = Vec4::Zero();
  Eigen::Matrix<double, 6, 4> l_6x4;
  for (size_t r = 0; r < 6; r++) {
    l_6x4.row(r) << L(r, 0), L(r, 1), L(r, 3), L(r, 6);
  }
  Eigen::JacobiSVD<Mat> svd_of_l4(l_6x4,
                                  Eigen::ComputeFullU | Eigen::ComputeFullV);
  Vec4 b4 = svd_of_l4.solve(rho);
  if ((l_6x4 * b4).isApprox(rho, 1e-3)) {
    if (b4(0) < 0) {
      b4 = -b4;
    }
    b4(0) =  std::sqrt(b4(0));
    betas << b4(0), b4(1) / b4(0), b4(2) / b4(0), b4(3) / b4(0);
    ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera);
    AbsoluteOrientation(X_world, X_camera, &Rs[0], &ts[0]);
    rmse(0) = RootMeanSquareError(x_camera, X_world, K, Rs[0], ts[0]);
  } else {
    LOG(ERROR) << "First approximation of beta not good enough.";
    ts[0].setZero();
    rmse(0) = std::numeric_limits<double>::max();
  }

  // Find the second possible solution for R, t corresponding to:
  // Betas          = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33]
  // Betas_approx_2 = [b00 b01 b11]
  betas.setZero();
  Eigen::Matrix<double, 6, 3> l_6x3;
  l_6x3 = L.block(0, 0, 6, 3);
  Eigen::JacobiSVD<Mat> svdOfL3(l_6x3,
                                Eigen::ComputeFullU | Eigen::ComputeFullV);
  Vec3 b3 = svdOfL3.solve(rho);
  VLOG(2) << " rho = " << rho;
  VLOG(2) << " l_6x3 * b3 = " << l_6x3 * b3;
  if ((l_6x3 * b3).isApprox(rho, 1e-3)) {
    if (b3(0) < 0) {
      betas(0) = std::sqrt(-b3(0));
      betas(1) = (b3(2) < 0) ? std::sqrt(-b3(2)) : 0;
    } else {
      betas(0) = std::sqrt(b3(0));
      betas(1) = (b3(2) > 0) ? std::sqrt(b3(2)) : 0;
    }
    if (b3(1) < 0) {
      betas(0) = -betas(0);
    }
    betas(2) = 0;
    betas(3) = 0;
    ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera);
    AbsoluteOrientation(X_world, X_camera, &Rs[1], &ts[1]);
    rmse(1) = RootMeanSquareError(x_camera, X_world, K, Rs[1], ts[1]);
  } else {
    LOG(ERROR) << "Second approximation of beta not good enough.";
    ts[1].setZero();
    rmse(1) = std::numeric_limits<double>::max();
  }

  // Find the third possible solution for R, t corresponding to:
  // Betas          = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33]
  // Betas_approx_3 = [b00 b01 b11 b02 b12]
  betas.setZero();
  Eigen::Matrix<double, 6, 5> l_6x5;
  l_6x5 = L.block(0, 0, 6, 5);
  Eigen::JacobiSVD<Mat> svdOfL5(l_6x5,
                                Eigen::ComputeFullU | Eigen::ComputeFullV);
  Vec5 b5 = svdOfL5.solve(rho);
  if ((l_6x5 * b5).isApprox(rho, 1e-3)) {
    if (b5(0) < 0) {
      betas(0) = std::sqrt(-b5(0));
      if (b5(2) < 0) {
        betas(1) = std::sqrt(-b5(2));
      } else {
        b5(2) = 0;
      }
    } else {
      betas(0) = std::sqrt(b5(0));
      if (b5(2) > 0) {
        betas(1) = std::sqrt(b5(2));
      } else {
        b5(2) = 0;
      }
    }
    if (b5(1) < 0) {
      betas(0) = -betas(0);
    }
    betas(2) = b5(3) / betas(0);
    betas(3) = 0;
    ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera);
    AbsoluteOrientation(X_world, X_camera, &Rs[2], &ts[2]);
    rmse(2) = RootMeanSquareError(x_camera, X_world, K, Rs[2], ts[2]);
  } else {
    LOG(ERROR) << "Third approximation of beta not good enough.";
    ts[2].setZero();
    rmse(2) = std::numeric_limits<double>::max();
  }

  // Finally, with all three solutions, select the (R, t) with the best RMSE.
  VLOG(2) << "RMSE for solution 0: " << rmse(0);
  VLOG(2) << "RMSE for solution 1: " << rmse(0);
  VLOG(2) << "RMSE for solution 2: " << rmse(0);
  size_t n = 0;
  if (rmse(1) < rmse(0)) {
    n = 1;
  }
  if (rmse(2) < rmse(n)) {
    n = 2;
  }
  if (rmse(n) == std::numeric_limits<double>::max()) {
    LOG(ERROR) << "All three possibilities failed. Reporting failure.";
    return false;
  }

  VLOG(1) << "RMSE for best solution #" << n << ": " << rmse(n);
  *R = Rs[n];
  *t = ts[n];

  // TODO(julien): Improve the solutions with non-linear refinement.
  return true;
}

} // namespace resection
} // namespace libmv


// Software License Agreement (BSD-3-Clause)
//
// Copyright 2018 The University of North Carolina at Chapel Hill
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above
//    copyright notice, this list of conditions and the following
//    disclaimer in the documentation and/or other materials provided
//    with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
// OF THE POSSIBILITY OF SUCH DAMAGE.

//! @author Jeff Ichnowski

#pragma once
#ifndef NIGH_METRIC_IMPL_SO2_HPP
#define NIGH_METRIC_IMPL_SO2_HPP

#include <type_traits>
#include <cmath>
#include <Eigen/Dense>
#include "constants.hpp"

namespace unc::robotics::nigh::impl::so2 {
    template <typename S>
    std::enable_if_t<std::is_floating_point_v<S>, S>
    angularDistance(S a, S b) {
        S d = std::fmod(std::abs(a - b), impl::PI<S> * 2);
        return std::min(d, impl::PI<S>*2 - d);
    }

    template <int p, typename A, typename B>
    std::common_type_t<typename A::Scalar, typename B::Scalar>
    angularDistance(const Eigen::ArrayBase<A>& a, const Eigen::ArrayBase<B>& b) {
        using Scalar = std::common_type_t<typename A::Scalar, typename B::Scalar>;
        Eigen::Array<Scalar, A::RowsAtCompileTime, A::ColsAtCompileTime> r = (a - b).cwiseAbs();
        r -= (r / (2*impl::PI<Scalar>)).floor() * (2*impl::PI<Scalar>);
        r = (r < -impl::PI<Scalar>).select(r + 2*impl::PI<Scalar>, r);
        r = (r >  impl::PI<Scalar>).select(r - 2*impl::PI<Scalar>, r);
        return r.matrix().template lpNorm<p>();
    }

    template <int p, typename A, typename B>
    auto angularDistance(const Eigen::MatrixBase<A>& a, const Eigen::MatrixBase<B>& b) {
        return angularDistance<p>(a.array(), b.array());
    }

        // Returns the argument bound to the range -PI..PI
    template <typename Scalar>
    std::enable_if_t<std::is_floating_point_v<Scalar>, Scalar>
    bound(Scalar a) {
        if ((a = std::fmod(a, PI<Scalar>*2)) <= -PI<Scalar>)
            return a + PI<Scalar>*2;
        return (a > PI<Scalar>) ? a - PI<Scalar>*2 : a;
    }

    // computes the counter-clockwise distance from a to b
    template <typename Scalar>
    Scalar ccwDist(Scalar a, Scalar b) {
        Scalar d = std::fmod(b - a, PI<Scalar>*2);
        return d < 0 ? d + PI<Scalar>*2 : d;
    }

    // This method is exactly the same ad ccwDist except that two
    // overlapping values are considered 2*PI instead of 0.
    template <typename Scalar>
    Scalar ccwRange(Scalar a, Scalar b) {
        Scalar d = std::fmod(b - a, PI<Scalar>*2);
        return d <= 0 ? d + PI<Scalar>*2 : d;
    }

    template <typename Scalar>
    Scalar antipode(Scalar a) {
        return a <= 0 ? (a + PI<Scalar>) : (a - PI<Scalar>);
    }

    // Finds a split on a set of SO(2) values normalized to the range
    // [-pi,pi].  The split divides the values into two evenly sized
    // (_N/2) sets, and maximizes the distance of the bounds to the
    // split.
    template <typename Iter>
    auto split(Iter first, Iter last) {
        using Scalar = typename std::iterator_traits<Iter>::value_type;
        std::size_t n = std::distance(first, last);
        assert(n > 1); // can only split more than 1 element

        // The loop finds the `i` at which half elements are to the
        // left of the line bisecting line it define.  To find it we
        // test for `j = i + N/2`, that the ccw distance to `[j] < pi`
        // and `[j+1] > pi`.  The "best" split is the one that
        // maximizes the distance between the points closest to the
        // split, thus `[i]`, `[i+1]`, `[j]`, and `[j+1]`.

        // distances: pi-D(i,j), D(i,j+1)-pi, D(i,i+1)
        //
        //        i
        //        |@@@
        //        |@@@@
        //   -----X@@@@
        //   ####/|\@@@
        //    ##/ | \@
        //     j    j+1

        //          /
        // j+1 ----X
        //     ###/|\     |
        //     ##/ |@\    |
        //     #/  |@@\   |
        //     i       j

        //        j+1 j
        //      ###| /@
        //     ####|/@@@
        //     ####X@@@@
        //     ###/ \@@@
        //      #/   \@
        //      i

        std::sort(first, last);
        Scalar dBest = -1, split = 0;
        Iter i1 = first;
        Iter j1 = first + n/2;
        do {
            Iter i0 = i1;
            Iter j0 = j1;
            if (++i1 == last) i1 = first;
            if (++j1 == last) j1 = first;
            Scalar d0 = PI<Scalar> - so2::ccwDist(*i0, *j0); // vals[i], vals[j%n]);
            Scalar d1 = PI<Scalar> - so2::ccwDist(*j1, *i0); // vals[(j+1)%n], vals[i]);

            if (d0 >= 0 && d1 >= 0) {
                Scalar di = so2::ccwDist(*i0, *i1); // vals[i], vals[(i+1)%n]);
                // Scalar split = vals[i] + std::min(di, d1) * 0.5;

                Scalar range = 2*PI<Scalar> - (d0+d1);

                if (range < PI<Scalar>) {
                    // The range of values is less than half a circle.
                    // split halfway between i0 and i1
                    if (range > dBest) {
                        dBest = range;
                        split = *i0 + di * 0.5;
                    }
                } else {
                    // The range of values is more than half a circle
                    // split considering both sides of split plane.
                    // we cannot split halfway between i0 and i1 since
                    // it could result in moving j1 to the other side
                    // of the split.
                    //
                    // An easy split to do would be i0 + min(di,d1)/2,
                    // since that would be halfway from the split to
                    // the next bound.  This is also guaranteed to be
                    // in the bounds.
                    //
                    // A possibly better split is to try to maximize
                    // the sum of square distances from the split.
                    //
                    // define x as the split offset from i0.  The
                    // distances from the split plane are thus:
                    //
                    //   i0 to split = x
                    //   split to i1 = di - x
                    //   j0 to split = d0 + x
                    //   split to j1 = d1 - x
                    //
                    // summing the square of the above quantities,
                    // then differentiating and solving for 0, we get:
                    //
                    // x = (di + d1 - d0) / 4;

                    // Compute the range as the sum of distances from
                    // the split.  We add PI so that we prefer
                    // splitting these axes over axes that are already
                    // split.
                    Scalar dSum = PI<Scalar> + (di+d0+d1)/2; // std::min({di, d0, d1});
                    if (dSum > dBest) {
                        dBest = dSum;
                        split = *i0 + std::min(di, d1) * Scalar(0.5);
                    }
                }
            }
        } while (i1 != first);

        //if (sBest > PI<Scalar>) sBest -= 2*PI<Scalar>;
        split = so2::bound(split);

        assert(0 <= dBest && dBest <= 2*PI<Scalar>);
        assert(-PI<Scalar> <= split && split <= PI<Scalar>);

        return std::make_pair(dBest, split);
    }

    template <typename Container>
    auto split(Container& container) {
        return split(container.begin(), container.end());
    }
}

#endif


#include <gtest/gtest.h>
#include "../util/util.h"
#include <Eigen/Core>
#include <string>
#include <algorithm>

#ifndef _MSC_VER
extern "C" {
#include <csim/memory_ops.h>
#include <csim/stat_ops.h>
#include <csim/update_ops.h>
#include <csim/init_ops.h>
}
#else
#include <csim/memory_ops.h>
#include <csim/stat_ops.h>
#include <csim/update_ops.h>
#include <csim/init_ops.h>
#endif

TEST(UpdateTest, MultiQubitDiagonalMatrixTest) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;

	std::vector<UINT> index_list;
	for (UINT i = 0; i < n; ++i) index_list.push_back(i);

	Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U1, U2, U3;

	auto state = allocate_quantum_state(dim);
	initialize_Haar_random_state(state, dim);
	Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
	for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

	for (UINT gate_size = 1; gate_size <= 1; ++gate_size) {
		ITYPE gate_dim = (1ULL) << gate_size;
		for (UINT r = 0; r < max_repeat; ++r) {
			std::random_shuffle(index_list.begin(), index_list.end());
			auto diag = get_eigen_diagonal_matrix_random_multi_qubit_unitary(gate_size);
			Eigen::MatrixXcd matrix = Eigen::MatrixXcd::Zero(dim, dim);
			for (ITYPE i = 0; i < dim; ++i) {
				UINT ti = 0;
				for (int j = 0; j < gate_size; ++j) {
					UINT gi = index_list[j];
					ti += ((i >> gi) % 2) * (1 << j);
				}
				matrix(i, i) = diag[ti];
			}
			//std::cout << test_state << std::endl;
			//std::cout << diag << std::endl;
			//std::cout << matrix << std::endl;
			multi_qubit_diagonal_matrix_gate(index_list.data(), gate_size, (CTYPE*)diag.data(), state, dim);
			test_state = matrix * test_state;
			state_equal(state, test_state, dim, "diagonal gate");
		}
	}
	release_quantum_state(state);
}

TEST(UpdateTest, MultiQubitDiagonalMatrixTest2) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;

	std::vector<UINT> index_list;
	for (UINT i = 0; i < n; ++i) index_list.push_back(i);

	Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U1, U2, U3;

	auto state = allocate_quantum_state(dim);
	initialize_Haar_random_state(state, dim);
	Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
	for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

	for (UINT gate_size = 1; gate_size <= 1; ++gate_size) {
		ITYPE gate_dim = (1ULL) << gate_size;
		for (UINT r = 0; r < max_repeat; ++r) {
			std::random_shuffle(index_list.begin(), index_list.end());
			auto diag = get_eigen_diagonal_matrix_random_multi_qubit_unitary(gate_size);
			Eigen::MatrixXcd matrix = Eigen::MatrixXcd::Zero(dim, dim);
			for (ITYPE i = 0; i < dim; ++i) {
				UINT ti = 0;
				for (int j = 0; j < gate_size; ++j) {
					UINT gi = index_list[j];
					ti += ((i >> gi) % 2) * (1 << j);
				}
				matrix(i, i) = diag[ti];
			}
			//std::cout << test_state << std::endl;
			//std::cout << diag << std::endl;
			//std::cout << matrix << std::endl;
			multi_qubit_control_multi_qubit_diagonal_matrix_gate({}, {}, 0, index_list.data(), gate_size, (CTYPE*)diag.data(), state, dim);
			test_state = matrix * test_state;
			state_equal(state, test_state, dim, "diagonal gate");
		}
	}
	release_quantum_state(state);
}

TEST(UpdateTest, TwoQubitControlTwoQubitDiagonalMatrixTest) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;

	std::vector<UINT> index_list;
	for (UINT i = 0; i < n; ++i) index_list.push_back(i);

	Eigen::MatrixXcd P0(2, 2), P1(2, 2);
	P0 << 1, 0, 0, 0;
	P1 << 0, 0, 0, 1;

	Eigen::VectorXcd d, d2;

	UINT targets[2], controls[2], mvalues[2];

	auto state = allocate_quantum_state(dim);
	initialize_Haar_random_state(state, dim);
	Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
	for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

	Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

	for (UINT rep = 0; rep < max_repeat; ++rep) {

		// two qubit control-11 two qubit gate
		d = get_eigen_diagonal_matrix_random_multi_qubit_unitary(1);
		d2 = get_eigen_diagonal_matrix_random_multi_qubit_unitary(1);
		std::random_shuffle(index_list.begin(), index_list.end());
		targets[0] = index_list[0];
		targets[1] = index_list[1];
		controls[0] = index_list[2];
		controls[1] = index_list[3];

		mvalues[0] = 1; mvalues[1] = 1;
		Eigen::Vector4cd dmerge;
		dmerge(0) = d[0] * d2[0];
		dmerge(1) = d[1] * d2[0];
		dmerge(2) = d[0] * d2[1];
		dmerge(3) = d[1] * d2[1];
		multi_qubit_control_multi_qubit_diagonal_matrix_gate(controls, mvalues, 2, targets, 2, (CTYPE*)dmerge.data(), state, dim);

		Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U, U2;
		U(0, 0) = d[0]; U(1, 1) = d[1]; U(0, 1) = 0; U(1, 0) = 0;
		U2(0, 0) = d2[0]; U2(1, 1) = d2[1]; U2(0, 1) = 0; U2(1, 0) = 0;

		test_state = (
			get_expanded_eigen_matrix_with_identity(controls[0], P0, n)*get_expanded_eigen_matrix_with_identity(controls[1], P0, n) +
			get_expanded_eigen_matrix_with_identity(controls[0], P0, n)*get_expanded_eigen_matrix_with_identity(controls[1], P1, n) +
			get_expanded_eigen_matrix_with_identity(controls[0], P1, n)*get_expanded_eigen_matrix_with_identity(controls[1], P0, n) +
			get_expanded_eigen_matrix_with_identity(controls[0], P1, n)*get_expanded_eigen_matrix_with_identity(controls[1], P1, n)*get_expanded_eigen_matrix_with_identity(targets[0], U, n)*get_expanded_eigen_matrix_with_identity(targets[1], U2, n)
			) * test_state;
		state_equal(state, test_state, dim, "two qubit control two qubit diagonal gate");
	}
	release_quantum_state(state);
}


/*
 * Copyright 2017 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
 */

/*
 * File:   GeoreferencingTest.hpp
 * Author: glm, jordan
 */

#ifndef GEOREFERENCINGTEST_HPP
#define GEOREFERENCINGTEST_HPP

#include "catch.hpp"
#include <Eigen/Dense>
#include "../src/Position.hpp"
#include "../src/georeferencing/Georeferencing.hpp"
#include "../src/math/Boresight.hpp"
#include "../src/math/CoordinateTransform.hpp"
#include "../src/utils/Constants.hpp"
#include "../src/svp/SoundVelocityProfileFactory.hpp"
#include "../src/svp/CarisSvpFile.hpp"

#define POSITION_PRECISION 0.00000001

TEST_CASE("Georeferencing LGF test") {

    GeoreferencingLGF georef;

    /*Build centroid position*/
    double latitudeCentroidDegrees = 0.859286627204 * R2D;
    double longitudeCentroidDegrees = -1.189078930041 * R2D;
    double ellipsoidalCentroidHeight = -25.711914675768;
    Position positionCentroid(0, latitudeCentroidDegrees, longitudeCentroidDegrees, ellipsoidalCentroidHeight);

    georef.setCentroid(positionCentroid);

    /*Build attitude*/
    double rollDegrees = 0.0273983876 * R2D; // roll at receive
    double pitchDegrees = 0.0243184966 * R2D; // pitch at transmit
    double headingDegrees = 0.1056083942 * R2D; // heading at transmit
    Attitude attitude(0, rollDegrees, pitchDegrees, headingDegrees);

    /*Build Position*/
    double latitudeDegrees = 0.859282504615 * R2D;
    double longitudeDegrees = -1.189079133235 * R2D;
    double ellipsoidalHeight = -25.753195024025;
    Position position(0, latitudeDegrees, longitudeDegrees, ellipsoidalHeight);

    /*Build Ping*/
    uint64_t microEpoch = 0;
    long id = 0;
    uint32_t quality = 0;
    double intensity = 0;
    double surfaceSoundSpeed = 1446.4250488;
    double twoWayTravelTime = 0.0091418369 * 2;
    double alongTrackAngle = 0.0;
    double acrossTrackAngle = 0.7031931281 * R2D;

    Ping ping(
            microEpoch,
            id,
            quality,
            intensity,
            surfaceSoundSpeed,
            twoWayTravelTime,
            alongTrackAngle,
            acrossTrackAngle
            );

    /*Obtain svp*/
    std::string svpFilePath = "test/data/rayTracingTestData/SVP-0.svp";
    CarisSvpFile svps;
    svps.readSvpFile(svpFilePath);
    SoundVelocityProfile * svp = svps.getSvps()[0];

    Eigen::Vector3d leverArm;
    leverArm << 0.0, 0.0, 0.0;

    /*Build Boresight Matrix*/
    double rollBoresightDegrees = 0.62;
    double pitchBoresightDegrees = 0.0;
    double headingBoresightDegrees = 0.0;
    Attitude boresightAngles(0, rollBoresightDegrees, pitchBoresightDegrees, headingBoresightDegrees);
    Eigen::Matrix3d boresightMatrix;
    Boresight::buildMatrix(boresightMatrix, boresightAngles);

    /*Perform georeferencing in LGF*/
    Eigen::Vector3d georeferencedPing;
    georef.georeference(georeferencedPing, attitude, position, ping, *svp, leverArm, boresightMatrix);

    Eigen::Vector3d expectedGeoreferencedPing;
    expectedGeoreferencedPing << -26.8825997032, 7.3549385469, 10.4758625062;

    double georefTestTreshold = 2e-2; // 2cm
    REQUIRE(std::abs(expectedGeoreferencedPing(0) - georeferencedPing(0)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeoreferencedPing(1) - georeferencedPing(1)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeoreferencedPing(2) - georeferencedPing(2)) < georefTestTreshold);
}

TEST_CASE("Georeference TRF with position and downward ping only") {
    Eigen::Vector3d georefedPing;

    Attitude attitude(0, 0, 0, 0);
    Position position(0, 48.4525, -68.5232, 15.401);
    Ping ping(0, 0, 0, 0, 0, 0.01, 0.0, 0.0);
    SoundVelocityProfile * svp = SoundVelocityProfileFactory::buildFreshWaterModel();
    ping.setSurfaceSoundSpeed(svp->getSpeeds()(0)); //important now that raytracing uses it
    Eigen::Vector3d leverArm(0, 0, 0);
    Eigen::Matrix3d boresight = Eigen::Matrix3d::Identity();

    GeoreferencingTRF geo;
    geo.georeference(georefedPing, attitude, position, ping, *svp, leverArm, boresight);

    //std::cerr << "GEOREF TRF: " << georefedPing << std::endl;

    Position georefPosition(0, 0, 0, 0);

    CoordinateTransform::convertECEFToLongitudeLatitudeElevation(georefedPing, georefPosition);

    //std::cerr << "Final position: " << std::endl << georefPosition << std::endl << std::endl;

    REQUIRE(abs(georefPosition.getLongitude() - position.getLongitude()) < POSITION_PRECISION);
    REQUIRE(abs(georefPosition.getLatitude() - position.getLatitude()) < POSITION_PRECISION);
    REQUIRE(abs(georefPosition.getEllipsoidalHeight() - (position.getEllipsoidalHeight() - 7.4)) < POSITION_PRECISION);

    delete svp;
}

TEST_CASE("Georeference LGF with position and downward ping only") {

    Eigen::Vector3d georefedPing;

    Attitude attitude(0, 0, 0, 0);
    Position position(0, 48.4525, -68.5232, 15.401);
    Ping ping(0, 0, 0, 0, 0, 0.01, 0, 0);
    SoundVelocityProfile * svp = SoundVelocityProfileFactory::buildFreshWaterModel();
    ping.setSurfaceSoundSpeed(svp->getSpeeds()(0)); //important now that raytracing uses it
    Eigen::Vector3d leverArm(0, 0, 0);
    Eigen::Matrix3d boresight = Eigen::Matrix3d::Identity();

    /*georef LGF*/
    /*Build Centroid Position*/
    double centroidLatitude = 48.4525;
    double centroidLongitude = -68.5232;
    double centroidEllipsoidHeight = 15.401;
    Position centroidPosition(0, centroidLatitude, centroidLongitude, centroidEllipsoidHeight);

    GeoreferencingLGF geo;
    geo.setCentroid(centroidPosition);

    geo.georeference(georefedPing, attitude, position, ping, *svp, leverArm, boresight);

    //std::cerr << "GEOREF LGF: " << std::endl << georefedPing << std::endl << std::endl;

    Eigen::Vector3d expectedPosition(0, 0, 7.4);
    REQUIRE(georefedPing.isApprox(expectedPosition, POSITION_PRECISION));

    delete svp;
}

TEST_CASE("Georeference TRF with position and perpendicular unit vector ping and non-zero attitude") {

    /*Build Centroid Position*/
    double centroidLatitude = 48.4525;
    double centroidLongitude = -68.5232;
    double centroidEllipsoidHeight = 15.401;
    Position centroidPosition(0, centroidLatitude, centroidLongitude, centroidEllipsoidHeight);

    /*Build attitude*/
    double roll = 1.0;
    double pitch = 2.0;
    double heading = 45.0;
    Attitude attitude(0, roll, pitch, heading);

    /*Build Position*/
    double latitude = 48.4525;
    double longitude = -68.5232;
    double ellipsoidHeight = 15.401;
    Position position(0, latitude, longitude, ellipsoidHeight);

    /*Build Ping*/
    uint64_t microEpoch = 0;
    long id = 0;
    uint32_t quality = 0;
    double intensity = 0;
    double surfaceSoundSpeed = 0;
    double twoWayTravelTime = 0.01;
    double alongTrackAngle = 0.0;
    double acrossTrackAngle = 0.0;

    Ping ping(
            microEpoch,
            id,
            quality,
            intensity,
            surfaceSoundSpeed,
            twoWayTravelTime,
            alongTrackAngle,
            acrossTrackAngle
            );

    /*Build SVP*/
    SoundVelocityProfile * svp = SoundVelocityProfileFactory::buildFreshWaterModel();
    ping.setSurfaceSoundSpeed(svp->getSpeeds()(0)); //important now that raytracing uses it

    /*Build lever arm*/
    Eigen::Vector3d leverArm(0, 0, 0);

    /*Build boresight matrix*/
    Eigen::Matrix3d boresight = Eigen::Matrix3d::Identity();

    /*georef TRF*/
    GeoreferencingTRF geo;

    Eigen::Vector3d georefedPingTRF;
    geo.georeference(georefedPingTRF, attitude, position, ping, *svp, leverArm, boresight);

    /*georef LGF*/
    GeoreferencingLGF geoLGF;
    geoLGF.setCentroid(centroidPosition);

    Eigen::Vector3d georefedPingLGF;
    geoLGF.georeference(georefedPingLGF, attitude, position, ping, *svp, leverArm, boresight);

    /*Test georefTRF*/
    Eigen::Vector3d centroidTRF;
    CoordinateTransform::getPositionECEF(centroidTRF, centroidPosition);

    Eigen::Matrix3d ned2ecef;
    CoordinateTransform::ned2ecef(ned2ecef,centroidPosition);

    Eigen::Vector3d expectedGeorefedPingTRF = centroidTRF + ned2ecef*georefedPingLGF;

    double georefTestTreshold = 1e-5;
    REQUIRE(std::abs(expectedGeorefedPingTRF(0) - georefedPingTRF(0)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPingTRF(1) - georefedPingTRF(1)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPingTRF(2) - georefedPingTRF(2)) < georefTestTreshold);

    delete svp;
}

TEST_CASE("Georeference LGF with position and perpendicular unit vector ping and non-zero attitude"){
    /*Build Centroid Position*/
    double centroidLatitude = 48.4525;
    double centroidLongitude = -68.5232;
    double centroidEllipsoidHeight = 15.401;
    Position centroidPosition(0, centroidLatitude, centroidLongitude, centroidEllipsoidHeight);

    /*Build attitude*/
    double roll = 1.0; // 0.017453293
    double pitch = 2.0; // 0.034906585
    double heading = 45.0; // 0.785398185
    Attitude attitude(0, roll, pitch, heading);

    /*Build Position*/
    double latitude = 48.4525;
    double longitude = -68.5232;
    double ellipsoidHeight = 15.401;
    Position position(0, latitude, longitude, ellipsoidHeight);

    /*Build Ping*/
    uint64_t microEpoch = 0;
    long id = 0;
    uint32_t quality = 0;
    double intensity = 0;
    double surfaceSoundSpeed = 0;
    double twoWayTravelTime = 0.01;
    double alongTrackAngle = 0.0;
    double acrossTrackAngle = 0.0;

    Ping ping(
            microEpoch,
            id,
            quality,
            intensity,
            surfaceSoundSpeed,
            twoWayTravelTime,
            alongTrackAngle,
            acrossTrackAngle
            );

    /*Build SVP*/
    SoundVelocityProfile * svp = SoundVelocityProfileFactory::buildFreshWaterModel();
    ping.setSurfaceSoundSpeed(svp->getSpeeds()(0)); //important now that raytracing uses it

    /*Build lever arm*/
    Eigen::Vector3d leverArm(0, 0, 0);

    /*Build boresight matrix*/
    Eigen::Matrix3d boresight = Eigen::Matrix3d::Identity();

    /*georef LGF*/
    GeoreferencingLGF geo;
    geo.setCentroid(centroidPosition);

    Eigen::Vector3d georefedPing;
    geo.georeference(georefedPing, attitude, position, ping, *svp, leverArm, boresight);

    Eigen::Vector3d expectedGeorefedPing;
    expectedGeorefedPing << 0.2739082412, 0.0912656601, 7.3943657507;

    double georefTestTreshold = 1e-5;
    REQUIRE(std::abs(expectedGeorefedPing(0) - georefedPing(0)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPing(1) - georefedPing(1)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPing(2) - georefedPing(2)) < georefTestTreshold);

    delete svp;
}

TEST_CASE("Georeference TRF with position and perpendicular unit vector ping and non-zero attitude and non-zero lever-arm"){

    /*Build Centroid Position*/
    double centroidLatitude = 48.4525;
    double centroidLongitude = -68.5232;
    double centroidEllipsoidHeight = 15.401;
    Position centroidPosition(0, centroidLatitude, centroidLongitude, centroidEllipsoidHeight);

    /*Build attitude*/
    double roll = 1.0; // 0.017453293
    double pitch = 2.0; // 0.034906585
    double heading = 45.0; // 0.785398185
    Attitude attitude(0, roll, pitch, heading);

    /*Build Position*/
    double latitude = 48.4525;
    double longitude = -68.5232;
    double ellipsoidHeight = 15.401;
    Position position(0, latitude, longitude, ellipsoidHeight);

    /*Build Ping*/
    uint64_t microEpoch = 0;
    long id = 0;
    uint32_t quality = 0;
    double intensity = 0;
    double surfaceSoundSpeed = 0;
    double twoWayTravelTime = 0.01;
    double alongTrackAngle = 0.0;
    double acrossTrackAngle = 0.0;

    Ping ping(
            microEpoch,
            id,
            quality,
            intensity,
            surfaceSoundSpeed,
            twoWayTravelTime,
            alongTrackAngle,
            acrossTrackAngle
            );

    /*Build SVP*/
    SoundVelocityProfile * svp = SoundVelocityProfileFactory::buildFreshWaterModel();
    ping.setSurfaceSoundSpeed(svp->getSpeeds()(0)); //important now that raytracing uses it

    /*Build lever arm*/
    Eigen::Vector3d leverArm(1, 2, 3);

    /*Build boresight matrix*/
    Eigen::Matrix3d boresight = Eigen::Matrix3d::Identity();

    /*georef TRF*/
    GeoreferencingTRF geo;

    Eigen::Vector3d georefedPingTRF;
    geo.georeference(georefedPingTRF, attitude, position, ping, *svp, leverArm, boresight);

    /*georef LGF*/
    GeoreferencingLGF geoLGF;
    geoLGF.setCentroid(centroidPosition);

    Eigen::Vector3d georefedPingLGF;
    geoLGF.georeference(georefedPingLGF, attitude, position, ping, *svp, leverArm, boresight);

    /*Test georefTRF*/
    Eigen::Vector3d centroidTRF;
    CoordinateTransform::getPositionECEF(centroidTRF, centroidPosition);

    Eigen::Matrix3d ned2ecef;
    CoordinateTransform::ned2ecef(ned2ecef,centroidPosition);

    Eigen::Vector3d expectedGeorefedPingTRF = centroidTRF + ned2ecef*georefedPingLGF;

    double georefTestTreshold = 1e-5;
    REQUIRE(std::abs(expectedGeorefedPingTRF(0) - georefedPingTRF(0)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPingTRF(1) - georefedPingTRF(1)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPingTRF(2) - georefedPingTRF(2)) < georefTestTreshold);

    delete svp;
}

TEST_CASE("Georeference LGF with position and perpendicular unit vector ping and non-zero attitude and non-zero lever arm"){

    /*Build Centroid Position*/
    double centroidLatitude = 48.4525;
    double centroidLongitude = -68.5232;
    double centroidEllipsoidHeight = 15.401;
    Position centroidPosition(0, centroidLatitude, centroidLongitude, centroidEllipsoidHeight);

    /*Build attitude*/
    double roll = 1.0; // 0.017453293
    double pitch = 2.0; // 0.034906585
    double heading = 45.0; // 0.785398185
    Attitude attitude(0, roll, pitch, heading);

    /*Build Position*/
    double latitude = 48.4525;
    double longitude = -68.5232;
    double ellipsoidHeight = 15.401;
    Position position(0, latitude, longitude, ellipsoidHeight);

    /*Build Ping*/
    uint64_t microEpoch = 0;
    long id = 0;
    uint32_t quality = 0;
    double intensity = 0;
    double surfaceSoundSpeed = 0;
    double twoWayTravelTime = 0.01;
    double alongTrackAngle = 0.0;
    double acrossTrackAngle = 0.0;

    Ping ping(
            microEpoch,
            id,
            quality,
            intensity,
            surfaceSoundSpeed,
            twoWayTravelTime,
            alongTrackAngle,
            acrossTrackAngle
            );

    /*Build SVP*/
    SoundVelocityProfile * svp = SoundVelocityProfileFactory::buildFreshWaterModel();
    ping.setSurfaceSoundSpeed(svp->getSpeeds()(0)); //important now that raytracing uses it

    /*Build lever arm*/
    Eigen::Vector3d leverArm(1, 2, 3);

    /*Build boresight matrix*/
    Eigen::Matrix3d boresight = Eigen::Matrix3d::Identity();

    /*georef LGF*/
    GeoreferencingLGF geo;
    geo.setCentroid(centroidPosition);

    Eigen::Vector3d georefedPing;
    geo.georeference(georefedPing, attitude, position, ping, *svp, leverArm, boresight);

    Eigen::Vector3d expectedGeorefedPing;
    expectedGeorefedPing << -0.3215086477, 2.2498008231, 10.3920656486;

    double georefTestTreshold = 1e-5;
    REQUIRE(std::abs(expectedGeorefedPing(0) - georefedPing(0)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPing(1) - georefedPing(1)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPing(2) - georefedPing(2)) < georefTestTreshold);

    delete svp;
}

TEST_CASE("Georeference TRF with position and perpendicular unit vector ping and non-zero attitude and non-zero lever-arm and non-zero boresight"){
    /*Build Centroid Position*/
    double centroidLatitude = 48.4525;
    double centroidLongitude = -68.5232;
    double centroidEllipsoidHeight = 15.401;
    Position centroidPosition(0, centroidLatitude, centroidLongitude, centroidEllipsoidHeight);

    /*Build attitude*/
    double roll = 1.0; // 0.017453293
    double pitch = 2.0; // 0.034906585
    double heading = 45.0; // 0.785398185
    Attitude attitude(0, roll, pitch, heading);

    /*Build Position*/
    double latitude = 48.4525;
    double longitude = -68.5232;
    double ellipsoidHeight = 15.401;
    Position position(0, latitude, longitude, ellipsoidHeight);

    /*Build Ping*/
    uint64_t microEpoch = 0;
    long id = 0;
    uint32_t quality = 0;
    double intensity = 0;
    double surfaceSoundSpeed = 0;
    double twoWayTravelTime = 0.01;
    double alongTrackAngle = 0.0;
    double acrossTrackAngle = 0.0;

    Ping ping(
            microEpoch,
            id,
            quality,
            intensity,
            surfaceSoundSpeed,
            twoWayTravelTime,
            alongTrackAngle,
            acrossTrackAngle
            );

    /*Build SVP*/
    SoundVelocityProfile * svp = SoundVelocityProfileFactory::buildFreshWaterModel();
    ping.setSurfaceSoundSpeed(svp->getSpeeds()(0)); //important now that raytracing uses it

    /*Build lever arm*/
    Eigen::Vector3d leverArm(1, 2, 3);

    /*Build boresight matrix*/
    double rollBoresightDegrees = 0.4;
    double pitchBoresightDegrees = 0.5;
    double headingBoresightDegrees = 0.6;
    Attitude boresightAngles(0, rollBoresightDegrees, pitchBoresightDegrees, headingBoresightDegrees);
    Eigen::Matrix3d boresight;
    Boresight::buildMatrix(boresight, boresightAngles);

    /*georef TRF*/
    GeoreferencingTRF geo;

    Eigen::Vector3d georefedPingTRF;
    geo.georeference(georefedPingTRF, attitude, position, ping, *svp, leverArm, boresight);

    /*georef LGF*/
    GeoreferencingLGF geoLGF;
    geoLGF.setCentroid(centroidPosition);

    Eigen::Vector3d georefedPingLGF;
    geoLGF.georeference(georefedPingLGF, attitude, position, ping, *svp, leverArm, boresight);

    /*Test georefTRF*/
    Eigen::Vector3d centroidTRF;
    CoordinateTransform::getPositionECEF(centroidTRF, centroidPosition);

    Eigen::Matrix3d ned2ecef;
    CoordinateTransform::ned2ecef(ned2ecef,centroidPosition);

    Eigen::Vector3d expectedGeorefedPingTRF = centroidTRF + ned2ecef*georefedPingLGF;

    double georefTestTreshold = 1e-5;
    REQUIRE(std::abs(expectedGeorefedPingTRF(0) - georefedPingTRF(0)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPingTRF(1) - georefedPingTRF(1)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPingTRF(2) - georefedPingTRF(2)) < georefTestTreshold);

    delete svp;
}

TEST_CASE("Georeference LGF with position and perpendicular unit vector ping and non-zero attitude and non-zero lever arm and non-zero boresight"){

    /*Build Centroid Position*/
    double centroidLatitude = 48.4525;
    double centroidLongitude = -68.5232;
    double centroidEllipsoidHeight = 15.401;
    Position centroidPosition(0, centroidLatitude, centroidLongitude, centroidEllipsoidHeight);

    /*Build attitude*/
    double roll = 1.0; // 0.017453293
    double pitch = 2.0; // 0.034906585
    double heading = 45.0; // 0.785398185
    Attitude attitude(0, roll, pitch, heading);

    /*Build Position*/
    double latitude = 48.4525;
    double longitude = -68.5232;
    double ellipsoidHeight = 15.401;
    Position position(0, latitude, longitude, ellipsoidHeight);

    /*Build Ping*/
    uint64_t microEpoch = 0;
    long id = 0;
    uint32_t quality = 0;
    double intensity = 0;
    double surfaceSoundSpeed = 0;
    double twoWayTravelTime = 0.01;
    double alongTrackAngle = 0.0;
    double acrossTrackAngle = 0.0;

    Ping ping(
            microEpoch,
            id,
            quality,
            intensity,
            surfaceSoundSpeed,
            twoWayTravelTime,
            alongTrackAngle,
            acrossTrackAngle
            );

    /*Build SVP*/
    SoundVelocityProfile * svp = SoundVelocityProfileFactory::buildFreshWaterModel();
    ping.setSurfaceSoundSpeed(svp->getSpeeds()(0)); //important now that raytracing uses it

    /*Build lever arm*/
    Eigen::Vector3d leverArm(1, 2, 3);

    /*Build boresight matrix*/
    double rollBoresightDegrees = 0.4;
    double pitchBoresightDegrees = 0.5;
    double headingBoresightDegrees = 0.6;
    Attitude boresightAngles(0, rollBoresightDegrees, pitchBoresightDegrees, headingBoresightDegrees);
    Eigen::Matrix3d boresight;
    Boresight::buildMatrix(boresight, boresightAngles);

    /*georef LGF*/
    GeoreferencingLGF geo;
    geo.setCentroid(centroidPosition);

    Eigen::Vector3d georefedPing;
    geo.georeference(georefedPing, attitude, position, ping, *svp, leverArm, boresight);

    Eigen::Vector3d expectedGeorefedPing;
    expectedGeorefedPing << -0.2394900282, 2.2597419967, 10.3884422996;

    double georefTestTreshold = 1e-5;
    REQUIRE(std::abs(expectedGeorefedPing(0) - georefedPing(0)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPing(1) - georefedPing(1)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPing(2) - georefedPing(2)) < georefTestTreshold);

    delete svp;
}

TEST_CASE("Georeference TRF with position and perpendicular unit vector ping and non-zero attitude and non-zero lever-arm and non-zero boresight and centroid not colocated with position"){
    /*Build Centroid Position*/
    double centroidLatitude = 48.4525033333222;
    double centroidLongitude = -68.52320333332072;
    double centroidEllipsoidHeight = 15.267333333333;
    Position centroidPosition(0, centroidLatitude, centroidLongitude, centroidEllipsoidHeight);

    /*Build attitude*/
    double roll = 1.0; // 0.017453293
    double pitch = 2.0; // 0.034906585
    double heading = 45.0; // 0.785398185
    Attitude attitude(0, roll, pitch, heading);

    /*Build Position*/
    double latitude = 48.4525;
    double longitude = -68.5232;
    double ellipsoidHeight = 15.401;
    Position position(0, latitude, longitude, ellipsoidHeight);

    /*Build Ping*/
    uint64_t microEpoch = 0;
    long id = 0;
    uint32_t quality = 0;
    double intensity = 0;
    double surfaceSoundSpeed = 0;
    double twoWayTravelTime = 0.01;
    double alongTrackAngle = 0.0;
    double acrossTrackAngle = 0.0;

    Ping ping(
            microEpoch,
            id,
            quality,
            intensity,
            surfaceSoundSpeed,
            twoWayTravelTime,
            alongTrackAngle,
            acrossTrackAngle
            );

    /*Build SVP*/
    SoundVelocityProfile * svp = SoundVelocityProfileFactory::buildFreshWaterModel();
    ping.setSurfaceSoundSpeed(svp->getSpeeds()(0)); //important now that raytracing uses it

    /*Build lever arm*/
    Eigen::Vector3d leverArm(1, 2, 3);

    /*Build boresight matrix*/
    double rollBoresightDegrees = 0.4;
    double pitchBoresightDegrees = 0.5;
    double headingBoresightDegrees = 0.6;
    Attitude boresightAngles(0, rollBoresightDegrees, pitchBoresightDegrees, headingBoresightDegrees);
    Eigen::Matrix3d boresight;
    Boresight::buildMatrix(boresight, boresightAngles);

    /*georef TRF*/
    GeoreferencingTRF geo;

    Eigen::Vector3d georefedPingTRF;
    geo.georeference(georefedPingTRF, attitude, position, ping, *svp, leverArm, boresight);

    /*georef LGF*/
    GeoreferencingLGF geoLGF;
    geoLGF.setCentroid(centroidPosition);

    Eigen::Vector3d georefedPingLGF;
    geoLGF.georeference(georefedPingLGF, attitude, position, ping, *svp, leverArm, boresight);

    /*Test georefTRF*/
    Eigen::Vector3d centroidTRF;
    CoordinateTransform::getPositionECEF(centroidTRF, centroidPosition);

    Eigen::Matrix3d ned2ecef;
    CoordinateTransform::ned2ecef(ned2ecef,centroidPosition);

    Eigen::Vector3d expectedGeorefedPingTRF = centroidTRF + ned2ecef*georefedPingLGF;

    double georefTestTreshold = 1e-5;
    REQUIRE(std::abs(expectedGeorefedPingTRF(0) - georefedPingTRF(0)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPingTRF(1) - georefedPingTRF(1)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPingTRF(2) - georefedPingTRF(2)) < georefTestTreshold);

    delete svp;
}

TEST_CASE("Georeference LGF with position and perpendicular unit vector ping and non-zero attitude and non-zero lever arm and non-zero boresight and centroid not colocated with position"){

    /*Build Centroid Position*/
    double centroidLatitude = 48.4525033333222;
    double centroidLongitude = -68.52320333332072;
    double centroidEllipsoidHeight = 15.267333333333;
    Position centroidPosition(0, centroidLatitude, centroidLongitude, centroidEllipsoidHeight);

    /*Build attitude*/
    double roll = 1.0; // 0.017453293
    double pitch = 2.0; // 0.034906585
    double heading = 45.0; // 0.785398185
    Attitude attitude(0, roll, pitch, heading);

    /*Build Position*/
    double latitude = 48.4525;
    double longitude = -68.5232;
    double ellipsoidHeight = 15.401;
    Position position(0, latitude, longitude, ellipsoidHeight);

    /*Build Ping*/
    uint64_t microEpoch = 0;
    long id = 0;
    uint32_t quality = 0;
    double intensity = 0;
    double surfaceSoundSpeed = 0;
    double twoWayTravelTime = 0.01;
    double alongTrackAngle = 0.0;
    double acrossTrackAngle = 0.0;

    Ping ping(
            microEpoch,
            id,
            quality,
            intensity,
            surfaceSoundSpeed,
            twoWayTravelTime,
            alongTrackAngle,
            acrossTrackAngle
            );

    /*Build SVP*/
    SoundVelocityProfile * svp = SoundVelocityProfileFactory::buildFreshWaterModel();
    ping.setSurfaceSoundSpeed(svp->getSpeeds()(0)); //important now that raytracing uses it

    /*Build lever arm*/
    Eigen::Vector3d leverArm(1, 2, 3);

    /*Build boresight matrix*/
    double rollBoresightDegrees = 0.4;
    double pitchBoresightDegrees = 0.5;
    double headingBoresightDegrees = 0.6;
    Attitude boresightAngles(0, rollBoresightDegrees, pitchBoresightDegrees, headingBoresightDegrees);
    Eigen::Matrix3d boresight;
    Boresight::buildMatrix(boresight, boresightAngles);

    /*georef LGF*/
    GeoreferencingLGF geo;
    geo.setCentroid(centroidPosition);

    Eigen::Vector3d georefedPing;
    geo.georeference(georefedPing, attitude, position, ping, *svp, leverArm, boresight);

    Eigen::Vector3d expectedGeorefedPing;
    expectedGeorefedPing << -0.6101546444, 2.5063106918, 10.2547756501;

    double georefTestTreshold = 1e-5;
    REQUIRE(std::abs(expectedGeorefedPing(0) - georefedPing(0)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPing(1) - georefedPing(1)) < georefTestTreshold);
    REQUIRE(std::abs(expectedGeorefedPing(2) - georefedPing(2)) < georefTestTreshold);

    delete svp;
}

#endif /* GEOREFERENCINGTEST_HPP */