// Copyright John Maddock 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.
// 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/math/tools/roots.hpp>
#include <boost/math/special_functions/ellint_1.hpp>
#include <boost/math/special_functions/ellint_2.hpp>
template <class T>
struct cbrt_functor_noderiv
{
   //  cube root of x using only function - no derivatives.
   cbrt_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)
   {
      T fx = x*x*x - a; // Difference (estimate x^3 - a).
      return fx;
   }
private:
   T a; // to be 'cube_rooted'.
};
//] [/root_finding_noderiv_1

template <class T>
boost::uintmax_t cbrt_noderiv(T x, T guess)
{
   // return cube root of x using bracket_and_solve (no derivatives).
   using namespace std;                          // Help ADL of std functions.
   using namespace boost::math::tools;           // For bracket_and_solve_root.

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

   const boost::uintmax_t maxit = 20;            // 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.
   int digits = std::numeric_limits<T>::digits;  // Maximum possible binary digits accuracy for type T.
   // Some fraction of digits is used to control how accurate to try to make the result.
   int get_digits = digits - 3;                  // We have to have a non-zero interval at each step, so
   // maximum accuracy is digits - 1.  But we also have to
   // allow for inaccuracy in f(x), otherwise the last few
   // iterations just thrash around.
   eps_tolerance<T> tol(get_digits);             // Set the tolerance.
   bracket_and_solve_root(cbrt_functor_noderiv<T>(x), guess, factor, is_rising, tol, it);
   return it;
}

template <class T>
struct cbrt_functor_deriv
{ // Functor also returning 1st derivative.
   cbrt_functor_deriv(T const& to_find_root_of) : a(to_find_root_of)
   { // Constructor stores value a to find root of,
      // for example: calling cbrt_functor_deriv<T>(a) to use to get cube root of a.
   }
   std::pair<T, T> operator()(T const& x)
   {
      // Return both f(x) and f'(x).
      T fx = x*x*x - a;                // Difference (estimate x^3 - value).
      T dx = 3 * x*x;                 // 1st derivative = 3x^2.
      return std::make_pair(fx, dx);   // 'return' both fx and dx.
   }
private:
   T a;                               // Store value to be 'cube_rooted'.
};

template <class T>
boost::uintmax_t cbrt_deriv(T x, T guess)
{
   // return cube root of x using 1st derivative and Newton_Raphson.
   using namespace boost::math::tools;
   T min = guess / 100;                     // We don't really know what this should be!
   T max = guess * 100;                     // We don't really know what this should be!
   const int digits = std::numeric_limits<T>::digits;  // Maximum possible binary digits accuracy for type T.
   int get_digits = static_cast<int>(digits * 0.6);    // Accuracy doubles with each step, so stop when we have
   // just over half the digits correct.
   const boost::uintmax_t maxit = 20;
   boost::uintmax_t it = maxit;
   newton_raphson_iterate(cbrt_functor_deriv<T>(x), guess, min, max, get_digits, it);
   return it;
}

template <class T>
struct cbrt_functor_2deriv
{
   // Functor returning both 1st and 2nd derivatives.
   cbrt_functor_2deriv(T const& to_find_root_of) : a(to_find_root_of)
   { // Constructor stores value a to find root of, for example:
      // calling cbrt_functor_2deriv<T>(x) to get cube root of x,
   }
   std::tuple<T, T, T> operator()(T const& x)
   {
      // Return both f(x) and f'(x) and f''(x).
      T fx = x*x*x - a;                     // Difference (estimate x^3 - value).
      T dx = 3 * x*x;                       // 1st derivative = 3x^2.
      T d2x = 6 * x;                        // 2nd derivative = 6x.
      return std::make_tuple(fx, dx, d2x);  // 'return' fx, dx and d2x.
   }
private:
   T a; // to be 'cube_rooted'.
};

template <class T>
boost::uintmax_t cbrt_2deriv(T x, T guess)
{
   // return cube root of x using 1st and 2nd derivatives and Halley.
   //using namespace std;  // Help ADL of std functions.
   using namespace boost::math::tools;
   T min = guess / 100;                     // We don't really know what this should be!
   T max = guess * 100;                     // We don't really know what this should be!
   const int digits = std::numeric_limits<T>::digits;  // Maximum possible binary digits accuracy for type T.
   // digits used to control how accurate to try to make the result.
   int get_digits = static_cast<int>(digits * 0.4);    // Accuracy triples with each step, so stop when just
   // over one third of the digits are correct.
   boost::uintmax_t maxit = 20;
   halley_iterate(cbrt_functor_2deriv<T>(x), guess, min, max, get_digits, maxit);
   return maxit;
}

template <class T>
boost::uintmax_t cbrt_2deriv_s(T x, T guess)
{
   // return cube root of x using 1st and 2nd derivatives and Halley.
   //using namespace std;  // Help ADL of std functions.
   using namespace boost::math::tools;
   T min = guess / 100;                     // We don't really know what this should be!
   T max = guess * 100;                     // We don't really know what this should be!
   const int digits = std::numeric_limits<T>::digits;  // Maximum possible binary digits accuracy for type T.
   // digits used to control how accurate to try to make the result.
   int get_digits = static_cast<int>(digits * 0.4);    // Accuracy triples with each step, so stop when just
   // over one third of the digits are correct.
   boost::uintmax_t maxit = 20;
   schroder_iterate(cbrt_functor_2deriv<T>(x), guess, min, max, get_digits, maxit);
   return maxit;
}

template <typename T = double>
struct elliptic_root_functor_noderiv
{
   elliptic_root_functor_noderiv(T const& arc, T const& radius) : m_arc(arc), m_radius(radius)
   { // Constructor just stores value a to find root of.
   }
   T operator()(T const& x)
   {
      // return the difference between required arc-length, and the calculated arc-length for an
      // ellipse with radii m_radius and x:
      T a = (std::max)(m_radius, x);
      T b = (std::min)(m_radius, x);
      T k = sqrt(1 - b * b / (a * a));
      return 4 * a * boost::math::ellint_2(k) - m_arc;
   }
private:
   T m_arc;     // length of arc.
   T m_radius;  // one of the two radii of the ellipse
}; // template <class T> struct elliptic_root_functor_noderiv

template <class T = double>
boost::uintmax_t elliptic_root_noderiv(T radius, T arc, T guess)
{ // return the other radius of an ellipse, given one radii and the arc-length
   using namespace std;  // Help ADL of std functions.
   using namespace boost::math::tools; // For bracket_and_solve_root.

   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;              // arc-length increases if one radii increases, so function is rising
   // Define a termination condition, stop when nearly all digits are correct, but allow for
   // the fact that we are returning a range, and must have some inaccuracy in the elliptic integral:
   eps_tolerance<T> tol(std::numeric_limits<T>::digits - 2);
   // Call bracket_and_solve_root to find the solution, note that this is a rising function:
   bracket_and_solve_root(elliptic_root_functor_noderiv<T>(arc, radius), guess, factor, is_rising, tol, it);
   return it;
}

template <class T = double>
struct elliptic_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!");

   elliptic_root_functor_1deriv(T const& arc, T const& radius) : m_arc(arc), m_radius(radius)
   { // Constructor just stores value a to find root of.
   }
   std::pair<T, T> operator()(T const& x)
   {
      // Return the difference between required arc-length, and the calculated arc-length for an
      // ellipse with radii m_radius and x, plus it's derivative.
      // See http://www.wolframalpha.com/input/?i=d%2Fda+[4+*+a+*+EllipticE%281+-+b^2%2Fa^2%29]
      // We require two elliptic integral calls, but from these we can calculate both
      // the function and it's derivative:
      T a = (std::max)(m_radius, x);
      T b = (std::min)(m_radius, x);
      T a2 = a * a;
      T b2 = b * b;
      T k = sqrt(1 - b2 / a2);
      T Ek = boost::math::ellint_2(k);
      T Kk = boost::math::ellint_1(k);
      T fx = 4 * a * Ek - m_arc;
      T dfx = 4 * (a2 * Ek - b2 * Kk) / (a2 - b2);
      return std::make_pair(fx, dfx);
   }
private:
   T m_arc;     // length of arc.
   T m_radius;  // one of the two radii of the ellipse
};  // struct elliptic_root__functor_1deriv

template <class T = double>
boost::uintmax_t elliptic_root_1deriv(T radius, T arc, T guess)
{
   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!");

   T min = 0;   // Minimum possible value is zero.
   T max = arc; // Maximum possible value is the arc length.

   // Accuracy doubles at each step, so stop when just over half of the digits are
   // correct, and rely on that step to polish off the remainder:
   int get_digits = static_cast<int>(std::numeric_limits<T>::digits * 0.6);
   const boost::uintmax_t maxit = 20;
   boost::uintmax_t it = maxit;
   newton_raphson_iterate(elliptic_root_functor_1deriv<T>(arc, radius), guess, min, max, get_digits, it);
   return it;
}

template <class T = double>
struct elliptic_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!");

   elliptic_root_functor_2deriv(T const& arc, T const& radius) : m_arc(arc), m_radius(radius) {}
   std::tuple<T, T, T> operator()(T const& x)
   {
      // Return the difference between required arc-length, and the calculated arc-length for an
      // ellipse with radii m_radius and x, plus it's derivative.
      // See http://www.wolframalpha.com/input/?i=d^2%2Fda^2+[4+*+a+*+EllipticE%281+-+b^2%2Fa^2%29]
      // for the second derivative.
      T a = (std::max)(m_radius, x);
      T b = (std::min)(m_radius, x);
      T a2 = a * a;
      T b2 = b * b;
      T k = sqrt(1 - b2 / a2);
      T Ek = boost::math::ellint_2(k);
      T Kk = boost::math::ellint_1(k);
      T fx = 4 * a * Ek - m_arc;
      T dfx = 4 * (a2 * Ek - b2 * Kk) / (a2 - b2);
      T dfx2 = 4 * b2 * ((a2 + b2) * Kk - 2 * a2 * Ek) / (a * (a2 - b2) * (a2 - b2));
      return std::make_tuple(fx, dfx, dfx2);
   }
private:
   T m_arc;     // length of arc.
   T m_radius;  // one of the two radii of the ellipse
};

template <class T = double>
boost::uintmax_t elliptic_root_2deriv(T radius, T arc, T guess)
{
   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!");

   T min = 0;                                   // Minimum possible value is zero.
   T max = arc;                                 // radius can't be larger than the arc length.

   // Accuracy triples at each step, so stop when just over one-third of the digits
   // are correct, and the last iteration will polish off the remaining digits:
   int get_digits = static_cast<int>(std::numeric_limits<T>::digits * 0.4);
   const boost::uintmax_t maxit = 20;
   boost::uintmax_t it = maxit;
   halley_iterate(elliptic_root_functor_2deriv<T>(arc, radius), guess, min, max, get_digits, it);
   return it;
} // nth_2deriv Halley
//]
// Using 1st and 2nd derivatives using Schroder algorithm.

template <class T = double>
boost::uintmax_t elliptic_root_2deriv_s(T radius, T arc, T guess)
{ // 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!");

   T min = 0; // Minimum possible value is zero.
   T max = arc; // radius can't be larger than the arc length.

   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;
   schroder_iterate(elliptic_root_functor_2deriv<T>(arc, radius), guess, min, max, get_digits, it);
   return it;
} // T elliptic_root_2deriv_s Schroder

int main()
{
   try
   {
      double to_root = 500;
      double answer = 7.93700525984;

      std::cout << "[table\n"
         << "[[Initial Guess=][-500% ([approx]1.323)][-100% ([approx]3.97)][-50% ([approx]3.96)][-20% ([approx]6.35)][-10% ([approx]7.14)][-5% ([approx]7.54)]"
         "[5% ([approx]8.33)][10% ([approx]8.73)][20% ([approx]9.52)][50% ([approx]11.91)][100% ([approx]15.87)][500 ([approx]47.6)]]\n";
      std::cout << "[[bracket_and_solve_root]["
         << cbrt_noderiv(to_root, answer / 6)
         << "][" << cbrt_noderiv(to_root, answer / 2)
         << "][" << cbrt_noderiv(to_root, answer - answer * 0.5)
         << "][" << cbrt_noderiv(to_root, answer - answer * 0.2)
         << "][" << cbrt_noderiv(to_root, answer - answer * 0.1)
         << "][" << cbrt_noderiv(to_root, answer - answer * 0.05)
         << "][" << cbrt_noderiv(to_root, answer + answer * 0.05)
         << "][" << cbrt_noderiv(to_root, answer + answer * 0.1)
         << "][" << cbrt_noderiv(to_root, answer + answer * 0.2)
         << "][" << cbrt_noderiv(to_root, answer + answer * 0.5)
         << "][" << cbrt_noderiv(to_root, answer + answer)
         << "][" << cbrt_noderiv(to_root, answer + answer * 5) << "]]\n";

      std::cout << "[[newton_iterate]["
         << cbrt_deriv(to_root, answer / 6)
         << "][" << cbrt_deriv(to_root, answer / 2)
         << "][" << cbrt_deriv(to_root, answer - answer * 0.5)
         << "][" << cbrt_deriv(to_root, answer - answer * 0.2)
         << "][" << cbrt_deriv(to_root, answer - answer * 0.1)
         << "][" << cbrt_deriv(to_root, answer - answer * 0.05)
         << "][" << cbrt_deriv(to_root, answer + answer * 0.05)
         << "][" << cbrt_deriv(to_root, answer + answer * 0.1)
         << "][" << cbrt_deriv(to_root, answer + answer * 0.2)
         << "][" << cbrt_deriv(to_root, answer + answer * 0.5)
         << "][" << cbrt_deriv(to_root, answer + answer)
         << "][" << cbrt_deriv(to_root, answer + answer * 5) << "]]\n";

      std::cout << "[[halley_iterate]["
         << cbrt_2deriv(to_root, answer / 6)
         << "][" << cbrt_2deriv(to_root, answer / 2)
         << "][" << cbrt_2deriv(to_root, answer - answer * 0.5)
         << "][" << cbrt_2deriv(to_root, answer - answer * 0.2)
         << "][" << cbrt_2deriv(to_root, answer - answer * 0.1)
         << "][" << cbrt_2deriv(to_root, answer - answer * 0.05)
         << "][" << cbrt_2deriv(to_root, answer + answer * 0.05)
         << "][" << cbrt_2deriv(to_root, answer + answer * 0.1)
         << "][" << cbrt_2deriv(to_root, answer + answer * 0.2)
         << "][" << cbrt_2deriv(to_root, answer + answer * 0.5)
         << "][" << cbrt_2deriv(to_root, answer + answer)
         << "][" << cbrt_2deriv(to_root, answer + answer * 5) << "]]\n";

      std::cout << "[[schr'''ö'''der_iterate]["
         << cbrt_2deriv_s(to_root, answer / 6)
         << "][" << cbrt_2deriv_s(to_root, answer / 2)
         << "][" << cbrt_2deriv_s(to_root, answer - answer * 0.5)
         << "][" << cbrt_2deriv_s(to_root, answer - answer * 0.2)
         << "][" << cbrt_2deriv_s(to_root, answer - answer * 0.1)
         << "][" << cbrt_2deriv_s(to_root, answer - answer * 0.05)
         << "][" << cbrt_2deriv_s(to_root, answer + answer * 0.05)
         << "][" << cbrt_2deriv_s(to_root, answer + answer * 0.1)
         << "][" << cbrt_2deriv_s(to_root, answer + answer * 0.2)
         << "][" << cbrt_2deriv_s(to_root, answer + answer * 0.5)
         << "][" << cbrt_2deriv_s(to_root, answer + answer)
         << "][" << cbrt_2deriv_s(to_root, answer + answer * 5) << "]]\n]\n\n";

      double radius_a = 10;
      double arc_length = 500;
      double radius_b = 123.6216507967705;

      std::cout << std::setprecision(4) << "[table\n"
         << "[[Initial Guess=][-500% ([approx]" << radius_b / 6 << ")][-100% ([approx]" << radius_b / 2 << ")][-50% ([approx]"
         << radius_b - radius_b * 0.5 << ")][-20% ([approx]" << radius_b - radius_b * 0.2 << ")][-10% ([approx]" << radius_b - radius_b * 0.1 << ")][-5% ([approx]" << radius_b - radius_b * 0.05 << ")]"
         "[5% ([approx]" << radius_b + radius_b * 0.05 << ")][10% ([approx]" << radius_b + radius_b * 0.1 << ")][20% ([approx]" << radius_b + radius_b * 0.2 << ")][50% ([approx]" << radius_b + radius_b * 0.5
         << ")][100% ([approx]" << radius_b + radius_b << ")][500 ([approx]" << radius_b + radius_b * 5 << ")]]\n";
      std::cout << "[[bracket_and_solve_root]["
         << elliptic_root_noderiv(radius_a, arc_length, radius_b / 6)
         << "][" << elliptic_root_noderiv(radius_a, arc_length, radius_b / 2)
         << "][" << elliptic_root_noderiv(radius_a, arc_length, radius_b - radius_b * 0.5)
         << "][" << elliptic_root_noderiv(radius_a, arc_length, radius_b - radius_b * 0.2)
         << "][" << elliptic_root_noderiv(radius_a, arc_length, radius_b - radius_b * 0.1)
         << "][" << elliptic_root_noderiv(radius_a, arc_length, radius_b - radius_b * 0.05)
         << "][" << elliptic_root_noderiv(radius_a, arc_length, radius_b + radius_b * 0.05)
         << "][" << elliptic_root_noderiv(radius_a, arc_length, radius_b + radius_b * 0.1)
         << "][" << elliptic_root_noderiv(radius_a, arc_length, radius_b + radius_b * 0.2)
         << "][" << elliptic_root_noderiv(radius_a, arc_length, radius_b + radius_b * 0.5)
         << "][" << elliptic_root_noderiv(radius_a, arc_length, radius_b + radius_b)
         << "][" << elliptic_root_noderiv(radius_a, arc_length, radius_b + radius_b * 5) << "]]\n";

      std::cout << "[[newton_iterate]["
         << elliptic_root_1deriv(radius_a, arc_length, radius_b / 6)
         << "][" << elliptic_root_1deriv(radius_a, arc_length, radius_b / 2)
         << "][" << elliptic_root_1deriv(radius_a, arc_length, radius_b - radius_b * 0.5)
         << "][" << elliptic_root_1deriv(radius_a, arc_length, radius_b - radius_b * 0.2)
         << "][" << elliptic_root_1deriv(radius_a, arc_length, radius_b - radius_b * 0.1)
         << "][" << elliptic_root_1deriv(radius_a, arc_length, radius_b - radius_b * 0.05)
         << "][" << elliptic_root_1deriv(radius_a, arc_length, radius_b + radius_b * 0.05)
         << "][" << elliptic_root_1deriv(radius_a, arc_length, radius_b + radius_b * 0.1)
         << "][" << elliptic_root_1deriv(radius_a, arc_length, radius_b + radius_b * 0.2)
         << "][" << elliptic_root_1deriv(radius_a, arc_length, radius_b + radius_b * 0.5)
         << "][" << elliptic_root_1deriv(radius_a, arc_length, radius_b + radius_b)
         << "][" << elliptic_root_1deriv(radius_a, arc_length, radius_b + radius_b * 5) << "]]\n";

      std::cout << "[[halley_iterate]["
         << elliptic_root_2deriv(radius_a, arc_length, radius_b / 6)
         << "][" << elliptic_root_2deriv(radius_a, arc_length, radius_b / 2)
         << "][" << elliptic_root_2deriv(radius_a, arc_length, radius_b - radius_b * 0.5)
         << "][" << elliptic_root_2deriv(radius_a, arc_length, radius_b - radius_b * 0.2)
         << "][" << elliptic_root_2deriv(radius_a, arc_length, radius_b - radius_b * 0.1)
         << "][" << elliptic_root_2deriv(radius_a, arc_length, radius_b - radius_b * 0.05)
         << "][" << elliptic_root_2deriv(radius_a, arc_length, radius_b + radius_b * 0.05)
         << "][" << elliptic_root_2deriv(radius_a, arc_length, radius_b + radius_b * 0.1)
         << "][" << elliptic_root_2deriv(radius_a, arc_length, radius_b + radius_b * 0.2)
         << "][" << elliptic_root_2deriv(radius_a, arc_length, radius_b + radius_b * 0.5)
         << "][" << elliptic_root_2deriv(radius_a, arc_length, radius_b + radius_b)
         << "][" << elliptic_root_2deriv(radius_a, arc_length, radius_b + radius_b * 5) << "]]\n";

      std::cout << "[[schr'''ö'''der_iterate]["
         << elliptic_root_2deriv_s(radius_a, arc_length, radius_b / 6)
         << "][" << elliptic_root_2deriv_s(radius_a, arc_length, radius_b / 2)
         << "][" << elliptic_root_2deriv_s(radius_a, arc_length, radius_b - radius_b * 0.5)
         << "][" << elliptic_root_2deriv_s(radius_a, arc_length, radius_b - radius_b * 0.2)
         << "][" << elliptic_root_2deriv_s(radius_a, arc_length, radius_b - radius_b * 0.1)
         << "][" << elliptic_root_2deriv_s(radius_a, arc_length, radius_b - radius_b * 0.05)
         << "][" << elliptic_root_2deriv_s(radius_a, arc_length, radius_b + radius_b * 0.05)
         << "][" << elliptic_root_2deriv_s(radius_a, arc_length, radius_b + radius_b * 0.1)
         << "][" << elliptic_root_2deriv_s(radius_a, arc_length, radius_b + radius_b * 0.2)
         << "][" << elliptic_root_2deriv_s(radius_a, arc_length, radius_b + radius_b * 0.5)
         << "][" << elliptic_root_2deriv_s(radius_a, arc_length, radius_b + radius_b)
         << "][" << elliptic_root_2deriv_s(radius_a, arc_length, radius_b + radius_b * 5) << "]]\n]\n\n";

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


#ifndef _SPHERE_MESH_GEN_H_
#define _SPHERE_MESH_GEN_H_
#include <array>
#include <map>
#include <vector>
#include <string>
#include <fstream>
#include <Eigen/Dense>
#include "compat.h"
template <typename Scalar>
class SphereMeshFactory{
    typedef mtao::compat::array<int, 3> Face;
    typedef mtao::compat::array<int, 2> Edge;
    public:
    typedef Scalar Scalar;
    typedef typename Eigen::Matrix<Scalar,3,1> Vector;
    using VecVector = mtao::vector<Vector>;
    SphereMeshFactory(int depth=3);
    void triforce(const Face & f, int depth);
    int add_edge(Edge e);
    void write(const std::string & filename);
    void write(std::ostream & outstream);
    const std::vector<Face> faces() const {return m_faces;}
    const VecVector vertices() const {return m_vertices;}

    private:
    const int m_depth = 0;
    VecVector m_vertices;
    std::vector<Face> m_faces;
    std::map<Edge,  int> m_edges;

};

template <typename T>
SphereMeshFactory<T>::SphereMeshFactory(int depth): m_depth(depth) {
    //Create icosahedron base

    Scalar gr = .5 * (1 + std::sqrt(Scalar(5)));
    m_vertices.resize(12);

    m_vertices[ 0] = Vector(     0,    - 1,     gr);
    m_vertices[ 1] = Vector(    gr,      0,      1);
    m_vertices[ 2] = Vector(    gr,      0,    - 1);
    m_vertices[ 3] = Vector(   -gr,      0,    - 1);
    m_vertices[ 4] = Vector(   -gr,      0,      1);
    m_vertices[ 5] = Vector(   - 1,     gr,      0);
    m_vertices[ 6] = Vector(     1,     gr,      0);
    m_vertices[ 7] = Vector(     1,    -gr,      0);
    m_vertices[ 8] = Vector(   - 1,    -gr,      0);
    m_vertices[ 9] = Vector(     0,    - 1,    -gr);
    m_vertices[10] = Vector(     0,      1,    -gr);
    m_vertices[11] = Vector(     0,      1,     gr);
    for(auto&& v: m_vertices) {
        v.normalize();
    }

    triforce({{ 1 ,  2 ,  6}},depth);
    triforce({{ 1 ,  7 ,  2}},depth);
    triforce({{ 3 ,  4 ,  5}},depth);
    triforce({{ 4 ,  3 ,  8}},depth);
    triforce({{ 6 ,  5 , 11}},depth);
    triforce({{ 5 ,  6 , 10}},depth);
    triforce({{ 9 , 10 ,  2}},depth);
    triforce({{10 ,  9 ,  3}},depth);
    triforce({{ 7 ,  8 ,  9}},depth);
    triforce({{ 8 ,  7 ,  0}},depth);
    triforce({{11 ,  0 ,  1}},depth);
    triforce({{ 0 , 11 ,  4}},depth);
    triforce({{ 6 ,  2 , 10}},depth);
    triforce({{ 1 ,  6 , 11}},depth);
    triforce({{ 3 ,  5 , 10}},depth);
    triforce({{ 5 ,  4 , 11}},depth);
    triforce({{ 2 ,  7 ,  9}},depth);
    triforce({{ 7 ,  1 ,  0}},depth);
    triforce({{ 3 ,  9 ,  8}},depth);
    triforce({{ 4 ,  8 ,  0}},depth);

}
template <typename T>
void SphereMeshFactory<T>::triforce(const Face & f, int depth) {
    if(depth <= 0) {
        m_faces.push_back(f);
    } else {
        int e01 = add_edge({{f[0],f[1]}});
        int e12 = add_edge({{f[1],f[2]}});
        int e02 = add_edge({{f[0],f[2]}});
        triforce({{f[0],e01,e02}},depth-1);
        triforce({{f[1],e12,e01}},depth-1);
        triforce({{f[2],e02,e12}},depth-1);
        triforce({{e01 ,e12,e02}},depth-1);
    }

}

template <typename T>
int SphereMeshFactory<T>::add_edge(Edge e) {
    if(e[0] > e[1]) {
        int tmp = e[0];
        e[0] = e[1];
        e[1] = tmp;
    }
    auto it = m_edges.find(e);
    if(it != m_edges.end()) {
        return it->second;
    } else {
        m_edges[e] = m_vertices.size();
        m_vertices.push_back(
                (m_vertices[e[0]] + m_vertices[e[1]]).normalized()
                );
        return m_vertices.size()-1;
    }

}

template <typename T>
void SphereMeshFactory<T>::write(const std::string & filename) {
    std::ofstream outstream(filename.c_str());
    write(outstream);
}

template <typename T>
void SphereMeshFactory<T>::write(std::ostream & outstream) {
    outstream << "#Icosahedral subdivision to depth " << m_depth << std::endl;
    for(auto&& v: m_vertices) {
        outstream << "v " << v.transpose() << std::endl;
    }

    for(auto&& f: m_faces) {
        outstream << "f " << f[0]+1 << " " << f[1]+1 << " " << f[2]+1 << std::endl;
    }
}
#endif


/*-----------------------------------------------------------------------------+
Interval Container Library
Author: Joachim Faulhaber
Copyright (c) 2007-2010: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
/** Example interval.cpp \file interval.cpp
    \brief Intervals for integral and continuous instance types.
           Closed and open interval borders.

    Much of the library code deals with intervals which are implemented
    by interval class templates. This program gives a very short samlpe of
    different interval instances.

    \include interval_/interval.cpp
*/
//[example_interval
#include <iostream>
#include <string>
#include <math.h>

// Dynamically bounded intervals
#include <boost/icl/discrete_interval.hpp>
#include <boost/icl/continuous_interval.hpp>

// Statically bounded intervals
#include <boost/icl/right_open_interval.hpp>
#include <boost/icl/left_open_interval.hpp>
#include <boost/icl/closed_interval.hpp>
#include <boost/icl/open_interval.hpp>

#include "../toytime.hpp"
#include <boost/icl/rational.hpp>

using namespace std;
using namespace boost;
using namespace boost::icl;

int main()
{
    cout << ">>Interval Container Library: Sample interval.cpp <<\n";
    cout << "----------------------------------------------------\n";

    // Class template discrete_interval can be used for discrete data types
    // like integers, date and time and other types that have a least steppable
    // unit.
    discrete_interval<int>      int_interval
        = construct<discrete_interval<int> >(3, 7, interval_bounds::closed());

    // Class template continuous_interval can be used for continuous data types
    // like double, boost::rational or strings.
    continuous_interval<double> sqrt_interval
        = construct<continuous_interval<double> >(1/sqrt(2.0), sqrt(2.0));
                                                 //interval_bounds::right_open() is default
    continuous_interval<string> city_interval
        = construct<continuous_interval<string> >("Barcelona", "Boston", interval_bounds::left_open());

    discrete_interval<Time>     time_interval
        = construct<discrete_interval<Time> >(Time(monday,8,30), Time(monday,17,20),
                                              interval_bounds::open());

    cout << "Dynamically bounded intervals:\n";
    cout << "  discrete_interval<int>:    " << int_interval  << endl;
    cout << "continuous_interval<double>: " << sqrt_interval << " does "
                                            << string(contains(sqrt_interval, sqrt(2.0))?"":"NOT")
                                            << " contain sqrt(2)" << endl;
    cout << "continuous_interval<string>: " << city_interval << " does "
                                            << string(contains(city_interval,"Barcelona")?"":"NOT")
                                            << " contain 'Barcelona'" << endl;
    cout << "continuous_interval<string>: " << city_interval << " does "
                                            << string(contains(city_interval, "Berlin")?"":"NOT")
                                            << " contain 'Berlin'" << endl;
    cout << "  discrete_interval<Time>:   " << time_interval << "\n\n";

    // There are statically bounded interval types with fixed interval borders
    right_open_interval<string>   fix_interval1; // You will probably use one kind of static intervals
                                                 // right_open_intervals are recommended.
    closed_interval<unsigned int> fix_interval2; // ... static closed, left_open and open intervals
    left_open_interval<float>     fix_interval3; // are implemented for sake of completeness but
    open_interval<short>          fix_interval4; // are of minor practical importance.

    right_open_interval<rational<int> > range1(rational<int>(0,1),  rational<int>(2,3));
    right_open_interval<rational<int> > range2(rational<int>(1,3),  rational<int>(1,1));

    // This middle third of the unit interval [0,1)
    cout << "Statically bounded interval:\n";
    cout << "right_open_interval<rational<int>>: " << (range1 & range2) << endl;

    return 0;
}

// Program output:

//>>Interval Container Library: Sample interval.cpp <<
//----------------------------------------------------
//Dynamically bounded intervals
//  discrete_interval<int>:    [3,7]
//continuous_interval<double>: [0.707107,1.41421) does NOT contain sqrt(2)
//continuous_interval<string>: (Barcelona,Boston] does NOT contain 'Barcelona'
//continuous_interval<string>: (Barcelona,Boston] does  contain 'Berlin'
//  discrete_interval<Time>:   (mon:08:30,mon:17:20)
//
//Statically bounded interval
//right_open_interval<rational<int>>: [1/3,2/3)

//]


//          Copyright Gavin Band 2008 - 2012.
// 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 METRO_MEAN_AND_VARIANCE
#define METRO_MEAN_AND_VARIANCE

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

namespace metro {
	// Compute mean and variance for a vector.
	// All values must be non-missing (i.e. not NaN.)
	template< typename Data >
	std::pair< double, double > compute_mean_and_variance( Data const& data ) {
		double const mean = data.sum() / data.size() ;
		double variance = std::numeric_limits< double >::quiet_NaN() ;
		if( data.size() > 1 ) {
			variance = ( data.array() - mean ).square().sum() / ( data.size() - 1  ) ;
		}
		return std::make_pair( mean, variance ) ;
	}

	// Compute mean and variance for a vector ignoring missing values.
	template< typename Data >
	std::pair< double, double > compute_mean_and_variance( Data const& data, Data const& nonmissingness ) {
		assert( data.size() == nonmissingness.size() ) ;
		// Ensure all non-missing values are zero.
		double const mean = ( data.array() * nonmissingness.array() ).sum() / nonmissingness.sum() ;
		double variance = std::numeric_limits< double >::quiet_NaN() ;
		if( data.size() > 1 ) {
			variance = (( data.array() - mean ) * nonmissingness.array() ).square().sum() / ( nonmissingness.sum() - 1 ) ;
		}
		return std::make_pair( mean, variance ) ;
	}

	// This struct implements an "on-line" algorithm for computing the mean and variance.
	// see http://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#On-line_algorithm
	// This implementation computes mean and per-element variance (but not covariance) of a matrix
	// of values.
	struct OnlineElementwiseMeanAndVariance {
		typedef Eigen::MatrixXd Storage ;
	public:
		template< typename Data, typename Nonmissingness >
		void accumulate( Data const& data, Nonmissingness const& nonmissingness ) {
			assert( data.rows() == nonmissingness.rows() ) ;
			assert( data.cols() == nonmissingness.cols() ) ;
			if( m_mean.rows() == 0 ) {
				m_nonmissingness = nonmissingness ;
				// resize storage to match data
				m_mean.setZero( data.rows(), data.cols() ) ;
				m_sum_of_squares_of_differences.setZero( data.rows(), data.cols() ) ;
			} else {
				assert( data.rows() == m_mean.rows() ) ;
				assert( data.cols() == m_mean.cols() ) ;
				assert( nonmissingness.rows() == m_mean.rows() ) ;
				assert( nonmissingness.cols() == m_mean.cols() ) ;
				m_nonmissingness += nonmissingness ;
			}
			m_delta = data - m_mean ;

			//std::cerr << "m_nonmissingness =\n" << m_nonmissingness.block( 0, 0, 10, 4 ) << "\n" ;
			//std::cerr << "m_delta =\n" << m_delta.block( 0, 0, 10, 4 ) << "\n" ;
			//std::cerr << "m_mean =\n" << m_mean.block( 0, 0, 10, 4 ) << "\n" ;
			m_mean.array() += nonmissingness.array() * ( m_delta.array() / ( m_nonmissingness.array() + ( m_nonmissingness.array() == 0 ).cast< double >() )) ;
			//std::cerr << "m_mean after update =\n" << m_mean.block( 0, 0, 10, 4 ) << "\n" ;
			m_sum_of_squares_of_differences.array() += nonmissingness.array() * ( m_delta.array() * ( data - m_mean ).array() ) ;
		}

		template< typename Data >
		void accumulate( Data const& data ) {
			Storage const nonmissingness = Storage::Constant( data.rows(), data.cols(), 1 ) ;
			this->accumulate( data, nonmissingness ) ;
		}

		Storage get_mean() const ;
		Storage get_variance() const ;
		double get_count( int row, int column ) const ;
		double get_mean( int row, int column ) const ;
		double get_variance( int row, int column ) const ;

	private:
		Storage m_nonmissingness ;
		Storage m_mean ;
		Storage m_sum_of_squares_of_differences ;
		Storage m_delta ;
	} ;
}

#endif


#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>

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

#include <opencv2/core/core.hpp>
#include <cmath>

using namespace std;
using namespace Eigen;

//顶点，即待优化变量，目标值
class CurveFittingVertex: public g2o::BaseVertex<4,Vector4d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    CurveFittingVertex():BaseVertex<4,Vector4d>()
    {

    }

    virtual void setToOriginImpl()
    {
        _estimate << 0,0,0,0;
    }

    virtual void oplusImpl(const double *update_) //更新顶点
    {
        Eigen::Map<const Vector4d> up(update_);
        _estimate += up;
        // cout<<"eee" <<_estimate<<endl;
    }

    bool read(std::istream& is){}
    bool write(std::ostream& os) const{}

};

//边，描述顶点之间的关系
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    CurveFittingEdge():g2o::BaseUnaryEdge<1,double,CurveFittingVertex>(){}
// 计算误差
    void computeError()
    {
        const CurveFittingVertex *v = static_cast<const CurveFittingVertex *>(_vertices[0]);
        const Vector4d abcd = v->estimate();
        double A = abcd[0],B = abcd[1],C = abcd[2],D = abcd[3];
        _error(0,0) = _measurement - (A*sin(B*_x)+C*cos(D*_x)+pow(_x,2)); // 观测量减去估计量
        // cout << "cee "<<_error << endl;

    }
// 计算雅可比矩阵
    void linearizeOplus()
    {
        CurveFittingVertex *vi = static_cast<CurveFittingVertex *>(_vertices[0]);
        Vector4d abcd = vi->estimate();
        double A = abcd[0],B = abcd[1],C = abcd[2],D = abcd[3];
        // cout << " ddd" << endl;
        //误差项对待优化变量的Jacobian
        _jacobianOplusXi(0,0) = -sin(B*_x);
        _jacobianOplusXi(0,1) = -A*_x*cos(B*_x);
        _jacobianOplusXi(0,2) = -cos(D*_x);
        _jacobianOplusXi(0,3) = C*_x*sin(D*_x);

    }

    bool read(istream &is){}
    bool write(ostream &os) const {}

public:
    double _x;
};

int main(int argc, char**argv)
{
    // double a = 5.0,b = 1.0,c = 10.0,d = 2.0;
    // int N = 100;

    // double w_sigma = 2.0;

    // cv::RNG rng;

    // double abcd[4] = {0.0,0.0,0.0};

    // vector<double> x_data,y_data;

    // cout << "generate data" << endl;

    // for (int i = 0; i < N; i++)
    // {
    //     double x = rng.uniform(-10,10);
    //     double y = a*sin(b*x)+c*cos(d*x)+rng.gaussian(w_sigma);
    //     x_data.push_back(x);
    //     y_data.push_back(y);

    //     // cout << x_data[i] << " ," << y_data[i] << endl;

    // }

    // // 每个误差项优化变量维度为 4，误差值维度为1
    // typedef g2o::BlockSolver<g2o::BlockSolverTraits<4,1>> Block;
    // // 线性方程求解器： 稠密的增量方程
    // Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();

    // // 矩阵块求解器
    // Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linearSolver));

    // // 梯度下降方法
    // // g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr));
    // g2o::OptimizationAlgorithmDogleg *solver = new g2o::OptimizationAlgorithmDogleg(std::unique_ptr<Block>(solver_ptr));

    // g2o::SparseOptimizer optimizer;
    // optimizer.setAlgorithm(solver);
    // optimizer.setVerbose(true);

    // CurveFittingVertex *v = new CurveFittingVertex();
    // // 初始值
    // v->setEstimate(Eigen::Vector4d(1.6,1.4,6.2,1.7));
    // v->setId(0);
    // v->setFixed(false);
    // optimizer.addVertex(v);//添加顶点

    // for(int i=0;i< N;i++)
    // {
    //     CurveFittingEdge *edge = new CurveFittingEdge();
    //     edge->setId(i+1);
    //     edge->setVertex(0,v);//设置连接的顶点
    //     edge->setMeasurement(y_data[i]);

    //     //信息矩阵： 协方差矩阵之逆
    //     edge->setInformation(Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma));
    //     edge->_x = x_data[i];
    //     optimizer.addEdge(edge);

    // }

    // cout << "start optimization" << endl;

    // chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    // optimizer.initializeOptimization();
    // optimizer.optimize(100);

    // chrono::steady_clock::time_point t2 = chrono::steady_clock::now();

    // chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    // cout << " time_used: " << time_used.count() << " seconds" << endl;

    // Eigen::Vector4d abcd_estimate = v->estimate();
    // cout << "estimated \n" << abcd_estimate << endl;

    // return 0;

    double a = 5.0, b = 1.0, c = 10.0, d = 2.0; // 真实参数值
    int N = 100;
    double w_sigma = 2.0;   // 噪声值Sigma
    cv::RNG rng;    // 随机数产生器OpenCV
    double abcd[4] = {0, 0, 0, 0};  // 参数的估计值abc

    vector<double> x_data, y_data;

    cout << "generate random data" << endl;

    for(int i = 0; i < N; i++)
    {
        //generate a random variable [-10 10]
        double x = rng.uniform(-10., 10.);
        double y = a * sin(b*x) + c* cos(d *x)+pow(x,2) + rng.gaussian(w_sigma);
        // double y = a * sin(b*x) + c * cos(d *x);
        x_data.push_back(x);
        y_data.push_back(y);

        // cout << x_data[i] << " , " << y_data[i] << endl;
    }

    // 构建图优化，先设定g2o
    // 矩阵块：每个误差项优化变量维度为4 ，误差值维度为1
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<4, 1> > Block;
    // 线性方程求解器：稠密的增量方程
    // Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();

    typedef g2o::LinearSolverDense<Block::PoseMatrixType> MyLinearSolver;
    // Block* solver_ptr = new Block(linearSolver);    // 矩阵块求解器

    // // 梯度下降方法，从GN, LM, DogLeg 中选
    // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    // 矩阵块求解器
    // Block* solver_ptr = new Block(std::make_unique<Block::LinearSolverType>(linearSolver));
    // g2o::OptimizationAlgorithmDogleg *solver = new g2o::OptimizationAlgorithmDogleg(std::unique_ptr<Block>(solver_ptr));
    g2o::SparseOptimizer optimizer;     // 图模型
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<Block>(g2o::make_unique<MyLinearSolver>()));
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg(g2o::make_unique<Block>(g2o::make_unique<MyLinearSolver>()));

    optimizer.setAlgorithm( solver );   // 设置求解器
    optimizer.setVerbose(true);     // 打开调试输出

    // 往图中增加顶点
    CurveFittingVertex *v = new CurveFittingVertex();
    // 设置优化初始估计值
    v->setEstimate( Eigen::Vector4d(1.6, 1.4, 6.2, 1.7));
    v->setId(0);
    // v->setFixed(false);
    optimizer.addVertex(v);

    // 往图中增加边
    for(int i = 0; i < N; i++)
    {
        CurveFittingEdge* edge = new CurveFittingEdge();
        edge->setId(i+1);
        edge->setVertex(0, v);      // 设置连接的顶点
        edge->setMeasurement( y_data[i] );      // 观测数值

        // 信息矩阵：协方差矩阵之逆
        edge->setInformation( Eigen::Matrix<double, 1, 1>::Identity() );

        edge->_x = x_data[i];

        optimizer.addEdge( edge );
    }

    // 执行优化
    cout << "strat optimization" << endl;

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    optimizer.initializeOptimization();
    optimizer.optimize(500);

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();

    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double> > (t2 - t1);
    cout << "solve time cost = " << time_used.count() << " seconds." << endl;

    // 输出优化值
    Eigen::Vector4d abcd_estimate = v->estimate();
    cout << "estimated module: " <<  endl << abcd_estimate << endl;

    return 0;

}


// Copyright (c) 2022 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <boost/geometry.hpp>
#include <cmath>
#include <tuple>

namespace pyinterp::detail::math {

/// Abstract class for bivariate interpolation
template <template <class> class Point, typename T>
struct Bivariate {
  /// Default constructor
  Bivariate() = default;

  /// Default destructor
  virtual ~Bivariate() = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  Bivariate(const Bivariate& rhs) = default;

  /// Move constructor
  ///
  /// @param rhs right value
  Bivariate(Bivariate&& rhs) noexcept = default;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  auto operator=(const Bivariate& rhs) -> Bivariate& = default;

  /// Move assignment operator
  ///
  /// @param rhs right value
  auto operator=(Bivariate&& rhs) noexcept -> Bivariate& = default;

  /// Performs the interpolation
  ///
  /// @param p Query point
  /// @param p0 Point of coordinate (x0, y0)
  /// @param p1 Point of coordinate (x1, y1)
  /// @param q00 Point value for the coordinate (x0, y0)
  /// @param q01 Point value for the coordinate (x0, y1)
  /// @param q10 Point value for the coordinate (x1, y0)
  /// @param q11 Point value for the coordinate (x1, y1)
  /// @return interpolated value at coordinate (x, y)
  virtual auto evaluate(const Point<T>& p, const Point<T>& p0,
                        const Point<T>& p1, const T& q00, const T& q01,
                        const T& q10, const T& q11) const -> T = 0;
};

/// Bilinear interpolation
template <template <class> class Point, typename T>
struct Bilinear : public Bivariate<Point, T> {
  /// Default constructor
  Bilinear() = default;

  /// Default destructor
  virtual ~Bilinear() = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  Bilinear(const Bilinear& rhs) = default;

  /// Move constructor
  ///
  /// @param rhs right value
  Bilinear(Bilinear&& rhs) noexcept = default;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  auto operator=(const Bilinear& rhs) -> Bilinear& = default;

  /// Move assignment operator
  ///
  /// @param rhs right value
  auto operator=(Bilinear&& rhs) noexcept -> Bilinear& = default;

  /// Performs the bilinear interpolation
  constexpr auto evaluate(const Point<T>& p, const Point<T>& p0,
                          const Point<T>& p1, const T& q00, const T& q01,
                          const T& q10, const T& q11) const -> T final {
    auto dx = boost::geometry::get<0>(p1) - boost::geometry::get<0>(p0);
    auto dy = boost::geometry::get<1>(p1) - boost::geometry::get<1>(p0);
    auto t = (boost::geometry::get<0>(p) - boost::geometry::get<0>(p0)) / dx;
    auto u = (boost::geometry::get<1>(p) - boost::geometry::get<1>(p0)) / dy;
    return (T(1) - t) * (T(1) - u) * q00 + t * (T(1) - u) * q10 +
           (T(1) - t) * u * q01 + t * u * q11;
  }
};

/// Inverse distance weighting interpolation
///
/// @see https://en.wikipedia.org/wiki/Inverse_distance_weighting
///
template <template <class> class Point, typename T>
struct InverseDistanceWeighting : public Bivariate<Point, T> {
  /// Default constructor (p=2)
  InverseDistanceWeighting() = default;

  /// Explicit definition of the parameter p.
  explicit InverseDistanceWeighting(const int exp) : exp_(exp) {}

  /// Return the exponent used by this instance
  [[nodiscard]] inline auto exp() const noexcept -> int { return exp_; }

  /// Default destructor
  virtual ~InverseDistanceWeighting() = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  InverseDistanceWeighting(const InverseDistanceWeighting& rhs) = default;

  /// Move constructor
  ///
  /// @param rhs right value
  InverseDistanceWeighting(InverseDistanceWeighting&& rhs) noexcept = default;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  auto operator=(const InverseDistanceWeighting& rhs)
      -> InverseDistanceWeighting& = default;

  /// Move assignment operator
  ///
  /// @param rhs right value
  auto operator=(InverseDistanceWeighting&& rhs) noexcept
      -> InverseDistanceWeighting& = default;

  /// Performs the interpolation
  inline auto evaluate(const Point<T>& p, const Point<T>& p0,
                       const Point<T>& p1, const T& q00, const T& q01,
                       const T& q10, const T& q11) const -> T final {
    auto distance = boost::geometry::distance(
        p, Point<T>{boost::geometry::get<0>(p0), boost::geometry::get<1>(p0)});
    if (distance <= std::numeric_limits<T>::epsilon()) {
      return q00;
    }

    auto w = 1 / std::pow(distance, exp_);
    auto wu = q00 * w;

    distance = boost::geometry::distance(
        p, Point<T>{boost::geometry::get<0>(p0), boost::geometry::get<1>(p1)});

    if (distance <= std::numeric_limits<T>::epsilon()) {
      return q01;
    }

    auto wi = 1 / std::pow(distance, exp_);
    w += wi;
    wu += q01 * wi;

    distance = boost::geometry::distance(
        p, Point<T>{boost::geometry::get<0>(p1), boost::geometry::get<1>(p0)});

    if (distance <= std::numeric_limits<T>::epsilon()) {
      return q10;
    }

    wi = 1 / std::pow(distance, exp_);
    w += wi;
    wu += q10 * wi;

    distance = boost::geometry::distance(
        p, Point<T>{boost::geometry::get<0>(p1), boost::geometry::get<1>(p1)});

    if (distance <= std::numeric_limits<T>::epsilon()) {
      return q11;
    }

    wi = 1 / std::pow(distance, exp_);
    w += wi;
    wu += q11 * wi;

    return wu / w;
  }

 private:
  int exp_{2};
};

/// Nearest interpolation
template <template <class> class Point, typename T>
struct Nearest : public Bivariate<Point, T> {
  /// Default constructor
  Nearest() = default;

  /// Default destructor
  virtual ~Nearest() = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  Nearest(const Nearest& rhs) = default;

  /// Move constructor
  ///
  /// @param rhs right value
  Nearest(Nearest&& rhs) noexcept = default;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  auto operator=(const Nearest& rhs) -> Nearest& = default;

  /// Move assignment operator
  ///
  /// @param rhs right value
  auto operator=(Nearest&& rhs) noexcept -> Nearest& = default;

  /// Performs the interpolation
  inline auto evaluate(const Point<T>& p, const Point<T>& p0,
                       const Point<T>& p1, const T& q00, const T& q01,
                       const T& q10, const T& q11) const -> T final {
    auto distance = boost::geometry::comparable_distance(
        p, Point<T>{boost::geometry::get<0>(p0), boost::geometry::get<1>(p0)});
    auto result = std::make_tuple(distance, q00);

    distance = boost::geometry::comparable_distance(
        p, Point<T>{boost::geometry::get<0>(p0), boost::geometry::get<1>(p1)});
    if (std::get<0>(result) > distance) {
      result = std::make_tuple(distance, q01);
    }

    distance = boost::geometry::comparable_distance(
        p, Point<T>{boost::geometry::get<0>(p1), boost::geometry::get<1>(p0)});
    if (std::get<0>(result) > distance) {
      result = std::make_tuple(distance, q10);
    }

    distance = boost::geometry::comparable_distance(
        p, Point<T>{boost::geometry::get<0>(p1), boost::geometry::get<1>(p1)});
    if (std::get<0>(result) > distance) {
      result = std::make_tuple(distance, q11);
    }
    return std::get<1>(result);
  }
};

}  // namespace pyinterp::detail::math


#include "socpInterface.hpp"

#include <array>
#include <iostream>
#include <chrono>

#include <Eigen/Dense>

// This example solves a simple random second order cone problem
// based on https://www.cvxpy.org/examples/basic/socp.html

int main()
{
    // Set up problem data.

    // number of second order cone constraints
    const size_t m = 3;
    // number of variables
    const size_t n = 10;
    // dimension of equality constraints
    const size_t p = 5;
    // dimension of second order cone constraints
    const size_t n_i = 5;

    std::array<Eigen::Matrix<double, n_i, n>, m> A;
    std::array<Eigen::Matrix<double, n_i, 1>, m> b;
    std::array<Eigen::Matrix<double, n, 1>, m> c;
    std::array<double, m> d;

    Eigen::Matrix<double, n, 1> x0;
    x0.setRandom();
    Eigen::Matrix<double, n, 1> f;
    f.setRandom();

    for (size_t i = 0; i < m; i++)
    {
        A[i].setRandom();
        b[i].setRandom();
        c[i].setRandom();
        d[i] = (A[i] * x0).norm() - c[i].dot(x0);
    }

    Eigen::Matrix<double, p, n> F;
    F.setRandom();
    Eigen::Matrix<double, p, 1> g = F * x0;

    // Formulate SOCP.
    auto t0 = std::chrono::high_resolution_clock::now();

    // Create the SOCP instance.
    op::SecondOrderConeProgram socp;

    // Add variables. Those can be scalars, vectors or matrices.
    op::Variable x = socp.createVariable("x", n);

    // Add constraints.
    for (size_t i = 0; i < m; i++)
    {
        socp.addConstraint(op::norm2(op::Parameter(A[i]) * x + op::Parameter(b[i])) <=
                           op::Parameter(c[i]).transpose() * x + op::Parameter(d[i]));
    }
    socp.addConstraint(op::Parameter(F) * x == op::Parameter(g));

    // Here we use a pointer to a parameter. This allows changing it dynamically.
    socp.addMinimizationTerm(op::Parameter(&f).transpose() * x);

    // Print the problem for inspection.
    std::cout << socp << "\n\n";

    // Create the solver instance.
    op::Solver solver(socp);
    solver.initialize();

    auto t = std::chrono::high_resolution_clock::now();
    auto t_setup = std::chrono::duration_cast<std::chrono::microseconds>(t - t0).count();
    std::cout << "\nSetup duration: " << t_setup << "μs.\n\n";

    // Solve the problem and show solver output.
    t0 = std::chrono::high_resolution_clock::now();
    const bool success = solver.solveProblem(true);
    if (not success)
    {
        // This should not happen in this example.
        throw std::runtime_error("Solver returned a critical error.");
    }
    std::cout << "Solver message: " << solver.getResultString() << "\n";

    // Check if the solver has produced a valid solution.
    assert(socp.isFeasible());

    t = std::chrono::high_resolution_clock::now();
    auto t_solve = std::chrono::duration_cast<std::chrono::microseconds>(t - t0).count();
    std::cout << "\nSolver duration: " << t_solve << "μs.\n\n";

    // Get Solution.
    Eigen::Matrix<double, n, 1> x_sol;
    socp.readSolution("x", x_sol);

    // Print the first solution.
    std::cout << "First solution:\n"
              << x_sol << "\n\n";

    // Change the problem parameters and solve again.
    f.setRandom();
    solver.solveProblem(false);
    socp.readSolution("x", x_sol);

    // Print the new solution.
    std::cout << "Solution after changing the cost function:\n"
              << x_sol << "\n\n";
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

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

using namespace std;

double f(double) { cout << "double\n"; return 1.0; }
complex<double> f(complex<double>) { cout << "complex\n"; return complex<double>(1.0, -1.0); }

template <typename Matrix>
void test(Matrix& A, const char* name)
{
    cout << "\n" << name << "\n";

    typedef typename mtl::Collection<Matrix>::size_type   size_type;
    typedef typename mtl::Collection<Matrix>::value_type  Scalar;
    typedef typename mtl::dense_vector<Scalar>            Vector;

    std::size_t size= num_cols(A);
    Matrix L(size, size), U(size, size);

    Scalar c= f(Scalar(1));
    cout << "c is: " << c << "\n";

    for (std::size_t i= 0; i < size; i++)
	for(std::size_t j= 0; j < size; j++) {
	    U[i][j]= i <= j ? c * Scalar(i+j+2) : Scalar(0);
	    L[i][j]= i > j ? c * Scalar(i+j+1) : (i == j ? Scalar(1) : Scalar(0));
	}

    cout << "L is:\n" << L << "U is:\n" << U;
    A= L * U;

    Vector v(size);
    for (std::size_t i= 0; i < size; i++)
	v[i]= Scalar(i);

    Vector w( A*v );

    cout << "A is:\n" << A;

    Matrix PLU(A);

    mtl::dense_vector<size_type> Pv(size);
    lu(PLU, Pv);
    typename mtl::mat::traits::permutation<>::type P(permutation(Pv));

    cout << "Permuted A is \n" << Matrix(P * A);

    Matrix I(size, size);
    I= Scalar(1);

    Matrix PL(I + strict_lower(PLU)), PU(upper(PLU)), PA2(PL * PU);
    cout << "L [permuted] is:\n" << PL << "U [permuted] is:\n" << PU
	 << "L * U [permuted] is:\n" << PA2
	 << "L * U is:\n" << Matrix(trans(P) * PA2);

    MTL_THROW_IF(one_norm(Matrix(trans(P) * PA2 - A)) > 0.1, mtl::runtime_error("Error in permuted LU factorization."));

    Matrix PUI(inv_upper(PU));
    cout << "inv(U) [permuted] is:\n" << PUI << "PUI * PU is:\n" << Matrix(PUI * PU);
    MTL_THROW_IF(one_norm(Matrix(PUI * PU - I)) > 0.1, mtl::runtime_error("Error in upper inversion."));

    Matrix PLI(inv_lower(PL));
    cout << "inv(L) [permuted] is:\n" << PLI << "PLI * PL is:\n" << Matrix(PLI * PL);
    MTL_THROW_IF(one_norm(Matrix(PLI * PL - I)) > 0.1, mtl::runtime_error("Error in lower inversion."));

    Matrix AI(PUI * PLI * P);
    cout << "inv(A) [inv(U) * inv(L) * P] is \n" << AI << "A * AI is\n" << Matrix(AI * A);
    MTL_THROW_IF(one_norm(Matrix(AI * A - I)) > 0.1, mtl::runtime_error("Error in inversion."));

    typename mtl::mat::traits::inv<Matrix>::type A_inv(inv(A));
    cout << "inv(A) is \n" << A_inv << "A * AI is\n" << Matrix(A_inv * A);
    MTL_THROW_IF(one_norm(Matrix(A_inv * A - I)) > 0.1, mtl::runtime_error("Error in inversion."));
}

int main(int, char**)
{
    using namespace mtl;
    std::size_t size= 4;

    dense2D<double>                                      dr(size, size);
    dense2D<complex<double> >                            dz(size, size);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);
    // compressed2D<double>                                 cr(size, size);

    test(dr, "Row-major dense");
    test(dz, "Row-major dense with complex numbers");
    test(dc, "Column-major dense");

    return 0;
}


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

extern "C" void kgain_(float *xENS, float *yEns, float *dy, int *nx, int *ny, int *nEns, float *s,
		       float *dx)
{
  int i, j;
  mat xENSa(*nx,*nEns),yENSa(*ny,*nEns);
  vec dya(*ny);
  //    kGain=dot(covXY,linalg.inv(covYY+R*eye(ny)))
  //    xRet=xEns.mean()+dot(kGain,dy)

  //vec dy=vec(*n),sol;
  for(i=0;i<*nEns;i++)
    for(j=0;j<*nx;j++)
      {
	xENSa(j,i)=xENS[j*(*nEns)+i];
      }
  for(j=0;j<*nx;j++)
    {
      float xmean=0;
      for(i=0;i<*nEns;i++)
	xmean+=xENSa(j,i);
      xENSa(j,span(0,*nEns-1))-=(xmean/(*nEns));
    }
  for(i=0;i<*nEns;i++)
    for(j=0;j<*ny;j++)
      {
	yENSa(j,i)=yEns[j*(*nEns)+i];
      }

  for(j=0;j<*ny;j++)
    {
      float ymean=0;
      for(i=0;i<*nEns;i++)
	ymean+=yENSa(j,i);
      yENSa(j,span(0,*nEns-1))-=(ymean/(*nEns));
    }
  mat covYY=(yENSa*yENSa.t())/(*nEns-1);
  mat covXY=(xENSa*yENSa.t())/(*nEns-1);
  //cout<<covYY;
  for(j=0;j<*ny;j++)
    {
      covYY(j,j)+=*s;
      dya(j)=dy[j];
    }
  vec sol,dxa;
  sol=solve(covYY,dya);
  dxa=covXY*sol;
  for(i=0;i<*nx;i++)
    dx[i]=dxa(i);

  /*
  for(i=0;i<*n;i++)
    printf("%6.2f ",zobs[i]);
  printf("\n");
  for(i=0;i<*n;i++)
    printf("%6.2f ",z[i]);
  printf("\n");
  for(i=0;i<*n;i++)
    printf("%6.2f ",sol[i]);
  printf("\n");
  printf("****\n");
  */
}

extern "C" void gauss_newton_(float *dzdn, float *z, float *zobs, int *n, float *s,
                            float *dn)
{
  int i, j;
  mat gradZ(*n,*n),temp;
  vec dy=vec(*n),sol;
  for(i=0;i<*n;i++)
    for(j=0;j<*n;j++)
    {
      gradZ(i,j)=dzdn[j*(*n)+i];
    }

  temp=gradZ.t()*gradZ;
  for(i=0;i<*n;i++)
    {
      temp(i,i)+=*s;
      if(zobs[i]>10 && z[i]>5)
	dy(i)=zobs[i]-z[i];
      else
	dy(i)=0;
    }
  vec graddy=gradZ.t()*dy;
  sol=solve(temp,graddy);
  for(i=0;i<*n;i++)
    dn[i]=sol(i);

  /*
  for(i=0;i<*n;i++)
    printf("%6.2f ",zobs[i]);
  printf("\n");
  for(i=0;i<*n;i++)
    printf("%6.2f ",z[i]);
  printf("\n");
  for(i=0;i<*n;i++)
    printf("%6.2f ",sol[i]);
  printf("\n");
  printf("****\n");
  */
}

extern "C" void interp_arm_(float *x, float *y, int *n,
                            float *xi, float *yi, int *ni)
{
  int i;
  vec xa=vec(*n);
  vec ya=vec(*n);
  vec xia=vec(*ni);
  vec yia=vec(*ni);
  for(i=0;i<*n;i++)
    {
      xa(i)=x[i];
      ya(i)=y[i];
    }
  for(i=0;i<*ni;i++)
    xia(i)=xi[i];

  interp1(xa, ya, xia, yia);
  for(i=0;i<*ni;i++)
    {
      yi[i]=yia(i);
      printf("%i %g \n",i,yia(i));
    }
}

extern "C" void kgainc_(float *dtb, int *n, float *s, float *kgain)
{
  mat A(*n,*n);
  int i,j;
  for(i=0;i<*n;i++)
    {
      for(j=0;j<*n;j++)
        A(i,j)=dtb[i]*dtb[j];
      A(i,i)=A(i,i)+(*s);
    }
  //A.print("A=:");
  mat B=pinv(A,0.0001);
  //B.print("B=:");

  for(i=0;i<*n;i++)
    {
      kgain[i]=0;
      for(j=0;j<*n;j++)
        kgain[i]+=dtb[j]*B(j,i);
    }
}
//pinv=linalg.pinv(dot(dtb.T,dtb)+eye(6)*4)
//  kgain=dot(dtb,pinv)

extern "C" void interp_armi_(int *x, float *y, int *n,
                             int *xi, float *yi, int *ni)
{
  int i;
  vec xa=vec(*n);
  vec ya=vec(*n);
  vec xia=vec(*ni);
  vec yia;
  for(i=0;i<*n;i++)
    {
      xa(i)=x[i];
      ya(i)=y[i];
    }
  for(i=0;i<*ni;i++)
    {
      xia(i)=xi[i];
      //printf("%g \n",xi[i]);
    }
  //  xia.print("xi:");

  interp1(xa, ya, xia, yia);
  //printf(" %i %i \n",*n,*ni);
  for(i=0;i<*ni;i++)
    {
      yi[i]=yia(i);
      //  printf("%i %lg %lg \n",i,xia(i),yia(i));
    }
}


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// 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)
//=======================================================================
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/ref.hpp>
#include <vector>

#include <boost/graph/planar_face_traversal.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace boost;

// Some planar face traversal visitors that will
// print the vertices and edges on the faces

struct output_visitor : public planar_face_traversal_visitor
{
  void begin_face() { std::cout << "New face: "; }
  void end_face() { std::cout << std::endl; }
};

struct vertex_output_visitor : public output_visitor
{
  template <typename Vertex>
  void next_vertex(Vertex v)
  {
    std::cout << v << " ";
  }
};

struct edge_output_visitor : public output_visitor
{
  template <typename Edge>
  void next_edge(Edge e)
  {
    std::cout << e << " ";
  }
};

int main(int argc, char** argv)
{

  typedef adjacency_list
    < vecS,
      vecS,
      undirectedS,
      property<vertex_index_t, int>,
      property<edge_index_t, int>
    >
    graph;

  // Create a graph - this is a biconnected, 3 x 3 grid.
  // It should have four small (four vertex/four edge) faces and
  // one large face that contains all but the interior vertex
  graph g(9);

  add_edge(0,1,g);
  add_edge(1,2,g);

  add_edge(3,4,g);
  add_edge(4,5,g);

  add_edge(6,7,g);
  add_edge(7,8,g);

  add_edge(0,3,g);
  add_edge(3,6,g);

  add_edge(1,4,g);
  add_edge(4,7,g);

  add_edge(2,5,g);
  add_edge(5,8,g);

  // Initialize the interior edge index
  property_map<graph, edge_index_t>::type e_index = get(edge_index, g);
  graph_traits<graph>::edges_size_type edge_count = 0;
  graph_traits<graph>::edge_iterator ei, ei_end;
  for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  // Test for planarity - we know it is planar, we just want to
  // compute the planar embedding as a side-effect
  typedef std::vector< graph_traits<graph>::edge_descriptor > vec_t;
  std::vector<vec_t> embedding(num_vertices(g));
  if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding =
                                       &embedding[0]
                                   )
      )
    std::cout << "Input graph is planar" << std::endl;
  else
    std::cout << "Input graph is not planar" << std::endl;

  std::cout << std::endl << "Vertices on the faces: " << std::endl;
  vertex_output_visitor v_vis;
  planar_face_traversal(g, &embedding[0], v_vis);

  std::cout << std::endl << "Edges on the faces: " << std::endl;
  edge_output_visitor e_vis;
  planar_face_traversal(g, &embedding[0], e_vis);

  return 0;
}


#include <cstdlib>
#include <iostream>
#include <type_traits>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/banded.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/lapack/driver.hpp>
#include "random.hpp"

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

int main(int argc, char *argv[]) {
  typedef ublas::vector<double> vector;
  typedef ublas::matrix<double, ublas::column_major> matrix;
  typedef ublas::banded_matrix<double, ublas::column_major> banded_matrix;
  typedef typename std::make_signed<vector::size_type>::type size_type;

  rand_normal<double>::reset();
  size_type n=128, k=1;
  banded_matrix A(n, n, k, k);
  for (size_type j=0; j<n; ++j) {
    for (size_type i=std::max(j-k, size_type(0)); i<=j; ++i) {
      A(i, j)=rand_normal<double>::get();
      A(j, i)=A(i, j);
    }
  }
  {
    vector d(n), e(n-1);
    for (size_type j=0; j<n; ++j) {
      d(j)=A(j, j);
      if (j<n-1)
	e(j)=A(j+1, j);
    }
    matrix vr(n ,n);
    int info=lapack::stev('V', n, d, e, vr);
    if (info==0) {
      for (int i=0; i<n; ++i) {
    	// res <- A*vr(i) - lambda(i)*vr(i)
	ublas::matrix_column<matrix> v(vr, i);
	vector res(ublas::prod(A, v)-d(i)*v);
	std::cout << "norm of residual (right eigen vector " << i
 		  << " ): " << blas::nrm2(res) << '\n';
      }
    } else
      if (info>0)
    	std::cout << "unable to compute all eigen values\n";
      else
    	std::cout << "illegal arguments\n";
  }
  return EXIT_SUCCESS;
}


//
// Created by krab1k on 31/10/18.
//

#include <vector>
#include <cmath>
#include <Eigen/LU>

#include "abeem.h"
#include "../parameters.h"
#include "../geometry.h"

CHARGEFW2_METHOD(ABEEM)

std::vector<double> ABEEM::calculate_charges(const Molecule &molecule) const {

    size_t n = molecule.atoms().size();
    size_t m = molecule.bonds().size();
    size_t mn = n + m + 1;

    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(mn, mn);
    Eigen::VectorXd b = Eigen::VectorXd::Zero(mn);

    const double k = parameters_->common()->parameter(common::k);

    // atom-atom part
    for (size_t i = 0; i < n; i++) {
        const auto &atom_i = molecule.atoms()[i];
        A(i, i) = parameters_->atom()->parameter(atom::b)(atom_i);
        b(i) = -parameters_->atom()->parameter(atom::a)(atom_i);
        for (size_t j = i + 1; j < n; j++) {
            const auto &atom_j = molecule.atoms()[j];
            double off = k / distance(atom_i, atom_j);
            A(i, j) = off;
            A(j, i) = off;
        }
    }

    // atom-bond part
    for (size_t i = 0; i < n; i++) {
        const auto &atom = molecule.atoms()[i];
        for (size_t j = 0; j < m; j++) {
            const auto &bond = molecule.bonds()[j];
            if (bond.hasAtom(atom)) {
                A(i, n + j) = parameters_->atom()->parameter(atom::c)(atom);
            } else {
                A(i, n + j) = k / distance(atom, bond, true);
            }
        }

    }

    // bond-atom part
    for (size_t i = 0; i < m; i++) {
        const auto &bond = molecule.bonds()[i];
        b(n + i) = -parameters_->bond()->parameter(bond::A)(bond);
        for (size_t j = 0; j < n; j++) {
            const auto &atom = molecule.atoms()[j];
            if (bond.hasAtom(atom)) {
                if (bond.first() == atom) {
                    A(n + i, j) = parameters_->bond()->parameter(bond::D)(bond);
                } else {
                    A(n + i, j) = parameters_->bond()->parameter(bond::C)(bond);
                }
            } else {
                A(n + i, j) = k / distance(atom, bond, true);
            }
        }
    }

    // bond-bond part
    for (size_t i = 0; i < m; i++) {
        const auto &bond_i = molecule.bonds()[i];
        A(n + i, n + i) = parameters_->bond()->parameter(bond::B)(bond_i);
        for (size_t j = i + 1; j < m; j++) {
            const auto &bond_j = molecule.bonds()[j];
            double off = k / distance(bond_i, bond_j, true);
            A(n + i, n + j) = off;
            A(n + j, n + i) = off;
        }
    }

    for (size_t i = 0; i < n + m; i++) {
        A(i, n + m) = 1;
        A(n + m, i) = 1;
    }

    A(n + m, n + m) = 0;
    b(n + m) = molecule.total_charge();

    Eigen::VectorXd q = A.partialPivLu().solve(b).head(mn);

    // Redistribute the bond charges to the corresponding atoms
    for(size_t i = 0; i < m; i++) {
        const auto &bond = molecule.bonds()[i];
        q(bond.first().index())+= 0.5 * q(n + i);
        q(bond.second().index()) += 0.5 * q(n + i);
    }

    return std::vector<double>(q.data(), q.data() + n);
}


#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseCholesky>

#include <vector>
#include <memory>

#include "SDOT/SemiDiscreteOT.h"
#include "SDOT/PolygonRasterize.h"
#include "SDOT/RegularGrid.h"
#include "SDOT/DiscretizedDistribution.h"
#include "SDOT/OptionUtilities.h"
#include "SDOT/Distances/Wasserstein2.h"

using namespace sdot;
using namespace sdot::distances;

void AddCircle(Eigen::MatrixXd &dens, double x, double y, double r, double dx, double dy)
{
  for(int j=0; j<dens.cols(); ++j){
    double yj = double(j)*dy;//  + 0.5*dy;

    for(int i=0; i<dens.rows(); ++i){
      double xi = double(i)*dx;// + 0.5*dx;

      if((xi-x)*(xi-x) + (yj-y)*(yj-y) < r*r)
        dens(i,j) = 1.0;
    }
  }
}

int main(int argc, char* argv[])
{
  OptionList opts;
  opts["Print Level"] = 1;
  opts["Max Steps"] = 300;
  opts["GTol Abs"] = 1e-8;
  opts["XTol Abs"] = 1e-9;

  int N = 1;

  Eigen::Matrix2Xd domain(2,4);
  domain << 0.0, 2.0, 2.0, 0.0,
            0.0, 0.0, 2.0, 2.0;

  // Construct the continuous distribution
  auto grid = std::make_shared<RegularGrid>(domain(0,0),domain(1,0), domain(0,2), domain(1,2), N, N);

  // Unnormalized density.  Will be normalized in DiscretizedDistribution constructor
  Eigen::MatrixXd density = Eigen::MatrixXd::Ones(grid->NumCells(0), grid->NumCells(1))/(N*N*grid->dx*grid->dy);

  //double radius = 0.1001;
  //AddCircle(density, 0.4,0.8, radius, grid->dx, grid->dy);
  //AddCircle(density, 1.4,0.4, radius, grid->dx, grid->dy);

  std::cout << "Density = \n" << density << std::endl;

  Eigen::MatrixXd pts(2,2);
  pts << 0.1, 1.5,
         1.0, 1.0;
  //pts << 1.26996324, 1.11996324, 1.11996324, 0.83003676, 0.68003676, 0.68003676,
  //       1.64491023, 1.73151277, 1.55830769, 0.60508977, 0.69169231, 0.51848723;
  unsigned int numPts = pts.cols();

  auto dist = std::make_shared<DiscretizedDistribution>(grid, density);

  // Evalaute the SDOT objective
  Eigen::VectorXd discrProbs = Eigen::VectorXd::Ones(pts.cols());
  discrProbs /= pts.cols();

  auto sdot = std::make_shared<SemidiscreteOT<Wasserstein2>>(dist, pts, discrProbs);

  Eigen::VectorXd optPrices;
  double optVal;
  std::tie(optPrices,optVal) = sdot->Solve(Eigen::VectorXd::Ones(numPts), opts);
  std::cout << "Optimal prices = " << optPrices.transpose() << std::endl;

  std::shared_ptr<LaguerreDiagram> lagDiag = sdot->Diagram();

  std::cout << "Laguerre cells = " << std::endl;
  for(int polyInd=0; polyInd<numPts; ++polyInd){
    std::cout << "Working on " <<  polyInd << std::endl;
    std::shared_ptr<PolygonRasterizeIter::Polygon_2> poly = lagDiag->GetCell(polyInd)->ToCGAL();

    if(poly->size()>0){
      auto vertIt = poly->vertices_begin();
      std::cout << "[[" << vertIt->x() << "," << vertIt->y() << "]";
      vertIt++;
      for(;  vertIt != poly->vertices_end(); ++vertIt){
        std::cout << ", [" << vertIt->x() << "," << vertIt->y() << "]";
      }
      std::cout << "]" << std::endl;
    }
  }

  // Check the gradient wrt the points
  Eigen::Matrix2Xd grad = sdot->PointGradient();

  // Compute a finite difference approximation
  double fdStep = 1e-5;
  Eigen::VectorXd newPrices;
  double newVal;

  for(int ptInd=0; ptInd<pts.cols(); ++ptInd){
    Eigen::MatrixXd newPts = pts;
    newPts(0,ptInd) += fdStep;

    auto newSdot = std::make_shared<SemidiscreteOT<Wasserstein2>>(dist, newPts, discrProbs);
    std::tie(newPrices,newVal) = newSdot->Solve(Eigen::VectorXd::Ones(numPts), opts);

    std::cout << "Point " << ptInd << std::endl;
    std::cout << "  FD Deriv:   " << (newVal-optVal)/fdStep << std::endl;
    std::cout << "  True Deriv: " << grad(0,ptInd) << std::endl;
  }

  return 0;
}


/*
expressionGenerator.cpp - This file is part of the Bayesembler (v1.1.1)

The MIT License (MIT)

Copyright (c) 2014 Lasse Maretty and Jonas Andreas Sibbesen

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

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

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

#include <expressionGenerator.h>
#include <boost/random/uniform_01.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/discrete_distribution.hpp>

typedef boost::random::mt19937* mt_rng_pt_t;
typedef boost::random::uniform_01<boost::random::mt19937*> uniform_01_sampler_t;
typedef boost::random::gamma_distribution<> gamma_distribution_t;
typedef boost::random::variate_generator<boost::random::mt19937*, boost::random::gamma_distribution<> > gamma_sampler_t;
typedef boost::random::uniform_int_distribution<> uniform_sampler_t;

ExpressionGenerator::ExpressionGenerator(int num_fragments_in, int num_transcripts_in, mt_rng_pt_t mt_rng_pt_in) {

	num_fragments = num_fragments_in;
	num_transcripts = num_transcripts_in;
	mt_rng_pt = mt_rng_pt_in;
}

ExpressionValueContainer ExpressionGenerator::generateEnsembleExpression(vector<int> indices, double gamma, CountValueContainer map_counts) {

	// Init container
	ExpressionValueContainer expression(num_transcripts);

	// Sample gammas for s-plus
	double norm_const_expression = 0;

	for (int i = 0; i < indices.size(); i++) {

	    int trans_id = indices[i];

	    gamma_distribution_t gamma_dist((map_counts.getCount(trans_id) + gamma),1);
        gamma_sampler_t sample_gamma(mt_rng_pt, gamma_dist);

        double gamma_sample = sample_gamma();

	    norm_const_expression += gamma_sample;
	    expression.setBinaryOn(trans_id);
	    expression.setValue(gamma_sample, trans_id);
        expression.addToPlus(trans_id);
	}

	expression.normalise(norm_const_expression);

	return expression;
}

ExpressionValueContainer ExpressionGenerator::generateExpression(int b, double gamma, CountValueContainer map_counts) {

	// Init container
	ExpressionValueContainer expression(num_transcripts);

	// Sample gammas for s-plus
	double norm_const_expression = 0;

	for (int i = 0; i < map_counts.getPlusSize(); i++) {

	    int trans_id = map_counts.getPlus(i);

	    gamma_distribution_t gamma_dist((map_counts.getCount(trans_id) + gamma),1);
        gamma_sampler_t sample_gamma(mt_rng_pt, gamma_dist);

        double gamma_sample = sample_gamma();

	    norm_const_expression += gamma_sample;
	    expression.setBinaryOn(trans_id);
	    expression.setValue(gamma_sample, trans_id);
        expression.addToPlus(trans_id);
	}

    gamma_distribution_t gamma_dist_base (gamma, 1);
    gamma_sampler_t sample_gamma_base (mt_rng_pt, gamma_dist_base);

	// Sample gammas for the expanded simplex
	for (int i = map_counts.getPlusSize(); i < b ; i++) {

	    uniform_sampler_t sample_trans(0,(map_counts.getNullSize()-1));

	    int s_null_idx = sample_trans(*mt_rng_pt);
	    int trans_id = map_counts.getNull(s_null_idx);

	    assert (map_counts.getCount(trans_id) == 0);

	    double gamma_base_sample = sample_gamma_base();
	    assert(gamma_base_sample >= double_underflow);

        // if (gamma_base_sample < almost_zero) {

        //     gamma_base_sample = almost_zero;
        // }

	    norm_const_expression += gamma_base_sample;
	    expression.setBinaryOn(trans_id);
	    expression.setValue(gamma_base_sample, trans_id);
        expression.addToPlus(trans_id);
	    map_counts.eraseNull(s_null_idx);
	}

	expression.normalise(norm_const_expression);

	return expression;
}


#include <iostream>
#include <chrono>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>

#include "utils/FileUtil.h"
#include "algorithm/BFFSolver.h"
#include "algorithm/BonSolver.h"

using namespace boost;
using namespace std;
using namespace std::chrono;

int **computeAllShortestPaths2(vector<pair<int, int>> &edges_vec, int n) {
    const int m = edges_vec.size();

    typedef adjacency_list<vecS, vecS, undirectedS, no_property,
            property<edge_weight_t, int, property<edge_weight2_t, int>>>
            Graph;

    typedef std::pair<int, int> Edge;
    Edge edges_array[m];
    for (int i = 0; i < m; ++i)
        edges_array[i] = Edge(edges_vec[i].first, edges_vec[i].second);

    Graph G(edges_array, edges_array + m, n);

    property_map<Graph, edge_weight_t>::type w = get(edge_weight, G);
    int weights[m];
    std::fill(weights, weights + m, 1);
    int *wp = weights;

    graph_traits<Graph>::edge_iterator e, e_end;
    for (boost::tie(e, e_end) = edges(G); e != e_end; ++e)
        w[*e] = *wp++;

    int **D = new int *[n];
    for (int i = 0; i < n; ++i) {
        D[i] = new int[n];
    }
    johnson_all_pairs_shortest_paths(G, D);
    return D;
}

int **computeAllShortestPaths(vector<vector<int>> &adj, int n) {

    int **D = new int *[n];
    for (int i = 0; i < n; ++i) {
        D[i] = new int[n];
        fill(D[i], D[i] + n, n);
        D[i][i] = 0;
    }

    for (int i = 0; i < n; ++i) {
        queue<int> q;
        vector<bool> visited(n);
        q.push(i);
        visited[i] = true;
        while (!q.empty()) {
            int s = q.front();
            q.pop();
            for (auto u: adj[s]) {
                if (!visited[u]) {
                    D[i][u] = D[i][s] + 1;
                    visited[u] = true;
                    q.push(u);
                }
            }
        }
    }

    return D;
}

int main(int argc, char **argv) {

    string input_file = argv[1];
    string alg = argv[2];

    // read instance
    vector<pair<int, int>> edges_vec;
    int n;
    tie(edges_vec, n) = FileUtil::load_graph(input_file);
    vector<vector<int>> adj = AlgUtils::createAdjList(edges_vec, n);

    // time of compute all shortest paths
    auto start = high_resolution_clock::now();
    int **D = computeAllShortestPaths(adj, n);

    auto stop = high_resolution_clock::now();
    auto duration = duration_cast<milliseconds>(stop - start);
    double time_APSP = duration.count() / (double) 1000;
    // end computations of all shortest paths
    cout << "Compute all shortest paths running time: " << time_APSP << " seconds" << endl;

    // time of algorithm
    vector<int> f;
    start = high_resolution_clock::now();

    if (alg == "bon") {
        BonSolver solver(n, D);
        f = solver.run();
    } else if (alg == "bff") {
        BFFSolver solver(n, D, adj);
        f = solver.run();
    } else if (alg == "bff+") {
        BFFSolver solver(n, D, adj);
        solver.setPlus(true);
        f = solver.run();
    } else {
        cerr << "Invalid algorithm!" << endl;
    }
    stop = high_resolution_clock::now();
    duration = duration_cast<milliseconds>(stop - start);
    double time_alg = duration.count() / (double) 1000;

    // print computations times and solution
    cout << "Algorithm running time: " << time_alg << " seconds" << endl;

    cout << "[";
    for (int i = 0; i < f.size() - 1; ++i) {
        cout << f[i] << ", ";
    }
    cout << f.back() << "]" << endl;
    cout << f.size() << endl;

    // clear memory
    for (int i = 0; i < n; ++i) {
        delete[] D[i];
    }
    return 0;
}


#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>
#include <unordered_map>

using namespace std;
using boost::multiprecision::cpp_int;

// Generate the n'th term for the factorial sequence. Uses a cache to speed up results.
cpp_int factorial(int n) {
	static unordered_map<int, cpp_int> cache;
	if(auto it = cache.find(n); it != cache.end()) {
		return (*it).second;
	}
	if(n > 1) {
		cpp_int res = n * factorial(n - 1);
		cache.insert({n, res});
		return res;
	} else {
		return 1;
	}
}

// Gets the combination.
cpp_int nCr(int n, int r) {
	return (factorial(n) / (factorial(r) * factorial(n - r)));
}

int main(int argc, char *argv[]) {
	int over_million_count = 0;
	for(int n = 1; n < 101; n++) {
		for(int r = 1; r <= n; r++) {
			if(nCr(n, r) > 1'000'000) {
				over_million_count++;
			}
		}
	}
	cout << over_million_count << endl;
	return 0;
}