// Copyright Paul A. Bristow 2015

// 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)

// Comparison of finding roots using TOMS748, Newton-Raphson, Halley & Schroder algorithms.
// root_n_finding_algorithms.cpp  Generalised for nth root version.

// http://en.wikipedia.org/wiki/Cube_root

// Note that this file contains Quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!
// This program also writes files in Quickbook tables mark-up format.

#include <boost/cstdlib.hpp>
#include <boost/config.hpp>
#include <boost/array.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/math/concepts/real_concept.hpp>
#include <boost/math/tools/roots.hpp>

//using boost::math::policies::policy;
//using boost::math::tools::eps_tolerance; // Binary functor for specified number of bits.
//using boost::math::tools::bracket_and_solve_root;
//using boost::math::tools::toms748_solve;
//using boost::math::tools::halley_iterate;
//using boost::math::tools::newton_raphson_iterate;
//using boost::math::tools::schroder_iterate;

#include <boost/math/special_functions/next.hpp> // For float_distance.
#include <boost/math/special_functions/pow.hpp> // For pow<N>.
#include <boost/math/tools/tuple.hpp> // for tuple and make_tuple.

#include <boost/multiprecision/cpp_bin_float.hpp> // is binary.
using boost::multiprecision::cpp_bin_float_100;
using boost::multiprecision::cpp_bin_float_50;

#include <boost/timer/timer.hpp>
#include <boost/system/error_code.hpp>
#include <boost/preprocessor/stringize.hpp>

// STL
#include <iostream>
#include <iomanip>
#include <string>
#include <vector>
#include <limits>
#include <fstream> // std::ofstream
#include <cmath>
#include <typeinfo> // for type name using typid(thingy).name();

#ifdef __FILE__
  std::string sourcefilename = __FILE__;
#else
  std::string sourcefilename("");
#endif

  std::string chop_last(std::string s)
  {
     std::string::size_type pos = s.find_last_of("\\/");
     if(pos != std::string::npos)
        s.erase(pos);
     else if(s.empty())
        abort();
     else
        s.erase();
     return s;
  }

  std::string make_root()
  {
     std::string result;
     if(sourcefilename.find_first_of(":") != std::string::npos)
     {
        result = chop_last(sourcefilename); // lose filename part
        result = chop_last(result);   // lose /example/
        result = chop_last(result);   // lose /math/
        result = chop_last(result);   // lose /libs/
     }
     else
     {
        result = chop_last(sourcefilename); // lose filename part
        if(result.empty())
           result = ".";
        result += "/../../..";
     }
     return result;
  }

  std::string short_file_name(std::string s)
  {
     std::string::size_type pos = s.find_last_of("\\/");
     if(pos != std::string::npos)
        s.erase(0, pos + 1);
     return s;
  }

  std::string boost_root = make_root();

std::string fp_hardware; // Any hardware features like SEE or AVX

const std::string roots_name = "libs/math/doc/roots/";

const std::string full_roots_name(boost_root + "/libs/math/doc/roots/");

const std::size_t nooftypes = 4;
const std::size_t noofalgos = 4;

double digits_accuracy = 1.0; // 1 == maximum possible accuracy.

std::stringstream ss;

std::ofstream fout;

std::vector<std::string> algo_names =
{
  "TOMS748", "Newton", "Halley", "Schr'''ö'''der"
};

std::vector<std::string> names =
{
  "float", "double", "long double", "cpp_bin_float50"
};

uintmax_t iters; // Global as value of iterations is not returned.

struct root_info
{ // for a floating-point type, float, double ...
  std::size_t max_digits10; // for type.
  std::string full_typename; // for type from type_id.name().
  std::string short_typename; // for type "float", "double", "cpp_bin_float_50" ....
  std::size_t bin_digits;  // binary in floating-point type numeric_limits<T>::digits;
  int get_digits; // fraction of maximum possible accuracy required.
  // = digits * digits_accuracy
  // Vector of values (4) for each algorithm, TOMS748, Newton, Halley & Schroder.
  //std::vector< boost::int_least64_t> times;  converted to int.
  std::vector<int> times; // arbitrary units (ticks).
  //boost::int_least64_t min_time = std::numeric_limits<boost::int_least64_t>::max(); // Used to normalize times (as int).
  std::vector<double> normed_times;
  int min_time = (std::numeric_limits<int>::max)(); // Used to normalize times.
  std::vector<uintmax_t> iterations;
  std::vector<long int> distances;
  std::vector<cpp_bin_float_100> full_results;
}; // struct root_info

std::vector<root_info> root_infos;  // One element for each floating-point type used.

inline std::string build_test_name(const char* type_name, const char* test_name)
{
  std::string result(BOOST_COMPILER);
  result += "|";
  result += BOOST_STDLIB;
  result += "|";
  result += BOOST_PLATFORM;
  result += "|";
  result += type_name;
  result += "|";
  result += test_name;
#if defined(_DEBUG) || !defined(NDEBUG)
  result += "|";
  result += " debug";
#else
  result += "|";
  result += " release";
#endif
  result += "|";
  return result;
} // std::string build_test_name

// Algorithms //////////////////////////////////////////////

// No derivatives - using TOMS748 internally.

template <int N, typename T = double>
struct nth_root_functor_noderiv
{ //  Nth root of x using only function - no derivatives.
  nth_root_functor_noderiv(T const& to_find_root_of) : a(to_find_root_of)
  { // Constructor just stores value a to find root of.
  }
  T operator()(T const& x)
  {
    using boost::math::pow;
    T fx = pow<N>(x) -a; // Difference (estimate x^n - a).
    return fx;
  }
private:
  T a; // to be 'cube_rooted'.
}; // template <int N, class T> struct nth_root_functor_noderiv

template <int N, class T = double>
T nth_root_noderiv(T x)
{ // return Nth root of x using bracket_and_solve (using NO derivatives).
  using namespace std;  // Help ADL of std functions.
  using namespace boost::math::tools; // For bracket_and_solve_root.

  typedef double guess_type;

  int exponent;
  frexp(static_cast<guess_type>(x), &exponent); // Get exponent of z (ignore mantissa).
  T guess = static_cast<T>(ldexp(static_cast<guess_type>(1.), exponent / N)); // Rough guess is to divide the exponent by n.
  //T min = static_cast<T>(ldexp(static_cast<guess_type>(1.) / 2, exponent / N)); // Minimum possible value is half our guess.
  //T max = static_cast<T>(ldexp(static_cast<guess_type>(2.), exponent / N)); // Maximum possible value is twice our guess.

  T factor = 2; // How big steps to take when searching.

  const boost::uintmax_t maxit = 50; // Limit to maximum iterations.
  boost::uintmax_t it = maxit; // Initially our chosen max iterations, but updated with actual.
  bool is_rising = true; // So if result if guess^3 is too low, then try increasing guess.
  // Some fraction of digits is used to control how accurate to try to make the result.
  int get_digits = std::numeric_limits<T>::digits - 2;
  eps_tolerance<T> tol(get_digits); // Set the tolerance.
  std::pair<T, T> r;
  r =  bracket_and_solve_root(nth_root_functor_noderiv<N, T>(x), guess, factor, is_rising, tol, it);
  iters = it;
  T result = r.first + (r.second - r.first) / 2;  // Midway between brackets.
  return result;
} // template <class T> T nth_root_noderiv(T x)

// Using 1st derivative only Newton-Raphson

template <int N, class T = double>
struct nth_root_functor_1deriv
{ // Functor also returning 1st derivative.
  BOOST_STATIC_ASSERT_MSG(boost::is_integral<T>::value == false, "Only floating-point type types can be used!");
  BOOST_STATIC_ASSERT_MSG((N > 0) == true, "root N must be > 0!");

  nth_root_functor_1deriv(T const& to_find_root_of) : a(to_find_root_of)
  { // Constructor stores value a to find root of, for example:
  }
  std::pair<T, T> operator()(T const& x)
  { // Return both f(x) and f'(x).
    using boost::math::pow; // // Compile-time integral power.
    T p = pow<N - 1>(x);
    return std::make_pair(p * x - a, N * p); // 'return' both fx and dx.
  }

private:
  T a; // to be 'nth_rooted'.
}; // struct nthroot__functor_1deriv

template <int N, class T = double>
T nth_root_1deriv(T x)
{ // return nth root of x using 1st derivative and Newton_Raphson.
  using namespace std;  // Help ADL of std functions.
  using namespace boost::math::tools; // For newton_raphson_iterate.

  BOOST_STATIC_ASSERT_MSG(boost::is_integral<T>::value == false, "Only floating-point type types can be used!");
  BOOST_STATIC_ASSERT_MSG((N > 0) == true, "root N must be > 0!");
  BOOST_STATIC_ASSERT_MSG((N > 1000) == false, "root N is too big!");

  typedef double guess_type;

  int exponent;
  frexp(static_cast<guess_type>(x), &exponent); // Get exponent of z (ignore mantissa).
  T guess = static_cast<T>(ldexp(static_cast<guess_type>(1.), exponent / N)); // Rough guess is to divide the exponent by n.
  T min = static_cast<T>(ldexp(static_cast<guess_type>(1.) / 2, exponent / N)); // Minimum possible value is half our guess.
  T max = static_cast<T>(ldexp(static_cast<guess_type>(2.), exponent / N)); // Maximum possible value is twice our guess.

  int digits = std::numeric_limits<T>::digits; // Maximum possible binary digits accuracy for type T.
  int get_digits = static_cast<int>(digits * 0.6);
  const boost::uintmax_t maxit = 20;
  boost::uintmax_t it = maxit;
  T result = newton_raphson_iterate(nth_root_functor_1deriv<N, T>(x), guess, min, max, get_digits, it);
  iters = it;
  return result;
} // T nth_root_1_deriv  Newton-Raphson

// Using 1st and 2nd derivatives with Halley algorithm.

template <int N, class T = double>
struct nth_root_functor_2deriv
{ // Functor returning both 1st and 2nd derivatives.
  BOOST_STATIC_ASSERT_MSG(boost::is_integral<T>::value == false, "Only floating-point type types can be used!");
  BOOST_STATIC_ASSERT_MSG((N > 0) == true, "root N must be > 0!");

  nth_root_functor_2deriv(T const& to_find_root_of) : a(to_find_root_of)
  { // Constructor stores value a to find root of, for example:
  }

  // using boost::math::tuple; // to return three values.
  std::tuple<T, T, T> operator()(T const& x)
  { // Return f(x), f'(x) and f''(x).
    using boost::math::pow; // Compile-time integral power.
    T p = pow<N - 2>(x);

    return std::make_tuple(p * x * x - a, p * x * N, p * N * (N - 1)); // 'return' fx, dx and d2x.
  }
private:
  T a; // to be 'nth_rooted'.
};

template <int N, class T = double>
T nth_root_2deriv(T x)
{ // return nth root of x using 1st and 2nd derivatives and Halley.

  using namespace std;  // Help ADL of std functions.
  using namespace boost::math::tools; // For halley_iterate.

  BOOST_STATIC_ASSERT_MSG(boost::is_integral<T>::value == false, "Only floating-point type types can be used!");
  BOOST_STATIC_ASSERT_MSG((N > 0) == true, "root N must be > 0!");
  BOOST_STATIC_ASSERT_MSG((N > 1000) == false, "root N is too big!");

  typedef double guess_type;

  int exponent;
  frexp(static_cast<guess_type>(x), &exponent); // Get exponent of z (ignore mantissa).
  T guess = static_cast<T>(ldexp(static_cast<guess_type>(1.), exponent / N)); // Rough guess is to divide the exponent by n.
  T min = static_cast<T>(ldexp(static_cast<guess_type>(1.) / 2, exponent / N)); // Minimum possible value is half our guess.
  T max = static_cast<T>(ldexp(static_cast<guess_type>(2.), exponent / N)); // Maximum possible value is twice our guess.

  int digits = std::numeric_limits<T>::digits; // Maximum possible binary digits accuracy for type T.
  int get_digits = static_cast<int>(digits * 0.4);
  const boost::uintmax_t maxit = 20;
  boost::uintmax_t it = maxit;
  T result = halley_iterate(nth_root_functor_2deriv<N, T>(x), guess, min, max, get_digits, it);
  iters = it;

  return result;
} // nth_2deriv Halley

template <int N, class T = double>
T nth_root_2deriv_s(T x)
{ // return nth root of x using 1st and 2nd derivatives and Schroder.

  using namespace std;  // Help ADL of std functions.
  using namespace boost::math::tools; // For schroder_iterate.

  BOOST_STATIC_ASSERT_MSG(boost::is_integral<T>::value == false, "Only floating-point type types can be used!");
  BOOST_STATIC_ASSERT_MSG((N > 0) == true, "root N must be > 0!");
  BOOST_STATIC_ASSERT_MSG((N > 1000) == false, "root N is too big!");

  typedef double guess_type;

  int exponent;
  frexp(static_cast<guess_type>(x), &exponent); // Get exponent of z (ignore mantissa).
  T guess = static_cast<T>(ldexp(static_cast<guess_type>(1.), exponent / N)); // Rough guess is to divide the exponent by n.
  T min = static_cast<T>(ldexp(static_cast<guess_type>(1.) / 2, exponent / N)); // Minimum possible value is half our guess.
  T max = static_cast<T>(ldexp(static_cast<guess_type>(2.), exponent / N)); // Maximum possible value is twice our guess.

  int get_digits = static_cast<int>(std::numeric_limits<T>::digits * 0.4);
  const boost::uintmax_t maxit = 20;
  boost::uintmax_t it = maxit;
  T result = schroder_iterate(nth_root_functor_2deriv<N, T>(x), guess, min, max, get_digits, it);
  iters = it;

  return result;
} // T nth_root_2deriv_s Schroder

//////////////////////////////////////////////////////// end of algorithms - perhaps in a separate .hpp?

//! Print 4 floating-point types info: max_digits10, digits and required accuracy digits as a Quickbook table.
int table_type_info(double digits_accuracy)
{
  std::string qbk_name = full_roots_name; // Prefix by boost_root file.

  qbk_name += "type_info_table";
  std::stringstream ss;
  ss.precision(3);
  ss << "_" << digits_accuracy * 100;
  qbk_name += ss.str();

#ifdef _MSC_VER
  qbk_name += "_msvc.qbk";
#else // assume GCC
  qbk_name += "_gcc.qbk";
#endif

  // Example: type_info_table_100_msvc.qbk
  fout.open(qbk_name, std::ios_base::out);

  if (fout.is_open())
  {
    std::cout << "Output type table to " << qbk_name << std::endl;
  }
  else
  { // Failed to open.
    std::cout << " Open file " << qbk_name << " for output failed!" << std::endl;
    std::cout << "errno " << errno << std::endl;
    return errno;
  }

  fout <<
    "[/"
    << qbk_name
    << "\n"
    "Copyright 2015 Paul A. Bristow.""\n"
    "Copyright 2015 John Maddock.""\n"
    "Distributed under the Boost Software License, Version 1.0.""\n"
    "(See accompanying file LICENSE_1_0.txt or copy at""\n"
    "http://www.boost.org/LICENSE_1_0.txt).""\n"
    "]""\n"
    << std::endl;

  fout << "[h6 Fraction of maximum possible bits of accuracy required is " << digits_accuracy << ".]\n" << std::endl;

  std::string table_id("type_info");
  table_id += ss.str(); // Fraction digits accuracy.

#ifdef _MSC_VER
  table_id += "_msvc";
#else // assume GCC
  table_id += "_gcc";
#endif

  fout << "[table:" << table_id << " Digits for float, double, long double and cpp_bin_float_50\n"
    << "[[type name] [max_digits10] [binary digits] [required digits]]\n";// header.

  // For all fout types:

  fout  << "[[" << "float" << "]"
    << "[" << std::numeric_limits<float>::max_digits10 << "]"  // max_digits10
    << "[" << std::numeric_limits<float>::digits << "]"// < "Binary digits
    << "[" << static_cast<int>(std::numeric_limits<float>::digits * digits_accuracy) << "]]\n"; // Accuracy digits.

  fout << "[[" << "float" << "]"
    << "[" << std::numeric_limits<double>::max_digits10 << "]"  // max_digits10
    << "[" << std::numeric_limits<double>::digits << "]"// < "Binary digits
    << "[" << static_cast<int>(std::numeric_limits<double>::digits * digits_accuracy) << "]]\n"; // Accuracy digits.

  fout << "[[" << "long double" << "]"
    << "[" << std::numeric_limits<long double>::max_digits10 << "]"  // max_digits10
    << "[" << std::numeric_limits<long double>::digits << "]"// < "Binary digits
    << "[" << static_cast<int>(std::numeric_limits<long double>::digits * digits_accuracy) << "]]\n"; // Accuracy digits.

  fout << "[[" << "cpp_bin_float_50" << "]"
    << "[" << std::numeric_limits<cpp_bin_float_50>::max_digits10 << "]"  // max_digits10
    << "[" << std::numeric_limits<cpp_bin_float_50>::digits << "]"// < "Binary digits
    << "[" << static_cast<int>(std::numeric_limits<cpp_bin_float_50>::digits * digits_accuracy) << "]]\n"; // Accuracy digits.

  fout << "] [/table table_id_msvc] \n" << std::endl; // End of table.

  fout.close();
  return 0;
} // type_table

//! Evaluate root N timing for each algorithm, and for one floating-point type T.
template <int N, typename T>
int test_root(cpp_bin_float_100 big_value, cpp_bin_float_100 answer, const char* type_name, std::size_t type_no)
{
  std::size_t max_digits = 2 + std::numeric_limits<T>::digits * 3010 / 10000;
  // For new versions use max_digits10
  // std::cout.precision(std::numeric_limits<T>::max_digits10);
  std::cout.precision(max_digits);
  std::cout << std::showpoint << std::endl; // Show trailing zeros too.

  root_infos.push_back(root_info());

  root_infos[type_no].max_digits10 = max_digits;
  root_infos[type_no].full_typename = typeid(T).name(); // Full typename.
  root_infos[type_no].short_typename = type_name; // Short typename.
  root_infos[type_no].bin_digits = std::numeric_limits<T>::digits;
  root_infos[type_no].get_digits = static_cast<int>(std::numeric_limits<T>::digits * digits_accuracy);

  T to_root = static_cast<T>(big_value);

  T result; // root
  T sum = 0;
  T ans = static_cast<T>(answer);

  using boost::timer::nanosecond_type;
  using boost::timer::cpu_times;
  using boost::timer::cpu_timer;

  int eval_count = boost::is_floating_point<T>::value ? 10000000 : 100000; // To give a sufficiently stable timing for the fast built-in types,
  //int eval_count = 1000000; // To give a sufficiently stable timing for the fast built-in types,
  // This takes an inconveniently long time for multiprecision cpp_bin_float_50 etc  types.

  cpu_times now; // Holds wall, user and system times.

  { // Evaluate times etc for each algorithm.
    //algorithm_names.push_back("TOMS748"); //
    cpu_timer ti; // Can start, pause, resume and stop, and read elapsed.
    ti.start();
    for (long i = 0; i < eval_count; ++i)
    {
      result = nth_root_noderiv<N, T>(to_root); //
      sum += result;
    }
    now = ti.elapsed();
    int time = static_cast<int>(now.user / eval_count);
    root_infos[type_no].times.push_back(time); // CPU time taken.
    if (time < root_infos[type_no].min_time)
    {
      root_infos[type_no].min_time = time;
    }
    ti.stop();
    long int distance = static_cast<int>(boost::math::float_distance<T>(result, ans));
    root_infos[type_no].distances.push_back(distance);
    root_infos[type_no].iterations.push_back(iters); //
    root_infos[type_no].full_results.push_back(result);
  }
  {
    // algorithm_names.push_back("Newton"); // algorithm
    cpu_timer ti; // Can start, pause, resume and stop, and read elapsed.
    ti.start();
    for (long i = 0; i < eval_count; ++i)
    {
      result = nth_root_1deriv<N, T>(to_root); //
      sum += result;
    }
    now = ti.elapsed();
    int time = static_cast<int>(now.user / eval_count);
    root_infos[type_no].times.push_back(time); // CPU time taken.
    if (time < root_infos[type_no].min_time)
    {
      root_infos[type_no].min_time = time;
    }

    ti.stop();
    long int distance = static_cast<int>(boost::math::float_distance<T>(result, ans));
    root_infos[type_no].distances.push_back(distance);
    root_infos[type_no].iterations.push_back(iters); //
    root_infos[type_no].full_results.push_back(result);
  }
  {
    //algorithm_names.push_back("Halley"); // algorithm
    cpu_timer ti; // Can start, pause, resume and stop, and read elapsed.
    ti.start();
    for (long i = 0; i < eval_count; ++i)
    {
      result = nth_root_2deriv<N>(to_root); //
      sum += result;
    }
    now = ti.elapsed();
    int time = static_cast<int>(now.user / eval_count);
    root_infos[type_no].times.push_back(time); // CPU time taken.
    ti.stop();
    if (time < root_infos[type_no].min_time)
    {
      root_infos[type_no].min_time = time;
    }
    long int distance = static_cast<int>(boost::math::float_distance<T>(result, ans));
    root_infos[type_no].distances.push_back(distance);
    root_infos[type_no].iterations.push_back(iters); //
    root_infos[type_no].full_results.push_back(result);
  }
  {
    // algorithm_names.push_back("Schroder"); // algorithm
    cpu_timer ti; // Can start, pause, resume and stop, and read elapsed.
    ti.start();
    for (long i = 0; i < eval_count; ++i)
    {
      result = nth_root_2deriv_s<N>(to_root); //
      sum += result;
    }
    now = ti.elapsed();
    int time = static_cast<int>(now.user / eval_count);
    root_infos[type_no].times.push_back(time); // CPU time taken.
    if (time < root_infos[type_no].min_time)
    {
      root_infos[type_no].min_time = time;
    }
    ti.stop();
    long int distance = static_cast<int>(boost::math::float_distance<T>(result, ans));
    root_infos[type_no].distances.push_back(distance);
    root_infos[type_no].iterations.push_back(iters); //
    root_infos[type_no].full_results.push_back(result);
  }
  for (size_t i = 0; i != root_infos[type_no].times.size(); i++) // For each time.
  { // Normalize times.
    root_infos[type_no].normed_times.push_back(static_cast<double>(root_infos[type_no].times[i]) / root_infos[type_no].min_time);
  }

  std::cout << "Accumulated result was: " << sum << std::endl;

  return 4;  // eval_count of how many algorithms used.
} // test_root

/*! Fill array of times, iterations, etc for Nth root for all 4 types,
 and write a table of results in Quickbook format.
 */
template <int N>
void table_root_info(cpp_bin_float_100 full_value)
{
   using std::abs;
  std::cout << nooftypes << " floating-point types tested:" << std::endl;
#if defined(_DEBUG) || !defined(NDEBUG)
  std::cout << "Compiled in debug mode." << std::endl;
#else
  std::cout << "Compiled in optimise mode." << std::endl;
#endif
  std::cout << "FP hardware " << fp_hardware << std::endl;
  // Compute the 'right' answer for root N at 100 decimal digits.
  cpp_bin_float_100 full_answer = nth_root_noderiv<N, cpp_bin_float_100>(full_value);

  root_infos.clear(); // Erase any previous data.
  // Fill the elements of the array for each floating-point type.

  test_root<N, float>(full_value, full_answer, "float", 0);
  test_root<N, double>(full_value, full_answer, "double", 1);
  test_root<N, long double>(full_value, full_answer, "long double", 2);
  test_root<N, cpp_bin_float_50>(full_value, full_answer, "cpp_bin_float_50", 3);

  // Use info from 4 floating point types to

  // Prepare Quickbook table for a single root
  // with columns of times, iterations, distances repeated for various floating-point types,
  // and 4 rows for each algorithm.

  std::stringstream table_info;
  table_info.precision(3);
  table_info << "[table:root_" << N << " " << N << "th root(" << static_cast<float>(full_value) << ") for float, double, long double and cpp_bin_float_50 types";
  if (fp_hardware != "")
  {
    table_info << ", using " << fp_hardware;
  }
  table_info << std::endl;

  fout << table_info.str()
    << "[[][float][][][] [][double][][][] [][long d][][][] [][cpp50][][]]\n"
    << "[[Algo     ]";
  for (size_t tp = 0; tp != nooftypes; tp++)
  { // For all types:
    fout << "[Its]" << "[Times]" << "[Norm]" << "[Dis]" << "[ ]";
  }
  fout << "]" << std::endl;

  // Row for all algorithms.
  for (std::size_t algo = 0; algo != noofalgos; algo++)
  {
    fout << "[[" << std::left << std::setw(9) << algo_names[algo] << "]";
    for (size_t tp = 0; tp != nooftypes; tp++)
    { // For all types:
      fout
        << "[" << std::right << std::showpoint
        << std::setw(3) << std::setprecision(2) << root_infos[tp].iterations[algo] << "]["
        << std::setw(5) << std::setprecision(5) << root_infos[tp].times[algo] << "][";
      fout << std::setw(3) << std::setprecision(3);
        double normed_time = root_infos[tp].normed_times[algo];
        if (abs(normed_time - 1.00) <= 0.05)
        { // At or near the best time, so show as blue.
          fout << "[role blue " << normed_time << "]";
        }
        else if (abs(normed_time) > 4.)
        { // markedly poor so show as red.
          fout << "[role red " << normed_time << "]";
        }
        else
        { // Not the best, so normal black.
          fout << normed_time;
        }
        fout << "]["
        << std::setw(3) << std::setprecision(2) << root_infos[tp].distances[algo] << "][ ]";
    } // tp
    fout << "]" << std::endl;
  } // for algo
  fout << "] [/end of table root]\n";
} // void table_root_info

/*! Output program header, table of type info, and tables for 4 algorithms and 4 floating-point types,
 for Nth root required digits_accuracy.
 */

int roots_tables(cpp_bin_float_100 full_value, double digits_accuracy)
{
  ::digits_accuracy = digits_accuracy;
  // Save globally so that it is available to root-finding algorithms. Ugly :-(

#if defined(_DEBUG) || !defined(NDEBUG)
  std::string debug_or_optimize("Compiled in debug mode.");
#else
     std::string debug_or_optimize("Compiled in optimise mode.");
#endif

  // Create filename for roots_table
  std::string qbk_name = full_roots_name;
  qbk_name += "roots_table";

  std::stringstream ss;
  ss.precision(3);
  // ss << "_" << N // now put all the tables in one .qbk file?
    ss << "_" << digits_accuracy * 100
    << std::flush;
  // Assume only save optimize mode runs, so don't add any  _DEBUG info.
  qbk_name += ss.str();

#ifdef _MSC_VER
  qbk_name += "_msvc";
#else // assume GCC
  qbk_name += "_gcc";
#endif
  if (fp_hardware != "")
  {
    qbk_name += fp_hardware;
  }
  qbk_name += ".qbk";

  fout.open(qbk_name, std::ios_base::out);

  if (fout.is_open())
  {
    std::cout << "Output root table to " << qbk_name << std::endl;
  }
  else
  { // Failed to open.
    std::cout << " Open file " << qbk_name << " for output failed!" << std::endl;
    std::cout << "errno " << errno << std::endl;
    return errno;
  }

  fout <<
    "[/"
    << qbk_name
    << "\n"
    "Copyright 2015 Paul A. Bristow.""\n"
    "Copyright 2015 John Maddock.""\n"
    "Distributed under the Boost Software License, Version 1.0.""\n"
    "(See accompanying file LICENSE_1_0.txt or copy at""\n"
    "http://www.boost.org/LICENSE_1_0.txt).""\n"
    "]""\n"
    << std::endl;

  // Print out the program/compiler/stdlib/platform names as a Quickbook comment:
  fout << "\n[h6 Program " << sourcefilename << ",\n "
    << BOOST_COMPILER << ", "
    << BOOST_STDLIB << ", "
    << BOOST_PLATFORM << "\n"
    << debug_or_optimize
    << ((fp_hardware != "") ? ", " + fp_hardware : "")
    << "]" // [h6 close].
    << std::endl;

  fout << "Fraction of full accuracy " << digits_accuracy << std::endl;

  table_root_info<5>(full_value);
  table_root_info<7>(full_value);
  table_root_info<11>(full_value);

  fout.close();

  //   table_type_info(digits_accuracy);

  return 0;
} // roots_tables

int main()
{
  using namespace boost::multiprecision;
  using namespace boost::math;

  try
  {
    std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << ", ";

// How to: Configure Visual C++ Projects to Target 64-Bit Platforms
// https://msdn.microsoft.com/en-us/library/9yb4317s.aspx

#ifdef _M_X64 // Defined for compilations that target x64 processors.
    std::cout << "X64 " << std::endl;
    fp_hardware += "_X64";
#else
#  ifdef _M_IX86
     std::cout << "X32 " << std::endl;
     fp_hardware += "_X86";
#  endif
#endif

#ifdef _M_AMD64
    std::cout << "AMD64 " << std::endl;
 //   fp_hardware += "_AMD64";
#endif

// https://msdn.microsoft.com/en-us/library/7t5yh4fd.aspx
// /arch (x86) options /arch:[IA32|SSE|SSE2|AVX|AVX2]
// default is to use SSE and SSE2 instructions by default.
// https://msdn.microsoft.com/en-us/library/jj620901.aspx
// /arch (x64) options /arch:AVX and /arch:AVX2

// MSVC doesn't bother to set these SSE macros!
// http://stackoverflow.com/questions/18563978/sse-sse2-is-enabled-control-in-visual-studio
// https://msdn.microsoft.com/en-us/library/b0084kay.aspx  predefined macros.

// But some of these macros are *not* defined by MSVC,
// unlike AVX (but *are* defined by GCC and Clang).
// So the macro code above does define them.
#if (defined(_M_AMD64) || defined (_M_X64))
#ifndef _M_X64
#  define _M_X64
#endif
#ifndef __SSE2__
#  define __SSE2__
#endif
#else
#  ifdef _M_IX86_FP // Expands to an integer literal value indicating which /arch compiler option was used:
    std::cout << "Floating-point _M_IX86_FP = " << _M_IX86_FP << std::endl;
#  if (_M_IX86_FP == 2) // 2 if /arch:SSE2, /arch:AVX or /arch:AVX2
#    define __SSE2__ // x32
#  elif (_M_IX86_FP == 1) // 1 if /arch:SSE was used.
#    define __SSE__ // x32
#  elif (_M_IX86_FP == 0) // 0 if /arch:IA32 was used.
#    define _X32 // No special FP instructions.
#  endif
# endif
#endif
// Set the fp_hardware that is used in the .qbk filename.
#ifdef __AVX2__
    std::cout << "Floating-point AVX2 " << std::endl;
    fp_hardware += "_AVX2";
#  else
#    ifdef __AVX__
    std::cout << "Floating-point AVX " << std::endl;
    fp_hardware += "_AVX";
#    else
#      ifdef __SSE2__
    std::cout << "Floating-point SSE2 " << std::endl;
    fp_hardware += "_SSE2";
#      else
#        ifdef __SSE__
    std::cout << "Floating-point SSE " << std::endl;
    fp_hardware += "_SSE";
#        endif
#      endif
#   endif
# endif

#ifdef _M_IX86
    std::cout << "Floating-point X86 _M_IX86 = " << _M_IX86 << std::endl;
    // https://msdn.microsoft.com/en-us/library/aa273918%28v=vs.60%29.aspx#_predir_table_1..3
    // 600 = Pentium Pro
#endif

#ifdef _MSC_FULL_VER
    std::cout << "Floating-point _MSC_FULL_VER " << _MSC_FULL_VER << std::endl;
#endif

#ifdef __MSVC_RUNTIME_CHECKS
    std::cout << "Runtime __MSVC_RUNTIME_CHECKS " << std::endl;
#endif

    BOOST_MATH_CONTROL_FP;

    cpp_bin_float_100 full_value("28.");
    // Compute full answer to more than precision of tests.
    //T value = 28.; // integer (exactly representable as floating-point)
    // whose cube root is *not* exactly representable.
    // Wolfram Alpha command N[28 ^ (1 / 3), 100] computes cube root to 100 decimal digits.
    // 3.036588971875662519420809578505669635581453977248111123242141654169177268411884961770250390838097895

    std::cout.precision(100);
    std::cout << "value " << full_value << std::endl;
   // std::cout << ",\n""answer = " << full_answer << std::endl;
    std::cout.precision(6);
   // cbrt cpp_bin_float_100 full_answer("3.036588971875662519420809578505669635581453977248111123242141654169177268411884961770250390838097895");

    // Output the table of types, maxdigits10 and digits and required digits for some accuracies.

    // Output tables for some roots at full accuracy.
    roots_tables(full_value, 1.);

    // Output tables for some roots at less accuracy.
    //roots_tables(full_value, 0.75);

    return boost::exit_success;
  }
  catch (std::exception const& ex)
  {
    std::cout << "exception thrown: " << ex.what() << std::endl;
    return boost::exit_failure;
  }
} // int main()

/*

*/


// g++ -I.. sparse_lu.cpp -O3 -g0 -I /usr/include/superlu/ -lsuperlu -lgfortran -DSIZE=1000 -DDENSITY=.05 && ./a.out

#define EIGEN_SUPERLU_SUPPORT
#define EIGEN_UMFPACK_SUPPORT
#include <Eigen/Sparse>

#define NOGMM
#define NOMTL

#ifndef SIZE
#define SIZE 10
#endif

#ifndef DENSITY
#define DENSITY 0.01
#endif

#ifndef REPEAT
#define REPEAT 1
#endif

#include "BenchSparseUtil.h"

#ifndef MINDENSITY
#define MINDENSITY 0.0004
#endif

#ifndef NBTRIES
#define NBTRIES 10
#endif

#define BENCH(X) \
  timer.reset(); \
  for (int _j=0; _j<NBTRIES; ++_j) { \
    timer.start(); \
    for (int _k=0; _k<REPEAT; ++_k) { \
        X  \
  } timer.stop(); }

typedef Matrix<Scalar,Dynamic,1> VectorX;

#include <Eigen/LU>

template<int Backend>
void doEigen(const char* name, const EigenSparseMatrix& sm1, const VectorX& b, VectorX& x, int flags = 0)
{
  std::cout << name << "..." << std::flush;
  BenchTimer timer; timer.start();
  SparseLU<EigenSparseMatrix,Backend> lu(sm1, flags);
  timer.stop();
  if (lu.succeeded())
    std::cout << ":\t" << timer.value() << endl;
  else
  {
    std::cout << ":\t FAILED" << endl;
    return;
  }

  bool ok;
  timer.reset(); timer.start();
  ok = lu.solve(b,&x);
  timer.stop();
  if (ok)
    std::cout << "  solve:\t" << timer.value() << endl;
  else
    std::cout << "  solve:\t" << " FAILED" << endl;

  //std::cout << x.transpose() << "\n";
}

int main(int argc, char *argv[])
{
  int rows = SIZE;
  int cols = SIZE;
  float density = DENSITY;
  BenchTimer timer;

  VectorX b = VectorX::Random(cols);
  VectorX x = VectorX::Random(cols);

  bool densedone = false;

  //for (float density = DENSITY; density>=MINDENSITY; density*=0.5)
//   float density = 0.5;
  {
    EigenSparseMatrix sm1(rows, cols);
    fillMatrix(density, rows, cols, sm1);

    // dense matrices
    #ifdef DENSEMATRIX
    if (!densedone)
    {
      densedone = true;
      std::cout << "Eigen Dense\t" << density*100 << "%\n";
      DenseMatrix m1(rows,cols);
      eiToDense(sm1, m1);

      BenchTimer timer;
      timer.start();
      FullPivLU<DenseMatrix> lu(m1);
      timer.stop();
      std::cout << "Eigen/dense:\t" << timer.value() << endl;

      timer.reset();
      timer.start();
      lu.solve(b,&x);
      timer.stop();
      std::cout << "  solve:\t" << timer.value() << endl;
//       std::cout << b.transpose() << "\n";
//       std::cout << x.transpose() << "\n";
    }
    #endif

    #ifdef EIGEN_UMFPACK_SUPPORT
    x.setZero();
    doEigen<Eigen::UmfPack>("Eigen/UmfPack (auto)", sm1, b, x, 0);
    #endif

    #ifdef EIGEN_SUPERLU_SUPPORT
    x.setZero();
    doEigen<Eigen::SuperLU>("Eigen/SuperLU (nat)", sm1, b, x, Eigen::NaturalOrdering);
//     doEigen<Eigen::SuperLU>("Eigen/SuperLU (MD AT+A)", sm1, b, x, Eigen::MinimumDegree_AT_PLUS_A);
//     doEigen<Eigen::SuperLU>("Eigen/SuperLU (MD ATA)", sm1, b, x, Eigen::MinimumDegree_ATA);
    doEigen<Eigen::SuperLU>("Eigen/SuperLU (COLAMD)", sm1, b, x, Eigen::ColApproxMinimumDegree);
    #endif

  }

  return 0;
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

#include <iostream>
#include <Eigen/Eigen>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Array<double,1,3> x(8,25,3),
                  e(1./3.,0.5,2.);
cout << "[" << x << "]^[" << e << "] = " << x.pow(e) << endl; // using ArrayBase::pow
cout << "[" << x << "]^[" << e << "] = " << pow(x,e) << endl; // using Eigen::pow

  return 0;
}


#include <opengv/optimization_tools/solver_tools/SolverTools.hpp>
#include <opengv/optimization_tools/objective_function_tools/ObjectiveFunctionInfo.hpp>
#include <Eigen/Dense>
#include <opengv/types.hpp>

class SolverToolsNoncentralRelativePose : public SolverTools {
public:
  Eigen::Matrix3d exp_R( Eigen::Matrix3d & X );

  opengv::rotation_t rotation_solver(opengv::rotation_t & state_rotation, const opengv::translation_t & translation,
                                     double &tol, ObjectiveFunctionInfo * info_function, int & k);

  opengv::translation_t translation_solver(const opengv::rotation_t & rotation,
                                           opengv::translation_t & translation, double &tol, ObjectiveFunctionInfo * info_function, double &step, int & k);

};


#include <NTL/ZZ_pX.h>
#include <NTL/ZZX.h>
#include <NTL/BasicThreadPool.h>

NTL_CLIENT

#define ITER (500)

void multest()
{
   cerr << "mul";
   for (long iter = 0; iter < ITER; iter++) {
      if (iter % 100 == 0) cerr << ".";

      long da = RandomBnd(5000) + 100;
      long db = RandomBnd(5000) + 100;

      ZZ_pX a, b, c1, c2;

      random(a, da);
      random(b, db);

      if (deg(a) < 80 || deg(b) < 80) {
         cerr << "*";
         continue;
      }

      FFTMul(c1, a, b);

      ZZX A, B, C;
      conv(A, a);
      conv(B, b);
      mul(C, A, B);
      conv(c2, C);

      if (c1 != c2) {
         cerr << "******* oops\n";
         break;
      }
   }

   cerr << "\n";
}

void sqrtest()
{
   cerr << "sqr";
   for (long iter = 0; iter < ITER; iter++) {
      if (iter % 100 == 0) cerr << ".";

      long da = RandomBnd(5000) + 100;
      long db = RandomBnd(5000) + 100;

      ZZ_pX a, b, c1, c2;

      random(a, da);

      if (deg(a) < 80) {
         cerr << "*";
         continue;
      }

      FFTSqr(c1, a);

      ZZX A, B, C;
      conv(A, a);
      sqr(C, A);
      conv(c2, C);

      if (c1 != c2) {
         cerr << "******* oops\n";
         break;
      }
   }

   cerr << "\n";
}

void mulmodtest()
{
   cerr << "mulmod";
   for (long iter = 0; iter < ITER; iter++) {
      if (iter % 100 == 0) cerr << ".";

      long n = RandomBnd(5000) + 300;
      long da = RandomBnd(n)+1;
      long db = RandomBnd(n)+1;

      if (RandomBnd(2)) { da = n; db = n; }

      ZZ_pX f;
      random(f, n);
      SetCoeff(f, n);
      ZZ_pXModulus F(f);

      ZZ_pX a, b, c1, c2;
      random(a, da);
      random(b, db);

      MulMod(c1, a, b, F);

      ZZX A, B, C;
      conv(A, a);
      conv(B, b);
      mul(C, A, B);
      conv(c2, C);
      rem(c2, c2, F);

      if (c1 != c2) {
         cerr << "******** oops\n";
         break;
      }
   }

   cerr << "\n";
}

void sqrmodtest()
{
   cerr << "sqrmod";
   for (long iter = 0; iter < ITER; iter++) {
      if (iter % 100 == 0) cerr << ".";

      long n = RandomBnd(5000) + 300;
      long da = RandomBnd(n)+1;
      long db = RandomBnd(n)+1;

      if (RandomBnd(2)) { da = n; db = n; }

      ZZ_pX f;
      random(f, n);
      SetCoeff(f, n);
      ZZ_pXModulus F(f);

      ZZ_pX a, b, c1, c2;
      random(a, da);
      random(b, db);

      SqrMod(c1, a, F);

      ZZX A, B, C;
      conv(A, a);
      conv(B, b);
      sqr(C, A);
      conv(c2, C);
      rem(c2, c2, F);

      if (c1 != c2) {
         cerr << "******** oops\n";
         break;
      }
   }

   cerr << "\n";
}

void mulmod1test()
{
   cerr << "mulmod1";
   for (long iter = 0; iter < ITER; iter++) {
      if (iter % 100 == 0) cerr << ".";

      long n = RandomBnd(5000) + 300;
      long da = RandomBnd(n)+1;
      long db = RandomBnd(n)+1;

      if (RandomBnd(2)) { da = n; db = n; }

      ZZ_pX f;
      random(f, n);
      SetCoeff(f, n);
      ZZ_pXModulus F(f);

      ZZ_pX a, b, c1, c2;
      random(a, da);
      random(b, db);

      ZZ_pXMultiplier bb;
      build(bb, b, F);

      MulMod(c1, a, bb, F);

      ZZX A, B, C;
      conv(A, a);
      conv(B, b);
      mul(C, A, B);
      conv(c2, C);
      rem(c2, c2, F);

      if (c1 != c2) {
         cerr << "******** oops\n";
         break;
      }
   }

   cerr << "\n";
}

namespace NTL {

void CopyReverse(ZZ_pX& x, const ZZ_pX& a, long lo, long hi);

}

struct ZZ_pXTransMultiplier {
   ZZ_pX f0, fbi, b;
   long shamt, shamt_fbi, shamt_b;
};

void build(ZZ_pXTransMultiplier& B, const ZZ_pX& b, const ZZ_pXModulus& F)
{
   long db = deg(b);

   if (db >= F.n) LogicError("build TransMultiplier: bad args");

   ZZ_pX t;

   LeftShift(t, b, F.n-1);
   div(t, t, F);

   // we optimize for low degree b

   long d;

   d = deg(t);
   if (d < 0)
      B.shamt_fbi = 0;
   else
      B.shamt_fbi = F.n-2 - d;

   CopyReverse(B.fbi, t, 0, d);

   // The following code optimizes the case when
   // f = X^n + low degree poly

   trunc(t, F.f, F.n);
   d = deg(t);
   if (d < 0)
      B.shamt = 0;
   else
      B.shamt = d;

   CopyReverse(B.f0, t, 0, d);

   if (db < 0)
      B.shamt_b = 0;
   else
      B.shamt_b = db;

   CopyReverse(B.b, b, 0, db);
}

void TransMulMod(ZZ_pX& x, const ZZ_pX& a, const ZZ_pXTransMultiplier& B,
               const ZZ_pXModulus& F)
{
   if (deg(a) >= F.n) LogicError("TransMulMod: bad args");

   ZZ_pX t1, t2;

   mul(t1, a, B.b);
   RightShift(t1, t1, B.shamt_b);

   mul(t2, a, B.f0);
   RightShift(t2, t2, B.shamt);
   trunc(t2, t2, F.n-1);

   mul(t2, t2, B.fbi);
   if (B.shamt_fbi > 0) LeftShift(t2, t2, B.shamt_fbi);
   trunc(t2, t2, F.n-1);
   LeftShift(t2, t2, 1);

   sub(x, t1, t2);
}

void UpdateMap(vec_ZZ_p& x, const vec_ZZ_p& a,
         const ZZ_pXTransMultiplier& B, const ZZ_pXModulus& F)
{
   ZZ_pX xx;
   TransMulMod(xx, to_ZZ_pX(a), B, F);
   x = xx.rep;
}

void updatetest()
{
   cerr << "update";
   for (long iter = 0; iter < ITER; iter++) {
      if (iter % 100 == 0) cerr << ".";

      long n = RandomBnd(5000) + 300;
      long da = RandomBnd(n)+1;
      long db = RandomBnd(n)+1;

      if (RandomBnd(2)) { da = n; db = n; }

      ZZ_pX f;
      random(f, n);
      SetCoeff(f, n);
      ZZ_pXModulus F(f);

      ZZ_pX a, b;
      random(a, da);
      random(b, db);

      ZZ_pXMultiplier bb1;
      build(bb1, b, F);

      ZZ_pXTransMultiplier bb2;
      build(bb2, b, F);

      Vec<ZZ_p> x1, x2;

      UpdateMap(x1, a.rep, bb1, F);
      UpdateMap(x2, a.rep, bb2, F);

      if (x1 != x2) {
         cerr << "******** oops\n";
         break;
      }
   }

   cerr << "\n";
}

void divremtest()
{
   cerr << "divrem";
   for (long iter = 0; iter < ITER; iter++) {
      if (iter % 100 == 0) cerr << ".";

      long n = RandomBnd(5000) + 300;
      long dq = RandomBnd(n);

      ZZ_pX f;
      random(f, n);
      SetCoeff(f, n);
      ZZ_pXModulus F(f);

      ZZ_pX a, q, r, q1, r1;

      random(a, 2*n-1);

      DivRem(q, r, a, F);
      rem(r1, a, F);
      div(q1, a, F);

      if (deg(r) >= n || a != q*f + r || q != q1 || r != r1) {
         cerr << "******** oops\n";
         break;
      }
   }

   cerr << "\n";
}

int main()
{
   ZZ p;
   GenPrime(p, 100);

   ZZ_p::init(p);

   multest();
   sqrtest();
   mulmodtest();
   sqrmodtest();
   mulmod1test();
   divremtest();
   updatetest();

#ifdef NTL_THREAD_BOOST

   GenPrime(p, 500);
   ZZ_p::init(p);

   SetNumThreads(4);
   cerr << "numthreads=4\n";

   multest();
   sqrtest();
   mulmodtest();
   sqrmodtest();
   mulmod1test();
   divremtest();
   updatetest();

#endif

}


#include <mpi.h> // has to be before plasma
#ifdef USE_MKL
#include <mkl.h>
#include <mkl_cblas.h>
#include <mkl_lapacke.h>
#else
#include <cblas.h>
#include <lapacke.h>
#endif
#include <Eigen/Core> // after MKL
#include <Eigen/Dense>
#include <fstream>
#include <array>
#include <random>
#include <mutex>
#include <iostream>
#include <map>
#include <tuple>

#include "tasktorrent/tasktorrent.hpp"
#include "util_shared.hpp"

using namespace std;
using namespace Eigen;
using namespace ttor;

int VERB = 0;
bool LOG = false;
bool TEST = true;
int n_threads_ = 2;
int n_ = 2;
int M_ = 4;
int N_ = 2;

// multi-threaded MKL dgeqp3 code -- test only on  a single node
int mkl_dlaqps(int n_threads, int n, int M, int N)
{
    // MPI info
    const int rank = comm_rank();
    const int nranks = comm_size();
    if(VERB) printf("[%d] Hello from %s\n", comm_rank(), processor_name().c_str());

    assert(nranks==1);

    int nb = min(32, n); // inner blocking size

    // Warmup MKL
    warmup_mkl(n_threads);

    mkl_set_num_threads(n_threads);

    MatrixXd A = MatrixXd::Zero(0,0);
    MatrixXd Acopy = MatrixXd::Zero(0,0);

    auto val = [&](int i, int j) { return 1/(double)((i-j)*(i-j)+1); };
    auto val_vec = [&](int i) {return 1/(double)(i*i+1);};

    if (TEST) {
        MatrixXd X =  MatrixXd::NullaryExpr(M*n,M*n,val);
        VectorXd d = VectorXd::Zero(M*n);
        d.head(N*n) = VectorXd::LinSpaced(N*n,1, N*n);

        DiagonalMatrix<double, Eigen::Dynamic> D(M*n);
        D = d.asDiagonal();
        A = X*D*X.inverse(); // A is square

        Acopy = A;
        if(rank == 0 && VERB) {
            cout << A << endl;
        }
    } // X and D gets deleted here
    else {
        A = MatrixXd::NullaryExpr(M*n,M*n,val);
    }
    // A is a square M*n x M*n matrix
    VectorXd ht = VectorXd(min(M*n, M*n));
    VectorXi jpvt = VectorXi::Zero(M*n);
    int matrix_rank=0;

    auto t0 = wctime();
    laqps(&A, &jpvt, &ht, N*n, matrix_rank);
    auto t1 = wctime();

    if(matrix_rank != N*n) {
        cout << matrix_rank << endl;
        return -1;
    }

    if (rank == 0){
        cout << "Time taken: " << elapsed(t0, t1) << endl;
    }

    if (rank == 0 && TEST){
        MatrixXd v = A.leftCols(N*n);
        VectorXd h = ht.topRows(N*n);
        orgqr(&v, &h);

        double error = (Acopy - v*(v.transpose()*Acopy)).norm();
        cout << "Error solve: " << error << endl;
        assert(error<=1e-8);
    }

    return 0;
}

int main(int argc, char **argv)
{
    int req = MPI_THREAD_FUNNELED;
    int prov = -1;

    MPI_Init_thread(NULL, NULL, req, &prov);

    assert(prov == req);

    if (argc >= 2)
    {
        n_threads_ = atoi(argv[1]);
    }

    if (argc >= 3)
    {
        n_ = atoi(argv[2]);
    }

    if (argc >= 4)
    {
        M_ = atoi(argv[3]);
    }

    if (argc >= 5)
    {
        N_ = atoi(argv[4]);
    }

    if (argc >= 6)
    {
        VERB = atoi(argv[5]);
    }

    if (argc >= 7)
    {
        LOG = atoi(argv[6]);
    }

    if (argc >= 8)
    {
        TEST = atoi(argv[7]);
    }

    const int return_flag = mkl_dlaqps(n_threads_, n_, M_, N_);
    MPI_Finalize();
    return return_flag;
}


//#########################################################//
//#                                                       #//
//# gaussian_mixture_models  pca.cpp                      #//
//# Roberto Capobianco  <capobianco@dis.uniroma1.it>      #//
//#                                                       #//
//#########################################################//

#include <stdexcept>

#include <Eigen/SVD>
#include <iostream>

#include <particle_filter/pca.h>

namespace gmms {
Eigen::MatrixXd
PCA::pca(const Eigen::MatrixXd& dataset, double& retained_variance) {
    if (num_components_ > dataset.cols()) {
        throw std::runtime_error(
                "Number of components greater than dataset size");
    }

    Eigen::MatrixXd centered = dataset.rowwise() - dataset.colwise().mean();

    Eigen::RowVectorXd maxValues = centered.colwise().maxCoeff();
    Eigen::RowVectorXd minValues = centered.colwise().minCoeff();
    Eigen::MatrixXd normalized_dataset =
            centered.array().rowwise() / (maxValues - minValues).array();

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(
            normalized_dataset, Eigen::ComputeThinV);

    Eigen::VectorXd S = svd.singularValues();
    Eigen::MatrixXd W = svd.matrixV().leftCols(num_components_);

    retained_variance = S.head(num_components_).sum() / S.sum();

    Eigen::MatrixXd projected = normalized_dataset * W;

    return projected;
}
}  // namespace gmms


#pragma once

#include <Eigen/Core>

#include <ipc/distance/point_point.hpp>

namespace ipc {

/// @brief Compute the distance between a point and line in 2D or 3D.
/// @note The distance is actually squared distance.
/// @param p The point.
/// @param e0 The first vertex of the edge defining the line.
/// @param e1 The second vertex of the edge defining the line.
/// @return The distance between the point and line.
template <typename DerivedP, typename DerivedE0, typename DerivedE1>
auto point_line_distance(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1)
{
    assert(p.size() == 2 || p.size() == 3);
    assert(e0.size() == 2 || e0.size() == 3);
    assert(e1.size() == 2 || e1.size() == 3);

    if (p.size() == 2) {
        auto e = e1 - e0;
        auto numerator =
            (e[1] * p[0] - e[0] * p[1] + e1[0] * e0[1] - e1[1] * e0[0]);
        return numerator * numerator / e.squaredNorm();
    } else {
        return cross(e0 - p, e1 - p).squaredNorm() / (e1 - e0).squaredNorm();
    }
}

// Symbolically generated derivatives;
namespace autogen {
    void point_line_distance_gradient_2D(
        double v01,
        double v02,
        double v11,
        double v12,
        double v21,
        double v22,
        double g[6]);

    void point_line_distance_gradient_3D(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double g[9]);

    void point_line_distance_hessian_2D(
        double v01,
        double v02,
        double v11,
        double v12,
        double v21,
        double v22,
        double H[36]);

    void point_line_distance_hessian_3D(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double H[81]);
} // namespace autogen

/// @brief Compute the gradient of the distance between a point and line.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] e0 The first vertex of the edge defining the line.
/// @param[in] e1 The second vertex of the edge defining the line.
/// @param[out] grad The gradient of the distance wrt p, e0, and e1.
template <
    typename DerivedP,
    typename DerivedE0,
    typename DerivedE1,
    typename DerivedGrad>
void point_line_distance_gradient(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    assert(p.size() == 2 || p.size() == 3);
    assert(e0.size() == 2 || e0.size() == 3);
    assert(e1.size() == 2 || e1.size() == 3);

    grad.resize(p.size() + e0.size() + e1.size());
    if (p.size() == 2) {
        autogen::point_line_distance_gradient_2D(
            p[0], p[1], e0[0], e0[1], e1[0], e1[1], grad.data());
    } else {
        autogen::point_line_distance_gradient_3D(
            p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2],
            grad.data());
    }
}

/// @brief Compute the hessian of the distance between a point and line.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] e0 The first vertex of the edge defining the line.
/// @param[in] e1 The second vertex of the edge defining the line.
/// @param[out] hess The hessian of the distance wrt p, e0, and e1.
template <
    typename DerivedP,
    typename DerivedE0,
    typename DerivedE1,
    typename DerivedHess>
void point_line_distance_hessian(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    assert(p.size() == 2 || p.size() == 3);
    assert(e0.size() == 2 || e0.size() == 3);
    assert(e1.size() == 2 || e1.size() == 3);

    hess.resize(
        p.size() + e0.size() + e1.size(), p.size() + e0.size() + e1.size());
    if (p.size() == 2) {
        autogen::point_line_distance_hessian_2D(
            p[0], p[1], e0[0], e0[1], e1[0], e1[1], hess.data());
    } else {
        autogen::point_line_distance_hessian_3D(
            p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2],
            hess.data());
    }
}

} // namespace ipc


// restored from rdiff-backup: 2016-09-19T17:24:46+02:00

#include <omp.h>
#include <Eigen/Sparse>
#include <algorithm>
#include <boost/math/constants/constants.hpp>
#include <boost/program_options.hpp>
#include <fstream>
#include <iostream>
#include <numeric>
#include <string>

#include <base/eigen2hdf.hpp>
#include <fft/ft_grid_helpers.hpp>
#include <matrices/matrix_entries.hpp>
#include <ridgelet/construction/ft.hpp>
#include <ridgelet/construction/translation_grid.hpp>
#include <ridgelet/lambda.hpp>
#include <ridgelet/ridgelet_frame.hpp>
#include "base/init.hpp"

using namespace std;

const double Lx = 1.0;
const double Ly = 1.0;
const double tol = 1e-10;

const double PI = boost::math::constants::pi<double>();

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

  namespace po = boost::program_options;

  po::options_description options("options");
  options.add_options()("help", "produce help message")
      ("Jxy,J", po::value<int>(&J)->default_value(3), "J")
      ("rho, r", po::value<int>(&rho)->default_value(1), "rho_x")
      ("transport", "compute transport part too");
  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, options), vm);
    po::notify(vm);
    if (vm.count("help")) {
      std::cout << options << "\n";
      return 0;
    }
  } catch (std::exception& e) {
    std::cout << e.what() << "\n";
  }

  std::cout << "J:   " << J << "\n"
            << "rho: " << rho << "\n";

  RidgeletFrame frame(J, J, rho, rho);

  std::string fname = std::string("mass_matrix_entries") + std::to_string(J) + ".dat";
  // compute the translation grid sizes for reach lambda
  // and
  cout << "Translation grid..."
       << "\n";
  std::vector<unsigned int> tgrid_size(frame.size());
  std::vector<unsigned int> num_elements(frame.size() + 1, 0);
  auto& lambdas = frame.lambdas();
  for (unsigned int i = 0; i < frame.size(); ++i) {
    auto T = tgrid_dim(lambdas[i], frame);
    tgrid_size[i] = std::get<0>(T) * std::get<1>(T);
  }
  std::partial_sum(tgrid_size.begin(), tgrid_size.end(), num_elements.begin() + 1);
  {
    std::ofstream fout(std::string("num_elems") + std::to_string(J) + ".dat");
    std::for_each(
        num_elements.begin(), num_elements.end(), [&fout](unsigned int i) { fout << i << "\n"; });
    fout.close();
  }

  // Normalization ------------------------------
  cout << "Normalization..."
       << "\n";
  std::vector<double> ft_coeff_norms(frame.size());
#pragma omp parallel for
  for (unsigned int i = 0; i < frame.size(); ++i) {
    auto lambda = frame.lambdas()[i];
    if (lambda.t == rt_type::S) {
      ft_coeff_norms[i] = std::sqrt(frame.get_dense(lambda).cwiseAbs2().sum());
    } else {
      ft_coeff_norms[i] = std::sqrt(frame.get_sparse(lambda).cwiseAbs2().sum());
    }
  }

  /*
   *  key = (l_i, l_j, t_d)
   *  l_i, l_j, integer:
   *  ------------------
   *
   *  hint: lambda_i = lambdas[l_i]
   *
   *  difference translation grid t_d:
   *  -----------------
   *  tuple t_d = {tx_i - tx_j, ty_i - ty_j}
   */
  typedef std::tuple<int, int> tgrid_diff_key; /*  type of t_d */
  typedef std::tuple<int, int, tgrid_diff_key> key_type;
  std::unordered_map<key_type, double> mass_matrix_entries;
  std::unordered_map<key_type, double> transport_matrix_entries;

  cout << "Computing integrals..."
       << "\n";
#pragma omp parallel for schedule(dynamic, 1)
  for (unsigned int i = 0; i < frame.size(); ++i) {
    for (unsigned int j = 0; j <= i; ++j) {
      // skip pairs that are known to evaluate to zero
      if (std::abs(lambdas[i].j - lambdas[j].j) > 1) continue;
      if ((lambdas[i].t == rt_type::X && lambdas[j].t == rt_type::Y) ||
          (lambdas[i].t == rt_type::Y && lambdas[j].t == rt_type::X))
        continue;
      if ((lambdas[i].t == lambdas[j].t) && (lambdas[i].j == lambdas[j].j) &&
          (std::abs(lambdas[i].k - lambdas[j].k) > 1))
        continue;

      auto& Psih_i = frame.get_sparse(lambdas[i]);
      // Psih_i, Psih_j sparse
      auto& Psih_j = frame.get_sparse(lambdas[j]);

      auto T1 = tgrid_dim(lambdas[i], frame);
      auto T2 = tgrid_dim(lambdas[j], frame);

      int tx = std::max(std::get<0>(T1), std::get<0>(T2));
      int ty = std::max(std::get<1>(T1), std::get<1>(T2));

      // translation grid
      auto vtx = Eigen::ArrayXd::LinSpaced(tx, 0, (1 - 1 / double(tx)) * Lx);
      auto vty = Eigen::ArrayXd::LinSpaced(ty, 0, (1 - 1 / double(ty)) * Ly);

      double fnorm = 1 / (ft_coeff_norms[i] * ft_coeff_norms[j]);
      for (int idx_tx = 0; idx_tx < tx; ++idx_tx) {
        for (int idx_ty = 0; idx_ty < ty; ++idx_ty) {
          auto k = std::make_tuple(idx_tx, idx_ty);
          auto key = std::make_tuple(i, j, k);
          double v = compute_mass_entry(Psih_i, Psih_j, vtx[idx_tx], vty[idx_ty]);
          v *= fnorm;
          if (std::abs(v) > tol) {
#pragma omp critical
            mass_matrix_entries[key] = v;
          }

          if (vm.count("transport")) {
            double vxy = compute_tentry(
                Psih_i, Psih_j, vtx[idx_tx], vty[idx_ty], derivative_t::dX, derivative_t::dY);
            vxy *= fnorm;
            double vxx = compute_tentry(
                Psih_i, Psih_j, vtx[idx_tx], vty[idx_ty], derivative_t::dX, derivative_t::dX);
            vxx *= fnorm;
            double vyy = compute_tentry(
                Psih_i, Psih_j, vtx[idx_tx], vty[idx_ty], derivative_t::dY, derivative_t::dY);
            vyy *= fnorm;

            double vt = 2 * vxy + vxx + vyy;

            if (std::abs(vt) > tol) {
#pragma omp critical
              transport_matrix_entries[key] = vt;
            }
          }
        }
      }
    }  // end for inner lambda
  }    // end for outer lambda

  std::cout << "mass_matrix_entries.size: " << mass_matrix_entries.size() << "\n";
  std::cout << "writing results to " << fname << "\n";
  // write to disk
  std::ofstream fout(fname);
  for (auto& elem : mass_matrix_entries) {
    auto& key = elem.first;
    auto val = elem.second;
    int i = std::get<0>(key);
    int j = std::get<1>(key);
    int tx, ty;
    std::tie(tx, ty) = std::get<2>(key);
    fout << i << "\t" << j << "\t" << tx << "\t" << ty << "\t" << val << "\n";
  }
  fout.close();
  cout << "\tdone"
       << "\n";

  if (vm.count("transport")) {
    std::string fname = std::string("transport_matrix_" + std::to_string(J) + ".dat");
    std::ofstream fout(fname);
    for (auto& elem : transport_matrix_entries) {
      auto& key = elem.first;
      auto val = elem.second;
      int i = std::get<0>(key);
      int j = std::get<1>(key);
      int tx, ty;
      std::tie(tx, ty) = std::get<2>(key);
      fout << i << "\t" << j << "\t" << tx << "\t" << ty << "\t" << val << "\n";
    }
    fout.close();
  }

  return 0;
}


// Copyright 2021 Apex.AI, Inc.
//
// 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_
#define HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_

#include <Eigen/Cholesky>

namespace autoware
{
namespace common
{
namespace helper_functions
{
/// \brief Calculate square of mahalanobis distance
/// \tparam T Type of elements in the matrix
/// \tparam kNumOfStates Number of states
/// \param sample Single column matrix containing sample whose distance needs to be computed
/// \param mean Single column matrix containing mean of samples received so far
/// \param covariance_factor Covariance matrix
/// \return Square of mahalanobis distance
template<typename T, std::int32_t kNumOfStates>
types::float32_t calculate_squared_mahalanobis_distance(
  const Eigen::Matrix<T, kNumOfStates, 1> & sample,
  const Eigen::Matrix<T, kNumOfStates, 1> & mean,
  const Eigen::Matrix<T, kNumOfStates, kNumOfStates> & covariance_factor)
{
  using Vector = Eigen::Matrix<T, kNumOfStates, 1>;
  // This is equivalent to the squared Mahalanobis distance of the form: diff.T * C.inv() * diff
  // Instead of the covariance matrix C we have its lower-triangular factor L, such that C = L * L.T
  // squared_mahalanobis_distance = diff.T * C.inv() * diff
  // = diff.T * (L * L.T).inv() * diff
  // = diff.T * L.T.inv() * L.inv() * diff
  // = (L.inv() * diff).T * (L.inv() * diff)
  // this allows us to efficiently find the squared Mahalanobis distance using (L.inv() * diff),
  // which can be found as a solution to: L * x = diff.
  const Vector diff = sample - mean;
  const Vector x = covariance_factor.ldlt().solve(diff);
  return x.transpose() * x;
}

/// \brief Calculate mahalanobis distance
/// \tparam T Type of elements in the matrix
/// \tparam kNumOfStates Number of states
/// \param sample Single column matrix containing sample whose distance needs to be computed
/// \param mean Single column matrix containing mean of samples received so far
/// \param covariance_factor Covariance matrix
/// \return Mahalanobis distance
template<typename T, std::int32_t kNumOfStates>
types::float32_t calculate_mahalanobis_distance(
  const Eigen::Matrix<T, kNumOfStates, 1> & sample,
  const Eigen::Matrix<T, kNumOfStates, 1> & mean,
  const Eigen::Matrix<T, kNumOfStates, kNumOfStates> & covariance_factor
)
{
  return sqrtf(calculate_squared_mahalanobis_distance(sample, mean, covariance_factor));
}
}  // namespace helper_functions
}  // namespace common
}  // namespace autoware

#endif  // HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_


// Boost.Geometry

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

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

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

#ifndef BOOST_GEOMETRY_FORMULAS_MAXIMUM_LONGITUDE_HPP
#define BOOST_GEOMETRY_FORMULAS_MAXIMUM_LONGITUDE_HPP

#include <boost/geometry/formulas/spherical.hpp>
#include <boost/geometry/formulas/flattening.hpp>

#include <boost/mpl/assert.hpp>

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

namespace boost { namespace geometry { namespace formula
{

/*!
\brief Algorithm to compute the vertex longitude of a geodesic segment. Vertex is
a point on the geodesic that maximizes (or minimizes) the latitude. The algorithm
is given the vertex latitude.
*/

//Classes for spesific CS

template <typename CT>
class vertex_longitude_on_sphere
{

public:

    template <typename T>
    static inline CT apply(T const& lat1, //segment point 1
                           T const& lat2, //segment point 2
                           T const& lat3, //vertex latitude
                           T const& sin_l12,
                           T const& cos_l12) //lon1 -lon2
    {
        //https://en.wikipedia.org/wiki/Great-circle_navigation#Finding_way-points
        CT const A = sin(lat1) * cos(lat2) * cos(lat3) * sin_l12;
        CT const B = sin(lat1) * cos(lat2) * cos(lat3) * cos_l12
                - cos(lat1) * sin(lat2) * cos(lat3);
        CT lon = atan2(B, A);
        return lon + math::pi<CT>();
    }
};

template <typename CT>
class vertex_longitude_on_spheroid
{
    template<typename T>
    static inline void normalize(T& x, T& y)
    {
        T h = boost::math::hypot(x, y);
        x /= h;
        y /= h;
    }

public:

    template <typename T, typename Spheroid>
    static inline CT apply(T const& lat1, //segment point 1
                           T const& lat2, //segment point 2
                           T const& lat3, //vertex latitude
                           T& alp1,
                           Spheroid const& spheroid)
    {
        // We assume that segment points lay on different side w.r.t.
        // the vertex

        // Constants
        CT const c0 = 0;
        CT const c2 = 2;
        CT const half_pi = math::pi<CT>() / c2;
        if (math::equals(lat1, half_pi)
                || math::equals(lat2, half_pi)
                || math::equals(lat1, -half_pi)
                || math::equals(lat2, -half_pi))
        {
            // one segment point is the pole
            return c0;
        }

        // More constants
        CT const f = flattening<CT>(spheroid);
        CT const pi = math::pi<CT>();
        CT const c1 = 1;
        CT const cminus1 = -1;

        // First, compute longitude on auxiliary sphere

        CT const one_minus_f = c1 - f;
        CT const bet1 = atan(one_minus_f * tan(lat1));
        CT const bet2 = atan(one_minus_f * tan(lat2));
        CT const bet3 = atan(one_minus_f * tan(lat3));

        CT cos_bet1 = cos(bet1);
        CT cos_bet2 = cos(bet2);
        CT const sin_bet1 = sin(bet1);
        CT const sin_bet2 = sin(bet2);
        CT const sin_bet3 = sin(bet3);

        CT omg12 = 0;

        if (bet1 < c0)
        {
            cos_bet1 *= cminus1;
            omg12 += pi;
        }
        if (bet2 < c0)
        {
            cos_bet2 *= cminus1;
            omg12 += pi;
        }

        CT const sin_alp1 = sin(alp1);
        CT const cos_alp1 = math::sqrt(c1 - math::sqr(sin_alp1));

        CT const norm = math::sqrt(math::sqr(cos_alp1) + math::sqr(sin_alp1 * sin_bet1));
        CT const sin_alp0 = sin(atan2(sin_alp1 * cos_bet1, norm));

        BOOST_ASSERT(cos_bet2 != c0);
        CT const sin_alp2 = sin_alp1 * cos_bet1 / cos_bet2;

        CT const cos_alp0 = math::sqrt(c1 - math::sqr(sin_alp0));
        CT const cos_alp2 = math::sqrt(c1 - math::sqr(sin_alp2));

        CT const sig1 = atan2(sin_bet1, cos_alp1 * cos_bet1);
        CT const sig2 = atan2(sin_bet2, -cos_alp2 * cos_bet2); //lat3 is a vertex

        CT const cos_sig1 = cos(sig1);
        CT const sin_sig1 = math::sqrt(c1 - math::sqr(cos_sig1));

        CT const cos_sig2 = cos(sig2);
        CT const sin_sig2 = math::sqrt(c1 - math::sqr(cos_sig2));

        CT const omg1 = atan2(sin_alp0 * sin_sig1, cos_sig1);
        CT const omg2 = atan2(sin_alp0 * sin_sig2, cos_sig2);

        omg12 += omg1 - omg2;

        CT const sin_omg12 = sin(omg12);
        CT const cos_omg12 = cos(omg12);

        CT omg13 = geometry::formula::vertex_longitude_on_sphere<CT>
                ::apply(bet1, bet2, bet3, sin_omg12, cos_omg12);

        if (lat1 * lat2 < c0)//different hemispheres
        {
            if ((lat2 - lat1) * lat3  > c0)// ascending segment
            {
                omg13 = pi - omg13;
            }
        }

        // Second, compute the ellipsoidal longitude

        CT const e2 = f * (c2 - f);
        CT const ep = math::sqrt(e2 / (c1 - e2));
        CT const k2 = math::sqr(ep * cos_alp0);
        CT const sqrt_k2_plus_one = math::sqrt(c1 + k2);
        CT const eps = (sqrt_k2_plus_one - c1) / (sqrt_k2_plus_one + c1);
        CT const eps2 = eps * eps;
        CT const n = f / (c2 - f);

        // sig3 is the length from equator to the vertex
        CT sig3;
        if(sin_bet3 > c0)
        {
            sig3 = half_pi;
        } else {
            sig3 = -half_pi;
        }
        CT const cos_sig3 = 0;
        CT const sin_sig3 = 1;

        CT sig13 = sig3 - sig1;
        if (sig13 > pi)
        {
            sig13 -= 2 * pi;
        }

        // Order 2 approximation
        CT const c1over2 = 0.5;
        CT const c1over4 = 0.25;
        CT const c1over8 = 0.125;
        CT const c1over16 = 0.0625;
        CT const c4 = 4;
        CT const c8 = 8;

        CT const A3 = 1 - (c1over2 - c1over2 * n) * eps - c1over4 * eps2;
        CT const C31 = (c1over4 - c1over4 * n) * eps + c1over8 * eps2;
        CT const C32 = c1over16 * eps2;

        CT const sin2_sig3 = c2 * cos_sig3 * sin_sig3;
        CT const sin4_sig3 = sin_sig3 * (-c4 * cos_sig3
                                         + c8 * cos_sig3 * cos_sig3 * cos_sig3);
        CT const sin2_sig1 = c2 * cos_sig1 * sin_sig1;
        CT const sin4_sig1 = sin_sig1 * (-c4 * cos_sig1
                                         + c8 * cos_sig1 * cos_sig1 * cos_sig1);
        CT const I3 = A3 * (sig13
                            + C31 * (sin2_sig3 - sin2_sig1)
                            + C32 * (sin4_sig3 - sin4_sig1));

        CT const sign = bet3 >= c0
                      ? c1
                      : cminus1;

        CT const dlon_max = omg13 - sign * f * sin_alp0 * I3;

        return dlon_max;
    }
};

//CS_tag dispatching

template <typename CT, typename CS_Tag>
struct compute_vertex_lon
{
    BOOST_MPL_ASSERT_MSG
    (
        false, NOT_IMPLEMENTED_FOR_THIS_COORDINATE_SYSTEM, (types<CS_Tag>)
    );

};

template <typename CT>
struct compute_vertex_lon<CT, spherical_equatorial_tag>
{
    template <typename Strategy>
    static inline CT apply(CT const& lat1,
                           CT const& lat2,
                           CT const& vertex_lat,
                           CT const& sin_l12,
                           CT const& cos_l12,
                           CT,
                           Strategy)
    {
        return vertex_longitude_on_sphere<CT>
                ::apply(lat1,
                        lat2,
                        vertex_lat,
                        sin_l12,
                        cos_l12);
    }
};

template <typename CT>
struct compute_vertex_lon<CT, geographic_tag>
{
    template <typename Strategy>
    static inline CT apply(CT const& lat1,
                           CT const& lat2,
                           CT const& vertex_lat,
                           CT,
                           CT,
                           CT& alp1,
                           Strategy const& azimuth_strategy)
    {
        return vertex_longitude_on_spheroid<CT>
                ::apply(lat1,
                        lat2,
                        vertex_lat,
                        alp1,
                        azimuth_strategy.model());
    }
};

// Vertex longitude interface
// Assume that lon1 < lon2 and vertex_lat is the latitude of the vertex

template <typename CT, typename CS_Tag>
class vertex_longitude
{
public :
    template <typename Strategy>
    static inline CT apply(CT& lon1,
                           CT& lat1,
                           CT& lon2,
                           CT& lat2,
                           CT const& vertex_lat,
                           CT& alp1,
                           Strategy const& azimuth_strategy)
    {
        CT const c0 = 0;
        CT pi = math::pi<CT>();

        //Vertex is a segment's point
        if (math::equals(vertex_lat, lat1))
        {
            return lon1;
        }
        if (math::equals(vertex_lat, lat2))
        {
            return lon2;
        }

        //Segment lay on meridian
        if (math::equals(lon1, lon2))
        {
            return (std::max)(lat1, lat2);
        }
        BOOST_ASSERT(lon1 < lon2);

        CT dlon = compute_vertex_lon<CT, CS_Tag>::apply(lat1, lat2,
                                                        vertex_lat,
                                                        sin(lon1 - lon2),
                                                        cos(lon1 - lon2),
                                                        alp1,
                                                        azimuth_strategy);

        CT vertex_lon = std::fmod(lon1 + dlon, 2 * pi);

        if (vertex_lat < c0)
        {
            vertex_lon -= pi;
        }

        if (std::abs(lon1 - lon2) > pi)
        {
            vertex_lon -= pi;
        }

        return vertex_lon;
    }
};

}}} // namespace boost::geometry::formula
#endif // BOOST_GEOMETRY_FORMULAS_MAXIMUM_LONGITUDE_HPP


/*
 * random.cpp
 *
 */
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <random>

#include <boost/assert.hpp>

#include "core/random.h"

using namespace std;
using namespace boost;
using namespace yann;

namespace yann {

typedef mt19937 DefaultGenerator;

////////////////////////////////////////////////////////////////////////////////////////////////
//
// yann::RandomGenerator_NormalDistribution implementation
//
class RandomGenerator_NormalDistribution : public RandomGenerator {
  friend class RandomGenerator;

public:
  RandomGenerator_NormalDistribution(const Value & mean, const Value & stddev, optional<Value> seed) :
    _gen(seed ? (unsigned)(*seed) : _rd()),
    _dist(mean, stddev)
  {
  }

  // RandomGenerator overwrites
  Value next() { return _dist(_gen); }

private:
  RandomGenerator_NormalDistribution(const RandomGenerator_NormalDistribution &) = delete;
  RandomGenerator_NormalDistribution& operator=(const RandomGenerator_NormalDistribution &) = delete;

private:
  random_device _rd;
  DefaultGenerator _gen;
  std::normal_distribution<Value> _dist;
}; // class RandomGenerator_NormalDistribution

////////////////////////////////////////////////////////////////////////////////////////////////
//
// yann::RandomGenerator_UniformDistribution implementation
//
class RandomGenerator_UniformDistribution : public RandomGenerator {
  friend class RandomGenerator;

public:
  RandomGenerator_UniformDistribution(const Value & aa, const Value & bb, optional<Value> seed) :
    _gen(seed ? (unsigned)(*seed) : _rd()),
    _dist(aa, bb)
  {
  }

  // RandomGenerator overwrites
  Value next() { return _dist(_gen); }

private:
  RandomGenerator_UniformDistribution(const RandomGenerator_UniformDistribution &) = delete;
  RandomGenerator_UniformDistribution& operator=(const RandomGenerator_UniformDistribution &) = delete;

private:
  random_device _rd;
  DefaultGenerator _gen;
  uniform_real_distribution<Value> _dist;
}; // class RandomGenerator_NormalDistribution

}; // RandomGenerator

////////////////////////////////////////////////////////////////////////////////////////////////
//
// yann::RandomGenerator implementation
//
unique_ptr<RandomGenerator> yann::RandomGenerator::normal_distribution(
    const Value & mean, const Value & stddev, optional<Value> seed)
{
  return make_unique<RandomGenerator_NormalDistribution>(mean, stddev, seed);
}

unique_ptr<RandomGenerator> yann::RandomGenerator::uniform_distribution(
    const Value & aa, const Value & bb, optional<Value> seed)
{
  return make_unique<RandomGenerator_UniformDistribution>(aa, bb, seed);
}

void yann::RandomGenerator::generate(Value & val)
{
  val = next();
}

void yann::RandomGenerator::generate(RefMatrix mm)
{
  for(auto ii = 0; ii < mm.rows(); ++ii) {
    for(auto jj = 0; jj < mm.cols(); ++jj) {
      mm(ii, jj) = next();
    }
  }
}


/*
 * ClusterRotate.cpp
 *
 *  Created on: 04-Mar-2009
 *      Author: sbutler
 */

#define EIGEN2_SUPPORT

#include "ClusterRotate.h"

#include <map>
#include <Eigen/Array>

ClusterRotate::ClusterRotate(int method) :
	mMethod(method),
	mMaxQuality(0)
{

}

std::vector<std::vector<int> > ClusterRotate::cluster(Eigen::MatrixXd& X)
{
	mMaxQuality = 0;
	std::vector<std::vector<int> > clusters;
	Eigen::MatrixXd vecRot;
	Eigen::MatrixXd vecIn = X.block(0, 0, X.rows(), 2);
	Evrot* e = NULL;
	for (int g = 2; g <= X.cols(); g++)
	{
		// make it incremental (used already aligned vectors)
		if (g > 2)
		{
			vecIn.resize(X.rows(), g);
			vecIn.block(0, 0, vecIn.rows(), g - 1) = e->getRotatedEigenVectors();
			vecIn.block(0, g - 1, X.rows(), 1) = X.block(0, g - 1, X.rows(), 1);
			delete e;
		}
		//perform the rotation for the current number of dimensions
		e = new Evrot(vecIn, mMethod);

		//save max quality
		if (e->getQuality() > mMaxQuality)
		{
			mMaxQuality = e->getQuality();
		}
		//save cluster data for max cluster or if we're near the max cluster (so prefer more clusters)
		if ((e->getQuality() > mMaxQuality) || (mMaxQuality - e->getQuality() <= 0.001))
		{
			clusters = e->getClusters();
			vecRot = e->getRotatedEigenVectors();
		}
	}

	Eigen::MatrixXd clusterCentres = Eigen::MatrixXd::Zero(clusters.size(), vecRot.cols());
	for (unsigned int i = 0; i < clusters.size(); i++)
	{
		for (unsigned int j = 0; j < clusters[i].size(); j++)
		{
			//sum points within cluster
			clusterCentres.row(i) += vecRot.row(clusters[i][j]);
		}
	}
	for (unsigned int i = 0; i < clusters.size(); i++)
	{
		//find average point within cluster
		clusterCentres.row(i) = clusterCentres.row(i) / clusters[i].size();
	}

	//order clustered points by (ascending) distance to cluster centre
	for (unsigned int i = 0; i < clusters.size(); i++)
	{
		std::multimap<double, int> clusterDistance;
		for (unsigned int j = 0; j < clusters[i].size(); j++)
		{
			double d2 = (vecRot.row(clusters[i][j]) - clusterCentres.row(i)).squaredNorm();
			clusterDistance.insert(std::make_pair(d2, clusters[i][j]));
		}
		//the map will be sorted based on the key so just loop through it
		//to get set of data indices sorted on the distance to cluster
		clusters[i].clear();
		for (std::multimap<double, int>::iterator it = clusterDistance.begin(); it != clusterDistance.end(); it++)
		{
			clusters[i].push_back(it->second);
		}
	}

	return clusters;
}


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS

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

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

 /*!
  @ingroup group-euler
    Function object implementing erfc capabilities

  Computes the complementary error function
   \f$\displaystyle \frac{2}{\sqrt\pi}\int_{x}^{\infty} e^{-t^2}\mbox{d}t\f$

  @par Semantic:

  For every parameter of floating type T

  @code
  T r = erfc(x);
  @endcode

  is similar to:

  @code
  T0 r = oneminus(erf(x));
  @endcode

  @par Decorators

  std_ for floating entries provides access to @c std::erfc

  @see erf, erfcx, oneminus

  **/
  Value erfc(Value const & v0);
} }
#endif

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

#endif


// Copyright 2019 Hans Dembinski
//
// 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)

//[ guide_custom_accumulators_advanced

#include <boost/format.hpp>
#include <boost/histogram.hpp>
#include <iostream>
#include <sstream>

int main() {
  using namespace boost::histogram;

  // Accumulator accepts two samples and an optional weight and computes the mean of each.
  struct multi_mean {
    accumulators::mean<> mx, my;

    // called when no weight is passed
    void operator()(double x, double y) {
      mx(x);
      my(y);
    }

    // called when a weight is passed
    void operator()(weight_type<double> w, double x, double y) {
      mx(w, x);
      my(w, y);
    }
  };
  // Note: The implementation can be made more efficient by sharing the sum of weights.

  // Create a 1D histogram that uses the custom accumulator.
  auto h = make_histogram_with(dense_storage<multi_mean>(), axis::integer<>(0, 2));
  h(0, sample(1, 2));            // samples go to first cell
  h(0, sample(3, 4));            // samples go to first cell
  h(1, sample(5, 6), weight(2)); // samples go to second cell
  h(1, sample(7, 8), weight(3)); // samples go to second cell

  std::ostringstream os;
  for (auto&& bin : indexed(h)) {
    os << boost::format("index %i mean-x %.1f mean-y %.1f\n") % bin.index() %
              bin->mx.value() % bin->my.value();
  }
  std::cout << os.str() << std::flush;
  assert(os.str() == "index 0 mean-x 2.0 mean-y 3.0\n"
                     "index 1 mean-x 6.2 mean-y 7.2\n");
}

//]


#include "distances.h"
#include "EMD_wrapper.h"

#define EIGEN_NO_DEBUG
#define EIGEN_DONT_PARALLELIZE
#include <Eigen/Dense>

#include <cmath> // for std::isnormal

DistanceIndexPairs lp_distances(int Mp_i, const Options& opts, const Manifold& M, const Manifold& Mp,
                                std::vector<int> inpInds)
{
  std::vector<int> inds;
  std::vector<double> dists;

  // Compare every observation in the M manifold to the
  // Mp_i'th observation in the Mp manifold.
  for (int i : inpInds) {
    // Calculate the distance between M[i] and Mp[Mp_i]
    double dist_i = 0.0;

    // If we have panel data and the M[i] / Mp[Mp_j] observations come from different panels
    // then add the user-supplied penalty/distance for the mismatch.
    if (opts.panelMode && opts.idw > 0) {
      dist_i += opts.idw * (M.panel(i) != Mp.panel(Mp_i));
    }

    for (int j = 0; j < M.E_actual(); j++) {
      // Get the sub-distance between M[i,j] and Mp[Mp_i, j]
      double dist_ij;

      // If either of these values is missing, the distance from
      // M[i,j] to Mp[Mp_i, j] is opts.missingdistance.
      // However, if the user doesn't specify this, then the entire
      // M[i] to Mp[Mp_i] distance is set as missing.
      if ((M(i, j) == MISSING_SENTINEL) || (Mp(Mp_i, j) == MISSING_SENTINEL)) {
        if (opts.missingdistance == 0) {
          dist_i = MISSING_SENTINEL;
          break;
        } else {
          dist_ij = opts.missingdistance;
        }
      } else { // Neither M[i,j] nor Mp[Mp_i, j] is missing.
        // How do we compare them? Do we treat them like continuous values and subtract them,
        // or treat them like unordered categorical variables and just check if they're the same?
        if (opts.metrics[j] == Metric::Diff) {
          dist_ij = M(i, j) - Mp(Mp_i, j);
        } else { // Metric::CheckSame
          dist_ij = (M(i, j) != Mp(Mp_i, j));
        }
      }

      if (opts.distance == Distance::MeanAbsoluteError) {
        dist_i += abs(dist_ij) / M.E_actual();
      } else { // Distance::Euclidean
        dist_i += dist_ij * dist_ij;
      }
    }

    if (dist_i != 0 && dist_i != MISSING_SENTINEL) {
      if (opts.distance == Distance::MeanAbsoluteError) {
        dists.push_back(dist_i);
      } else { // Distance::Euclidean
        dists.push_back(sqrt(dist_i));
      }
      inds.push_back(i);
    }
  }

  return { inds, dists };
}

// This function compares the M(i,.) multivariate time series to the Mp(j,.) multivariate time series.
// The M(i,.) observation has data for E consecutive time points (e.g. time(i), time(i+1), ..., time(i+E-1)) and
// the Mp(j,.) observation corresponds to E consecutive time points (e.g. time(j), time(j+1), ..., time(j+E-1)).
// At each time instant we observe n >= 1 pieces of data.
// These may either be continuous data or unordered categorical data.
//
// The Wasserstein distance (using the 'curve-matching' strategy) is equivalent to the (minimum) cost of turning
// the first time series into the second time series. In a simple example, say E = 2 and n = 1, and
//         M(i,.) = [ 1, 2 ] and Mp(j,.) = [ 2, 2 ].
// To turn M(i,.) into Mp(j,.) the first element needs to be increased by 1, so the overall cost is
//         Wasserstein( M(i,.), Mp(j,.) ) = 1.
// The distance can also reorder the points, so for example say
//         M(i,.) = [ 1, 100 ] and Mp(j,.) = [ 100, 1 ].
// If we just change the 1 to 100 and the 100 to 1 then the cost of each is 99 + 99 = 198.
// However, Wasserstein can instead reorder these points at a cost of
//         Wasserstein( M(i,.), Mp(j,.) ) = 2 * gamma * (time(1)-time(2))
// so if the observations occur on a regular grid so time(i) = i then the distance will just be 2 * gamma.
//
// The return value of this function is a matrix which shows the pairwise costs associated to each
// potential Wasserstein solution. E.g. the (n,m) element of the returned matrix shows the cost
// of turning the individual point M(i, n) into Mp(j, m).
//
// When there are missing values in one or other observation, we can either ignore this time period
// and compute the Wasserstein for the mismatched regime where M(i,.) is of size len_i and Mp(j,.) is
// of size len_j, where len_i != len_j is possible. Alternatively, we can fill in the affected elements
// of the cost matrix with some user-supplied 'missingDistance' value and then len_i == len_j is upheld.
std::unique_ptr<double[]> wasserstein_cost_matrix(const Manifold& M, const Manifold& Mp, int i, int j,
                                                  const Options& opts, int& len_i, int& len_j)
{
  // The M(i,.) observation will be stored as one flat vector of length M.E_actual():
  // - the first M.E() observations will the lagged version of the main time series
  // - the next M.E() observations will be the lagged 'dt' time series (if it is included, i.e., if M.E_dt() > 0)
  // - the next n * M.E() observations will be the n lagged extra variables,
  //   so in total that is M.E_lagged_extras() = n * M.E() observations
  // - the remaining M.E_actual() - M.E() - M.E_dt() - M.E_lagged_extras() are the unlagged extras and the distance
  //   between those two vectors forms a kind of minimum distance which is added to the time-series curve matching
  //   Wasserstein distance.

  bool skipMissing = (opts.missingdistance == 0);

  auto M_i = M.laggedObsMap(i);
  auto Mp_j = Mp.laggedObsMap(j);

  auto M_i_missing = (M_i.array() == M.missing()).colwise().any();
  auto Mp_j_missing = (Mp_j.array() == Mp.missing()).colwise().any();

  if (skipMissing) {
    // N.B. Can't .sum() a vector of bools to count them in Eigen.
    len_i = M.E() - M_i_missing.count();
    len_j = Mp.E() - Mp_j_missing.count();
  } else {
    len_i = M.E();
    len_j = Mp.E();
  }

  double gamma = 1.0;
  if (M.E_dt() > 0) {
    // Imagine the M_i time series as a plot, and calculate the
    // aspect ratio of this plot, so we can rescale the time variable
    // to get the user-supplied aspect ratio.
    double minData = std::numeric_limits<double>::max();
    double maxData = std::numeric_limits<double>::min();
    double maxTime = 0.0;
    for (int t = 0; t < M_i.cols(); t++) {
      if (M_i(0, t) != MISSING_SENTINEL) {
        if (M_i(0, t) < minData) {
          minData = M_i(0, t);
        }
        if (M_i(0, t) > maxData) {
          maxData = M_i(0, t);
        }
      }
      if (M_i(1, t) != MISSING_SENTINEL && M_i(1, t) > maxTime) {
        maxTime = M_i(1, t);
      }
    }

    double epsilon = 1e-6; // Some small number in case the following ratio gets wildly large/small
    gamma = opts.aspectRatio * (maxData - minData + epsilon) / (maxTime + epsilon);
  }

  int timeSeriesDim = M_i.rows();

  double unlaggedDist = 0.0;
  int numUnlaggedExtras = M.E_extras() - M.E_lagged_extras();
  for (int e = 0; e < numUnlaggedExtras; e++) {
    double x = M.unlagged_extras(i, e), y = Mp.unlagged_extras(j, e);
    bool eitherMissing = (x == M.missing()) || (y == M.missing());

    if (eitherMissing) {
      unlaggedDist += opts.missingdistance;
    } else {
      if (opts.metrics[timeSeriesDim + e] == Metric::Diff) {
        unlaggedDist += abs(x - y);
      } else {
        unlaggedDist += x != y;
      }
    }
  }

  // If we have panel data and the M[i] / Mp[j] observations come from different panels
  // then add the user-supplied penalty/distance for the mismatch.
  if (opts.panelMode && opts.idw > 0) {
    unlaggedDist += opts.idw * (M.panel(i) != Mp.panel(j));
  }

  auto flatCostMatrix = std::make_unique<double[]>(len_i * len_j);
  std::fill_n(flatCostMatrix.get(), len_i * len_j, unlaggedDist);
  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> costMatrix(flatCostMatrix.get(),
                                                                                len_i, len_j);
  for (int k = 0; k < timeSeriesDim; k++) {
    int n = 0;
    for (int nn = 0; nn < M_i.cols(); nn++) {
      if (skipMissing && M_i_missing[nn]) {
        continue;
      }

      int m = 0;

      for (int mm = 0; mm < Mp_j.cols(); mm++) {
        if (skipMissing && Mp_j_missing[mm]) {
          continue;
        }
        double dist;
        bool eitherMissing = M_i_missing[nn] || Mp_j_missing[mm];

        if (eitherMissing) {
          dist = opts.missingdistance;
        } else {
          if (opts.metrics[k] == Metric::Diff) {
            dist = abs(M_i(k, nn) - Mp_j(k, mm));
          } else {
            dist = M_i(k, nn) != Mp_j(k, mm);
          }
        }

        // For the time data, we add in the 'gamma' scaling factor calculated earlier
        if ((M.E_dt() > 0) && (k == 1)) {
          dist *= gamma;
        }

        costMatrix(n, m) += dist;

        m += 1;
      }

      n += 1;
    }
  }
  return flatCostMatrix;
}

// TODO: Subtract the D(x,x) and D(y,y) parts from this.
double approx_wasserstein(double* C, int len_i, int len_j, double eps, double stopErr)
{
  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> costMatrix(C, len_i, len_j);

  double r = 1.0 / len_i;
  double c = 1.0 / len_j;

  Eigen::MatrixXd K = Eigen::exp(-costMatrix.array() / eps);
  Eigen::MatrixXd Kp = len_i * K.array();

  Eigen::VectorXd u = Eigen::VectorXd::Ones(len_i) / len_i;
  Eigen::VectorXd v = Eigen::VectorXd::Ones(len_j) / len_j;

  int maxIter = 10000;
  for (int iter = 0; iter < maxIter; iter++) {

    v = c / (K.transpose() * u).array();
    u = 1.0 / (Kp * v).array();

    if (iter % 10 == 0) {
      // Compute right marginal (diag(u) K diag(v))^T1
      Eigen::VectorXd tempColSums = (u.asDiagonal() * K * v.asDiagonal()).colwise().sum();
      double LInfErr = (tempColSums.array() - c).abs().maxCoeff();
      if (LInfErr < stopErr) {
        break;
      }
    }
  }

  Eigen::MatrixXd transportPlan = u.asDiagonal() * K * v.asDiagonal();
  double dist = (transportPlan.array() * costMatrix.array()).sum();
  return dist;
}

double wasserstein(double* C, int len_i, int len_j)
{
  // Create vectors which are just 1/len_i and 1/len_j of length len_i and len_j.
  auto w_1 = std::make_unique<double[]>(len_i);
  std::fill_n(w_1.get(), len_i, 1.0 / len_i);
  auto w_2 = std::make_unique<double[]>(len_j);
  std::fill_n(w_2.get(), len_j, 1.0 / len_j);

  int maxIter = 10000;
  double cost;
  EMD_wrap(len_i, len_j, w_1.get(), w_2.get(), C, &cost, maxIter);
  return cost;
}

DistanceIndexPairs wasserstein_distances(int Mp_i, const Options& opts, const Manifold& M, const Manifold& Mp,
                                         std::vector<int> inpInds)
{
  std::vector<int> inds;
  std::vector<double> dists;

  // Compare every observation in the M manifold to the
  // Mp_i'th observation in the Mp manifold.
  for (int i : inpInds) {
    int len_i, len_j;
    auto C = wasserstein_cost_matrix(M, Mp, i, Mp_i, opts, len_i, len_j);

    if (len_i > 0 && len_j > 0) {
      double dist_i = wasserstein(C.get(), len_i, len_j);

      // Alternatively, the approximate version based on Sinkhorn's algorithm can be called with something like:
      // double dist_i = approx_wasserstein(C.get(), len_i, len_j, 0.1, 0.1)
      // In that case, the "std::isnormal" is really needed on the next line, as some
      // instability gives us some 'nan' distances using that method.

      if (dist_i != 0 && std::isnormal(dist_i)) {
        dists.push_back(dist_i);
        inds.push_back(i);
      }
    }
  }

  return { inds, dists };
}