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

#pragma once

#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

namespace math {

inline static
bool is_power_of_two(int number){
	return !(number == 0) && !(number & (number - 1));
}

template<typename ScalarA, typename ScalarB>
inline static
bool are_dimensions_equal(
		const Eigen::Matrix<ScalarA,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor>& container_a,
		const Eigen::Matrix<ScalarB,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor>& container_b){
	return container_a.rows() == container_b.rows() && container_a.cols() == container_b.cols();
}

template<typename ScalarA, typename ScalarB, typename ScalarC>
inline static
bool are_dimensions_equal(
		const Eigen::Matrix<ScalarA,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor>& container_a,
		const Eigen::Matrix<ScalarB,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor>& container_b,
		const Eigen::Matrix<ScalarC,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor>& container_c){
	return container_a.rows() == container_b.rows() && container_a.cols() == container_b.cols()
			&& container_c.rows() == container_b.rows() && container_c.cols() == container_b.cols();
}

template<typename ScalarA, typename ScalarB>
inline static
bool are_dimensions_equal(
		const Eigen::Tensor<ScalarA,3,Eigen::ColMajor>& container_a,
		const Eigen::Tensor<ScalarB,3,Eigen::ColMajor>& container_b){
	for (int i_dim = 0; i_dim < 3; i_dim++) {
		if (container_a.dimension(i_dim) != container_b.dimension(i_dim)) {
			return false;
		}
	}
	return true;
}

template<typename ScalarA, typename ScalarB, typename ScalarC>
inline static
bool are_dimensions_equal(
		const Eigen::Tensor<ScalarA,3,Eigen::ColMajor>& container_a,
		const Eigen::Tensor<ScalarB,3,Eigen::ColMajor>& container_b,
		const Eigen::Tensor<ScalarC,3,Eigen::ColMajor>& container_c){
	for (int i_dim = 0; i_dim < 3; i_dim++) {
		if (container_a.dimension(i_dim) != container_b.dimension(i_dim) ||
				container_c.dimension(i_dim) != container_c.dimension(i_dim)) {
			return false;
		}
	}
	return true;
}

} //namespace math


/* Copyright (c) 2018-2019 Bradley Worley <geekysuavo@gmail.com>
 * Released under the MIT License.
 */

#pragma once
#include <random>
#include <vector>
#include <string>
#include <iostream>
#include <Eigen/Dense>

/* problem data initializers:
 *  @unif: whether or not to use uniform-amplitude impulses.
 *  @k: number of impulses in the weight vector.
 *  @sigma: measurement noise standard deviation.
 *  @seed: pseudorandom number generator seed.
 */
bool unif = true;
std::size_t k = 10;
double sigma = 0.001;
std::size_t seed = 47351;

/* weight prior parameters:
 *  @alpha0: shape.
 *  @beta0: rate.
 */
double alpha0 = 0.001;
double beta0 = 0.001;

/* noise prior parameters:
 *  @nu0: shape.
 *  @lambda0: rate.
 */
double nu0 = 0.001;
double lambda0 = 0.001;

/* algorithm parameters:
 *  @iters: iteration count.
 *  @burn: (monte carlo) burn-in iteration count.
 *  @thin: (monte carlo) thinning iteration count.
 */
std::size_t iters = 1000;
std::size_t burn = 10;
std::size_t thin = 1;

/* instance-constant expressions:
 *  @alpha: weight posterior shape parameter.
 *  @nu: noise posterior shape parameter.
 */
constexpr double pi = 3.14159265358979323846264338327950288;
double alpha;
double nu;

/* global pseudorandom number generator:
 */
std::default_random_engine gen;

/* instance data:
 *  @A: measurement matrix.
 *  @x0: true weight vector.
 *  @y: data vector.
 */
Eigen::Matrix<double, m, n> A;
Eigen::Matrix<double, n, 1> x0;
Eigen::Matrix<double, m, 1> y;

/* precomputed data:
 *  @a: vector of diagonal elements of the measurement gramian.
 *  @L: twice the maximal eigenvalue of the measurement gramian.
 *  @phi0: constant offset term of the universal sbl objective.
 */
Eigen::Matrix<double, n, 1> a;
double L, phi0;

/* instance_init(): initialize the current problem instance.
 */
static void instance_init (int argc, char **argv) {
  /* parse runtime arguments. */
  for (std::size_t i = 1; i < argc; i++) {
    /* get the current argument. */
    std::string arg(argv[i]);
    auto idx = arg.find_first_of('=');
    if (idx == std::string::npos)
      continue;

    /* split the argument into key=val. */
    auto key = arg.substr(0, idx);
    auto val = arg.substr(idx + 1);

    /* run some dirty argument parsing. */
    if (key.compare("unif") == 0) {
      if (val.compare("true") == 0)       unif = true;
      else if (val.compare("false") == 0) unif = false;
    }
    else if (key.compare("k")     == 0) { k = std::stoi(val); }
    else if (key.compare("sigma") == 0) { sigma = std::stod(val); }
    else if (key.compare("seed")  == 0) { seed = std::stoull(val); }
    else if (key.compare("alpha0") == 0) { alpha0 = std::stod(val); }
    else if (key.compare("beta0")  == 0) { beta0 = std::stod(val); }
    else if (key.compare("nu0")     == 0) { nu0 = std::stod(val); }
    else if (key.compare("lambda0") == 0) { lambda0 = std::stod(val); }
    else if (key.compare("iters") == 0) { iters = std::stoul(val); }
    else if (key.compare("burn")  == 0) { burn = std::stoul(val); }
    else if (key.compare("thin")  == 0) { thin = std::stoul(val); }
  }

  /* update the posterior shape parameters. */
  alpha = alpha0 + 0.5;
  nu = nu0 + 0.5 * m;

  /* prepare to sample from two distributions:
   *  @idx: {0, 1, 2, ..., n-1}.
   *  @nrm: N(0, 1).
   */
  std::uniform_int_distribution<std::size_t> idx{0, n - 1};
  std::uniform_int_distribution<std::size_t> bin{0, 1};
  std::normal_distribution<double> nrm{0, 1};
  gen.seed(seed);

  /* compute each row of the measurement matrix. */
  for (std::size_t i = 0; i < m; i++) {
    /* sample the row elements from a standard normal. */
    for (std::size_t j = 0; j < n; j++)
      A(i,j) = nrm(gen);

    /* normalize the row to unit length. */
    A.row(i).normalize();
  }

  /* fill the feature vector with spikes. */
  std::size_t spikes = 0;
  x0.setZero();
  do {
    /* sample a new element index. */
    std::size_t j = idx(gen);
    if (x0(j) != 0)
      continue;

    /* sample a random spike intensity. */
    double xj = 0;
    if (unif)
      xj = (bin(gen) ? 1 : -1);
    else
      xj = nrm(gen);

    /* store the spike intensity. */
    x0(j) = xj;
    spikes++;
  }
  while (spikes < k);

  /* compute the noise-free data vector. */
  y = A * x0;

  /* check if the noise is nonzero. */
  if (sigma > 0) {
    /* add noise to the data vector. */
    for (std::size_t i = 0; i < m; i++)
      y(i) += sigma * nrm(gen);
  }

  /* construct an eigenvalue solver for the measurement matrix gramian. */
  Eigen::Matrix<double, n, n> AtA = A.transpose() * A;
  Eigen::EigenSolver<decltype(AtA)> es(AtA, false);

  /* get the diagonal elements of the gramian. */
  a = AtA.diagonal();

  /* identify the maximal eigenvalue of the gramian. */
  L = es.eigenvalues()(0).real();
  for (std::size_t j = 1; j < n; j++)
    L = std::max(L, es.eigenvalues()(j).real());

  /* double the result (should be twice the maximal eigenvalue). */
  L *= 2;

  /* compute the constant offset to the sbl objective. */
  phi0 = 0.5 * (m + n) * std::log(2 * pi)
       - nu0 * std::log(lambda0)
       - n * alpha0 * std::log(beta0)
       - (std::lgamma(nu) - std::lgamma(nu0))
       - n * (std::lgamma(alpha) - std::lgamma(alpha0));
}


#include <iostream>
#include <boost/numeric/interval.hpp>

int main()
{
	auto range1 = boost::numeric::hull(0, 100);
	//auto range2 = boost::numeric::hull(20.5, 120);
	auto range2 = boost::numeric::hull(20.5, 120.0);

	std::cout << range1.lower() << " ~ "
		<< range1.upper() << std::endl;

	std::cout << range2.lower() << " ~ "
		<< range2.upper() << std::endl;

	return 0;
}


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

#ifndef BOOST_MATH_AIRY_HPP
#define BOOST_MATH_AIRY_HPP

#include <limits>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/cbrt.hpp>
#include <boost/math/special_functions/detail/airy_ai_bi_zero.hpp>
#include <boost/math/tools/roots.hpp>

namespace boost{ namespace math{

namespace detail{

template <class T, class Policy>
T airy_ai_imp(T x, const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(x < 0)
   {
      T p = (-x * sqrt(-x) * 2) / 3;
      T v = T(1) / 3;
      T j1 = boost::math::cyl_bessel_j(v, p, pol);
      T j2 = boost::math::cyl_bessel_j(-v, p, pol);
      T ai = sqrt(-x) * (j1 + j2) / 3;
      //T bi = sqrt(-x / 3) * (j2 - j1);
      return ai;
   }
   else if(fabs(x * x * x) / 6 < tools::epsilon<T>())
   {
      T tg = boost::math::tgamma(constants::twothirds<T>(), pol);
      T ai = 1 / (pow(T(3), constants::twothirds<T>()) * tg);
      //T bi = 1 / (sqrt(boost::math::cbrt(T(3))) * tg);
      return ai;
   }
   else
   {
      T p = 2 * x * sqrt(x) / 3;
      T v = T(1) / 3;
      //T j1 = boost::math::cyl_bessel_i(-v, p, pol);
      //T j2 = boost::math::cyl_bessel_i(v, p, pol);
      //
      // Note that although we can calculate ai from j1 and j2, the accuracy is horrible
      // as we're subtracting two very large values, so use the Bessel K relation instead:
      //
      T ai = cyl_bessel_k(v, p, pol) * sqrt(x / 3) / boost::math::constants::pi<T>();  //sqrt(x) * (j1 - j2) / 3;
      //T bi = sqrt(x / 3) * (j1 + j2);
      return ai;
   }
}

template <class T, class Policy>
T airy_bi_imp(T x, const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(x < 0)
   {
      T p = (-x * sqrt(-x) * 2) / 3;
      T v = T(1) / 3;
      T j1 = boost::math::cyl_bessel_j(v, p, pol);
      T j2 = boost::math::cyl_bessel_j(-v, p, pol);
      //T ai = sqrt(-x) * (j1 + j2) / 3;
      T bi = sqrt(-x / 3) * (j2 - j1);
      return bi;
   }
   else if(fabs(x * x * x) / 6 < tools::epsilon<T>())
   {
      T tg = boost::math::tgamma(constants::twothirds<T>(), pol);
      //T ai = 1 / (pow(T(3), constants::twothirds<T>()) * tg);
      T bi = 1 / (sqrt(boost::math::cbrt(T(3))) * tg);
      return bi;
   }
   else
   {
      T p = 2 * x * sqrt(x) / 3;
      T v = T(1) / 3;
      T j1 = boost::math::cyl_bessel_i(-v, p, pol);
      T j2 = boost::math::cyl_bessel_i(v, p, pol);
      T bi = sqrt(x / 3) * (j1 + j2);
      return bi;
   }
}

template <class T, class Policy>
T airy_ai_prime_imp(T x, const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(x < 0)
   {
      T p = (-x * sqrt(-x) * 2) / 3;
      T v = T(2) / 3;
      T j1 = boost::math::cyl_bessel_j(v, p, pol);
      T j2 = boost::math::cyl_bessel_j(-v, p, pol);
      T aip = -x * (j1 - j2) / 3;
      return aip;
   }
   else if(fabs(x * x) / 2 < tools::epsilon<T>())
   {
      T tg = boost::math::tgamma(constants::third<T>(), pol);
      T aip = 1 / (boost::math::cbrt(T(3)) * tg);
      return -aip;
   }
   else
   {
      T p = 2 * x * sqrt(x) / 3;
      T v = T(2) / 3;
      //T j1 = boost::math::cyl_bessel_i(-v, p, pol);
      //T j2 = boost::math::cyl_bessel_i(v, p, pol);
      //
      // Note that although we can calculate ai from j1 and j2, the accuracy is horrible
      // as we're subtracting two very large values, so use the Bessel K relation instead:
      //
      T aip = -cyl_bessel_k(v, p, pol) * x / (boost::math::constants::root_three<T>() * boost::math::constants::pi<T>());
      return aip;
   }
}

template <class T, class Policy>
T airy_bi_prime_imp(T x, const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(x < 0)
   {
      T p = (-x * sqrt(-x) * 2) / 3;
      T v = T(2) / 3;
      T j1 = boost::math::cyl_bessel_j(v, p, pol);
      T j2 = boost::math::cyl_bessel_j(-v, p, pol);
      T aip = -x * (j1 + j2) / constants::root_three<T>();
      return aip;
   }
   else if(fabs(x * x) / 2 < tools::epsilon<T>())
   {
      T tg = boost::math::tgamma(constants::third<T>(), pol);
      T bip = sqrt(boost::math::cbrt(T(3))) / tg;
      return bip;
   }
   else
   {
      T p = 2 * x * sqrt(x) / 3;
      T v = T(2) / 3;
      T j1 = boost::math::cyl_bessel_i(-v, p, pol);
      T j2 = boost::math::cyl_bessel_i(v, p, pol);
      T aip = x * (j1 + j2) / boost::math::constants::root_three<T>();
      return aip;
   }
}

template <class T, class Policy>
T airy_ai_zero_imp(int m, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names, needed for log, sqrt.

   // Handle cases when a negative zero (negative rank) is requested.
   if(m < 0)
   {
      return policies::raise_domain_error<T>("boost::math::airy_ai_zero<%1%>(%1%, int)",
                                             "Requested the %1%'th zero, but the rank must be 1 or more !", m, pol);
   }

   // Handle case when the zero'th zero is requested.
   if(m == 0U)
   {
      return policies::raise_domain_error<T>("boost::math::airy_ai_zero<%1%>(%1%,%1%)",
        "The requested rank of the zero is %1%, but must be 1 or more !", static_cast<T>(m), pol);
   }

   // Set up the initial guess for the upcoming root-finding.
   const T guess_root = boost::math::detail::airy_zero::airy_ai_zero_detail::initial_guess<T>(m);

   // Select the maximum allowed iterations based on the number
   // of decimal digits in the numeric type T, being at least 12.
   const int my_digits10 = static_cast<int>(static_cast<float>(policies::digits<T, Policy>() * 0.301F));

   const boost::uintmax_t iterations_allowed = static_cast<boost::uintmax_t>((std::max)(12, my_digits10 * 2));

   boost::uintmax_t iterations_used = iterations_allowed;

   // Use a dynamic tolerance because the roots get closer the higher m gets.
   T tolerance;

   if     (m <=   10) { tolerance = T(0.3F); }
   else if(m <=  100) { tolerance = T(0.1F); }
   else if(m <= 1000) { tolerance = T(0.05F); }
   else               { tolerance = T(1) / sqrt(T(m)); }

   // Perform the root-finding using Newton-Raphson iteration from Boost.Math.
   const T am =
      boost::math::tools::newton_raphson_iterate(
         boost::math::detail::airy_zero::airy_ai_zero_detail::function_object_ai_and_ai_prime<T, Policy>(pol),
         guess_root,
         T(guess_root - tolerance),
         T(guess_root + tolerance),
         policies::digits<T, Policy>(),
         iterations_used);

   static_cast<void>(iterations_used);

   return am;
}

template <class T, class Policy>
T airy_bi_zero_imp(int m, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names, needed for log, sqrt.

   // Handle cases when a negative zero (negative rank) is requested.
   if(m < 0)
   {
      return policies::raise_domain_error<T>("boost::math::airy_bi_zero<%1%>(%1%, int)",
                                             "Requested the %1%'th zero, but the rank must 1 or more !", m, pol);
   }

   // Handle case when the zero'th zero is requested.
   if(m == 0U)
   {
      return policies::raise_domain_error<T>("boost::math::airy_bi_zero<%1%>(%1%,%1%)",
        "The requested rank of the zero is %1%, but must be 1 or more !", static_cast<T>(m), pol);
   }
   // Set up the initial guess for the upcoming root-finding.
   const T guess_root = boost::math::detail::airy_zero::airy_bi_zero_detail::initial_guess<T>(m);

   // Select the maximum allowed iterations based on the number
   // of decimal digits in the numeric type T, being at least 12.
   const int my_digits10 = static_cast<int>(static_cast<float>(policies::digits<T, Policy>() * 0.301F));

   const boost::uintmax_t iterations_allowed = static_cast<boost::uintmax_t>((std::max)(12, my_digits10 * 2));

   boost::uintmax_t iterations_used = iterations_allowed;

   // Use a dynamic tolerance because the roots get closer the higher m gets.
   T tolerance;

   if     (m <=   10) { tolerance = T(0.3F); }
   else if(m <=  100) { tolerance = T(0.1F); }
   else if(m <= 1000) { tolerance = T(0.05F); }
   else               { tolerance = T(1) / sqrt(T(m)); }

   // Perform the root-finding using Newton-Raphson iteration from Boost.Math.
   const T bm =
      boost::math::tools::newton_raphson_iterate(
         boost::math::detail::airy_zero::airy_bi_zero_detail::function_object_bi_and_bi_prime<T, Policy>(pol),
         guess_root,
         T(guess_root - tolerance),
         T(guess_root + tolerance),
         policies::digits<T, Policy>(),
         iterations_used);

   static_cast<void>(iterations_used);

   return bm;
}

} // namespace detail

template <class T, class Policy>
inline typename tools::promote_args<T>::type airy_ai(T x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, Policy>(detail::airy_ai_imp<value_type>(static_cast<value_type>(x), forwarding_policy()), "boost::math::airy<%1%>(%1%)");
}

template <class T>
inline typename tools::promote_args<T>::type airy_ai(T x)
{
   return airy_ai(x, policies::policy<>());
}

template <class T, class Policy>
inline typename tools::promote_args<T>::type airy_bi(T x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, Policy>(detail::airy_bi_imp<value_type>(static_cast<value_type>(x), forwarding_policy()), "boost::math::airy<%1%>(%1%)");
}

template <class T>
inline typename tools::promote_args<T>::type airy_bi(T x)
{
   return airy_bi(x, policies::policy<>());
}

template <class T, class Policy>
inline typename tools::promote_args<T>::type airy_ai_prime(T x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, Policy>(detail::airy_ai_prime_imp<value_type>(static_cast<value_type>(x), forwarding_policy()), "boost::math::airy<%1%>(%1%)");
}

template <class T>
inline typename tools::promote_args<T>::type airy_ai_prime(T x)
{
   return airy_ai_prime(x, policies::policy<>());
}

template <class T, class Policy>
inline typename tools::promote_args<T>::type airy_bi_prime(T x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, Policy>(detail::airy_bi_prime_imp<value_type>(static_cast<value_type>(x), forwarding_policy()), "boost::math::airy<%1%>(%1%)");
}

template <class T>
inline typename tools::promote_args<T>::type airy_bi_prime(T x)
{
   return airy_bi_prime(x, policies::policy<>());
}

template <class T, class Policy>
inline T airy_ai_zero(int m, const Policy& /*pol*/)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename policies::evaluation<T, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   BOOST_STATIC_ASSERT_MSG(    false == std::numeric_limits<T>::is_specialized
                           || (   true  == std::numeric_limits<T>::is_specialized
                               && false == std::numeric_limits<T>::is_integer),
                           "Airy value type must be a floating-point type.");

   return policies::checked_narrowing_cast<T, Policy>(detail::airy_ai_zero_imp<value_type>(m, forwarding_policy()), "boost::math::airy_ai_zero<%1%>(unsigned)");
}

template <class T>
inline T airy_ai_zero(int m)
{
   return airy_ai_zero<T>(m, policies::policy<>());
}

template <class T, class OutputIterator, class Policy>
inline OutputIterator airy_ai_zero(
                         int start_index,
                         unsigned number_of_zeros,
                         OutputIterator out_it,
                         const Policy& pol)
{
   typedef T result_type;

   BOOST_STATIC_ASSERT_MSG(    false == std::numeric_limits<T>::is_specialized
                           || (   true  == std::numeric_limits<T>::is_specialized
                               && false == std::numeric_limits<T>::is_integer),
                           "Airy value type must be a floating-point type.");

   for(unsigned i = 0; i < number_of_zeros; ++i)
   {
      *out_it = boost::math::airy_ai_zero<result_type>(start_index + i, pol);
      ++out_it;
   }
   return out_it;
}

template <class T, class OutputIterator>
inline OutputIterator airy_ai_zero(
                         int start_index,
                         unsigned number_of_zeros,
                         OutputIterator out_it)
{
   return airy_ai_zero<T>(start_index, number_of_zeros, out_it, policies::policy<>());
}

template <class T, class Policy>
inline T airy_bi_zero(int m, const Policy& /*pol*/)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename policies::evaluation<T, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   BOOST_STATIC_ASSERT_MSG(    false == std::numeric_limits<T>::is_specialized
                           || (   true  == std::numeric_limits<T>::is_specialized
                               && false == std::numeric_limits<T>::is_integer),
                           "Airy value type must be a floating-point type.");

   return policies::checked_narrowing_cast<T, Policy>(detail::airy_bi_zero_imp<value_type>(m, forwarding_policy()), "boost::math::airy_bi_zero<%1%>(unsigned)");
}

template <typename T>
inline T airy_bi_zero(int m)
{
   return airy_bi_zero<T>(m, policies::policy<>());
}

template <class T, class OutputIterator, class Policy>
inline OutputIterator airy_bi_zero(
                         int start_index,
                         unsigned number_of_zeros,
                         OutputIterator out_it,
                         const Policy& pol)
{
   typedef T result_type;

   BOOST_STATIC_ASSERT_MSG(    false == std::numeric_limits<T>::is_specialized
                           || (   true  == std::numeric_limits<T>::is_specialized
                               && false == std::numeric_limits<T>::is_integer),
                           "Airy value type must be a floating-point type.");

   for(unsigned i = 0; i < number_of_zeros; ++i)
   {
      *out_it = boost::math::airy_bi_zero<result_type>(start_index + i, pol);
      ++out_it;
   }
   return out_it;
}

template <class T, class OutputIterator>
inline OutputIterator airy_bi_zero(
                         int start_index,
                         unsigned number_of_zeros,
                         OutputIterator out_it)
{
   return airy_bi_zero<T>(start_index, number_of_zeros, out_it, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_AIRY_HPP


/*
 * Copyright (c) 2016 Shanghai Jiao Tong University.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 * For more about this software visit:
 *
 *      http://ipads.se.sjtu.edu.cn/projects/wukong
 *
 */
#pragma once
#include <Eigen/Dense>

#include "optimizer/stats_type.hpp"

#include "utils/logger2.hpp"

namespace wukong {

#define L2U_FACTOR_NUM 2
#define K2U_FACTOR_NUM 4
#define K2L_FACTOR_NUM 5
#define K2K_FACTOR_NUM 5

using namespace Eigen;

class CostModel{
    private:
     MatrixXd matrix_l2u;
     VectorXd vector_l2u;
     double b_l2u = 0.046;  // explore, const
     double d_l2u = 0;

     MatrixXd matrix_k2u;
     VectorXd vector_k2u;
     double a1_k2u = 0.07;
     double a2_k2u = 0.09;
     double b_k2u = 0.01;
     double d_k2u = 0;  // init-prune, prune, explore, const

     MatrixXd matrix_k2l;
     VectorXd vector_k2l;
     double a1_k2l = 0.013;
     double a2_k2l = 0.015;
     double b_k2l = 0.012;
     double c_k2l = 0;
     double d_k2l = 0;  // init-prune, prune, explore, match, const

     MatrixXd matrix_k2k;
     VectorXd vector_k2k;
     double a1_k2k = 0.44;
     double a2_k2k = 0.01;
     double b_k2k = -0.022;
     double c_k2k = 0;
     double d_k2k = 0;  // init-prune, prune, explore, match, const

    public:
     void print(){
        logstream(LOG_INFO) << "cost model factor: l2u " << b_l2u << "  " << d_l2u << " " << LOG_endl;
        logstream(LOG_INFO)
            << "cost model factor: k2u " << a1_k2u << "  " << a2_k2u << " "
            << b_k2u << "  " << d_k2u << " " << LOG_endl;
        logstream(LOG_INFO)
            << "cost model factor: k2l " << a1_k2l << "  " << a2_k2l << " "
            << b_k2l << "  " << c_k2l << "  " << d_k2l << " " << LOG_endl;
        logstream(LOG_INFO)
            << "cost model factor: k2k " << a1_k2k << "  " << a2_k2k << " "
            << b_k2k << "  " << c_k2k << "  " << d_k2k << " " << LOG_endl;
     }

     void calculate(CostResult &result){
        if(result.current_model != model_t::L2U && result.init_bind == 0){
            result.add_cost = 0;
            return;
        }
        switch (result.current_model)
         {
         case model_t::T2U:
         case model_t::L2U:
             result.add_cost = result.explore_bind * b_l2u + d_l2u;
             break;
         case model_t::K2U:
             result.add_cost = (result.init_bind-result.prune_bind) * a1_k2u + result.prune_bind * a2_k2u +
                        result.explore_bind * b_k2u + d_k2u;
             break;
         case model_t::K2L:{
             result.add_cost = (result.init_bind - result.prune_bind) * a1_k2l +
                               result.prune_bind * a2_k2l + result.explore_bind * b_k2l + d_k2l;
         }
             break;
         case model_t::K2K:{
             result.add_cost = (result.init_bind - result.prune_bind) * a1_k2k +
                               result.prune_bind * a2_k2k + result.explore_bind * b_k2k + d_k2k;
         }
             break;
         default:
             break;
         }

         if(result.add_cost < 0)
            result.add_cost = 0;
     }

     void generate_factor(model_t model_type=model_t::ALL){
         VectorXd result;
         switch (model_type)
         {
         case model_t::ALL:
             result = matrix_l2u.jacobiSvd(ComputeThinU | ComputeThinV).solve(vector_l2u);
             b_l2u = abs(result(0)); d_l2u = result(1);
             result = matrix_k2u.jacobiSvd(ComputeThinU | ComputeThinV).solve(vector_k2u);
             a1_k2u = abs(result(0)); a2_k2u = abs(result(1)); b_k2u = abs(result(2));
             d_k2u = result(3);
             result = matrix_k2l.jacobiSvd(ComputeThinU | ComputeThinV).solve(vector_k2l);
             a1_k2l = abs(result(0)); a2_k2l = abs(result(1)); b_k2l = abs(result(2));
             c_k2l = abs(result(3)); d_k2l = result(4);
             break;
         case model_t::L2U:
             result = matrix_l2u.jacobiSvd(ComputeThinU | ComputeThinV).solve(vector_l2u);
             b_l2u = result(0); d_l2u = result(1);
             break;
         case model_t::K2U:
             result = matrix_k2u.jacobiSvd(ComputeThinU | ComputeThinV).solve(vector_k2u);
             a1_k2u = result(0); a2_k2u = result(1); b_k2u = result(2);d_k2u = result(3);
             break;
         case model_t::K2L:
             result = matrix_k2l.jacobiSvd(ComputeThinU | ComputeThinV).solve(vector_k2l);
             a1_k2l = result(0); a2_k2l = result(1); b_k2l = result(2);
             b_k2l = result(3); d_k2l = result(4);
             break;
         case model_t::K2K:
             result = matrix_k2k.jacobiSvd(ComputeThinU | ComputeThinV).solve(vector_k2k);
             a1_k2k = result(0); a2_k2k = result(1); b_k2k = result(2);
             c_k2k = result(3); d_k2k = result(4);
             break;
         default:
             break;
         }

         print();
     }

     void add_l2u_sample(int sample_index, int row_explore, int latency){
         matrix_l2u.row(sample_index) << row_explore , 1;
         vector_l2u.row(sample_index) << latency;
     }

     void add_k2u_sample(int sample_index, int row_init, int row_prune, int row_explore, int latency) {
         matrix_k2u.row(sample_index) << (row_init-row_prune), row_prune, row_explore, 1;
         vector_k2u.row(sample_index) << latency;
     }

     void add_k2l_sample(int sample_index, int row_init, int row_prune, int row_explore,
                                                int row_match, int latency) {
         matrix_k2l.row(sample_index) << (row_init-row_prune), row_prune, row_explore, 0, 1;
         vector_k2l.row(sample_index) << latency;
     }

     void add_k2k_sample(int sample_index, int row_init, int row_prune, int row_explore,
                                                int row_match, int latency) {
         matrix_k2k.row(sample_index) << (row_init-row_prune), row_prune, row_explore, 0, 1;
         vector_k2k.row(sample_index) << latency;
     }

     void resize(int sample_num, model_t model_type=model_t::K2K){
         switch (model_type)
         {
         case model_t::ALL:
             matrix_l2u = MatrixXd(sample_num, L2U_FACTOR_NUM);
             vector_l2u = VectorXd(sample_num);
             matrix_k2u = MatrixXd(sample_num, K2U_FACTOR_NUM);
             vector_k2u = VectorXd(sample_num);
             matrix_k2l = MatrixXd(sample_num, K2L_FACTOR_NUM);
             vector_k2l = VectorXd(sample_num);
             break;
         case model_t::L2U:
             matrix_l2u = MatrixXd(sample_num, L2U_FACTOR_NUM);
             vector_l2u = VectorXd(sample_num);
             break;
         case model_t::K2U:
             matrix_k2u = MatrixXd(sample_num, K2U_FACTOR_NUM);
             vector_k2u = VectorXd(sample_num);
             break;
         case model_t::K2L:
             matrix_k2l = MatrixXd(sample_num, K2L_FACTOR_NUM);
             vector_k2l = VectorXd(sample_num);
             break;
         case model_t::K2K:
             matrix_k2k.resize(sample_num, K2K_FACTOR_NUM);
             vector_k2k.resize(sample_num);
             break;
         default:
             break;
         }
     }

     void init(int sample_num, model_t model_type=model_t::ALL){
         switch (model_type)
         {
         case model_t::ALL:
             matrix_l2u = MatrixXd(sample_num, L2U_FACTOR_NUM);
             vector_l2u = VectorXd(sample_num);
             matrix_k2u = MatrixXd(sample_num, K2U_FACTOR_NUM);
             vector_k2u = VectorXd(sample_num);
             matrix_k2l = MatrixXd(sample_num, K2L_FACTOR_NUM);
             vector_k2l = VectorXd(sample_num);
             break;
         case model_t::L2U:
             matrix_l2u = MatrixXd(sample_num, L2U_FACTOR_NUM);
             vector_l2u = VectorXd(sample_num);
             break;
         case model_t::K2U:
             matrix_k2u = MatrixXd(sample_num, K2U_FACTOR_NUM);
             vector_k2u = VectorXd(sample_num);
             break;
         case model_t::K2L:
             matrix_k2l = MatrixXd(sample_num, K2L_FACTOR_NUM);
             vector_k2l = VectorXd(sample_num);
             break;
         case model_t::K2K:
             matrix_k2k = MatrixXd(sample_num, K2K_FACTOR_NUM);
             vector_k2k = VectorXd(sample_num);
             break;
         default:
             break;
         }
     }
};

} // namespace wukong


/**
 * Copyright (c) 2018, University Osnabrück
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * 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.
 *     * Neither the name of the University Osnabrück 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 University Osnabrück 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.
 */

/**
 * EigenSVDPointAlign.cpp
 *
 *  @date Feb 21, 2014
 *  @author Thomas Wiemann
 */
#include <lvr2/registration/EigenSVDPointAlign.hpp>

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

using namespace Eigen;
using std::numeric_limits;

namespace lvr2
{

template <typename BaseVecT>
double EigenSVDPointAlign<BaseVecT>::alignPoints(const PointPairVector<BaseVecT>& pairs,
        const BaseVecT centroid_m, const BaseVecT centroid_d, Matrix4<BaseVecT>& alignfx)
{
    double error = 0;
    double sum = 0.0;

    // Get centered PtPairs
    double** m = new double*[pairs.size()];
    double** d = new double*[pairs.size()];

    for(unsigned int i = 0; i <  pairs.size(); i++){
        m[i] = new double[3];
        d[i] = new double[3];
        m[i][0] = pairs[i].first.x - centroid_m[0];
        m[i][1] = pairs[i].first.y - centroid_m[1];
        m[i][2] = pairs[i].first.z - centroid_m[2];
        d[i][0] = pairs[i].second.x - centroid_d[0];
        d[i][1] = pairs[i].second.y - centroid_d[1];
        d[i][2] = pairs[i].second.z - centroid_d[2];

        sum += pow(pairs[i].first.x - pairs[i].second.x, 2)
             + pow(pairs[i].first.y - pairs[i].second.y, 2)
             + pow(pairs[i].first.z - pairs[i].second.z, 2) ;

    }

    error = sqrt(sum / (double)pairs.size());

    // Fill H matrix
    Matrix3d H, R;
    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < 3; j++)
        {
            H(i,j) = 0.0;
            R(i,j) = 0.0;
        }
    }

    for(size_t i = 0; i < pairs.size(); i++){
        for(int j = 0; j < 3; j++){
            for(int k = 0; k < 3; k++){
                H(j, k) += d[i][j]*m[i][k];
            }
        }
    }

    JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);

    Matrix3d U = svd.matrixU();
    Matrix3d V = svd.matrixV();

    R = V * U.transpose();

    // Calculate translation
    double translation[3];

    MatrixXd col_vec(3,1);
    for(int j = 0; j < 3; j++)
        col_vec(j,0) = centroid_d[j];

    MatrixXd r_time_colVec(3,1);

    r_time_colVec = R * col_vec;
    translation[0] = centroid_m[0] - r_time_colVec(0);
    translation[1] = centroid_m[1] - r_time_colVec(1);
    translation[2] = centroid_m[2] - r_time_colVec(2);

    // Fill result
    alignfx[0] = R(0,0);
    alignfx[1] = R(1,0);
    alignfx[2] = 0;
    alignfx[2] = R(2,0);
    alignfx[3] = 0;
    alignfx[4] = R(0,1);
    alignfx[5] = R(1,1);
    alignfx[6] = R(2,1);
    alignfx[7] = 0;
    alignfx[8] = R(0,2);
    alignfx[9] = R(1,2);
    alignfx[10] = R(2,2);
    alignfx[11] = 0;
    alignfx[12] = translation[0];
    alignfx[13] = translation[1];
    alignfx[14] = translation[2];
    alignfx[15] = 1;

    for(unsigned int i = 0; i <  pairs.size(); i++){
        delete m[i];
        delete d[i];
    }
    delete[] m;
    delete[] d;

    return error;
}

} // namespace lvr2


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    cpp_int N; cin >> N;

    cpp_int three = 3;
    for (int i = 1; i <= 37; i++) {
        cpp_int five = 5;
        for (int j = 1; j <= 25; j++) {
            if (three + five == N) {
                cout << i << " " << j << endl;
                return 0;
            }
            else if (three + five > N) break;
            five *= 5;
        }
        three *= 3;
    }

    cout << -1 << endl;
    return 0;
}


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * 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.
//
//     * Neither the name of The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>
#include <glog/logging.h>
#include <vector>

#include "theia/vision/sfm/triangulation/triangulation.h"

namespace theia {
using Eigen::MatrixXd;
using Eigen::Matrix4d;
using Eigen::Vector2d;
using Eigen::Vector3d;

// Triangulates 2 posed views
bool Triangulate(const ProjectionMatrix& pose_left,
                 const ProjectionMatrix& pose_right,
                 const Vector2d& point_left, const Vector2d& point_right,
                 Vector3d* triangulated_point) {
  Matrix4d design_matrix;
  design_matrix.row(0) = point_left[0] * pose_left.row(2) - pose_left.row(0);
  design_matrix.row(1) = point_left[1] * pose_left.row(2) - pose_left.row(1);
  design_matrix.row(2) = point_right[0] * pose_right.row(2) - pose_right.row(0);
  design_matrix.row(3) = point_right[1] * pose_right.row(2) - pose_right.row(1);

  // Extract nullspace.
  Eigen::Vector4d homog_triangulated_point =
      design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
  if (homog_triangulated_point[3] != 0) {
    *triangulated_point = homog_triangulated_point.hnormalized();
    return true;
  } else {
    return false;
  }
}

// Triangulates N views by computing SVD that minimizes the error.
bool TriangulateNViewSVD(const std::vector<ProjectionMatrix>& poses,
                         const std::vector<Vector2d>& points,
                         Vector3d* triangulated_point) {
  CHECK_EQ(poses.size(), points.size());

  MatrixXd design_matrix(3 * points.size(), 4 + points.size());

  for (int i = 0; i < points.size(); i++) {
    design_matrix.block<3, 4>(3 * i, 0) = -poses[i].matrix();
    design_matrix.block<3, 1>(3 * i, 4 + i) = points[i].homogeneous();
  }

  // Computing SVD on A'A is more efficient and gives the same null-space.
  Eigen::Vector4d homog_triangulated_point =
      (design_matrix.transpose() * design_matrix).jacobiSvd(Eigen::ComputeFullV)
          .matrixV().rightCols<1>().head(4);
  if (homog_triangulated_point[3] != 0) {
    *triangulated_point = homog_triangulated_point.hnormalized();
    return true;
  } else {
    return false;
  }
}

bool TriangulateNView(const std::vector<ProjectionMatrix>& poses,
                      const std::vector<Vector2d>& points,
                      Vector3d* triangulated_point) {
  CHECK_EQ(poses.size(), points.size());

  Matrix4d design_matrix = Matrix4d::Zero();
  for (int i = 0; i < points.size(); i++) {
    const Vector3d norm_point = points[i].homogeneous().normalized();
    const Eigen::Matrix<double, 3, 4> cost_term =
        poses[i].matrix() -
        norm_point * norm_point.transpose() * poses[i].matrix();
    design_matrix = design_matrix + cost_term.transpose() * cost_term;
  }

  Eigen::SelfAdjointEigenSolver<Matrix4d> eigen_solver(design_matrix);
  Eigen::Vector4d homog_triangulated_point = eigen_solver.eigenvectors().col(0);
  if (homog_triangulated_point[3] != 0) {
    *triangulated_point = homog_triangulated_point.hnormalized();
    return true;
  } else {
    return false;
  }
}

}  // namespace theia


#ifndef SVDSENSITIVITY_HH
#define SVDSENSITIVITY_HH
#include <Eigen/Dense>
#include <array>
#include <MeshFEM/EnergyDensities/Tensor.hh>

// 2x2 case only for now, also can be sped up.
struct SVDSensitivity {
    using M2d = Eigen::Matrix2d;
    using V2d = Eigen::Vector2d;

    SVDSensitivity() { }

    template<typename Derived>
    SVDSensitivity(const Eigen::MatrixBase<Derived> &A) { setMatrix(A); }

    template<typename Derived>
    void setMatrix(const Eigen::MatrixBase<Derived> &A) {
        static_assert((Derived::RowsAtCompileTime == 2) && (Derived::ColsAtCompileTime == 2), "Only 2x2 supported for now");
        Eigen::JacobiSVD<M2d> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
        m_U     = svd.matrixU();
        m_V     = svd.matrixV();
        m_Sigma = svd.singularValues();

        // Cache first derivatives (needed for computing second derivatives)
        m_dSigma[0] = m_U.col(0) * m_V.col(0).transpose();
        m_dSigma[1] = m_U.col(1) * m_V.col(1).transpose();

        const double sigmaSqDiff = m_Sigma[0] * m_Sigma[0] - m_Sigma[1] * m_Sigma[1];
        if (std::abs(sigmaSqDiff) < 1e-15) { m_degenerate =  true; m_invSigmaSqDiff = 0.0; }
        else                               { m_degenerate = false; m_invSigmaSqDiff = 1.0 / sigmaSqDiff; }

        m_y = m_invSigmaSqDiff * (m_Sigma[1] * m_U.col(0) * m_V.col(1).transpose()
                                + m_Sigma[0] * m_U.col(1) * m_V.col(0).transpose());
        m_du0[0] =  m_y * m_U(0, 1);
        m_du0[1] =  m_y * m_U(1, 1);
        m_du1[0] = -m_y * m_U(0, 0);
        m_du1[1] = -m_y * m_U(1, 0);

        m_z = m_invSigmaSqDiff * (m_Sigma[0] * m_U.col(0) * m_V.col(1).transpose()
                                + m_Sigma[1] * m_U.col(1) * m_V.col(0).transpose());
        m_dv0[0] =  m_z * m_V(0, 1);
        m_dv0[1] =  m_z * m_V(1, 1);
        m_dv1[0] = -m_z * m_V(0, 0);
        m_dv1[1] = -m_z * m_V(1, 0);
    }

    // Access SVD
    const M2d &    U() const { return m_U; }
    const M2d &    V() const { return m_V; }
    const V2d &Sigma() const { return m_Sigma; }

    auto       u(size_t i) const { return m_U.col(i); }
    auto       v(size_t i) const { return m_V.col(i); }
    double sigma(size_t i) const { return m_Sigma[i]; }

    ////////////////////////////////////////////////////////////////////////////
    // First derivative expressions
    ////////////////////////////////////////////////////////////////////////////
    M2d dsigma(size_t i) const { return m_dSigma.at(i); }
    M2d du0   (size_t i) const { return m_du0   .at(i); }
    M2d du1   (size_t i) const { return m_du1   .at(i); }
    M2d dv0   (size_t i) const { return m_dv0   .at(i); }
    M2d dv1   (size_t i) const { return m_dv1   .at(i); }

    template<typename M2d_, EnableIfMatrixOfSize<M2d_, 2, 2, int> = 0> V2d dSigma(const M2d_ &dA) const { return V2d(doubleContract(m_dSigma[0], dA), doubleContract(m_dSigma[1], dA)); }
    template<typename M2d_, EnableIfMatrixOfSize<M2d_, 2, 2, int> = 0> V2d du0   (const M2d_ &dA) const { return V2d(doubleContract(m_du0   [0], dA), doubleContract(m_du0   [1], dA)); }
    template<typename M2d_, EnableIfMatrixOfSize<M2d_, 2, 2, int> = 0> V2d du1   (const M2d_ &dA) const { return V2d(doubleContract(m_du1   [0], dA), doubleContract(m_du1   [1], dA)); }
    template<typename M2d_, EnableIfMatrixOfSize<M2d_, 2, 2, int> = 0> V2d dv0   (const M2d_ &dA) const { return V2d(doubleContract(m_dv0   [0], dA), doubleContract(m_dv0   [1], dA)); }
    template<typename M2d_, EnableIfMatrixOfSize<M2d_, 2, 2, int> = 0> V2d dv1   (const M2d_ &dA) const { return V2d(doubleContract(m_dv1   [0], dA), doubleContract(m_dv1   [1], dA)); }

    template<typename M2d_, EnableIfMatrixOfSize<M2d_, 2, 2, int> = 0>
    double dsigma(size_t i, const M2d_ &dA) const { return doubleContract(m_dSigma[i], dA); }

    ////////////////////////////////////////////////////////////////////////////
    // Second derivative expressions
    ////////////////////////////////////////////////////////////////////////////
    // Note, to avoid using high order tensors, we only provide the contraction of the
    // singular value/vector Hessians with perturbation matrices.

    // Second derivative of singular values with respect to variables inducing
    // perturbations dA_1 and dA_2, respectively.
    template<typename M2d_, EnableIfMatrixOfSize<M2d_, 2, 2, int> = 0>
    V2d d2Sigma(const M2d_ &dA_1, const M2d_ &dA_2) const {
        return V2d(du0(dA_2).dot((dA_1 * m_V.col(0)).matrix()) + m_U.col(0).dot((dA_1 * dv0(dA_2)).matrix()),
                   du1(dA_2).dot((dA_1 * m_V.col(1)).matrix()) + m_U.col(1).dot((dA_1 * dv1(dA_2)).matrix()));
    }
    template<typename M2d_, EnableIfMatrixOfSize<M2d_, 2, 2, int> = 0>
    double d2sigma(size_t i, const M2d_ &dA_1, const M2d_ &dA_2) const {
        if (i == 0) return du0(dA_2).dot((dA_1 * m_V.col(0)).matrix()) + m_U.col(0).dot((dA_1 * dv0(dA_2)).matrix());
        if (i == 1) return du1(dA_2).dot((dA_1 * m_V.col(1)).matrix()) + m_U.col(1).dot((dA_1 * dv1(dA_2)).matrix());
        throw std::runtime_error("Index out of bounds");
    }

    // Second derivative of first left singular vector with respect to variables inducing
    // perturbations dA_1 and dA_2, respectively.
    template<typename M2d_, EnableIfMatrixOfSize<M2d_, 2, 2, int> = 0>
    V2d d2u0(const M2d_ &dA_1, const M2d_ &dA_2) const {
        M2d Ut_dA1_V = m_U.transpose() * (dA_1 * m_V);

        V2d d_sigma_d2 = dSigma(dA_2);
        const double y1 = doubleContract(m_y, dA_1);
        const double y2 = doubleContract(m_y, dA_2);
        const double z2 = doubleContract(m_z, dA_2);

        const double dy1_d2 = m_invSigmaSqDiff * (d_sigma_d2[1] * (2 * m_Sigma[1] * y1 + Ut_dA1_V(0, 1)) + d_sigma_d2[0] * (Ut_dA1_V(1, 0) - 2 * m_Sigma[0] * y1)
                                                  + Ut_dA1_V(1, 1) * (m_Sigma[1] * y2 + m_Sigma[0] * z2)
                                                  - Ut_dA1_V(0, 0) * (m_Sigma[0] * y2 + m_Sigma[1] * z2));

        return dy1_d2 * m_U.col(1) - y1 * y2 * m_U.col(0);
    }

    // Second derivative of second right singular vector with respect to variables inducing
    // perturbations dA_1 and dA_2, respectively.
    //      -d z_1 / d2 v0 - z_1 z_2 v1
    // Note: "z" is the same as "y" with sigma_0 and sigma_1 swapped (apart from the m_invSigmaSqDiff factor).
    template<typename M2d_, EnableIfMatrixOfSize<M2d_, 2, 2, int> = 0>
    V2d d2v1(const M2d_ &dA_1, const M2d_ &dA_2) const {
        M2d Ut_dA1_V = m_U.transpose() * (dA_1 * m_V);

        V2d d_sigma_d2 = dSigma(dA_2);
        const double z1 = doubleContract(m_z, dA_1);
        const double z2 = doubleContract(m_z, dA_2);
        const double y2 = doubleContract(m_y, dA_2);

        const double dz1_d2 = m_invSigmaSqDiff * (d_sigma_d2[0] * (Ut_dA1_V(0, 1) - 2 * m_Sigma[0] * z1) + d_sigma_d2[1] * (Ut_dA1_V(1, 0) + 2 * m_Sigma[1] * z1)
                                                  + Ut_dA1_V(1, 1) * (m_Sigma[0] * y2 + m_Sigma[1] * z2)
                                                  - Ut_dA1_V(0, 0) * (m_Sigma[1] * y2 + m_Sigma[0] * z2));

        return -dz1_d2 * m_V.col(0) - z1 * z2 * m_V.col(1);
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    M2d m_U, m_V, m_y, m_z;
    V2d m_Sigma;

    bool m_degenerate;
    double m_invSigmaSqDiff;

    std::array<M2d, 2> m_dSigma;
    std::array<M2d, 2> m_du0, m_du1,
                       m_dv0, m_dv1;
};

#endif /* end of include guard: SVDSENSITIVITY_HH */


#include "Interrupt.h"
#include "OGR.h"
#include "ProjInfo.h"
#include "Shape_circle.h"
#include "Shape_rect.h"
#include "SpatialReference.h"
#include <boost/math/constants/constants.hpp>
#include <macgyver/Exception.h>
#include <ogr_geometry.h>
#include <ogr_spatialref.h>

#include <iostream>

namespace Fmi
{
const double epsilon = 1e-6;

const double wgs84radius = 6378137.0;

const int default_circle_segments = 360;

// Longitude to -180...180 range
double modlon(double lon)
{
  try
  {
    if (lon > 180)
      return fmod(lon + 180, 360) - 180;
    if (lon < -180)
      return -(fmod(-lon + 180, 360) - 180);
    return lon;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// Create a circle in WGS84 coordinates. Range may span from -360 to +360

OGRPolygon* make_circle(double lon, double lat, double radius, int segments)
{
  try
  {
    auto* poly = new OGRPolygon;
    auto* ring = new OGRLinearRing;

    // We start from -180 instead of zero for southern circles to avoid extra joining work
    const auto angle_offset = (lat >= 0 ? 0.0 : -M_PI);

    const auto lon1 = lon * boost::math::double_constants::degree;
    const auto lat1 = lat * boost::math::double_constants::degree;
    const auto dr = radius / wgs84radius;  // angular distance in radians

    const auto sindr = sin(dr);
    const auto cosdr = cos(dr);

    const auto sinlat1 = sin(lat1);
    const auto coslat1 = cos(lat1);

    for (int i = 0; i <= segments; i++)
    {
      const auto angle = 2 * M_PI * i / segments + angle_offset;

      auto la = asin(sinlat1 * cosdr + coslat1 * sindr * cos(angle));
      auto lo = lon1 + atan2(sin(angle) * sindr * coslat1, cosdr - sinlat1 * sin(la));

      la *= boost::math::double_constants::radian;
      lo *= boost::math::double_constants::radian;

      // Note: No mod 360 math here, we wish to preserve the overflow for clipping

      ring->addPoint(lo, la);
    }

    // Now we need to check if we must add either pole to close the ring

    const auto n = ring->getNumPoints();

    const auto x1 = ring->getX(0);
    const auto y1 = ring->getY(0);
    auto x2 = ring->getX(n - 1);
    auto y2 = ring->getY(n - 1);

    const double step = 10;  // degrees

    if (std::hypot(x1 - x2, y1 - y2) > 1e-3)
    {
      if (lat >= 0)
      {
        // close via north pole
        while (y2 + step < 90)
        {
          y2 += step;
          ring->addPoint(x2, y2);
        }
        while (x2 < x1)
        {
          ring->addPoint(x2, 90);
          x2 += step;
        }
        y2 = 90;
        while (y2 > y1)
        {
          ring->addPoint(x1, y2);
          y2 -= step;
        }
      }
      else
      {
        // close via south pole
        while (y2 - step > -90)
        {
          y2 -= step;
          ring->addPoint(x2, y2);
        }
        while (x2 > x1)
        {
          ring->addPoint(x2, -90);
          x2 -= step;
        }
        y2 = -90;
        while (y2 < y1)
        {
          ring->addPoint(x1, y2);
          y2 += step;
        }
      }
    }

    ring->closeRings();  // close if not already closed
    poly->addRingDirectly(ring);
    return poly;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// Create a rect

OGRPolygon* make_rect(double x1, double y1, double x2, double y2)
{
  try
  {
    auto* poly = new OGRPolygon;
    auto* ring = new OGRLinearRing;

    ring->addPoint(x1, y1);
    ring->addPoint(x1, y2);
    ring->addPoint(x2, y2);
    ring->addPoint(x2, y1);
    ring->addPoint(x1, y1);

    poly->addRingDirectly(ring);
    return poly;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// Create a circle cutgeometry

OGRGeometry* circle_cut(double lon,
                        double lat,
                        double radius,
                        int segments = default_circle_segments)
{
  try
  {
    // One circle
    auto* geom = make_circle(lon, lat, radius, segments);

    // Extract envelope

    OGREnvelope env;
    geom->getEnvelope(&env);

    // Nothing to do if the circle is fully within normal bounds
    if (env.MinX >= -180 && env.MaxX <= 180)
    {
      geom->assignSpatialReference(OGRSpatialReference::GetWGS84SRS());
      return geom;
    }

    // Otherwise we must take at least 2 intersections, maybe 3

    auto* result = new OGRGeometryCollection;

    auto* rect = make_rect(-180, -90, 180, 90);
    auto* cut = geom->Intersection(rect);
    if (cut != nullptr && cut->IsEmpty() == 0)
      result->addGeometryDirectly(cut);
    CPLFree(rect);

    if (env.MinX < -180)
    {
      auto* rect = make_rect(-540, -90, -180, 90);
      auto* cut = geom->Intersection(rect);
      OGR::translate(cut, +360, 0);
      if (cut != nullptr && cut->IsEmpty() == 0)
        result->addGeometryDirectly(cut);
      CPLFree(rect);
    }

    if (env.MaxX > 180)
    {
      auto* rect = make_rect(180, -90, 540, 90);
      auto* cut = geom->Intersection(rect);
      OGR::translate(cut, -360, 0);
      if (cut != nullptr && cut->IsEmpty() == 0)
        result->addGeometryDirectly(cut);
      CPLFree(rect);
    }

    CPLFree(geom);

    result->assignSpatialReference(OGRSpatialReference::GetWGS84SRS());
    return result;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

Shape_sptr make_vertical_cut(double lon, double lat1, double lat2)
{
  try
  {
    return std::make_shared<Shape_rect>(
        lon - epsilon, std::min(lat1, lat2), lon + epsilon, std::max(lat1, lat2));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

Shape_sptr make_horizontal_cut(double lat, double lon1, double lon2)
{
  try
  {
    return std::make_shared<Shape_rect>(
        std::min(lon1, lon2), lat - epsilon, std::max(lon1, lon2), lat + epsilon);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

Interrupt interruptGeometry(const SpatialReference& theSRS)
{
  try
  {
    Interrupt result;

    const auto opt_name = theSRS.projInfo().getString("proj");
    if (!opt_name)
      return result;

    const auto name = *opt_name;

    const auto opt_lon_0 = theSRS.projInfo().getDouble("lon_0");
    const auto lon_0 = opt_lon_0 ? *opt_lon_0 : 0.0;

    const auto opt_lat_0 = theSRS.projInfo().getDouble("lat_0");
    const auto lat_0 = opt_lat_0 ? *opt_lat_0 : 0.0;

    // If general oblique transformation such as rotated latlon, cut the Antarctic in half at the
    // central meridian. -60 is large enough to make the cut, since Drake passage is below
    // that latitude. In reality, the cut should be made for any polygon which spans the south pole,
    // and the cut should be made for that polygon only. Hence this code is not generic enough.
    // Similar logic would be needed for the north pole should there be a polygon covering it.
    //
    // The Interrupt struct should thus contain conditional cuts for individual polygons based
    // on the envelope of the individual polygon. The current implementation does not support this.
    //
    // The code commented out shows various tests used to find out how a nonzero lon_0 should be
    // handled, but the (random) experimental approach failed.

    if (theSRS.projInfo().getString("proj") == std::string("ob_tran"))
    {
      auto opt_lat_p = theSRS.projInfo().getDouble("o_lat_p");
      if (opt_lat_p)
      {
        const auto opt_lon_0 = theSRS.projInfo().getDouble("lon_0");
        const auto lon_0 = (opt_lon_0 ? *opt_lon_0 : 0.0);

        const auto lat_p = *opt_lat_p;

        result.shapeCuts.emplace_back(make_vertical_cut(0, -90, lat_p - 90));
        result.shapeCuts.emplace_back(make_vertical_cut(lon_0, -90, lat_p - 90));
        result.shapeCuts.emplace_back(make_vertical_cut(-lon_0, -90, lat_p - 90));

        result.shapeCuts.emplace_back(make_vertical_cut(lat_p, -90, lat_p - 90));
        result.shapeCuts.emplace_back(make_vertical_cut(-lat_p, -90, lat_p - 90));

        result.shapeCuts.emplace_back(make_horizontal_cut(-lat_p, -180, 180));
        result.shapeCuts.emplace_back(make_horizontal_cut(-90, -180, 180));
        result.shapeCuts.emplace_back(make_horizontal_cut(+90, -180, 180));
        result.shapeCuts.emplace_back(make_vertical_cut(+180, -90, 90));
        result.shapeCuts.emplace_back(make_vertical_cut(-180, -90, 90));
      }
    }

    // Geographic: cut everything at lon_wrap (default=Greenwich) antimeridians
    if (theSRS.isGeographic())
    {
      const auto opt_lon_wrap = theSRS.projInfo().getDouble("lon_wrap");
      const auto lon_wrap = (opt_lon_wrap ? *opt_lon_wrap : 0.0);

      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_wrap + 180), -90, 90));
      if (lon_wrap == 0)
        result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_wrap - 180), -90, 90));

      return result;
    }

    if (name == "laea")
    {
      // Poles always project to x-coordinate zero, we need to cut them out too
      // Cannot use epsilon here, the cut would be too small for PROJ.7
      result.shapeClips.push_back(std::make_shared<Shape_rect>(-178, -89.99, 178, 89.99));
      return result;
    }

    if (name == "nicol")
    {
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 180), -90, 90));
      if (lon_0 == 0)
        result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 180), -90, 90));

      // TODO: proj=nicol is hard to handle correctly since the projection seems to wrap
      // around itself around -+90 longitudes
      //
      // Very slow:
      // result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 90), -90, 90));
      // result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 90), -90, 90));
      return result;
    }

    if (name == "nsper")
    {
      // TODO: Something odd, maybe the result is shifted?
      const auto radius = 90 * wgs84radius * boost::math::double_constants::degree;
      result.andGeometry.reset(circle_cut(lon_0, lat_0, radius));
      return result;
    }

    if (name == "tcc")
    {
      // TODO: Figure out what's wrong in longitude range 90...130
      result.shapeCuts.emplace_back(std::make_shared<Shape_rect>(90, -90, 130, 90));
      return result;
    }

    if (name == "lcc")
    {
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 180), -90, 90));
      if (lon_0 == 0)
        result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 180), -90, 90));
      result.shapeCuts.emplace_back(make_horizontal_cut(-90, -180, 180));
    }

    if (name == "imw_p")
    {
      // TODO: Slow as hell, disabled for now
      // const auto radius = 80 * wgs84radius * boost::math::double_constants::degree;
      // result.andGeometry.reset(circle_cut(lon_0, lat_0, radius));
      return result;
    }

    if (name == "aeqd")
    {
      // TODO: 130 is just an experimental value getting some things right, but
      // this clipping is not even close to correct. Not sure what kind of clipping this needs.
      //
      // Also: The antarctic is missing completely. Probably the fault of the current
      // version of circle cutting.

      const auto radius = 130 * wgs84radius * boost::math::double_constants::degree;
      result.andGeometry.reset(circle_cut(lon_0, lat_0, radius));
      return result;
    }

    if (name == "tmerc")
    {
      // TODO: This is just experimental to get something out
      // const auto radius = 90 * wgs84radius * boost::math::double_constants::degree;
      // result.andGeometry.reset(circle_cut(lon_0, lat_0, radius));
      return result;
    }
    if (name == "gstmerc")
    {
      // 90 causes errors
      const auto radius = 89.5 * wgs84radius * boost::math::double_constants::degree;
      result.andGeometry.reset(circle_cut(lon_0, lat_0, radius));
      return result;
    }

    if (name == "gnom")
    {
      // TODO: Nothing seems to work, result is full of NaN values
      const auto radius = 89 * wgs84radius * boost::math::double_constants::degree;
      result.andGeometry.reset(circle_cut(lon_0, lat_0, radius));
      return result;
    }

    if (name == "airy" || name == "ortho")
    {
      const auto radius = 90 * wgs84radius * boost::math::double_constants::degree;
      result.andGeometry.reset(circle_cut(lon_0, lat_0, radius));
      return result;
    }

    if (name == "tpers")
    {
      // 50 was found experimentally
      const auto radius = 50 * wgs84radius * boost::math::double_constants::degree;
      result.andGeometry.reset(circle_cut(lon_0, lat_0, radius));
      return result;
    }

    if (name == "geos")
    {
      // 80 was found experimentally
      const auto radius = 80 * wgs84radius * boost::math::double_constants::degree;
      result.andGeometry.reset(circle_cut(lon_0, lat_0, radius));
      return result;
    }

    if (name == "adams_hemi")
    {
      // TODO: Just something that works on small scales not up to the maximum
      const auto radius = 90 * wgs84radius * boost::math::double_constants::degree;
      result.andGeometry.reset(circle_cut(lon_0, lat_0, radius));
      return result;
    }

    if (name == "bertin1953" || name == "peirce_q")
    {
      // TODO: No idea how to fix these
      return result;
    }

    if (name == "tpeqd")
    {
      const auto opt_lon_1 = theSRS.projInfo().getDouble("lon_1");
      const auto lon_1 = opt_lon_1 ? *opt_lon_1 : 0.0;

      const auto opt_lat_1 = theSRS.projInfo().getDouble("lat_1");
      const auto lat_1 = opt_lat_1 ? *opt_lat_1 : 0.0;

      const auto opt_lon_2 = theSRS.projInfo().getDouble("lon_2");
      const auto lon_2 = opt_lon_2 ? *opt_lon_2 : 0.0;

      const auto opt_lat_2 = theSRS.projInfo().getDouble("lat_2");
      const auto lat_2 = opt_lat_2 ? *opt_lat_2 : 0.0;

      // Hack solution: rough estimate on the center
      const auto lon = 0.5 * (lon_1 + lon_2);
      const auto lat = 0.5 * (lat_1 + lat_2);

      const auto radius = 145 * wgs84radius * boost::math::double_constants::degree;
      result.andGeometry.reset(circle_cut(lon, lat, radius));
      return result;
    }

    if (name == "igh")
    {
      // Interrupted Goode Homolosine
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 180), -90, 90));
      if (lon_0 == 0)
        result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 180), -90, 90));

      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 40), 0, 90));
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 100), -90, 0));
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 20), -90, 0));
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 80), -90, 0));

      return result;
    }

    if (name == "igh_o")
    {
      // Interrupted Goode Homolosine (Oseanic)

      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 180), -90, 90));
      if (lon_0 == 0)
        result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 180), -90, 90));
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 90), 0, 90));
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 60), 0, 90));
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 60), -90, 0));
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 90), -90, 0));
      return result;
    }

    if (name == "healpix")
    {
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 180), -90, 90));
      if (lon_0 == 0)
        result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 180), -90, 90));

      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 90), -90, -45));
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 90), 45, 90));
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0), -90, -45));
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0), 45, 90));
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 90), -90, -45));
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 90), 45, 90));

      return result;
    }

    if (name == "isea")
    {
      // Icosahedral Snyder Equal Area.
      // TODO: PROJ.7 implementation seems to have the cuts at odd locations, perhaps a bug?

      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 180), -90, 90));
      if (lon_0 == 0)
        result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 180), -90, 90));

      // result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 108), 30, 90));
      // result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 36), 30, 90));
      // result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 36), 30, 90));
      // result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 108), 30, 90));
      //
      // result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 144), -90, -30));
      // result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 72), -90, -30));
      // result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 0), -90, -30));
      // result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 72), -90, -30));
      // result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 144), -90, -30));

      return result;
    }

    // Regular geometric: cut everything at lon_0+180 antimeridian
    // lon_0 is needed for all remaining geometric projections

    result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 + 180), -90, 90));
    if (lon_0 == 0)
      result.shapeCuts.emplace_back(make_vertical_cut(modlon(lon_0 - 180), -90, 90));

    return result;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

OGREnvelope interruptEnvelope(const SpatialReference& theSRS)
{
  try
  {
    // Default answer covers nothing. User should check for this special case.
    OGREnvelope env;
    env.MinY = 0;
    env.MaxY = 0;
    env.MinX = 0;
    env.MaxX = 0;

    if (theSRS.isGeographic())
    {
      // geographic projections are modified by +lon_wrap which defines the wanter center longitude
      const auto opt_lon_wrap = theSRS.projInfo().getDouble("lon_wrap");
      const auto lon_wrap = (opt_lon_wrap ? *opt_lon_wrap : 0);

      env.MinY = -90;
      env.MaxY = +90;
      env.MinX = lon_wrap - 180;
      env.MaxX = lon_wrap + 180;
      return env;
    }

    // For geometric projections the default box is modified by +lon_0
    // whose default value is zero.  Anything not rectilinear must be
    // clipped to produce shorted edges at the antimeridians, or the
    // edges will not curve nicely enough when projected. In practise it
    // seems like only cylindrical projections and pseudocylindrical
    // projections with straight sides such as collg can be handled via
    // envelopes.

    const auto opt_name = theSRS.projInfo().getString("proj");
    if (!opt_name)
      return env;
    const auto name = *opt_name;

    if (name == "cc" || name == "cea" || name == "collg" || name == "comill" || name == "eqc" ||
        name == "fouc_s" || name == "gall" || name == "merc" || name == "mill" || name == "ocea" ||
        name == "patterson" || name == "webmerc")
    {
      const auto opt_lon_0 = theSRS.projInfo().getDouble("lon_0");
      const auto lon_0 = opt_lon_0 ? *opt_lon_0 : 0.0;

      env.MinY = -90;
      env.MaxY = +90;
      env.MinX = lon_0 - 180;
      env.MaxX = lon_0 + 180;
    }

    return env;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

}  // namespace Fmi


// ==BEGIN LICENSE==
//
// MIT License
//
// Copyright (c) 2018 SRI Lab, ETH Zurich
//
// 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.
//
// ==END LICENSE==

#include <iostream>
#include <math.h>
#include <boost/math/distributions/normal.hpp> // for normal_distribution
#include <dlfcn.h> // for loading shared library
#include "toms462.hpp"

using boost::math::normal; // typedef provides default type is double.

using namespace std;

typedef double numb;

// http://static.stevereads.com/papers_to_read/on_the_ratio_of_two_correlated_normal_random_variables.pdf -> page 3

// LOAD TOMS462 functions
#define STRINGIFY2(X) #X
#define STRINGIFY(X) STRINGIFY2(X)
typedef double (*bivnorlib_t)( double ah, double ak, double r );
double bivnor( double ah, double ak, double r ){
	void* lib = dlopen(STRINGIFY(MYPATH)"/libtoms462.so", RTLD_LAZY);
	bivnorlib_t bivnorlib = (bivnorlib_t)dlsym(lib, "bivnor" );
	double ret = bivnorlib(ah, ak, r);
	dlclose(lib);
	return ret;
}

// MY FUNCTIONS

numb a(numb w, numb sx, numb sy, numb rho){
	numb w2=w*w;
	numb sx2=sx*sx;
	numb sy2=sy*sy;
	return sqrt(w2/sx2-2*rho*w/(sx*sy)+1/sy2);
}

double b(double w, double mx, double my, double sx, double sy, double rho){
	double sx2=sx*sx;
	double sy2=sy*sy;
	return mx*w/sx2-rho*(mx+my*w)/(sx*sy)+my/sy2;
}

double c(double mx, double my, double sx, double sy, double rho){
	double mx2=mx*mx;
	double sx2=sx*sx;
	double my2=my*my;
	double sy2=sy*sy;
	return mx2/sx2-2*rho*mx*my/(sx*sy)+my2/sy2;
}

double d(double w, double mx, double my, double sx, double sy, double rho){
	double a_=a(w,sx,sy,rho);
	double a2=a_*a_;
	double b_=b(w,mx,my,sx,sy,rho);
	double b2=b_*b_;
	double c_=c(mx,my,sx,sy,rho);
	double rho2=rho*rho;
	return exp((b2-c_*a2)/(2*(1-rho2)*a2));
}

numb phi(numb x){
	normal s; // (default mean = zero, and standard deviation = unity)
	return cdf(s,x);
}

numb ratio_pdf(numb w, numb mx, numb my, numb sx, numb sy, numb rho){
	numb a_=a(w,sx,sy,rho);
	numb b_=b(w,mx,my,sx,sy,rho);
	numb c_=c(mx,my,sx,sy,rho);
	numb d_=d(w,mx,my,sx,sy,rho);

	numb a2=a_*a_;
	numb a3=a_*a_*a_;
	numb rho2=rho*rho;

	numb pi=M_PI;

	numb frac1=b_*d_/(sqrt(2*pi)*sx*sy*a3);
	numb arg1=b_/(sqrt(1-rho2)*a_);
	numb arg2=-b_/(sqrt(1-rho2)*a_);
	numb frac2=sqrt(1-rho2)/(pi*sx*sy*a2);
	numb e=exp(-c_/(2*(1-rho2)));

	return frac1*(phi(arg1)-phi(arg2))+frac2*e;
}

numb ratio_cdf(numb w, numb mx, numb my, numb sx, numb sy, numb rho){
	//cout << "Running ratio_cdf with " << w << "/" << mx << "/" << my << "/" << sx << "/" << sy << endl;

	numb a_=a(w,sx,sy,rho);

	numb h1=(mx-my*w)/(sx*sy*a_);
	numb k1=-my/sy;
	numb gamma=(sy*w-rho*sx)/(sx*sy*a_);
	//cout << "Running with " << h1 << "/" << k1 << "/" << gamma << endl;
	numb L1=bivnor(h1,k1,gamma);

	numb h2=(my*w-mx)/(sx*sy*a_);
	numb k2=my/sy;
	//cout << "Running with " << h2 << "/" << k2 << "/" << gamma << endl;
	numb L2=bivnor(h2,k2,gamma);

	numb ret=L1+L2;
	return ret;
}

double ratio_cdf(double lower, double upper, double mx, double my, double sx, double sy, double rho){
	//mpf_set_default_prec(n_bits);

	numb lower_=lower;
	numb upper_=upper;
	numb mx_=mx;
	numb my_=my;
	numb sx_=sx;
	numb sy_=sy;
	numb rho_=rho;

	numb cdf1=ratio_cdf(upper_, mx_, my_, sx_, sy_, rho_);
	numb cdf2=ratio_cdf(lower_, mx_, my_, sx_, sy_, rho_);

	numb ret=cdf1-cdf2;
	return ret;//ret.get_d();
}

double interval_fallback_threshold=1e-7;

double get_err_interval(double p1,double p2,double std1,double std2,double center, double confidence){
	double confidence_2=1-(1-confidence)/2;
	double z_2=sqrt(2)*boost::math::erf_inv(confidence_2);
	double d1=std1*z_2;
	double d2=std2*z_2;
	if (p2>p1){
		swap(p1,p2);
		swap(d1,d2);
	}
	double err=0;
	if (p2-d2<=0){
		// p2 may be 0
		if (p2+d2<=0){
			// p2 must be 0. Thus, eps must be infinity
			err=0;
		}else{
			// p2 may or may not be 0. Thus, eps may or may not be infinity
			err=numeric_limits<double>::infinity();
		}
	}else if (p1-d1<=0){
			// p1 may be 0. In that case, eps would have to be -inf
			err=numeric_limits<double>::infinity();
	}else{
		// neither p1 nor p2 can be 0
		double min_eps=log(p1-d1)-log(p2+d2);
		double max_eps=log(p1+d1)-log(p2-d2);
		max_eps=max(max_eps,abs(min_eps));
		min_eps=max(min_eps,0.0);

		double eps=center;
		err=max(max_eps-eps,eps-min_eps);
	}
	return err;
}

// returns: confidence interval for logarithm of ratio
// center: center of confidence interval
// err_goal: precision of confidence interval
double ratio_confidence_interval(double p1, double p2, double d1, double d2, double corr, double center, double confidence, double err_goal){
	if (d1<interval_fallback_threshold || d2<interval_fallback_threshold){
			cout << "Falling back to interval arithmetic..." << endl;
			return get_err_interval(p1,p2,d1,d2,center,confidence);
	}

	double d_old=0;
	double d=err_goal/2;
	while (true) {
		double min=center-d;
		double max=center+d;
		double p=ratio_cdf(exp(min),exp(max),p1,p2,d1,d2,corr);
		//cout << "Pr[ratio∊ " << center << "±" << d << "]=" << p << " (exponential search)" << endl;
		if (p>=confidence){
			break;
		}
		d_old=d;
		d*=2;
		if (d>10){
			return INFINITY;
		}
	}
	double d_lower=d_old;
	double d_upper=d;
	while(d_upper-d_lower>err_goal/200){
		double middle=(d_lower+d_upper)/2;
		double min=center-middle;
		double max=center+middle;
		double p=ratio_cdf(exp(min),exp(max),p1,p2,d1,d2,corr);
		//cout << "Pr[ratio∊ " << center << "±" << middle << "]=" << p << " (binary search)" << endl;
		if (p<confidence){
			d_lower=middle;
		}else{
			d_upper=middle;
		}
	}

	/*double l=center-d_upper;
	double u=center+d_upper;
	double p_too_small=ratio_cdf(l,p1,p2,d1,d2,corr);
	double p_correct=ratio_cdf(l,u,p1,p2,d1,d2,corr);
	double p_too_large=1-ratio_cdf(u,p1,p2,d1,d2,corr);
	cout << "Pr[ratio<" << center << "-" << d_upper << "]=" << p_too_small << endl;
	cout << "Pr[ratio∊ " << center << "±" << d_upper << "]=" << p_correct << endl;
	cout << "Pr[ratio>" << center << "+" << d_upper << "]=" << p_too_large << endl;
	cout << p_too_small + p_correct + p_too_large << endl;*/

	return d_upper;
}

extern "C" {

	double ratio_cdf_extern(double lower, double upper, double mx, double my, double sx, double sy, double rho) {
		return ratio_cdf(lower,upper,mx,my,sx,sy,rho);
	}

	double ratio_pdf_extern(double w, double mx, double my, double sx, double sy, double rho) {
		return ratio_pdf(w,mx,my,sx,sy,rho);
	}

	double ratio_confidence_interval_extern(double p1, double p2, double d1, double d2, double corr, double center, double confidence, double err_goal){
		// cout << p1 << " " << p2 << " " << d1 << " " << d2 << " " << corr << " " << center << " " << confidence << " " << err_goal << endl;
		return ratio_confidence_interval(p1, p2, d1, d2, corr, center, confidence, err_goal);
	}

}

int main() {
	double x = bivnor(1,0,0);
	cout << x << endl;
	return 0;
}


#pragma once

#include "definitions.hpp"

#include <Eigen/Dense>

namespace activation
{
class ActivationFunction
{
public:
    virtual Matrix evaluate(const MatrixRef& x) const      = 0;
    virtual Matrix derivative(const MatrixRef& x) const    = 0;
    virtual Matrix dblDerivative(const MatrixRef& x) const = 0;
};

template <typename F, typename DF, typename DDF>
class DerivedActivationFunction : public ActivationFunction
{
private:
    const F   f {};
    const DF  df {};
    const DDF ddf {};

public:
    Matrix evaluate(const MatrixRef& x) const override { return x.unaryExpr(f); }

    Matrix derivative(const MatrixRef& y) const override { return y.unaryExpr(df); }

    Matrix dblDerivative(const MatrixRef& y) const override { return y.unaryExpr(ddf); }
};

namespace functors
{
namespace relu
{
struct eval
{
    constexpr Real operator()(Real x) const { return x > 0 ? x : 0; }
};
struct deriv
{
    constexpr Real operator()(Real y) const { return y > 0 ? 1 : 0; }
};
struct dblDeriv
{
    constexpr Real operator()(Real y) const
    {
        (void) y;
        return 0;
    }
};
}  // namespace relu

namespace identity
{
struct eval
{
    constexpr Real operator()(Real x) const { return x; }
};
struct deriv
{
    constexpr Real operator()(Real y) const
    {
        (void) y;
        return 1;
    }
};
struct dblDeriv
{
    constexpr Real operator()(Real y) const
    {
        (void) y;
        return 0;
    }
};
}  // namespace identity

namespace sigmoid
{
struct eval
{
    constexpr Real operator()(Real x) const { return 1 / (1 + std::exp(-x)); }
};
struct deriv
{
    constexpr Real operator()(Real y) const { return y * (1 - y); }
};
struct dblDeriv
{
    constexpr Real operator()(Real y) const { return y * (1 - y) * (1 - 2 * y); }
};
}  // namespace sigmoid

namespace tanh
{
struct eval
{
    constexpr Real operator()(Real x) const { return std::tanh(x); }
};
struct deriv
{
    constexpr Real operator()(Real y) const { return 1 - y * y; }
};
struct dblDeriv
{
    constexpr Real operator()(Real y) const
    {
        Real x = std::atanh(y);
        return -2 * y / square(std::cosh(x));
    }
};
}  // namespace tanh

namespace exponential
{
struct eval
{
    constexpr Real operator()(Real x) const { return std::exp(x); }
};
struct deriv
{
    constexpr Real operator()(Real y) const { return y; }
};
struct dblDeriv
{
    constexpr Real operator()(Real y) const { return y; }
};
}  // namespace exponential

}  // namespace functors

using ReluActivation     = DerivedActivationFunction<functors::relu::eval,
                                                 functors::relu::deriv,
                                                 functors::relu::dblDeriv>;
using IdentityActivation = DerivedActivationFunction<functors::identity::eval,
                                                     functors::identity::deriv,
                                                     functors::identity::dblDeriv>;
using SigmoidActivation  = DerivedActivationFunction<functors::sigmoid::eval,
                                                    functors::sigmoid::deriv,
                                                    functors::sigmoid::dblDeriv>;
using TanhActivation     = DerivedActivationFunction<functors::tanh::eval,
                                                 functors::tanh::deriv,
                                                 functors::tanh::dblDeriv>;
using ExponentialActivation
    = DerivedActivationFunction<functors::exponential::eval,
                                functors::exponential::deriv,
                                functors::exponential::dblDeriv>;

extern ReluActivation        relu;
extern IdentityActivation    identity;
extern SigmoidActivation     sigmoid;
extern TanhActivation        tanh;
extern ExponentialActivation exponential;

}  // namespace activation


#include "numpy_utils.hpp"
#include <boost/foreach.hpp>
#include <boost/python.hpp>
#include <Eigen/Core>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <iostream>
#include <iostream>
#include <vector>

using namespace Eigen;
using namespace std;
namespace py = boost::python;

/////////////////

// http://en.wikipedia.org/wiki/Axis_angle#Log_map_from_SO.283.29_to_so.283.29
Vector3d LogMap(const Matrix3d& m) {
  double cosarg = (m.trace() - 1)/2;
  cosarg = fmin(cosarg, 1);
  cosarg = fmax(cosarg, -1);
  double theta = acos( cosarg );
  if (theta==0) return Vector3d::Zero();
  else return theta*(1/(2*sin(theta))) * Vector3d(m(2,1) - m(1,2), m(0,2)-m(2,0), m(1,0)-m(0,1));
}

double RotReg(const Matrix3d& b, const Vector3d& rot_coeffs, double scale_coeff) {
  // regularize rotation using polar decomposition
  JacobiSVD<Matrix3d> svd(b.transpose(), ComputeFullU | ComputeFullV);
  Vector3d s = svd.singularValues();
  if (b.determinant() <= 0) return INFINITY;
  return LogMap(svd.matrixU() * svd.matrixV().transpose()).cwiseAbs().dot(rot_coeffs) + s.array().log().square().sum()*scale_coeff;
}

Matrix3d RotRegGrad(const Matrix3d& b, const Vector3d& rot_coeffs, double scale_coeff) {
  Matrix3d out;
  double y0 = RotReg(b, rot_coeffs, scale_coeff);
  Matrix3d xpert = b;
  double epsilon = 1e-5;
  for (int i=0; i < 3; ++i) {
    for (int j=0; j < 3; ++j) {
      xpert(i,j) = b(i,j) + epsilon;
      out(i,j) = (RotReg(xpert, rot_coeffs, scale_coeff) - y0)/epsilon;
      xpert(i,j) = b(i,j);
    }
  }
  return out;
}

Vector3d gRotCoeffs;
double gScaleCoeff;

void PySetCoeffs(py::object rot_coeffs, py::object scale_coeff) {
  gRotCoeffs = Vector3d(py::extract<double>(rot_coeffs[0]), py::extract<double>(rot_coeffs[1]), py::extract<double>(rot_coeffs[2]));
  gScaleCoeff = py::extract<double>(scale_coeff);
}

double PyRotReg(const py::object& m ){
  const double* data = getPointer<double>(m);
  return RotReg( Map< const Matrix<double,3,3,RowMajor> >(data), gRotCoeffs, gScaleCoeff);
}

py::object PyRotRegGrad(const py::object& m) {
  static py::object np_mod = py::import("numpy");
  py::object out = np_mod.attr("empty")(py::make_tuple(3,3));
  const double* data = getPointer<double>(m);
  Matrix<double,3,3,RowMajor> g = RotRegGrad( Map< const Matrix<double,3,3,RowMajor> >(data), gRotCoeffs, gScaleCoeff);
  memcpy(getPointer<double>(out), g.data(), sizeof(double)*9);
  return out;
}

////////////////

void Interp(float x0, float x1, const VectorXf& y0, const VectorXf& y1, const VectorXf& newxs, MatrixXf& newys) {
  float dx = x1 - x0;
  for (int i=0; i < newxs.size(); ++i) {
    newys.row(i) = ((x1 - newxs[i])/dx) * y0 + ((newxs[i] - x0)/dx) * y1;
  }
}

vector<int> Resample(const MatrixXf& x, const VectorXf& _t, float max_err, float max_dx, float max_dt) {
  int N = x.rows(); // number of timesteps
  VectorXf t;
  if (_t.size() == 0){
    t.resize(N);
    for (int i=0; i < N; ++i) t[i] = i;
  }
  else t=_t;
  VectorXi cost(N); // shortest path cost
  VectorXi pred(N); // shortest path predecessor

  MatrixXf q(20, x.cols()); // scratch space for interpolation
  q.setConstant(-666);

  const int NOPRED = -666;
  const int BIGINT = 999999;

  pred.setConstant(NOPRED);
  cost.setConstant(BIGINT);
  cost(0) = 0;
  pred(0) = NOPRED;
  for (int iSrc = 0; iSrc < N; ++iSrc) {
    for (int iTarg = iSrc+1; iTarg < N; ++iTarg) {
      float dx = (x.row(iTarg) - x.row(iSrc)).maxCoeff();
      float dt = t(iTarg) - t(iSrc);
      int seglen = iTarg - iSrc + 1;
      if (q.rows() < seglen) q.resize(2*q.rows(), q.cols());
      Interp(t(iSrc), t(iTarg), x.row(iSrc), x.row(iTarg), t.middleRows(iSrc, seglen), q);
      float err = (q.block(0,0,seglen, x.cols()) - x.middleRows(iSrc, seglen)).cwiseAbs().maxCoeff();
      if ((dx <= max_dx) && (dt <= max_dt || iTarg == iSrc+1) && (err <= max_err)) {
        int newcost = cost(iSrc) + 1;
        if (newcost < cost(iTarg)) {
          cost(iTarg) = newcost;
          pred(iTarg) = iSrc;
        }
      }
      else break;
    }
  }

  int i=N-1;
  vector<int> revpath;
  while (i > 0) {
    revpath.push_back(i);
    i = pred(i);
  }
  revpath.push_back(0);
  std::reverse(revpath.begin(), revpath.end());
  return revpath;
}

py::object pyResample(py::object x, py::object t, float max_err, float max_dx, float max_dt) {
  x = np_mod.attr("array")(x, "float32");
  int xdim0 = py::extract<int>(x.attr("shape")[0]);
  int xdim1 = py::extract<int>(x.attr("shape")[1]);
  float* xdata = getPointer<float>(x);
  t = np_mod.attr("array")(t, "float32");
  int tdim0 = py::extract<int>(t.attr("__len__")());
  float* tdata = getPointer<float>(t);
  vector<int> inds = Resample(Map<MatrixXf>(xdata, xdim0, xdim1), Map<VectorXf>(tdata, tdim0), max_err, max_dx, max_dt);
  return toNdarray1<int>(inds.data(), inds.size());
}
//BOOST_PYTHON_FUNCTION_OVERLOADS(resample_overloads, pyResample, 3, 5);

///////////////////

BOOST_PYTHON_MODULE(fastrapp) {

  np_mod = py::import("numpy");

  py::def("resample", &pyResample, (py::arg("x"), py::arg("t"), py::arg("max_err"),   py::arg("max_dx")=INFINITY, py::arg("max_dt")=INFINITY));

  py::def("set_coeffs", &PySetCoeffs, (py::arg("rot_coeffs"), py::arg("scale_coeff")));
  py::def("rot_reg", &PyRotReg, (py::arg("B")));
  py::def("rot_reg_grad", &PyRotRegGrad, (py::arg("B")));

}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#ifndef ROKKO_UTILITY_XYZ_HAMILTONIAN_HPP
#define ROKKO_UTILITY_XYZ_HAMILTONIAN_HPP

#include <vector>
#include <boost/tuple/tuple.hpp>
#include <rokko/localized_matrix.hpp>

namespace rokko {

namespace xyz_hamiltonian {

template<typename T>
void multiply(int L, const std::vector<std::pair<int, int> >& lattice,
  const std::vector<boost::tuple<double, double, double> >& coupling, const T* v, T* w) {
  int N = 1 << L;
  for (int l = 0; l < lattice.size(); ++l) {
    int i = lattice[l].first;
    int j = lattice[l].second;
    double jx = coupling[l].get<0>();
    double jy = coupling[l].get<1>();
    double jz = coupling[l].get<2>();

    double diag_plus = jz / 4.0;
    double diag_minus = - jz / 4.0;
    double offdiag_plus = (jx + jy) / 4.0;
    double offdiag_minus = (jx - jy) / 4.0;

    int m1 = 1 << i;
    int m2 = 1 << j;
    int m3 = m1 + m2;
    for (int k=0; k<N; ++k) {
      if (((k & m3) == m1) || ((k & m3) == m2)) {
        // when (bit i == 1, bit j == 0) or (bit i == 0, bit j == 1)
        w[k] += diag_minus * v[k] + offdiag_plus * v[k^m3];
      } else {
        w[k] += diag_plus * v[k] + offdiag_minus * v[k^m3];
      }
    }
  }
}

template<typename T>
void multiply(int L, const std::vector<std::pair<int, int> >& lattice,
  const std::vector<boost::tuple<double, double, double> >& coupling, const std::vector<T>& v,
  std::vector<T>& w) {
  multiply(L, lattice, coupling, &v[0], &w[0]);
}

template<typename T>
void fill_diagonal(int L, const std::vector<std::pair<int, int> >& lattice,
  const std::vector<boost::tuple<double, double, double> >& coupling, T* w) {
  int N = 1 << L;
  for (int k=0; k<N; ++k) {
    w[k] = 0;
  }
  for (int l=0; l<lattice.size(); ++l) {
    int i = lattice[l].first;
    int j = lattice[l].second;
    double jx = coupling[l].get<0>();
    double jy = coupling[l].get<1>();
    double jz = coupling[l].get<2>();

    double diag_plus = jz / 4.0;
    double diag_minus = - jz / 4.0;
    double offdiag_plus = (jx + jy) / 4.0;
    double offdiag_minus = (jx - jy) / 4.0;

    int m1 = 1 << i;
    int m2 = 1 << j;
    int m3 = m1 + m2;
    for (int k=0; k<N; ++k) {
      if (((k & m3) == m1) || ((k & m3) == m2)) {
        // when (bit i == 1, bit j == 0) or (bit i == 0, bit j == 1)
        w[k] += diag_minus;
      } else {
        w[k] += diag_plus;
      }
    }
  }
}

template<typename T>
void fill_diagonal(int L, const std::vector<std::pair<int, int> >& lattice,
  const std::vector<boost::tuple<double, double, double> >& coupling, std::vector<T>& w) {
  fill_diagonal(L, lattice, coupling, &w[0]);
}

template<typename T, typename MATRIX_MAJOR>
void generate(int L, const std::vector<std::pair<int, int> >& lattice,
  const std::vector<boost::tuple<double, double, double> >& coupling,
  rokko::localized_matrix<T, MATRIX_MAJOR>& mat) {
  mat.set_zeros();
  int N = 1 << L;
  for (int l=0; l<lattice.size(); ++l) {
    int i = lattice[l].first;
    int j = lattice[l].second;
    double jx = coupling[l].get<0>();
    double jy = coupling[l].get<1>();
    double jz = coupling[l].get<2>();
    double diag_plus = jz / 4.0;
    double diag_minus = - jz/ 4.0;
    double offdiag_plus = (jx + jy) / 4.0;
    double offdiag_minus = (jx - jy) / 4.0;

    int m1 = 1 << i;
    int m2 = 1 << j;
    int m3 = m1 + m2;
    for (int k=0; k<N; ++k) {
      if (((k & m3) == m1) || ((k & m3) == m2)) {
        // when (bit i == 1, bit j == 0) or (bit i == 0, bit j == 1)
        mat(k^m3, k) += offdiag_plus;
        mat(k, k) += diag_minus;
      } else {
        mat(k^m3, k) += offdiag_minus;
        mat(k, k) += diag_plus;
      }
    }
  }
}

} // namespace xyz_hamiltonian

} // namespace rokko

#endif // ROKKO_UTILITY_XYZ_HAMILTONIAN_HPP