//  Copyright (c) 2006 Xiaogang Zhang, 2015 John Maddock
//  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)
//
//  History:
//  XZ wrote the original of this file as part of the Google
//  Summer of Code 2006.  JM modified it to fit into the
//  Boost.Math conceptual framework better, and to correctly
//  handle the p < 0 case.
//  Updated 2015 to use Carlson's latest methods.
//

#ifndef BOOST_MATH_ELLINT_RJ_HPP
#define BOOST_MATH_ELLINT_RJ_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/ellint_rc.hpp>
#include <boost/math/special_functions/ellint_rf.hpp>
#include <boost/math/special_functions/ellint_rd.hpp>

// Carlson's elliptic integral of the third kind
// R_J(x, y, z, p) = 1.5 * \int_{0}^{\infty} (t+p)^{-1} [(t+x)(t+y)(t+z)]^{-1/2} dt
// Carlson, Numerische Mathematik, vol 33, 1 (1979)

namespace boost { namespace math { namespace detail{

template <typename T, typename Policy>
T ellint_rc1p_imp(T y, const Policy& pol)
{
   using namespace boost::math;
   // Calculate RC(1, 1 + x)
   BOOST_MATH_STD_USING

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

   if(y == -1)
   {
      return policies::raise_domain_error<T>(function,
         "Argument y must not be zero but got %1%", y, pol);
   }

   // for 1 + y < 0, the integral is singular, return Cauchy principal value
   T result;
   if(y < -1)
   {
      result = sqrt(1 / -y) * detail::ellint_rc_imp(T(-y), T(-1 - y), pol);
   }
   else if(y == 0)
   {
      result = 1;
   }
   else if(y > 0)
   {
      result = atan(sqrt(y)) / sqrt(y);
   }
   else
   {
      if(y > -0.5)
      {
         T arg = sqrt(-y);
         result = (boost::math::log1p(arg, pol) - boost::math::log1p(-arg, pol)) / (2 * sqrt(-y));
      }
      else
      {
         result = log((1 + sqrt(-y)) / sqrt(1 + y)) / sqrt(-y);
      }
   }
   return result;
}

template <typename T, typename Policy>
T ellint_rj_imp(T x, T y, T z, T p, const Policy& pol)
{
   BOOST_MATH_STD_USING

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

   if(x < 0)
   {
      return policies::raise_domain_error<T>(function,
         "Argument x must be non-negative, but got x = %1%", x, pol);
   }
   if(y < 0)
   {
      return policies::raise_domain_error<T>(function,
         "Argument y must be non-negative, but got y = %1%", y, pol);
   }
   if(z < 0)
   {
      return policies::raise_domain_error<T>(function,
         "Argument z must be non-negative, but got z = %1%", z, pol);
   }
   if(p == 0)
   {
      return policies::raise_domain_error<T>(function,
         "Argument p must not be zero, but got p = %1%", p, pol);
   }
   if(x + y == 0 || y + z == 0 || z + x == 0)
   {
      return policies::raise_domain_error<T>(function,
         "At most one argument can be zero, "
         "only possible result is %1%.", std::numeric_limits<T>::quiet_NaN(), pol);
   }

   // for p < 0, the integral is singular, return Cauchy principal value
   if(p < 0)
   {
      //
      // We must ensure that x < y < z.
      // Since the integral is symmetrical in x, y and z
      // we can just permute the values:
      //
      if(x > y)
         std::swap(x, y);
      if(y > z)
         std::swap(y, z);
      if(x > y)
         std::swap(x, y);

      BOOST_MATH_ASSERT(x <= y);
      BOOST_MATH_ASSERT(y <= z);

      T q = -p;
      p = (z * (x + y + q) - x * y) / (z + q);

      BOOST_MATH_ASSERT(p >= 0);

      T value = (p - z) * ellint_rj_imp(x, y, z, p, pol);
      value -= 3 * ellint_rf_imp(x, y, z, pol);
      value += 3 * sqrt((x * y * z) / (x * y + p * q)) * ellint_rc_imp(T(x * y + p * q), T(p * q), pol);
      value /= (z + q);
      return value;
   }

   //
   // Special cases from http://dlmf.nist.gov/19.20#iii
   //
   if(x == y)
   {
      if(x == z)
      {
         if(x == p)
         {
            // All values equal:
            return 1 / (x * sqrt(x));
         }
         else
         {
            // x = y = z:
            return 3 * (ellint_rc_imp(x, p, pol) - 1 / sqrt(x)) / (x - p);
         }
      }
      else
      {
         // x = y only, permute so y = z:
         using std::swap;
         swap(x, z);
         if(y == p)
         {
            return ellint_rd_imp(x, y, y, pol);
         }
         else if((std::max)(y, p) / (std::min)(y, p) > 1.2)
         {
            return 3 * (ellint_rc_imp(x, y, pol) - ellint_rc_imp(x, p, pol)) / (p - y);
         }
         // Otherwise fall through to normal method, special case above will suffer too much cancellation...
      }
   }
   if(y == z)
   {
      if(y == p)
      {
         // y = z = p:
         return ellint_rd_imp(x, y, y, pol);
      }
      else if((std::max)(y, p) / (std::min)(y, p) > 1.2)
      {
         // y = z:
         return 3 * (ellint_rc_imp(x, y, pol) - ellint_rc_imp(x, p, pol)) / (p - y);
      }
      // Otherwise fall through to normal method, special case above will suffer too much cancellation...
   }
   if(z == p)
   {
      return ellint_rd_imp(x, y, z, pol);
   }

   T xn = x;
   T yn = y;
   T zn = z;
   T pn = p;
   T An = (x + y + z + 2 * p) / 5;
   T A0 = An;
   T delta = (p - x) * (p - y) * (p - z);
   T Q = pow(tools::epsilon<T>() / 5, -T(1) / 8) * (std::max)((std::max)(fabs(An - x), fabs(An - y)), (std::max)(fabs(An - z), fabs(An - p)));

   unsigned n;
   T lambda;
   T Dn;
   T En;
   T rx, ry, rz, rp;
   T fmn = 1; // 4^-n
   T RC_sum = 0;

   for(n = 0; n < policies::get_max_series_iterations<Policy>(); ++n)
   {
      rx = sqrt(xn);
      ry = sqrt(yn);
      rz = sqrt(zn);
      rp = sqrt(pn);
      Dn = (rp + rx) * (rp + ry) * (rp + rz);
      En = delta / Dn;
      En /= Dn;
      if((En < -0.5) && (En > -1.5))
      {
         //
         // Occasionally En ~ -1, we then have no means of calculating
         // RC(1, 1+En) without terrible cancellation error, so we
         // need to get to 1+En directly.  By substitution we have
         //
         // 1+E_0 = 1 + (p-x)*(p-y)*(p-z)/((sqrt(p) + sqrt(x))*(sqrt(p)+sqrt(y))*(sqrt(p)+sqrt(z)))^2
         //       = 2*sqrt(p)*(p+sqrt(x) * (sqrt(y)+sqrt(z)) + sqrt(y)*sqrt(z)) / ((sqrt(p) + sqrt(x))*(sqrt(p) + sqrt(y)*(sqrt(p)+sqrt(z))))
         //
         // And since this is just an application of the duplication formula for RJ, the same
         // expression works for 1+En if we use x,y,z,p_n etc.
         // This branch is taken only once or twice at the start of iteration,
         // after than En reverts to it's usual very small values.
         //
         T b = 2 * rp * (pn + rx * (ry + rz) + ry * rz) / Dn;
         RC_sum += fmn / Dn * detail::ellint_rc_imp(T(1), b, pol);
      }
      else
      {
         RC_sum += fmn / Dn * ellint_rc1p_imp(En, pol);
      }
      lambda = rx * ry + rx * rz + ry * rz;

      // From here on we move to n+1:
      An = (An + lambda) / 4;
      fmn /= 4;

      if(fmn * Q < An)
         break;

      xn = (xn + lambda) / 4;
      yn = (yn + lambda) / 4;
      zn = (zn + lambda) / 4;
      pn = (pn + lambda) / 4;
      delta /= 64;
   }

   T X = fmn * (A0 - x) / An;
   T Y = fmn * (A0 - y) / An;
   T Z = fmn * (A0 - z) / An;
   T P = (-X - Y - Z) / 2;
   T E2 = X * Y + X * Z + Y * Z - 3 * P * P;
   T E3 = X * Y * Z + 2 * E2 * P + 4 * P * P * P;
   T E4 = (2 * X * Y * Z + E2 * P + 3 * P * P * P) * P;
   T E5 = X * Y * Z * P * P;
   T result = fmn * pow(An, T(-3) / 2) *
      (1 - 3 * E2 / 14 + E3 / 6 + 9 * E2 * E2 / 88 - 3 * E4 / 22 - 9 * E2 * E3 / 52 + 3 * E5 / 26 - E2 * E2 * E2 / 16
      + 3 * E3 * E3 / 40 + 3 * E2 * E4 / 20 + 45 * E2 * E2 * E3 / 272 - 9 * (E3 * E4 + E2 * E5) / 68);

   result += 6 * RC_sum;
   return result;
}

} // namespace detail

template <class T1, class T2, class T3, class T4, class Policy>
inline typename tools::promote_args<T1, T2, T3, T4>::type
   ellint_rj(T1 x, T2 y, T3 z, T4 p, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2, T3, T4>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::ellint_rj_imp(
         static_cast<value_type>(x),
         static_cast<value_type>(y),
         static_cast<value_type>(z),
         static_cast<value_type>(p),
         pol), "boost::math::ellint_rj<%1%>(%1%,%1%,%1%,%1%)");
}

template <class T1, class T2, class T3, class T4>
inline typename tools::promote_args<T1, T2, T3, T4>::type
   ellint_rj(T1 x, T2 y, T3 z, T4 p)
{
   return ellint_rj(x, y, z, p, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_RJ_HPP


/*
 * Copyright 2009-2015 The VOTCA Development Team (http://www.votca.org)
 *
 * 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.
 *
 */

#include <votca/tools/linalg.h>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_eigen.h>

namespace votca { namespace tools {

using namespace std;

/**
 * ublas binding to GSL  Singular Value Decomposition
 *
 * A = U S V^T
 *
 * @param A MxN matrix do decompose. Becomes an MxN orthogonal matrix U
 * @param V NxN orthogonal square matrix
 * @param S N vector of non-negative numbers forming a non-increasing sequence
 * @return succeeded or not
 */

bool linalg_singular_value_decomposition(ub::matrix<double> &A, ub::matrix<double> &VT, ub::vector<double> &S )
{

        gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t M = A.size1();
        const size_t N = A.size2();
        // gsl does not handle conversion of a symmetric_matrix

         if (M>N){
        throw runtime_error("Matrix for svd has the wrong shape first dimension must be equal or larger than second.");
    }
	S.resize(N, false);
	VT.resize(N, N, false);

	gsl_matrix_view A_view = gsl_matrix_view_array(&A(0,0), M, N);
	gsl_vector_view S_view = gsl_vector_view_array(&S(0), N);
	gsl_matrix_view V_view = gsl_matrix_view_array(&VT(0,0), N, N);
	gsl_vector * work = gsl_vector_alloc(N);

        int status = gsl_linalg_SV_decomp (&A_view.matrix, &V_view.matrix, &S_view.vector, work);
	//gsl_eigen_symmv_sort(&E_view.vector, &V_view.matrix, GSL_EIGEN_SORT_ABS_ASC);
	gsl_set_error_handler(handler);
        gsl_vector_free (work);
        VT=ub::trans(VT);
	return (status != 0);

};

}}


/* vim:set ts=3 sw=3 sts=3 et: */
/**
 * Copyright © 2008-2013 Last.fm Limited
 *
 * This file is part of libmoost.
 *
 * 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.
 */

/**
 * @file spline_interpolation.hpp
 * @brief generate a cubic spline with natural boundary conditions
 * @author Ricky Cormier (thanks to Marcus for the suggestion)
 * @version See version.h (N/A if it doesn't exist)
 * @date 2012-04-26
 */

#ifndef MOOST_ALGORITHM_SPLINE_INTERPOLATION_HPP
#define MOOST_ALGORITHM_SPLINE_INTERPOLATION_HPP

#include <cassert>
#include <vector>
#include <algorithm>

#include <gsl/gsl_spline.h> // gnu scientific library

#include <boost/noncopyable.hpp>

#include "../utils/scope_exit.hpp"

namespace moost { namespace algorithm {

   /**
    * @brief Generate a spline interpolation of X and Y data points using
    *        the GNU Scientific Library.
    *
    * http://www.gnu.org/software/gsl/manual/html_node/Interpolation.html
    *
    */

   class spline_interpolation : private boost::noncopyable
   {
      private:
         typedef moost::utils::scope_exit::type<
            gsl_interp *>::call_free_function_with_val interp_t;

         typedef moost::utils::scope_exit::type<
            gsl_interp_accel *>::call_free_function_with_val accel_t;

      public:

         /**
          * @brief create a spline interpolation object
          *
          * @param x : A vector of X data points
          * @param y : A vector of Y data points
          *
          * @note The X vector MUST be sorted in ascending order.
          *
          * Given a vector of X/Y data points, this class will interpolate a
          * value of Y for any value of X providing the value of X is given
          * within the range of X_begin to X_end.
          */

         spline_interpolation(
               std::vector<double> const & x,
               std::vector<double> const & y)
            : interp_(gsl_interp_alloc(gsl_interp_cspline, x.size()), &gsl_interp_free)
              , accel_(gsl_interp_accel_alloc(), &gsl_interp_accel_free)
              , x_(x)
              , y_(y)

         {
            validate_construction(); // throws on fail
            gsl_interp_init(interp_->get(), &x_[0], &y_[0], x_.size());
         }

         /**
          * @brief Given a value of X return Y (or Z if X is out of range)
          *
          * @param x An X data point
          * @param y An interpolated Y data point
          * @param z Used as the default if X is out of range
          *
          * @note the value of X doesn't have to be one of the original values
          *       of the X vector, it just has to be within the range of the
          *       lowest to the highest X value (hence, Y is an interpolation).
          *
          * @return Interpolation of Y or Z if X is out of range
          */

         bool operator () (double const x, double & y, double const z = 0) const
         {
            bool ok =  0 == gsl_interp_eval_e(
                  interp_->get(),
                  &x_[0], &y_[0], x,
                  accel_->get(), &y
                  );

            if(!ok) { y = z;}

            return ok;
         }

         /**
          * @brief Given a value of X return Y (or throw if out of range)
          *
          * @param x An X data point
          *
          * @note the value of X doesn't have to be one of the original values
          *       of the X vector, it just has to be within the range of the
          *       lowest to the highest X value (hence, Y is an interpolation).
          *
          * @return An interpolation of Y data point
          */
         double operator () (double const x) const
         {
            double y = 0.0;

            if(!(*this)(x, y))
            {
               throw std::range_error("out of range");
            }

            return y;
         }

      private:

         void validate_construction() const
         {
            // x and y must be the same size
            assert(x_.size() == y_.size());
            if(x_.size() != y_.size())
            {
               throw std::runtime_error("the size of x and y must be the same");
            }

            // x must be sorted by-asc (not my rule, gsl demands it)
            bool const sorted = (std::adjacent_find(x_.begin(), x_.end(), std::greater_equal<double>()) == x_.end());
            assert(sorted);
            if(!sorted)
            {
               throw std::runtime_error("x must be sorted in assending order");
            }
         }

      private:
         interp_t interp_;
         accel_t  accel_;
         std::vector<double> const & x_;
         std::vector<double> const & y_;
   };

}}

#endif // MOOST_ALGORITHM_SPLINE_INTERPOLATION_HPP


#include "linear_system2d.h"
#include <boost/numeric/itl/itl.hpp>

void LinearSystem::Solve (std::vector<FunctionP1>& sol)
{
    if (!ins)
        throw "The system was already solved.";
    else {
        delete ins;
        ins = 0;

        // Set the initial guess
        mtl::dense_vector<double> x(unknowns);
        for (int i = 0; i < N; ++i) {
            for (int j = 0; j < nodes_num; ++j) {
                int eq = get_index(i, j);
                x[eq] = sol[i].values[j];
            }
        }

        // Solve the linear system
        itl::pc::ilu_0<mtl::compressed2D<double> > P(A);
        itl::basic_iteration<double> iter(b, param.max_linear_sys_iterations,
            param.linear_sys_tol);
        bicgstab(A, x, b, P, iter);

        // Set the solution
        for (int i = 0; i < N; ++i) {
            for (int j = 0; j < nodes_num; ++j) {
                int eq = get_index(i, j);
                sol[i].values[j] = x[eq];
            }
        }
    }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// 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)

// Qt Example

// Qt is a well-known and often used platform independent windows library

// To build and run this example:
// 1) download (from http://qt.nokia.com), configure and make QT
// 2) if necessary, adapt Qt clause in include path (note there is a Qt property sheet)

#include <sstream>

#include <QtGui>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/register/ring.hpp>

// Adapt a QPointF such that it can be handled by Boost.Geometry
BOOST_GEOMETRY_REGISTER_POINT_2D_GET_SET(QPointF, double, cs::cartesian, x, y, setX, setY)

// Adapt a QPolygonF as well.
// A QPolygonF has no holes (interiors) so it is similar to a Boost.Geometry ring
BOOST_GEOMETRY_REGISTER_RING(QPolygonF)

int main(int argc, char *argv[])
{
    // This usage QApplication and QLabel is adapted from
    // http://en.wikipedia.org/wiki/Qt_(toolkit)#Qt_hello_world
    QApplication app(argc, argv);

    // Declare a Qt polygon. The Qt Polygon can be used
    // in Boost.Geometry, just by its oneline registration above.
    QPolygonF polygon;

    // Use Qt to add points to polygon
    polygon
        << QPointF(10, 20) << QPointF(20, 30)
        << QPointF(30, 20) << QPointF(20, 10)
        << QPointF(10, 20);

    // Use Boost.Geometry e.g. to calculate area
    std::ostringstream out;
    out << "Boost.Geometry area: " << boost::geometry::area(polygon) << std::endl;

    // Some functionality is defined in both Qt and Boost.Geometry
    QPointF p(20,20);
    out << "Qt contains: "
        << (polygon.containsPoint(p, Qt::WindingFill) ? "yes" : "no")
        << std::endl
        << "Boost.Geometry within: "
        << (boost::geometry::within(p, polygon) ? "yes" : "no")
        << std::endl;
    // Detail: if point is ON boundary, Qt says yes, Boost.Geometry says no.

    // Qt defines an iterator
    // (which is required for of the Boost.Geometry ring-concept)
    // such that Boost.Geometry can use the points of this polygon
    QPolygonF::const_iterator it;
    for (it = polygon.begin(); it != polygon.end(); ++it)
    {
        // Stream Delimiter-Separated, just to show something Boost.Geometry can do
        out << boost::geometry::dsv(*it) << std::endl;
    }

    // Stream the polygon as well
    out << boost::geometry::dsv(polygon) << std::endl;

    // Just show what we did in a label
    QLabel label(out.str().c_str());
    label.show();
    return app.exec();
}


#include <Eigen/Dense>
#include <fmt/core.h>
#include <fmt/ranges.h>

#include <algorithm>
#include <array>
#include <fstream>
#include <iostream>
#include <numeric>
#include <sstream>
#include <string>
#include <vector>

auto parseFile(char const *file_name) {
  auto file = std::ifstream(file_name);
  std::string line;
  std::vector<std::vector<int>> temp;
  while (std::getline(file, line)) {
    std::vector<int> t;
    // Not sure why passing chars to stoi didn't work but what evers
    // Probably needs null termination
    for (auto i = 0; i < line.size(); ++i) {
      std::string c = {line[i]};
      if (c != "\n") {
        t.push_back(std::stoi(c));
      }
    }
    temp.push_back(std::move(t));
  }

  auto rows = temp.size();
  auto cols = temp[0].size();

  Eigen::MatrixXi out(rows, cols);
  out.setZero();

  for (auto i = 0; i < rows; ++i) {
    for (auto j = 0; j < cols; ++j) {
      out(i, j) = temp[i][j];
    }
  }

  return out;
}

auto findLow(auto const &m) {
  Eigen::MatrixXi out(m.rows(), m.cols());
  out.setConstant(-1);

  for (auto r = 0; r < m.rows(); ++r) {
    for (auto c = 0; c < m.cols(); ++c) {
      if (r == 0 && c == 0) {
        if (m(0, 0) < m(0, 1) && m(0, 0) < m(1, 0)) { // upper left
          out(r, c) = m(r, c);
        }
      } else if (r == (m.rows() - 1) && c == (m.cols() - 1)) { // bottom right
        if (m(r, c) < m(r - 1, c) && m(r, c) < m(r, c - 1)) {
          out(r, c) = m(r, c);
        }
      } else if (r == 0 && c == (m.cols() - 1)) { // upper right
        if (m(r, c) < m(r + 1, c) && m(r, c) < m(r, c - 1)) {
          out(r, c) = m(r, c);
        }
      } else if (r == (m.rows() - 1) && c == 0) { // lower left
        if (m(r, c) < m(r - 1, c) && m(r, c) < m(r, c + 1)) {
          out(r, c) = m(r, c);
        }
      } else if (r == 0) { // top row
        if (m(r, c) < m(r + 1, c) && m(r, c) < m(r, c + 1) &&
            m(r, c) < m(r, c - 1)) {
          out(r, c) = m(r, c);
        }
      } else if (r == (m.rows() - 1)) { // bottom row
        if (m(r, c) < m(r - 1, c) && m(r, c) < m(r, c + 1) &&
            m(r, c) < m(r, c - 1)) {
          out(r, c) = m(r, c);
        }
      } else if (c == 0) { // Left col
        if (m(r, c) < m(r, c + 1) && m(r, c) < m(r + 1, c) &&
            m(r, c) < m(r - 1, c)) {
          out(r, c) = m(r, c);
        }
      } else if (c == (m.cols() - 1)) { // right col
        if (m(r, c) < m(r, c - 1) && m(r, c) < m(r + 1, c) &&
            m(r, c) < m(r - 1, c)) {
          out(r, c) = m(r, c);
        }
      } else { // Whew in the middle
        if (m(r, c) < m(r, c + 1) && m(r, c) < m(r, c - 1) &&
            m(r, c) < m(r + 1, c) && m(r, c) < m(r - 1, c)) {
          out(r, c) = m(r, c);
        }
      }
    }
  }

  return out;
}

int discoverBasin(Eigen::MatrixXi &m, int i, int j) {
  if (i < 0 || i >= m.rows() || j < 0 || j >= m.cols() || m(i, j) == 9) {
    return 0; // Can't be here
  }

  if (m(i, j) == -1) { // Already been here
    return 0;
  }

  m(i,j) = -1; // Been here

  // The + 1 is so that we get credit for this node
  return discoverBasin(m, i + 1, j) + discoverBasin(m, i - 1, j) +
         discoverBasin(m, i, j + 1) + discoverBasin(m, i, j - 1) + 1;
}

int main(int _, char **argv) {
  auto M = parseFile(argv[1]);
  if (M.rows() < 11 && M.cols() < 11) {
    std::cout << "M:\n" << M << "\n\n";
  }
  auto lows = findLow(M);
  if (M.rows() < 11 && M.cols() < 11) {
    std::cout << "L:\n" << lows << "\n\n";
  }
  auto sum = 0;
  for (auto i = 0; i < lows.rows(); ++i) {
    for (auto j = 0; j < lows.cols(); ++j) {
      if (lows(i, j) != -1) {
        sum += lows(i, j) + 1;
      }
    }
  }

  fmt::print("Sum of low points: {}\n", sum);

  // for part two we need to do a search
  Eigen::MatrixXi D = M;
  std::vector<int> basins;
  for(auto i = 0; i < D.rows(); ++i){
    for(auto j = 0; j < D.cols(); ++j){
      if(D(i,j) != 9 && D(i,j) != -1){
        basins.push_back(discoverBasin(D, i, j));
      }
    }
  }

  std::sort(basins.begin(), basins.end(), std::greater<>{});
  fmt::print("Basins: {}\n", fmt::join(basins, ","));
  fmt::print("Prod of top 3: {}\n", basins[0] * basins[1] * basins[2]);
}


/**
 * @file subset_backtrack_test.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-08-14
 */

#include "paal/utils/algorithms/subset_backtrack.hpp"
#include "paal/utils/functors.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/range/algorithm_ext/iota.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/algorithm/remove.hpp>

#include <vector>
#include <random>

BOOST_AUTO_TEST_CASE(subset_backtrack) {
    const int N = 10, WANT_SUM = (1 << (N - 1)) * (N * (N + 1) / 2);
    std::vector<int> numbers(N);
    boost::iota(numbers, 1);
    int sum = 0, selected_sum = 0;
    auto backtrack = paal::make_subset_backtrack(numbers);
    backtrack.solve([&](int k) {
                        selected_sum += k;
                        sum += selected_sum;
                        return true;
                    },
                    [&](int k) { selected_sum -= k; });
    BOOST_CHECK_EQUAL(selected_sum, 0);
    BOOST_CHECK_EQUAL(sum, WANT_SUM);
}

BOOST_AUTO_TEST_CASE(subset_backtrack_empty_set_solved_two_times){
    auto backtrack=paal::make_subset_backtrack(std::vector<int>{});
    auto f=[](int k){BOOST_CHECK(false);return true;};
    backtrack.solve(f,f);
    backtrack.solve(f,f);
}

BOOST_AUTO_TEST_CASE(subset_backtrack_on_element_set_solved_two_times){
    auto backtrack=paal::make_subset_backtrack(std::vector<int>{0});
    auto t=paal::utils::always_true{};
    backtrack.solve(t,t);
    backtrack.solve(t,t);
}

BOOST_AUTO_TEST_CASE(subset_backtrack_max_one_even) {
    const int N = 20;
    assert(N % 4 == 0);
    const int WANT_SUM =
        (1 << (N / 2))         // number of sets of odd numbers
        * (N / 2 + 1)          // one or 0 even numbers
        * (N * N / 8 + N / 2); // average sum of odd and even numbers
    std::vector<int> numbers(N);
    boost::iota(numbers, 1);
    int sum = 0, even = 0, selected_sum = 0;
    auto backtrack = paal::make_subset_backtrack(numbers);
    auto is_even = [](int k) { return k % 2 == 0; };
    backtrack.solve([&](int k) {
                        if (even + is_even(k) > 1) return false;
                        even += is_even(k);
                        selected_sum += k;
                        sum += selected_sum;
                        return true;
                    },
                    [&](int k) {
                        even -= is_even(k);
                        selected_sum -= k;
                    },
                    [&](boost::iterator_range<std::vector<int>::iterator>
                            remaining_nums) {
                        if (even == 1) return boost::remove(remaining_nums, 8);
                        std::shuffle(remaining_nums.begin(), remaining_nums.end(),
                                    std::default_random_engine());
                        return remaining_nums.end();
                    });
    BOOST_CHECK_EQUAL(even, 0);
    BOOST_CHECK_EQUAL(selected_sum, 0);
    BOOST_CHECK_EQUAL(sum, WANT_SUM);
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.
// Copyright (c) 2009-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2015-2021.
// Modifications copyright (c) 2015-2021, Oracle and/or its affiliates.

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

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// 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_STRATEGIES_CARTESIAN_CENTROID_WEIGHTED_LENGTH_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_CENTROID_WEIGHTED_LENGTH_HPP

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include <boost/geometry/arithmetic/arithmetic.hpp>

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

#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/strategies/centroid.hpp>

#include <boost/geometry/util/algorithm.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace centroid
{

template
<
    typename Ignored1 = void,
    typename Ignored2 = void,
    typename CalculationType = void
>
class weighted_length
{
private :
    typedef geometry::strategy::distance::pythagoras<CalculationType> pythagoras_strategy;

    template <typename GeometryPoint, typename ResultPoint>
    struct calculation_type
    {
        // Below the distance between two GeometryPoints is calculated.
        // ResultPoint is taken into account by passing them together here.
        typedef typename pythagoras_strategy::template calculation_type
            <
                GeometryPoint, ResultPoint
            >::type type;
    };

    template <typename GeometryPoint, typename ResultPoint>
    class sums
    {
        friend class weighted_length;
        template <typename, typename> friend struct set_sum_div_length;

        typedef typename calculation_type<GeometryPoint, ResultPoint>::type calc_type;
        typedef typename geometry::model::point
            <
                calc_type,
                geometry::dimension<ResultPoint>::value,
                cs::cartesian
            > work_point;

        calc_type length;
        work_point average_sum;

    public:
        inline sums()
            : length(calc_type())
        {
            geometry::assign_zero(average_sum);
        }
    };

public :
    template <typename GeometryPoint, typename ResultPoint>
    struct state_type
    {
        typedef sums<GeometryPoint, ResultPoint> type;
    };

    template <typename GeometryPoint, typename ResultPoint>
    static inline void apply(GeometryPoint const& p1, GeometryPoint const& p2,
                             sums<GeometryPoint, ResultPoint>& state)
    {
        typedef typename calculation_type<GeometryPoint, ResultPoint>::type distance_type;

        distance_type const d = pythagoras_strategy::apply(p1, p2);
        state.length += d;

        distance_type const d_half = d / distance_type(2);
        geometry::detail::for_each_dimension<ResultPoint>([&](auto dimension)
        {
            distance_type const coord1 = get<dimension>(p1);
            distance_type const coord2 = get<dimension>(p2);
            distance_type const wm = (coord1 + coord2) * d_half; // weighted median
            set<dimension>(state.average_sum, get<dimension>(state.average_sum) + wm);
        });
    }

    template <typename GeometryPoint, typename ResultPoint>
    static inline bool result(sums<GeometryPoint, ResultPoint> const& state,
                              ResultPoint& centroid)
    {
        typedef typename calculation_type<GeometryPoint, ResultPoint>::type distance_type;

        distance_type const zero = distance_type();
        if (! geometry::math::equals(state.length, zero)
            && boost::math::isfinite(state.length)) // Prevent NaN centroid coordinates
        {
            // NOTE: above distance_type is checked, not the centroid coordinate_type
            // which means that the centroid can still be filled with INF
            // if e.g. distance_type is double and centroid contains floats
            geometry::detail::for_each_dimension<ResultPoint>([&](auto dimension)
            {
                typedef typename geometry::coordinate_type<ResultPoint>::type coordinate_type;
                geometry::set<dimension>(
                    centroid,
                    boost::numeric_cast<coordinate_type>(
                        geometry::get<dimension>(state.average_sum) / state.length
                    )
                );
            });
            return true;
        }

        return false;
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

// Register this strategy for linear geometries, in all dimensions

template <std::size_t N, typename Point, typename Geometry>
struct default_strategy
<
    cartesian_tag,
    linear_tag,
    N,
    Point,
    Geometry
>
{
    typedef weighted_length
        <
            Point,
            typename point_type<Geometry>::type
        > type;
};

} // namespace services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::centroid

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_CENTROID_WEIGHTED_LENGTH_HPP


// Boost.Geometry

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

// 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_SJOBERG_INTERSECTION_HPP
#define BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP

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

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

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

namespace boost { namespace geometry { namespace formula
{

/*!
\brief The intersection of two great circles as proposed by Sjoberg.
\see See
    - [Sjoberg02] Lars E. Sjoberg, Intersections on the sphere and ellipsoid, 2002
      http://link.springer.com/article/10.1007/s00190-001-0230-9
*/
template <typename CT>
struct sjoberg_intersection_spherical_02
{
    // TODO: if it will be used as standalone formula
    //       support segments on equator and endpoints on poles

    static inline bool apply(CT const& lon1, CT const& lat1, CT const& lon_a2, CT const& lat_a2,
                             CT const& lon2, CT const& lat2, CT const& lon_b2, CT const& lat_b2,
                             CT & lon, CT & lat)
    {
        CT tan_lat = 0;
        bool res = apply_alt(lon1, lat1, lon_a2, lat_a2,
                             lon2, lat2, lon_b2, lat_b2,
                             lon, tan_lat);

        if (res)
        {
            lat = atan(tan_lat);
        }

        return res;
    }

    static inline bool apply_alt(CT const& lon1, CT const& lat1, CT const& lon_a2, CT const& lat_a2,
                                 CT const& lon2, CT const& lat2, CT const& lon_b2, CT const& lat_b2,
                                 CT & lon, CT & tan_lat)
    {
        CT const cos_lon1 = cos(lon1);
        CT const sin_lon1 = sin(lon1);
        CT const cos_lon2 = cos(lon2);
        CT const sin_lon2 = sin(lon2);
        CT const sin_lat1 = sin(lat1);
        CT const sin_lat2 = sin(lat2);
        CT const cos_lat1 = cos(lat1);
        CT const cos_lat2 = cos(lat2);

        CT const tan_lat_a2 = tan(lat_a2);
        CT const tan_lat_b2 = tan(lat_b2);

        return apply(lon1, lon_a2, lon2, lon_b2,
                     sin_lon1, cos_lon1, sin_lat1, cos_lat1,
                     sin_lon2, cos_lon2, sin_lat2, cos_lat2,
                     tan_lat_a2, tan_lat_b2,
                     lon, tan_lat);
    }

private:
    static inline bool apply(CT const& lon1, CT const& lon_a2, CT const& lon2, CT const& lon_b2,
                             CT const& sin_lon1, CT const& cos_lon1, CT const& sin_lat1, CT const& cos_lat1,
                             CT const& sin_lon2, CT const& cos_lon2, CT const& sin_lat2, CT const& cos_lat2,
                             CT const& tan_lat_a2, CT const& tan_lat_b2,
                             CT & lon, CT & tan_lat)
    {
        // NOTE:
        // cos_lat_ = 0 <=> segment on equator
        // tan_alpha_ = 0 <=> segment vertical

        CT const tan_lat1 = sin_lat1 / cos_lat1; //tan(lat1);
        CT const tan_lat2 = sin_lat2 / cos_lat2; //tan(lat2);

        CT const dlon1 = lon_a2 - lon1;
        CT const sin_dlon1 = sin(dlon1);
        CT const dlon2 = lon_b2 - lon2;
        CT const sin_dlon2 = sin(dlon2);

        CT const c0 = 0;
        bool const is_vertical1 = math::equals(sin_dlon1, c0);
        bool const is_vertical2 = math::equals(sin_dlon2, c0);

        CT tan_alpha1 = 0;
        CT tan_alpha2 = 0;

        if (is_vertical1 && is_vertical2)
        {
            // circles intersect at one of the poles or are collinear
            return false;
        }
        else if (is_vertical1)
        {
            CT const cos_dlon2 = cos(dlon2);
            CT const tan_alpha2_x = cos_lat2 * tan_lat_b2 - sin_lat2 * cos_dlon2;
            tan_alpha2 = sin_dlon2 / tan_alpha2_x;

            lon = lon1;
        }
        else if (is_vertical2)
        {
            CT const cos_dlon1 = cos(dlon1);
            CT const tan_alpha1_x = cos_lat1 * tan_lat_a2 - sin_lat1 * cos_dlon1;
            tan_alpha1 = sin_dlon1 / tan_alpha1_x;

            lon = lon2;
        }
        else
        {
            CT const cos_dlon1 = cos(dlon1);
            CT const cos_dlon2 = cos(dlon2);

            CT const tan_alpha1_x = cos_lat1 * tan_lat_a2 - sin_lat1 * cos_dlon1;
            CT const tan_alpha2_x = cos_lat2 * tan_lat_b2 - sin_lat2 * cos_dlon2;
            tan_alpha1 = sin_dlon1 / tan_alpha1_x;
            tan_alpha2 = sin_dlon2 / tan_alpha2_x;

            CT const T1 = tan_alpha1 * cos_lat1;
            CT const T2 = tan_alpha2 * cos_lat2;
            CT const T1T2 = T1*T2;
            CT const tan_lon_y = T1 * sin_lon2 - T2 * sin_lon1 + T1T2 * (tan_lat1 * cos_lon1 - tan_lat2 * cos_lon2);
            CT const tan_lon_x = T1 * cos_lon2 - T2 * cos_lon1 - T1T2 * (tan_lat1 * sin_lon1 - tan_lat2 * sin_lon2);

            lon = atan2(tan_lon_y, tan_lon_x);
        }

        // choose closer result
        CT const pi = math::pi<CT>();
        CT const lon_2 = lon > c0 ? lon - pi : lon + pi;
        CT const lon_dist1 = (std::max)((std::min)(math::longitude_difference<radian>(lon1, lon),
                                                   math::longitude_difference<radian>(lon_a2, lon)),
                                        (std::min)(math::longitude_difference<radian>(lon2, lon),
                                                   math::longitude_difference<radian>(lon_b2, lon)));
        CT const lon_dist2 = (std::max)((std::min)(math::longitude_difference<radian>(lon1, lon_2),
                                                   math::longitude_difference<radian>(lon_a2, lon_2)),
                                        (std::min)(math::longitude_difference<radian>(lon2, lon_2),
                                                   math::longitude_difference<radian>(lon_b2, lon_2)));
        if (lon_dist2 < lon_dist1)
        {
            lon = lon_2;
        }

        CT const sin_lon = sin(lon);
        CT const cos_lon = cos(lon);

        if (math::abs(tan_alpha1) >= math::abs(tan_alpha2)) // pick less vertical segment
        {
            CT const sin_dlon_1 = sin_lon * cos_lon1 - cos_lon * sin_lon1;
            CT const cos_dlon_1 = cos_lon * cos_lon1 + sin_lon * sin_lon1;
            CT const lat_y_1 = sin_dlon_1 + tan_alpha1 * sin_lat1 * cos_dlon_1;
            CT const lat_x_1 = tan_alpha1 * cos_lat1;
            tan_lat = lat_y_1 / lat_x_1;
        }
        else
        {
            CT const sin_dlon_2 = sin_lon * cos_lon2 - cos_lon * sin_lon2;
            CT const cos_dlon_2 = cos_lon * cos_lon2 + sin_lon * sin_lon2;
            CT const lat_y_2 = sin_dlon_2 + tan_alpha2 * sin_lat2 * cos_dlon_2;
            CT const lat_x_2 = tan_alpha2 * cos_lat2;
            tan_lat = lat_y_2 / lat_x_2;
        }

        return true;
    }
};

/*! Approximation of dLambda_j [Sjoberg07], expanded into taylor series in e^2
    Maxima script:
    dLI_j(c_j, sinB_j, sinB) := integrate(1 / (sqrt(1 - c_j ^ 2 - x ^ 2)*(1 + sqrt(1 - e2*(1 - x ^ 2)))), x, sinB_j, sinB);
    dL_j(c_j, B_j, B) := -e2 * c_j * dLI_j(c_j, B_j, B);
    S: taylor(dLI_j(c_j, sinB_j, sinB), e2, 0, 3);
    assume(c_j < 1);
    assume(c_j > 0);
    L1: factor(integrate(sqrt(-x ^ 2 - c_j ^ 2 + 1) / (x ^ 2 + c_j ^ 2 - 1), x));
    L2: factor(integrate(((x ^ 2 - 1)*sqrt(-x ^ 2 - c_j ^ 2 + 1)) / (x ^ 2 + c_j ^ 2 - 1), x));
    L3: factor(integrate(((x ^ 4 - 2 * x ^ 2 + 1)*sqrt(-x ^ 2 - c_j ^ 2 + 1)) / (x ^ 2 + c_j ^ 2 - 1), x));
    L4: factor(integrate(((x ^ 6 - 3 * x ^ 4 + 3 * x ^ 2 - 1)*sqrt(-x ^ 2 - c_j ^ 2 + 1)) / (x ^ 2 + c_j ^ 2 - 1), x));

\see See
    - [Sjoberg07] Lars E. Sjoberg, Geodetic intersection on the ellipsoid, 2007
      http://link.springer.com/article/10.1007/s00190-007-0204-7
*/
template <unsigned int Order, typename CT>
inline CT sjoberg_d_lambda_e_sqr(CT const& sin_betaj, CT const& sin_beta,
                                 CT const& Cj, CT const& sqrt_1_Cj_sqr,
                                 CT const& e_sqr)
{
    using math::detail::bounded;

    if (Order == 0)
    {
        return 0;
    }

    CT const c1 = 1;
    CT const c2 = 2;

    CT const asin_B = asin(bounded(sin_beta / sqrt_1_Cj_sqr, -c1, c1));
    CT const asin_Bj = asin(sin_betaj / sqrt_1_Cj_sqr);
    CT const L0 = (asin_B - asin_Bj) / c2;

    if (Order == 1)
    {
        return -Cj * e_sqr * L0;
    }

    CT const c0 = 0;
    CT const c16 = 16;

    CT const X = sin_beta;
    CT const Xj = sin_betaj;
    CT const X_sqr = math::sqr(X);
    CT const Xj_sqr = math::sqr(Xj);
    CT const Cj_sqr = math::sqr(Cj);
    CT const Cj_sqr_plus_one = Cj_sqr + c1;
    CT const one_minus_Cj_sqr = c1 - Cj_sqr;
    CT const sqrt_Y = math::sqrt(bounded(-X_sqr + one_minus_Cj_sqr, c0));
    CT const sqrt_Yj = math::sqrt(-Xj_sqr + one_minus_Cj_sqr);
    CT const L1 = (Cj_sqr_plus_one * (asin_B - asin_Bj) + X * sqrt_Y - Xj * sqrt_Yj) / c16;

    if (Order == 2)
    {
        return -Cj * e_sqr * (L0 + e_sqr * L1);
    }

    CT const c3 = 3;
    CT const c5 = 5;
    CT const c128 = 128;

    CT const E = Cj_sqr * (c3 * Cj_sqr + c2) + c3;
    CT const F = X * (-c2 * X_sqr + c3 * Cj_sqr + c5);
    CT const Fj = Xj * (-c2 * Xj_sqr + c3 * Cj_sqr + c5);
    CT const L2 = (E * (asin_B - asin_Bj) + F * sqrt_Y - Fj * sqrt_Yj) / c128;

    if (Order == 3)
    {
        return -Cj * e_sqr * (L0 + e_sqr * (L1 + e_sqr * L2));
    }

    CT const c8 = 8;
    CT const c9 = 9;
    CT const c10 = 10;
    CT const c15 = 15;
    CT const c24 = 24;
    CT const c26 = 26;
    CT const c33 = 33;
    CT const c6144 = 6144;

    CT const G = Cj_sqr * (Cj_sqr * (Cj_sqr * c15 + c9) + c9) + c15;
    CT const H = -c10 * Cj_sqr - c26;
    CT const I = Cj_sqr * (Cj_sqr * c15 + c24) + c33;
    CT const J = X_sqr * (X * (c8 * X_sqr + H)) + X * I;
    CT const Jj = Xj_sqr * (Xj * (c8 * Xj_sqr + H)) + Xj * I;
    CT const L3 = (G * (asin_B - asin_Bj) + J * sqrt_Y - Jj * sqrt_Yj) / c6144;

    // Order 4 and higher
    return -Cj * e_sqr * (L0 + e_sqr * (L1 + e_sqr * (L2 + e_sqr * L3)));
}

/*!
\brief The representation of geodesic as proposed by Sjoberg.
\see See
    - [Sjoberg07] Lars E. Sjoberg, Geodetic intersection on the ellipsoid, 2007
      http://link.springer.com/article/10.1007/s00190-007-0204-7
    - [Sjoberg12] Lars E. Sjoberg, Solutions to the ellipsoidal Clairaut constant
      and the inverse geodetic problem by numerical integration, 2012
      https://www.degruyter.com/view/j/jogs.2012.2.issue-3/v10156-011-0037-4/v10156-011-0037-4.xml
*/
template <typename CT, unsigned int Order>
class sjoberg_geodesic
{
    sjoberg_geodesic() {}

    static int sign_C(CT const& alphaj)
    {
        CT const c0 = 0;
        CT const c2 = 2;
        CT const pi = math::pi<CT>();
        CT const pi_half = pi / c2;

        return (pi_half < alphaj && alphaj < pi) || (-pi_half < alphaj && alphaj < c0) ? -1 : 1;
    }

public:
    sjoberg_geodesic(CT const& lon, CT const& lat, CT const& alpha, CT const& f)
        : lonj(lon)
        , latj(lat)
        , alphaj(alpha)
    {
        CT const c0 = 0;
        CT const c1 = 1;
        CT const c2 = 2;
        //CT const pi = math::pi<CT>();
        //CT const pi_half = pi / c2;

        one_minus_f = c1 - f;
        e_sqr = f * (c2 - f);

        tan_latj = tan(lat);
        tan_betaj = one_minus_f * tan_latj;
        betaj = atan(tan_betaj);
        sin_betaj = sin(betaj);

        cos_betaj = cos(betaj);
        sin_alphaj = sin(alphaj);
        // Clairaut constant (lower-case in the paper)
        Cj = sign_C(alphaj) * cos_betaj * sin_alphaj;
        Cj_sqr = math::sqr(Cj);
        sqrt_1_Cj_sqr = math::sqrt(c1 - Cj_sqr);

        sign_lon_diff = alphaj >= 0 ? 1 : -1; // || alphaj == -pi ?
        //sign_lon_diff = 1;

        is_on_equator = math::equals(sqrt_1_Cj_sqr, c0);
        is_Cj_zero = math::equals(Cj, c0);

        t0j = c0;
        asin_tj_t0j = c0;

        if (! is_Cj_zero)
        {
            t0j = sqrt_1_Cj_sqr / Cj;
        }

        if (! is_on_equator)
        {
            //asin_tj_t0j = asin(tan_betaj / t0j);
            asin_tj_t0j = asin(tan_betaj * Cj / sqrt_1_Cj_sqr);
        }
    }

    struct vertex_data
    {
        //CT beta0j;
        CT sin_beta0j;
        CT dL0j;
        CT lon0j;
    };

    vertex_data get_vertex_data() const
    {
        CT const c2 = 2;
        CT const pi = math::pi<CT>();
        CT const pi_half = pi / c2;

        vertex_data res;

        if (! is_Cj_zero)
        {
            //res.beta0j = atan(t0j);
            //res.sin_beta0j = sin(res.beta0j);
            res.sin_beta0j = math::sign(t0j) * sqrt_1_Cj_sqr;
            res.dL0j = d_lambda(res.sin_beta0j);
            res.lon0j = lonj + sign_lon_diff * (pi_half - asin_tj_t0j + res.dL0j);
        }
        else
        {
            //res.beta0j = pi_half;
            //res.sin_beta0j = betaj >= 0 ? 1 : -1;
            res.sin_beta0j = 1;
            res.dL0j = 0;
            res.lon0j = lonj;
        }

        return res;
    }

    bool is_sin_beta_ok(CT const& sin_beta) const
    {
        CT const c1 = 1;
        return math::abs(sin_beta / sqrt_1_Cj_sqr) <= c1;
    }

    bool k_diff(CT const& sin_beta,
                CT & delta_k) const
    {
        if (is_Cj_zero)
        {
            delta_k = 0;
            return true;
        }

        // beta out of bounds and not close
        if (! (is_sin_beta_ok(sin_beta)
                || math::equals(math::abs(sin_beta), sqrt_1_Cj_sqr)) )
        {
            return false;
        }

        // NOTE: beta may be slightly out of bounds here but d_lambda handles that
        CT const dLj = d_lambda(sin_beta);
        delta_k = sign_lon_diff * (/*asin_t_t0j*/ - asin_tj_t0j + dLj);

        return true;
    }

    bool lon_diff(CT const& sin_beta, CT const& t,
                  CT & delta_lon) const
    {
        using math::detail::bounded;
        CT const c1 = 1;

        if (is_Cj_zero)
        {
            delta_lon = 0;
            return true;
        }

        CT delta_k = 0;
        if (! k_diff(sin_beta, delta_k))
        {
            return false;
        }

        CT const t_t0j = t / t0j;
        // NOTE: t may be slightly out of bounds here
        CT const asin_t_t0j = asin(bounded(t_t0j, -c1, c1));
        delta_lon = sign_lon_diff * asin_t_t0j + delta_k;

        return true;
    }

    bool k_diffs(CT const& sin_beta, vertex_data const& vd,
                 CT & delta_k_before, CT & delta_k_behind,
                 bool check_sin_beta = true) const
    {
        CT const pi = math::pi<CT>();

        if (is_Cj_zero)
        {
            delta_k_before = 0;
            delta_k_behind = sign_lon_diff * pi;
            return true;
        }

        // beta out of bounds and not close
        if (check_sin_beta
            && ! (is_sin_beta_ok(sin_beta)
                    || math::equals(math::abs(sin_beta), sqrt_1_Cj_sqr)) )
        {
            return false;
        }

        // NOTE: beta may be slightly out of bounds here but d_lambda handles that
        CT const dLj = d_lambda(sin_beta);
        delta_k_before = sign_lon_diff * (/*asin_t_t0j*/ - asin_tj_t0j + dLj);

        // This version require no additional dLj calculation
        delta_k_behind = sign_lon_diff * (pi /*- asin_t_t0j*/ - asin_tj_t0j + vd.dL0j + (vd.dL0j - dLj));

        // [Sjoberg12]
        //CT const dL101 = d_lambda(sin_betaj, vd.sin_beta0j);
        // WARNING: the following call might not work if beta was OoB because only the second argument is bounded
        //CT const dL_01 = d_lambda(sin_beta, vd.sin_beta0j);
        //delta_k_behind = sign_lon_diff * (pi /*- asin_t_t0j*/ - asin_tj_t0j + dL101 + dL_01);

        return true;
    }

    bool lon_diffs(CT const& sin_beta, CT const& t, vertex_data const& vd,
                   CT & delta_lon_before, CT & delta_lon_behind) const
    {
        using math::detail::bounded;
        CT const c1 = 1;
        CT const pi = math::pi<CT>();

        if (is_Cj_zero)
        {
            delta_lon_before = 0;
            delta_lon_behind = sign_lon_diff * pi;
            return true;
        }

        CT delta_k_before = 0, delta_k_behind = 0;
        if (! k_diffs(sin_beta, vd, delta_k_before, delta_k_behind))
        {
            return false;
        }

        CT const t_t0j = t / t0j;
        // NOTE: t may be slightly out of bounds here
        CT const asin_t_t0j = asin(bounded(t_t0j, -c1, c1));
        CT const sign_asin_t_t0j = sign_lon_diff * asin_t_t0j;
        delta_lon_before = sign_asin_t_t0j + delta_k_before;
        delta_lon_behind = -sign_asin_t_t0j + delta_k_behind;

        return true;
    }

    bool lon(CT const& sin_beta, CT const& t, vertex_data const& vd,
             CT & lon_before, CT & lon_behind) const
    {
        using math::detail::bounded;
        CT const c1 = 1;
        CT const pi = math::pi<CT>();

        if (is_Cj_zero)
        {
            lon_before = lonj;
            lon_behind = lonj + sign_lon_diff * pi;
            return true;
        }

        if (! (is_sin_beta_ok(sin_beta)
                || math::equals(math::abs(sin_beta), sqrt_1_Cj_sqr)) )
        {
            return false;
        }

        CT const t_t0j = t / t0j;
        CT const asin_t_t0j = asin(bounded(t_t0j, -c1, c1));
        CT const dLj = d_lambda(sin_beta);
        lon_before = lonj + sign_lon_diff * (asin_t_t0j - asin_tj_t0j + dLj);
        lon_behind = vd.lon0j + (vd.lon0j - lon_before);

        return true;
    }

    CT lon(CT const& delta_lon) const
    {
        return lonj + delta_lon;
    }

    CT lat(CT const& t) const
    {
        // t = tan(beta) = (1-f)tan(lat)
        return atan(t / one_minus_f);
    }

    void vertex(CT & lon, CT & lat) const
    {
        lon = get_vertex_data().lon0j;
        if (! is_Cj_zero)
        {
            lat = sjoberg_geodesic::lat(t0j);
        }
        else
        {
            CT const c2 = 2;
            lat = math::pi<CT>() / c2;
        }
    }

    CT lon_of_equator_intersection() const
    {
        CT const c0 = 0;
        CT const dLj = d_lambda(c0);
        CT const asin_tj_t0j = asin(Cj * tan_betaj / sqrt_1_Cj_sqr);
        return lonj - asin_tj_t0j + dLj;
    }

    CT d_lambda(CT const& sin_beta) const
    {
        return sjoberg_d_lambda_e_sqr<Order>(sin_betaj, sin_beta, Cj, sqrt_1_Cj_sqr, e_sqr);
    }

    // [Sjoberg12]
    /*CT d_lambda(CT const& sin_beta1, CT const& sin_beta2) const
    {
        return sjoberg_d_lambda_e_sqr<Order>(sin_beta1, sin_beta2, Cj, sqrt_1_Cj_sqr, e_sqr);
    }*/

    CT lonj;
    CT latj;
    CT alphaj;

    CT one_minus_f;
    CT e_sqr;

    CT tan_latj;
    CT tan_betaj;
    CT betaj;
    CT sin_betaj;
    CT cos_betaj;
    CT sin_alphaj;
    CT Cj;
    CT Cj_sqr;
    CT sqrt_1_Cj_sqr;

    int sign_lon_diff;

    bool is_on_equator;
    bool is_Cj_zero;

    CT t0j;
    CT asin_tj_t0j;
};

/*!
\brief The intersection of two geodesics as proposed by Sjoberg.
\see See
    - [Sjoberg02] Lars E. Sjoberg, Intersections on the sphere and ellipsoid, 2002
      http://link.springer.com/article/10.1007/s00190-001-0230-9
    - [Sjoberg07] Lars E. Sjoberg, Geodetic intersection on the ellipsoid, 2007
      http://link.springer.com/article/10.1007/s00190-007-0204-7
    - [Sjoberg12] Lars E. Sjoberg, Solutions to the ellipsoidal Clairaut constant
      and the inverse geodetic problem by numerical integration, 2012
      https://www.degruyter.com/view/j/jogs.2012.2.issue-3/v10156-011-0037-4/v10156-011-0037-4.xml
*/
template
<
    typename CT,
    template <typename, bool, bool, bool, bool, bool> class Inverse,
    unsigned int Order = 4
>
class sjoberg_intersection
{
    typedef sjoberg_geodesic<CT, Order> geodesic_type;
    typedef Inverse<CT, false, true, false, false, false> inverse_type;
    typedef typename inverse_type::result_type inverse_result;

    static bool const enable_02 = true;
    static int const max_iterations_02 = 10;
    static int const max_iterations_07 = 20;

public:
    template <typename T1, typename T2, typename Spheroid>
    static inline bool apply(T1 const& lona1, T1 const& lata1,
                             T1 const& lona2, T1 const& lata2,
                             T2 const& lonb1, T2 const& latb1,
                             T2 const& lonb2, T2 const& latb2,
                             CT & lon, CT & lat,
                             Spheroid const& spheroid)
    {
        CT const lon_a1 = lona1;
        CT const lat_a1 = lata1;
        CT const lon_a2 = lona2;
        CT const lat_a2 = lata2;
        CT const lon_b1 = lonb1;
        CT const lat_b1 = latb1;
        CT const lon_b2 = lonb2;
        CT const lat_b2 = latb2;

        inverse_result const res1 = inverse_type::apply(lon_a1, lat_a1, lon_a2, lat_a2, spheroid);
        inverse_result const res2 = inverse_type::apply(lon_b1, lat_b1, lon_b2, lat_b2, spheroid);

        return apply(lon_a1, lat_a1, lon_a2, lat_a2, res1.azimuth,
                     lon_b1, lat_b1, lon_b2, lat_b2, res2.azimuth,
                     lon, lat, spheroid);
    }

    // TODO: Currently may not work correctly if one of the endpoints is the pole
    template <typename Spheroid>
    static inline bool apply(CT const& lon_a1, CT const& lat_a1, CT const& lon_a2, CT const& lat_a2, CT const& alpha_a1,
                             CT const& lon_b1, CT const& lat_b1, CT const& lon_b2, CT const& lat_b2, CT const& alpha_b1,
                             CT & lon, CT & lat,
                             Spheroid const& spheroid)
    {
        // coordinates in radians

        CT const c0 = 0;
        CT const c1 = 1;

        CT const f = formula::flattening<CT>(spheroid);
        CT const one_minus_f = c1 - f;

        geodesic_type geod1(lon_a1, lat_a1, alpha_a1, f);
        geodesic_type geod2(lon_b1, lat_b1, alpha_b1, f);

        // Cj = 1 if on equator <=> sqrt_1_Cj_sqr = 0
        // Cj = 0 if vertical <=> sqrt_1_Cj_sqr = 1

        if (geod1.is_on_equator && geod2.is_on_equator)
        {
            return false;
        }
        else if (geod1.is_on_equator)
        {
            lon = geod2.lon_of_equator_intersection();
            lat = c0;
            return true;
        }
        else if (geod2.is_on_equator)
        {
            lon = geod1.lon_of_equator_intersection();
            lat = c0;
            return true;
        }

        // (lon1 - lon2) normalized to (-180, 180]
        CT const lon1_minus_lon2 = math::longitude_distance_signed<radian>(geod2.lonj, geod1.lonj);

        // vertical segments
        if (geod1.is_Cj_zero && geod2.is_Cj_zero)
        {
            CT const pi = math::pi<CT>();

            // the geodesics are parallel, the intersection point cannot be calculated
            if ( math::equals(lon1_minus_lon2, c0)
              || math::equals(lon1_minus_lon2 + (lon1_minus_lon2 < c0 ? pi : -pi), c0) )
            {
                return false;
            }

            lon = c0;

            // the geodesics intersect at one of the poles
            CT const pi_half = pi / CT(2);
            CT const abs_lat_a1 = math::abs(lat_a1);
            CT const abs_lat_a2 = math::abs(lat_a2);
            if (math::equals(abs_lat_a1, abs_lat_a2))
            {
                lat = pi_half;
            }
            else
            {
                // pick the pole closest to one of the points of the first segment
                CT const& closer_lat = abs_lat_a1 > abs_lat_a2 ? lat_a1 : lat_a2;
                lat = closer_lat >= 0 ? pi_half : -pi_half;
            }

            return true;
        }

        CT lon_sph = 0;

        // Starting tan(beta)
        CT t = 0;

        /*if (geod1.is_Cj_zero)
        {
            CT const k_base = lon1_minus_lon2 + geod2.sign_lon_diff * geod2.asin_tj_t0j;
            t = sin(k_base) * geod2.t0j;
            lon_sph = vertical_intersection_longitude(geod1.lonj, lon_b1, lon_b2);
        }
        else if (geod2.is_Cj_zero)
        {
            CT const k_base = lon1_minus_lon2 - geod1.sign_lon_diff * geod1.asin_tj_t0j;
            t = sin(-k_base) * geod1.t0j;
            lon_sph = vertical_intersection_longitude(geod2.lonj, lon_a1, lon_a2);
        }
        else*/
        {
            // TODO: Consider using betas instead of latitudes.
            //       Some function calls might be saved this way.
            CT tan_lat_sph = 0;
            sjoberg_intersection_spherical_02<CT>::apply_alt(lon_a1, lat_a1, lon_a2, lat_a2,
                                                             lon_b1, lat_b1, lon_b2, lat_b2,
                                                             lon_sph, tan_lat_sph);
            t = one_minus_f * tan_lat_sph; // tan(beta)
        }

        // TODO: no need to calculate atan here if reduced latitudes were used
        //       instead of latitudes above, in sjoberg_intersection_spherical_02
        CT const beta = atan(t);

        if (enable_02 && newton_method(geod1, geod2, beta, t, lon1_minus_lon2, lon, lat))
        {
            return true;
        }

        return converge_07(geod1, geod2, beta, t, lon1_minus_lon2, lon_sph, lon, lat);
    }

private:
    static inline bool newton_method(geodesic_type const& geod1, geodesic_type const& geod2, // in
                                     CT beta, CT t, CT const& lon1_minus_lon2, // in
                                     CT & lon, CT & lat) // out
    {
        CT const c0 = 0;
        CT const c1 = 1;

        CT const e_sqr = geod1.e_sqr;

        CT lon1_diff = 0;
        CT lon2_diff = 0;

        CT abs_dbeta_last = 0;

        // [Sjoberg02] converges faster than solution in [Sjoberg07]
        // Newton-Raphson method
        for (int i = 0; i < max_iterations_02; ++i)
        {
            CT const sin_beta = sin(beta);
            CT const cos_beta = cos(beta);
            CT const cos_beta_sqr = math::sqr(cos_beta);
            CT const G = c1 - e_sqr * cos_beta_sqr;

            CT f1 = 0;
            CT f2 = 0;

            if (!geod1.is_Cj_zero)
            {
                bool is_beta_ok = geod1.lon_diff(sin_beta, t, lon1_diff);

                if (is_beta_ok)
                {
                    CT const H = cos_beta_sqr - geod1.Cj_sqr;
                    f1 = geod1.Cj / cos_beta * math::sqrt(G / H);
                }
                else
                {
                    return false;
                }
            }

            if (!geod2.is_Cj_zero)
            {
                bool is_beta_ok = geod2.lon_diff(sin_beta, t, lon2_diff);

                if (is_beta_ok)
                {
                    CT const H = cos_beta_sqr - geod2.Cj_sqr;
                    f2 = geod2.Cj / cos_beta * math::sqrt(G / H);
                }
                else
                {
                    return false;
                }
            }

            // NOTE: Things may go wrong if the IP is near the vertex
            //   1. May converge into the wrong direction (from the other way around).
            //      This happens when the starting point is on the other side than the vertex
            //   2. During converging may "jump" into the other side of the vertex.
            //      In this case sin_beta/sqrt_1_Cj_sqr and t/t0j is not in [-1, 1]
            //   3. f1-f2 may be 0 which means that the intermediate point is on the vertex
            //      In this case it's not possible to check if this is the correct result

            CT const dbeta_denom = f1 - f2;
            //CT const dbeta_denom = math::abs(f1) + math::abs(f2);

            if (math::equals(dbeta_denom, c0))
            {
                return false;
            }

            // The sign of dbeta is changed WRT [Sjoberg02]
            CT const dbeta = (lon1_minus_lon2 + lon1_diff - lon2_diff) / dbeta_denom;

            CT const abs_dbeta = math::abs(dbeta);
            if (i > 0 && abs_dbeta > abs_dbeta_last)
            {
                // The algorithm is not converging
                // The intersection may be on the other side of the vertex
                return false;
            }
            abs_dbeta_last = abs_dbeta;

            if (math::equals(dbeta, c0))
            {
                // Result found
                break;
            }

            // Because the sign of dbeta is changed WRT [Sjoberg02] dbeta is subtracted here
            beta = beta - dbeta;

            t = tan(beta);
        }

        lat = geod1.lat(t);
        // NOTE: if Cj is 0 then the result is lonj or lonj+180
        lon = ! geod1.is_Cj_zero
                ? geod1.lon(lon1_diff)
                : geod2.lon(lon2_diff);

        return true;
    }

    struct geodesics_type
    {
        geodesics_type(geodesic_type const& g1, geodesic_type const& g2)
            : geod1(g1)
            , geod2(g2)
            , vertex1(geod1.get_vertex_data())
            , vertex2(geod2.get_vertex_data())
        {}

        geodesic_type const& geod1;
        geodesic_type const& geod2;
        typename geodesic_type::vertex_data vertex1;
        typename geodesic_type::vertex_data vertex2;
    };

    struct converge_07_result
    {
        converge_07_result()
            : lon1(0), lon2(0), k1_diff(0), k2_diff(0), t1(0), t2(0)
        {}

        CT lon1, lon2;
        CT k1_diff, k2_diff;
        CT t1, t2;
    };

    static inline bool converge_07(geodesic_type const& geod1, geodesic_type const& geod2,
                                   CT beta, CT t,
                                   CT const& lon1_minus_lon2, CT const& lon_sph,
                                   CT & lon, CT & lat)
    {
        //CT const c0 = 0;
        //CT const c1 = 1;
        //CT const c2 = 2;
        //CT const pi = math::pi<CT>();

        geodesics_type geodesics(geod1, geod2);
        converge_07_result result;

        // calculate first pair of longitudes
        if (!converge_07_step_one(CT(sin(beta)), t, lon1_minus_lon2, geodesics, lon_sph, result, false))
        {
            return false;
        }

        int t_direction = 0;

        CT lon_diff_prev = math::longitude_difference<radian>(result.lon1, result.lon2);

        // [Sjoberg07]
        for (int i = 2; i < max_iterations_07; ++i)
        {
            // pick t candidates from previous result based on dir
            CT t_cand1 = result.t1;
            CT t_cand2 = result.t2;
            // if direction is 0 the closer one is the first
            if (t_direction < 0)
            {
                t_cand1 = (std::min)(result.t1, result.t2);
                t_cand2 = (std::max)(result.t1, result.t2);
            }
            else if (t_direction > 0)
            {
                t_cand1 = (std::max)(result.t1, result.t2);
                t_cand2 = (std::min)(result.t1, result.t2);
            }
            else
            {
                t_direction = t_cand1 < t_cand2 ? -1 : 1;
            }

            CT t1 = t;
            CT beta1 = beta;
            // check if the further calculation is needed
            if (converge_07_update(t1, beta1, t_cand1))
            {
                break;
            }

            bool try_t2 = false;
            converge_07_result result_curr;
            if (converge_07_step_one(CT(sin(beta1)), t1, lon1_minus_lon2, geodesics, lon_sph, result_curr))
            {
                CT const lon_diff1 = math::longitude_difference<radian>(result_curr.lon1, result_curr.lon2);
                if (lon_diff_prev > lon_diff1)
                {
                    t = t1;
                    beta = beta1;
                    lon_diff_prev = lon_diff1;
                    result = result_curr;
                }
                else if (t_cand1 != t_cand2)
                {
                    try_t2 = true;
                }
                else
                {
                    // the result is not fully correct but it won't be more accurate
                    break;
                }
            }
            // ! converge_07_step_one
            else
            {
                if (t_cand1 != t_cand2)
                {
                    try_t2 = true;
                }
                else
                {
                    return false;
                }
            }

            if (try_t2)
            {
                CT t2 = t;
                CT beta2 = beta;
                // check if the further calculation is needed
                if (converge_07_update(t2, beta2, t_cand2))
                {
                    break;
                }

                if (! converge_07_step_one(CT(sin(beta2)), t2, lon1_minus_lon2, geodesics, lon_sph, result_curr))
                {
                    return false;
                }

                CT const lon_diff2 = math::longitude_difference<radian>(result_curr.lon1, result_curr.lon2);
                if (lon_diff_prev > lon_diff2)
                {
                    t_direction *= -1;
                    t = t2;
                    beta = beta2;
                    lon_diff_prev = lon_diff2;
                    result = result_curr;
                }
                else
                {
                    // the result is not fully correct but it won't be more accurate
                    break;
                }
            }
        }

        lat = geod1.lat(t);
        lon = ! geod1.is_Cj_zero ? result.lon1 : result.lon2;
        math::normalize_longitude<radian>(lon);

        return true;
    }

    static inline bool converge_07_update(CT & t, CT & beta, CT const& t_new)
    {
        CT const c0 = 0;

        CT const beta_new = atan(t_new);
        CT const dbeta = beta_new - beta;
        beta = beta_new;
        t = t_new;

        return math::equals(dbeta, c0);
    }

    static inline CT const& pick_t(CT const& t1, CT const& t2, int direction)
    {
        return direction < 0 ? (std::min)(t1, t2) : (std::max)(t1, t2);
    }

    static inline bool converge_07_step_one(CT const& sin_beta,
                                            CT const& t,
                                            CT const& lon1_minus_lon2,
                                            geodesics_type const& geodesics,
                                            CT const& lon_sph,
                                            converge_07_result & result,
                                            bool check_sin_beta = true)
    {
        bool ok = converge_07_one_geod(sin_beta, t, geodesics.geod1, geodesics.vertex1, lon_sph,
                                       result.lon1, result.k1_diff, check_sin_beta)
               && converge_07_one_geod(sin_beta, t, geodesics.geod2, geodesics.vertex2, lon_sph,
                                       result.lon2, result.k2_diff, check_sin_beta);

        if (!ok)
        {
            return false;
        }

        CT const k = lon1_minus_lon2 + result.k1_diff - result.k2_diff;

        // get 2 possible ts one lesser and one greater than t
        // t1 is the closer one
        calc_ts(t, k, geodesics.geod1, geodesics.geod2, result.t1, result.t2);

        return true;
    }

    static inline bool converge_07_one_geod(CT const& sin_beta, CT const& t,
                                            geodesic_type const& geod,
                                            typename geodesic_type::vertex_data const& vertex,
                                            CT const& lon_sph,
                                            CT & lon, CT & k_diff,
                                            bool check_sin_beta)
    {
        using math::detail::bounded;
        CT const c1 = 1;

        CT k_diff_before = 0;
        CT k_diff_behind = 0;

        bool is_beta_ok = geod.k_diffs(sin_beta, vertex, k_diff_before, k_diff_behind, check_sin_beta);

        if (! is_beta_ok)
        {
            return false;
        }

        CT const asin_t_t0j = ! geod.is_Cj_zero ? asin(bounded(t / geod.t0j, -c1, c1)) : 0;
        CT const sign_asin_t_t0j = geod.sign_lon_diff * asin_t_t0j;

        CT const lon_before = geod.lonj + sign_asin_t_t0j + k_diff_before;
        CT const lon_behind = geod.lonj - sign_asin_t_t0j + k_diff_behind;

        CT const lon_dist_before = math::longitude_distance_signed<radian>(lon_before, lon_sph);
        CT const lon_dist_behind = math::longitude_distance_signed<radian>(lon_behind, lon_sph);
        if (math::abs(lon_dist_before) <= math::abs(lon_dist_behind))
        {
            k_diff = k_diff_before;
            lon = lon_before;
        }
        else
        {
            k_diff = k_diff_behind;
            lon = lon_behind;
        }

        return true;
    }

    static inline void calc_ts(CT const& t, CT const& k,
                               geodesic_type const& geod1, geodesic_type const& geod2,
                               CT & t1, CT& t2)
    {
        CT const c1 = 1;
        CT const c2 = 2;

        CT const K = sin(k);

        BOOST_GEOMETRY_ASSERT(!geod1.is_Cj_zero || !geod2.is_Cj_zero);
        if (geod1.is_Cj_zero)
        {
            t1 = K * geod2.t0j;
            t2 = -t1;
        }
        else if (geod2.is_Cj_zero)
        {
            t1 = -K * geod1.t0j;
            t2 = -t1;
        }
        else
        {
            CT const A = math::sqr(geod1.t0j) + math::sqr(geod2.t0j);
            CT const B = c2 * geod1.t0j * geod2.t0j * math::sqrt(c1 - math::sqr(K));

            CT const K_t01_t02 = K * geod1.t0j * geod2.t0j;
            CT const D1 = math::sqrt(A + B);
            CT const D2 = math::sqrt(A - B);
            CT const t_new1 = K_t01_t02 / D1;
            CT const t_new2 = K_t01_t02 / D2;
            CT const t_new3 = -t_new1;
            CT const t_new4 = -t_new2;

            // Pick 2 nearest t_new, one greater and one lesser than current t
            CT const abs_t_new1 = math::abs(t_new1);
            CT const abs_t_new2 = math::abs(t_new2);
            CT const abs_t_max = (std::max)(abs_t_new1, abs_t_new2);
            t1 = -abs_t_max; // lesser
            t2 = abs_t_max; // greater
            if (t1 < t)
            {
                if (t_new1 < t && t_new1 > t1)
                    t1 = t_new1;
                if (t_new2 < t && t_new2 > t1)
                    t1 = t_new2;
                if (t_new3 < t && t_new3 > t1)
                    t1 = t_new3;
                if (t_new4 < t && t_new4 > t1)
                    t1 = t_new4;
            }
            if (t2 > t)
            {
                if (t_new1 > t && t_new1 < t2)
                    t2 = t_new1;
                if (t_new2 > t && t_new2 < t2)
                    t2 = t_new2;
                if (t_new3 > t && t_new3 < t2)
                    t2 = t_new3;
                if (t_new4 > t && t_new4 < t2)
                    t2 = t_new4;
            }
        }

        // the first one is the closer one
        if (math::abs(t - t2) < math::abs(t - t1))
        {
            std::swap(t2, t1);
        }
    }

    static inline CT fj(CT const& cos_beta, CT const& cos2_beta, CT const& Cj, CT const& e_sqr)
    {
        CT const c1 = 1;
        CT const Cj_sqr = math::sqr(Cj);
        return Cj / cos_beta * math::sqrt((c1 - e_sqr * cos2_beta) / (cos2_beta - Cj_sqr));
    }

    /*static inline CT vertical_intersection_longitude(CT const& ip_lon, CT const& seg_lon1, CT const& seg_lon2)
    {
        CT const c0 = 0;
        CT const lon_2 = ip_lon > c0 ? ip_lon - pi : ip_lon + pi;

        return (std::min)(math::longitude_difference<radian>(ip_lon, seg_lon1),
                          math::longitude_difference<radian>(ip_lon, seg_lon2))
            <=
               (std::min)(math::longitude_difference<radian>(lon_2, seg_lon1),
                          math::longitude_difference<radian>(lon_2, seg_lon2))
            ? ip_lon : lon_2;
    }*/
};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP


/*
 *  Distributed under the MIT License (See accompanying file /LICENSE )
 */
#include <doctest/doctest.h>  // for ResultBuilder

#include <array>                             // for operator==
#include <boost/multiprecision/cpp_int.hpp>  // for cpp_int
#include <ostream>                           // for operator<<
#include <tuple>                             // for tuple
#include <type_traits>                       // for move

#include "projgeom/ck_plane.hpp"              // for check_sine...
#include "projgeom/common_concepts.h"         // for Value_type
#include "projgeom/euclid_plane.hpp"          // for uc_point, Ar
#include "projgeom/euclid_plane_measure.hpp"  // for quadrance
#include "projgeom/fractions.hpp"             // for operator*
#include "projgeom/pg_common.hpp"             // for sq, cross
#include "projgeom/pg_line.hpp"               // for meet
#include "projgeom/pg_object.hpp"             // for operator*
#include "projgeom/pg_point.hpp"              // for pg_point
#include "projgeom/proj_plane.hpp"            // for coincident
#include "projgeom/proj_plane_measure.hpp"    // for R
// #include <iostream>

using namespace fun;

static const auto Zero = doctest::Approx(0).epsilon(0.01);

/**
 * @brief
 *
 * @param[in] a
 * @return true
 * @return false
 */
template <typename T> inline auto ApproxZero(const T& a) -> bool {
    return a[0] == Zero && a[1] == Zero && a[2] == Zero;
}

/**
 * @brief
 *
 * @tparam T
 * @param[in] triangle
 */
template <Projective_plane_prim2 P> void chk_euclid(const Triple<P>& triangle) {
    auto trilateral = tri_dual(triangle);

    const auto& [a1, a2, a3] = triangle;
    const auto& [l1, l2, l3] = trilateral;

    // using P = decltype(a1);
    using L = decltype(l1);
    using K = Value_type<P>;
    static_assert(Projective_plane_prim2<L>);
    static_assert(ring<K>);

    // auto zero = std::array<K, 3> {0, 0, 0};

    auto [t1, t2, t3] = tri_altitude(triangle);

    auto t4 = harm_conj(t1, t2, t3);
    auto o = orthocenter(triangle);
    auto tau = reflect(l1);
    auto Q = tri_quadrance(triangle);
    auto S = tri_spread(trilateral);

    auto [m12, m23, m13] = tri_midpoint(triangle);

    auto mt1 = a1 * m23;
    auto mt2 = a2 * m13;
    auto mt3 = a3 * m12;

    const auto& [q1, q2, q3] = Q;
    const auto& [s1, s2, s3] = S;

    auto tqf = sq(q1 + q2 + q3) - 2 * (q1 * q1 + q2 * q2 + q3 * q3);
    auto tsf = sq(s1 + s2 + s3) - 2 * (s1 * s1 + s2 * s2 + s3 * s3) - 4 * s1 * s2 * s3;
    auto c3 = sq(q1 + q2 - q3) / (4 * q1 * q2);

    auto a3p = plucker(3, a1, 4, a2);
    auto q1p = quadrance(a2, a3p);
    auto q2p = quadrance(a1, a3p);
    auto q3p = quadrance(a1, a2);
    auto tqf2 = Ar(q1p, q2p, q3p);  // get 0

    if constexpr (Integral<K>) {
        CHECK(!is_parallel(l1, l2));
        CHECK(!is_parallel(l2, l3));
        CHECK(is_perpendicular(t1, l1));
        CHECK(spread(t1, l1) == K(1));
        CHECK(coincident(t1 * t2, t3));
        CHECK(coincident(t1 * t2, t3, t4));
        CHECK(R(t1, t2, t3, t4) == K(-1));
        CHECK(o == t2 * t3);
        CHECK(tau(tau(a1)) == a1);
        CHECK(spread(l1, l1) == K(0));
        CHECK(quadrance(a1, a1) == K(0));
        CHECK(check_sine_law(Q, S));
        CHECK(check_sine_law(S, Q));
        CHECK(coincident(mt1 * mt2, mt3));
        // CHECK(cross_s(l1, l2) == c3);
        CHECK((c3 + s3) == K(1));
        CHECK(tqf == Ar(q1, q2, q3));
        CHECK(tsf == K(0));
        CHECK(tqf2 == K(0));
        // auto o2 = orthocenter(
        //               std::tuple {std::move(o), std::move(a2),
        //               std::move(a3)});
        // CHECK(a1 == o2);
    } else {
        CHECK(cross2(l1, l2) != Zero);
        CHECK(cross2(l2, l3) != Zero);
        CHECK(dot1(t1, l1) == Zero);
        CHECK(spread(t1, l1) - 1 == Zero);
        CHECK(t1.dot(t2 * t3) == Zero);
        CHECK(R(t1, t2, t3, t4) + 1 == Zero);
        CHECK(ApproxZero(cross(meet(t2, t3), o)));
        CHECK(ApproxZero(cross(tau(tau(a1)), a1)));
        CHECK(mt1.dot(mt2 * mt3) == Zero);
        CHECK(spread(l1, l1) == Zero);
        CHECK(quadrance(a1, a1) == Zero);
        CHECK(angle(l1, l1) == Zero);
        CHECK(distance(a1, a1) == Zero);
        // CHECK(cross_s(l1, l2) == doctest::Approx(c3).epsilon(0.01));
        CHECK((c3 + s3) - 1 == Zero);
        CHECK(tqf - Ar(q1, q2, q3) == Zero);
        CHECK(tsf == Zero);
        CHECK(tqf2 == Zero);
        // CHECK(ApproxEqual(a1, orthocenter(std::tuple{std::move(o), // not
        // quite accurate
        //                     std::move(a2), std::move(a3)})));
    }
}

template <typename T> void chk_cyclic(const T& quadangle) {
    auto& [u1, u2, u3, u4] = quadangle;

    auto q12 = quadrance(u1, u2);
    auto q23 = quadrance(u2, u3);
    auto q34 = quadrance(u3, u4);
    auto q14 = quadrance(u1, u4);
    auto q24 = quadrance(u2, u4);
    auto q13 = quadrance(u1, u3);

    using P = decltype(u1);
    using K = Value_type<P>;

    if constexpr (Integral<K>) {
        auto okay = Ptolemy(std::tuple{std::move(q12), std::move(q23), std::move(q34),
                                       std::move(q14), std::move(q24), std::move(q13)});
        CHECK(okay);
    } else {
        auto t = Ar(q12 * q34, q23 * q14, q13 * q24);
        CHECK(t == Zero);
    }
}

TEST_CASE("Euclid plane (cpp_int)") {
    using boost::multiprecision::cpp_int;

    auto a1 = pg_point<cpp_int>{1, 3, 1};
    auto a2 = pg_point<cpp_int>{4, 2, 1};
    auto a3 = pg_point<cpp_int>{4, -3, 1};

    auto triangle = std::tuple{std::move(a1), std::move(a2), std::move(a3)};
    chk_euclid(triangle);
}

TEST_CASE("Euclid plane (floating point)") {
    auto a1 = pg_point{1., 3., 1.};
    auto a2 = pg_point{4., 2., 1.};
    auto a3 = pg_point{4., -3., 1.};

    auto triangle = std::tuple{std::move(a1), std::move(a2), std::move(a3)};
    chk_euclid(triangle);
}

TEST_CASE("Euclid Cyclic Points (cpp_int)") {
    using boost::multiprecision::cpp_int;
    using P = pg_point<cpp_int>;

    auto u1 = uc_point<P>(1, 0);
    auto u2 = uc_point<P>(3, 4);
    auto u3 = uc_point<P>(-1, 2);
    auto u4 = uc_point<P>(0, 1);

    auto quadangle = std::tuple{std::move(u1), std::move(u2), std::move(u3), std::move(u4)};
    chk_cyclic(quadangle);
}

TEST_CASE("Euclid Cyclic Points (double)") {
    using P = pg_point<double>;

    auto u1 = uc_point<P>(1, 0);
    auto u2 = uc_point<P>(3, 4);
    auto u3 = uc_point<P>(-1, 2);
    auto u4 = uc_point<P>(0, 1);

    auto quadangle = std::tuple{std::move(u1), std::move(u2), std::move(u3), std::move(u4)};
    chk_cyclic(quadangle);
}


///////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::location_scale::log_unnormalized_pdf.hpp //
//                                                                               //
//  (C) Copyright 2009 Erwann Rogard                                             //
//  Use, modification and distribution are subject to the                        //
//  Boost Software License, Version 1.0. (See accompanying file                  //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)             //
///////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_LOCATION_SCALE_DERIVATIVE_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_LOCATION_SCALE_DERIVATIVE_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#include <boost/concept/assert.hpp>
#include <boost/statistics/detail/distribution_common/meta/value.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/location_scale.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace toolkit{

    template<typename Z,typename T>
    T
    derivative_log_unnormalized_pdf(
    	const location_scale_distribution<Z>& d,
        const T& x
    ){
        typedef location_scale_distribution<Z> dist_;

        T z = (x-d.mu())/d.sigma();
        T result = derivative_log_unnormalized_pdf(d.z(),z);
        result /= d.sigma(); // = dz/dx
        return result;
    }

}// toolkit
}// distribution
}// detail
}// statistics
}// boost

#endif


#include "parametrization/parametrization.hpp"

#include <vector>
#include <cmath>
#include <chrono>
#include <Eigen/IterativeLinearSolvers>

using namespace Eigen;
using namespace std;

typedef Triplet<double> Td;

vector<MatrixXd> parametrize(vector<const MatrixXd*> &Vs, vector<const MatrixXi*> &Ts) {
  cout << "Started parametrization:..." << flush;
  auto start = chrono::high_resolution_clock::now();

  vector<MatrixXd> Us;
  Us.reserve(Vs.size());
  for(int i = 0; i < Vs.size(); i++)
    Us.emplace_back(parametrize(*Vs[i], *Ts[i]));

  auto finish = chrono::high_resolution_clock::now();
  cout << "done: " << chrono::duration<double>(finish-start).count() << " s" << endl;

  return Us;
}

MatrixXd parametrize(const MatrixXd &V, const MatrixXi &T) {
  const int np = T.rows(), n = V.rows();

  pair<int,int> diam = approximateDiameter(V);

  // List of triplets for sparse matrix creation
  vector<Td> ta, tb, tu;
  ta.reserve(np * 12);

  // For each triangle
  for (int i = 0; i < np; i++) {
    // Calculate x,y
    // First vertex as (0,0), second as (s,0), third as t(cos(a), sin(a))
    RowVectorXd p[3];
    double x[3], y[3];

    p[0] = V.row(T(i, 0));
    p[1] = V.row(T(i, 1));
    p[2] = V.row(T(i, 2));

    double a = angleBetweenSides(p[1] - p[0], p[2] - p[0]);
    double s = (p[1] - p[0]).norm();
    double t = (p[2] - p[0]).norm();

    x[0] = y[0] = 0;
    x[1] = s, y[1] = 0;
    x[2] = t * cos(a), y[2] = t * sin(a);

    double d = sqrt((x[0] * y[1] - y[0] * x[1]) + (x[1] * y[2] - y[1] * x[2]) + (x[2] * y[0] - y[2] * x[0]));
//    d=1;

    // Calculate W
    double Wr[3], Wi[3];
    Wr[0] = (x[2] - x[1]) / d, Wi[0] = (y[2] - y[1]) / d;
    Wr[1] = (x[0] - x[2]) / d, Wi[1] = (y[0] - y[2]) / d;
    Wr[2] = (x[1] - x[0]) / d, Wi[2] = (y[1] - y[0]) / d;

    // Push values to A and B
    emplaceAB(ta, tb, i, T(i,0), n, np, Wr[0], Wi[0], diam);
    emplaceAB(ta, tb, i, T(i,1), n, np, Wr[1], Wi[1], diam);
    emplaceAB(ta, tb, i, T(i,2), n, np, Wr[2], Wi[2], diam);
  }

  // Build A and B
  SparseMatrix<double> A(2 * np, 2 * (n - 2));
  SparseMatrix<double> B(2 * np, 4);
  A.setFromTriplets(ta.begin(), ta.end());
  ta.clear();
  B.setFromTriplets(tb.begin(), tb.end());
  tb.clear();

  // Build up
  VectorXd up(4);
  up(0) = 0;
  up(1) = 1;
  up(2) = 0;
  up(3) = 1;

  VectorXd b = - B * up;

  // Solve least squares using QR decomposition
  // A.makeCompressed();
  // SparseQR<SparseMatrix<double>, COLAMDOrdering<int> > solver(A);
  // VectorXd uf = solver.solve(b);

  // Solve least squares using iterative CG
  A.makeCompressed();
  LeastSquaresConjugateGradient<SparseMatrix<double>> solver(A);
  VectorXd uf = solver.solve(b);

  // Join uf and up
  MatrixXd U(n, 2);
  for(int i = 0; i < n; i++) {
    if(i != diam.first and i != diam.second) {
      int ii = i;
      if(i > diam.second) ii--;
      if(i > diam.first) ii--;
      U(i,0) = uf(ii);
      U(i,1) = uf(ii + n - 2);
    }
    else
      U(i,0) = U(i,1) = (i==diam.second);
  }

  return U;
}

double angleBetweenSides(const RowVectorXd &a, const RowVectorXd &b) {
  return acos(a.dot(b) / a.norm() / b.norm());
}

pair<int,int> approximateDiameter(const MatrixXd &V) {
  const int n = V.rows();
  double m = 0;
  pair<int,int> best{0,1};
  for(int i = 0; i < n; i++) {
    for(int j = i+1; j < n; j++) {
      double d = (V.row(i)-V.row(j)).norm();
      if(d>m) {
        m = d;
        best = pair<int,int>{i,j};
      }
    }
  }
  return best;
}

void emplaceAB(vector<Td> &ta, vector<Td> &tb, int i, int j, int n, int np, double wr, double wi, pair<int,int> &diam) {
  if (j != diam.first and j != diam.second) {
    if(j > diam.second) j--;
    if(j > diam.first) j--;
    ta.emplace_back(i, j, wr);
    ta.emplace_back(i + np, j + n - 2, wr);
    ta.emplace_back(i, j + n - 2, -wi);
    ta.emplace_back(i + np, j, wi);
  } else {
    j = (j==diam.second);
    tb.emplace_back(i, j, wr);
    tb.emplace_back(i + np, j + 2, wr);
    tb.emplace_back(i, j + 2, -wi);
    tb.emplace_back(i + np, j, wi);
  }
}


// -------------------------------------------------------------------------------------------------
//                              Copyright 2016 - NumScale SAS
//
//                   Distributed under the Boost Software License, Version 1.0.
//                        See accompanying file LICENSE.txt or copy at
//                            http://www.boost.org/LICENSE_1_0.txt
// -------------------------------------------------------------------------------------------------

#include <simd_bench.hpp>
#include <boost/simd/function/simd/ilogb.hpp>
#include <boost/simd/pack.hpp>

namespace nsb = ns::bench;
namespace bs =  boost::simd;

DEFINE_SIMD_BENCH(simd_ilogb, bs::ilogb);

DEFINE_BENCH_MAIN()
{
  nsb::for_each<simd_ilogb, NS_BENCH_NUMERIC_TYPES>(0, 10);
}


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);
// intentionally remove indentation of snippet
{
Vector3f boxMin(Vector3f::Zero()), boxMax(Vector3f::Ones());
Vector3f p0 = Vector3f::Random(), p1 = Vector3f::Random().cwiseAbs();
// let's check if p0 and p1 are inside the axis aligned box defined by the corners boxMin,boxMax:
cout << "Is (" << p0.transpose() << ") inside the box: "
     << ((boxMin.array()<p0.array()).all() && (boxMax.array()>p0.array()).all()) << endl;
cout << "Is (" << p1.transpose() << ") inside the box: "
     << ((boxMin.array()<p1.array()).all() && (boxMax.array()>p1.array()).all()) << endl;

}
  return 0;
}


// 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.
//
// Written by Jiahu Deng and Peter Gottschling

#include <iostream>
#include <cstdio>
#include <boost/numeric/mtl/detail/dilated_int.hpp>

using namespace std;

struct morton_exception {};

template <typename T>
void test_inc(string s, T dil, typename T::value_type exp_increment)
{
    printf("%s %x, bit mask is %x, negated mask is %x\n", s.c_str(), dil.i, dil.bit_mask, dil.anti_mask);
    if ((++dil).i != exp_increment) throw morton_exception();

    for (typename T::value_type i = 1; i < 6; ++i) {
	T dilated(i);
	if (dilated.undilate() != i) throw morton_exception();

	cout << "dilated(" << i << ") = " << dilated <<  "\n";
	if (dil.i != dilated.i) throw morton_exception();
	// check both pre and post increment
	i & 1 ? ++dil : dil++;
    }

    printf("dilated_zero = %x, dilated_one = %x\n", dil.dilated_zero, dil.dilated_one);

    T dec_dil(6);
    for (typename T::value_type i = 6; i > 0; --i) {
	cout << "dilated(" << i << ") = " << dec_dil <<  "\n";
	if (dec_dil != T(i)) throw morton_exception();

	// check both pre and post decrement
	i & 1 ? --dec_dil : dec_dil--;
    }
}

template <typename T>
void test_plus1(T)
{
    T a(1), b(3), c(4);
    cout << "a = " << a << ", b = " << b << ", a + b = " << a + b << ", c = " << c << "\n";
    if (a + b != c) throw morton_exception();

    cout << "c - b = " << c - b << "\n";
    if (c - b != a) throw morton_exception();
}

template <typename T>
void test_plus2(T)
{
    T a(22), b(33), c(55);
    cout << "a = " << a << ", b = " << b << ", a + b = " << a + b << ", c = " << c << "\n";
    if (a + b != c) throw morton_exception();

    cout << "c - b = " << c - b << "\n";
    if (c - b != a) throw morton_exception();
}

template <typename T>
void test_dilated(string s, T dil, typename T::value_type exp_increment)
{
    test_inc(s, dil, exp_increment);
    test_plus1(dil); test_plus2(dil);
}

int main(int , char**)
{
    using namespace mtl;

    dilated_int<unsigned, dilated::odd_bits<unsigned>::value, true>     dil1;
    test_dilated( "Odd normalized", dil1, 2);

    dilated_int<unsigned, dilated::even_bits<unsigned>::value, true>     dil2;
    test_dilated( "Even normalized", dil2, 1);

    dilated_int<unsigned, dilated::odd_bits<unsigned>::value, false>     dil3;
    test_dilated( "Odd anti-normalized", dil3, 0x55555557);

    dilated_int<unsigned, dilated::even_bits<unsigned>::value, false>     dil4;
    test_dilated( "Even anti-normalized", dil4, 0xaaaaaaab);

    return 0;
}


#include <iostream>
#include <sys/time.h>
#include <Eigen/Core>

#include "celerite/celerite.h"
//#include "celerite/carma.h"
#include "celerite/utils.h"
#include "../include/KF.h"
#include "../include/dsho.h"

// This code benchmarks a single DSHO, celerite vs gpstate

// Timer for the benchmark.
double get_timestamp ()
{
  struct timeval now;
  gettimeofday (&now, NULL);
  return double(now.tv_usec) * 1.0e-6 + double(now.tv_sec);
}

int main (int argc, char* argv[])
{
  srand(42);

  size_t N_max = pow(2, 19);
  if (argc >= 2) N_max = atoi(argv[1]);
  size_t niter = 5;
  if (argc >= 3) niter = atoi(argv[2]);
  size_t niter_celerite = niter;
  if (argc >= 4) niter_celerite = atoi(argv[3]);
  std::cout << "N:" << N_max << " gpstate:" << niter << " celerite:" << niter_celerite << std::endl;

 // Generate some fake data.
  Eigen::VectorXd x = Eigen::VectorXd::Random(N_max),
                  yerr = Eigen::VectorXd::Random(N_max),
                  y, diag;
  yerr.array() *= 0.1;
  yerr.array() += 1.0;
  diag = yerr.array() * yerr.array();
  std::sort(x.data(), x.data() + x.size());
  y = sin(x.array());

  //set up the DSHO parameters
  size_t nterms=3;
  double omega0=1.0;
  double Q = 1.0;
  double varf = 1.0;
  int flag;

  // translate into corresponding CARMA parameters for use in celerite
  Eigen::VectorXd carma_arparams(nterms);
  Eigen::VectorXd carma_maparams(nterms-1);
  carma_arparams << omega0*omega0, omega0/Q, 1.0;
  carma_maparams << 1.0, 0.0;

  Eigen::VectorXd alpha_real, beta_real;
  Eigen::VectorXd alpha_complex_real(1), alpha_complex_imag(1),
                    beta_complex_real(1), beta_complex_imag(1);

  double temp = std::sqrt(4.0*Q*Q - 1.0);
  double S0 = varf* std::pow(Q,-2) * std::sqrt(M_PI) / std::sqrt(2);
  alpha_complex_real(0) = S0 * omega0 * Q;
  alpha_complex_imag(0) = S0 * omega0 * Q/temp;
  beta_complex_real(0) = 0.5*omega0 / Q;
  beta_complex_imag(0) = 0.5*temp*omega0 / Q;

//f = np.sqrt(4.0 * Q**2-1)
 //       return (
 //           S0 * w0 * Q,
 //           S0 * w0 * Q / f,
 //           0.5 * w0 / Q,
 //           0.5 * w0 / Q * f,
 //       )

  // the following pieces of code verify that the log likelihoods are
  // consistent between celerite and gpstate
  celerite::solver::CholeskySolver<double> solver;
  solver.compute(0.0, alpha_real, beta_real, alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag, x, diag);
  double celerite_ll = -0.5*(solver.dot_solve(y) + solver.log_determinant() + x.rows() * log(2.0 * M_PI));
  std::cout << celerite_ll << std::endl;

  gpstate::dsho::DSHOSolver dsho(x,y,yerr,omega0, Q, 1.0);
  double log_likelihood = dsho.KF_log_likelihood();
  std::cout << "logL: " << log_likelihood << std::endl;

  // now benchmark, knowing that we do get consistent log-likelohood values
  double strt;

for (size_t N = 64; N <= N_max; N *= 2) {

  double celerite_time = 0.0;
  double gpstate_time = 0.0;

  if (niter_celerite > 0) {
    //celerite::solver::CholeskySolver<double> solver;
    //solver.compute(0.0, alpha_real, beta_real, alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag, x, diag);
    //celerite_ll = -0.5*(solver.dot_solve(y) + solver.log_determinant() + x.rows() * log(2.0 * M_PI));
    //std::cout << "logL(celerite)" << celerite_ll << std::endl;

    for (size_t i = 0; i < niter_celerite; ++i) {
      strt = get_timestamp();
      solver.compute(0.0, alpha_real, beta_real, alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag, x.head(N), diag.head(N));
      celerite_ll = -0.5*(solver.dot_solve(y.head(N)) + solver.log_determinant() + x.head(N).rows() * log(2.0 * M_PI));
      celerite_time += get_timestamp()-strt;
    }
  }
  if (niter > 0) {
    //gpstate::dsho::DSHOSolver dsho1(x,y,yerr,omega0, Q, 1.0);
    //log_likelihood = dsho1.KF_log_likelihood();
    //std::cout << "logL: " << log_likelihood << std::endl;

    for (size_t i = 0; i < niter; ++i) {
      strt = get_timestamp();
      gpstate::dsho::DSHOSolver dsho(x.head(N),y.head(N),yerr.head(N),omega0, Q, 1.0);
      double log_likelihood = dsho.KF_log_likelihood();
      gpstate_time += get_timestamp()-strt;
      }
    }
   // Print the results.
   if ((niter > 0) & (niter_celerite > 0)){
     std::cout << N;
     std::cout << " ";
     std::cout << celerite_time / niter_celerite;
     std::cout << " ";
     std::cout << gpstate_time / niter;
     std::cout << "\n";
   }
}
}