/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2012 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file adaptiverungekutta.hpp
    \brief Runge-Kutta ODE integration

    Runge Kutta method with adaptive stepsize as described in
    Numerical Recipes in C, Chapter 16.2
*/

#ifndef quantlib_adaptive_runge_kutta_hpp
#define quantlib_adaptive_runge_kutta_hpp

#include <ql/types.hpp>
#include <ql/errors.hpp>
#include <ql/utilities/disposable.hpp>
#include <boost/function.hpp>
#include <vector>
#include <cmath>

namespace QuantLib {

    template <class T = Real>
    class AdaptiveRungeKutta {
      public:
        typedef boost::function<
          Disposable<std::vector<T> >(const Real,
                                      const std::vector<T>&)> OdeFct;
        typedef boost::function<T(const Real, const T)> OdeFct1d;

        /*! The class is constructed with the following inputs:
            - eps       prescribed error for the solution
            - h1        start step size
            - hmin      smallest step size allowed
        */

        AdaptiveRungeKutta(const Real eps=1.0e-6,
                           const Real h1=1.0e-4,
                           const Real hmin=0.0)
        : eps_(eps), h1_(h1), hmin_(hmin),
          a2(0.2), a3(0.3), a4(0.6), a5(1.0), a6(0.875),
          b21(0.2), b31(3.0/40.0), b32(9.0/40.0), b41(0.3), b42(-0.9), b43(1.2),
          b51(-11.0/54.0), b52(2.5), b53(-70.0/27.0), b54(35.0/27.0),
          b61(1631.0/55296.0), b62(175.0/512.0), b63(575.0/13824.0),
          b64(44275.0/110592.0), b65(253.0/4096.0),
          c1(37.0/378.0), c3(250.0/621.0), c4(125.0/594.0), c6(512.0/1771.0),
          dc1(c1-2825.0/27648.0), dc3(c3-18575.0/48384.0),
          dc4(c4-13525.0/55296.0), dc5(-277.0/14336.0), dc6(c6-0.25),
          ADAPTIVERK_MAXSTP(10000), ADAPTIVERK_TINY(1.0E-30),
          ADAPTIVERK_SAFETY(0.9), ADAPTIVERK_PGROW(-0.2),
          ADAPTIVERK_PSHRINK(-0.25), ADAPTIVERK_ERRCON(1.89E-4) {}

        /*! Integrate the ode from \f$ x1 \f$ to \f$ x2 \f$ with
            initial value condition \f$ f(x1)=y1 \f$.

            The ode is given by a function \f$ F: R \times K^n
            \rightarrow K^n \f$ as \f$ f'(x) = F(x,f(x)) \f$, $K=R,
            C$ */
        Disposable<std::vector<T> > operator()(const OdeFct& ode,
                                               const std::vector<T>& y1,
                                               const Real x1,
                                               const Real x2);
        T operator()(const OdeFct1d& ode,
                     const T y1,
                     const Real x1,
                     const Real x2);

    private:
        void rkqs(std::vector<T>& y,
                  const std::vector<T>& dydx,
                  Real& x,
                  const Real htry,
                  const Real eps,
                  const std::vector<Real>& yScale,
                  Real &hdid,
                  Real &hnext,
                  const OdeFct& derivs);
        void rkck(const std::vector<T>& y,
                  const std::vector<T>& dydx,
                  Real x,
                  const Real h,
                  std::vector<T>& yout,
                  std::vector<T>& yerr,
                  const OdeFct& derivs);

        const std::vector<T> yStart_;
        const Real eps_, h1_, hmin_;
        const Real a2,a3,a4,a5,a6,
                   b21,b31,b32,b41,b42,b43,b51,b52,b53,b54,b61,b62,b63,b64,b65,
                   c1,c3,c4,c6,dc1,dc3,dc4,dc5,dc6;
        const double ADAPTIVERK_MAXSTP, ADAPTIVERK_TINY, ADAPTIVERK_SAFETY,
                   ADAPTIVERK_PGROW, ADAPTIVERK_PSHRINK, ADAPTIVERK_ERRCON;
    };

    template<class T>
    Disposable<std::vector<T> > AdaptiveRungeKutta<T>::operator()(
                                                     const OdeFct& ode,
                                                     const std::vector<T>& y1,
                                                     const Real x1,
                                                     const Real x2) {
        Size n = y1.size();
        std::vector<T> y(y1);
        std::vector<Real> yScale(n);
        Real x = x1;
        Real h = h1_* (x1<=x2 ? 1 : -1);
        Real hnext,hdid;

        for (Size nstp=1; nstp<=ADAPTIVERK_MAXSTP; nstp++) {
            std::vector<T> dydx=ode(x,y);
            for (Size i=0;i<n;i++)
                yScale[i] = std::abs(y[i])+std::abs(dydx[i]*h)+ADAPTIVERK_TINY;
            if ((x+h-x2)*(x+h-x1) > 0.0)
                h=x2-x;
            rkqs(y,dydx,x,h,eps_,yScale,hdid,hnext,ode);

            if ((x-x2)*(x2-x1) >= 0.0)
                return y;

            if (std::fabs(hnext) <= hmin_)
                QL_FAIL("Step size (" << hnext << ") too small ("
                        << hmin_ << " min) in AdaptiveRungeKutta");
            h=hnext;
        }
        QL_FAIL("Too many steps (" << ADAPTIVERK_MAXSTP
                << ") in AdaptiveRungeKutta");
    }

    namespace detail {

        template <class T>
        struct OdeFctWrapper {
            typedef typename AdaptiveRungeKutta<T>::OdeFct1d OdeFct1d;
            OdeFctWrapper(const OdeFct1d& ode1d)
            : ode1d_(ode1d) {}
            Disposable<std::vector<T> > operator()(const Real x,
                                                   const std::vector<T>& y) {
                std::vector<T> res(1,ode1d_(x,y[0]));
                return res;
            }
            const OdeFct1d& ode1d_;
        };

    }

    template<class T>
    T AdaptiveRungeKutta<T>::operator()(const OdeFct1d& ode,
                                        const T y1,
                                        const Real x1,
                                        const Real x2) {
        return operator()(detail::OdeFctWrapper<T>(ode),
                          std::vector<T>(1,y1),x1,x2)[0];
    }

    template<class T>
    void AdaptiveRungeKutta<T>::rkqs(std::vector<T>& y,
                                     const std::vector<T>& dydx,
                                     Real& x,
                                     const Real htry,
                                     const Real eps,
                                     const std::vector<Real>& yScale,
                                     Real& hdid,
                                     Real& hnext,
                                     const OdeFct& derivs) {
        Size n=y.size();
        Real errmax,htemp,xnew;
        std::vector<T> yerr(n),ytemp(n);

        Real h=htry;

        for(;;) {
            rkck(y,dydx,x,h,ytemp,yerr,derivs);
            errmax=0.0;
            for (Size i=0;i<n;i++)
                errmax=std::max(errmax,std::abs(yerr[i]/yScale[i]));
            errmax/=eps;
            if (errmax>1.0) {
                htemp=ADAPTIVERK_SAFETY*h*std::pow(errmax,ADAPTIVERK_PSHRINK);
                h = (h>=0.0 ? std::max(htemp,h/10) : std::min(htemp,h/10));
                xnew=x+h;
                if (xnew==x)
                    QL_FAIL("Stepsize (" << xnew
                            << ") underflow in AdaptiveRungeKutta::rkqs");
                continue;
            } else {
                if (errmax>ADAPTIVERK_ERRCON)
                    hnext=ADAPTIVERK_SAFETY*h*std::pow(errmax,ADAPTIVERK_PGROW);
                else
                    hnext=5.0*h;
                x+=(hdid=h);
                for (Size i=0;i<n;i++)
                    y[i]=ytemp[i];
                break;
            }
        }
    }

    template <class T>
    void AdaptiveRungeKutta<T>::rkck(const std::vector<T>& y,
                                     const std::vector<T>& dydx,
                                     Real x,
                                     const Real h,
                                     std::vector<T>& yout,
                                     std::vector<T> &yerr,
                                     const OdeFct& derivs) {

        Size n=y.size();
        std::vector<T> ak2(n),ak3(n),ak4(n),ak5(n),ak6(n),ytemp(n);

        // first step
        for (Size i=0;i<n;i++)
            ytemp[i]=y[i]+b21*h*dydx[i];

        // second step
        ak2=derivs(x+a2*h,ytemp);
        for (Size i=0;i<n;i++)
            ytemp[i]=y[i]+h*(b31*dydx[i]+b32*ak2[i]);

        // third step
        ak3=derivs(x+a3*h,ytemp);
        for (Size i=0;i<n;i++)
            ytemp[i]=y[i]+h*(b41*dydx[i]+b42*ak2[i]+b43*ak3[i]);

        // fourth step
        ak4=derivs(x+a4*h,ytemp);
        for (Size i=0;i<n;i++)
            ytemp[i]=y[i]+h*(b51*dydx[i]+b52*ak2[i]+b53*ak3[i]+b54*ak4[i]);

        // fifth step
        ak5=derivs(x+a5*h,ytemp);
        for (Size i=0;i<n;i++)
            ytemp[i]=y[i]+h*(b61*dydx[i]+b62*ak2[i]+b63*ak3[i]+b64*ak4[i]+b65*ak5[i]);

        // sixth step
        ak6=derivs(x+a6*h,ytemp);
        for (Size i=0;i<n;i++) {
            yout[i]=y[i]+h*(c1*dydx[i]+c3*ak3[i]+c4*ak4[i]+c6*ak6[i]);
            yerr[i]=h*(dc1*dydx[i]+dc3*ak3[i]+dc4*ak4[i]+dc5*ak5[i]+dc6*ak6[i]);
        }
    }

}

#endif


// Copyright (C) 2013  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/python.h>
#include <dlib/statistics.h>

using namespace dlib;
namespace py = pybind11;

typedef std::vector<std::pair<unsigned long,double> > sparse_vect;

struct cca_outputs
{
    matrix<double,0,1> correlations;
    matrix<double> Ltrans;
    matrix<double> Rtrans;
};

cca_outputs _cca1 (
    const std::vector<sparse_vect>& L,
    const std::vector<sparse_vect>& R,
    unsigned long num_correlations,
    unsigned long extra_rank,
    unsigned long q,
    double regularization
)
{
    pyassert(num_correlations > 0 && L.size() > 0 && R.size() > 0 && L.size() == R.size() && regularization >= 0,
        "Invalid inputs");

    cca_outputs temp;
    temp.correlations = cca(L,R,temp.Ltrans,temp.Rtrans,num_correlations,extra_rank,q,regularization);
    return temp;
}

// ----------------------------------------------------------------------------------------

unsigned long sparse_vector_max_index_plus_one (
    const sparse_vect& v
)
{
    return max_index_plus_one(v);
}

matrix<double,0,1> apply_cca_transform (
    const matrix<double>& m,
    const sparse_vect& v
)
{
    pyassert((long)max_index_plus_one(v) <= m.nr(), "Invalid Inputs");
    return sparse_matrix_vector_multiply(trans(m), v);
}

void bind_cca(py::module& m)
{
    py::class_<cca_outputs>(m, "cca_outputs")
        .def_readwrite("correlations", &cca_outputs::correlations)
        .def_readwrite("Ltrans", &cca_outputs::Ltrans)
        .def_readwrite("Rtrans", &cca_outputs::Rtrans);

    m.def("max_index_plus_one", sparse_vector_max_index_plus_one, py::arg("v"),
"ensures    \n\
    - returns the dimensionality of the given sparse vector.  That is, returns a    \n\
      number one larger than the maximum index value in the vector.  If the vector    \n\
      is empty then returns 0.   "
    );

    m.def("apply_cca_transform", apply_cca_transform, py::arg("m"), py::arg("v"),
"requires    \n\
    - max_index_plus_one(v) <= m.nr()    \n\
ensures    \n\
    - returns trans(m)*v    \n\
      (i.e. multiply m by the vector v and return the result)   "
    );

    m.def("cca", _cca1, py::arg("L"), py::arg("R"), py::arg("num_correlations"), py::arg("extra_rank")=5, py::arg("q")=2, py::arg("regularization")=0,
"requires    \n\
    - num_correlations > 0    \n\
    - len(L) > 0     \n\
    - len(R) > 0     \n\
    - len(L) == len(R)    \n\
    - regularization >= 0    \n\
    - L and R must be properly sorted sparse vectors.  This means they must list their  \n\
      elements in ascending index order and not contain duplicate index values.  You can use \n\
      make_sparse_vector() to ensure this is true.  \n\
ensures    \n\
    - This function performs a canonical correlation analysis between the vectors    \n\
      in L and R.  That is, it finds two transformation matrices, Ltrans and    \n\
      Rtrans, such that row vectors in the transformed matrices L*Ltrans and    \n\
      R*Rtrans are as correlated as possible (note that in this notation we    \n\
      interpret L as a matrix with the input vectors in its rows).  Note also that    \n\
      this function tries to find transformations which produce num_correlations    \n\
      dimensional output vectors.    \n\
    - Note that you can easily apply the transformation to a vector using     \n\
      apply_cca_transform().  So for example, like this:     \n\
        - apply_cca_transform(Ltrans, some_sparse_vector)    \n\
    - returns a structure containing the Ltrans and Rtrans transformation matrices    \n\
      as well as the estimated correlations between elements of the transformed    \n\
      vectors.    \n\
    - This function assumes the data vectors in L and R have already been centered    \n\
      (i.e. we assume the vectors have zero means).  However, in many cases it is    \n\
      fine to use uncentered data with cca().  But if it is important for your    \n\
      problem then you should center your data before passing it to cca().   \n\
    - This function works with reduced rank approximations of the L and R matrices.    \n\
      This makes it fast when working with large matrices.  In particular, we use    \n\
      the dlib::svd_fast() routine to find reduced rank representations of the input    \n\
      matrices by calling it as follows: svd_fast(L, U,D,V, num_correlations+extra_rank, q)     \n\
      and similarly for R.  This means that you can use the extra_rank and q    \n\
      arguments to cca() to influence the accuracy of the reduced rank    \n\
      approximation.  However, the default values should work fine for most    \n\
      problems.    \n\
    - The dimensions of the output vectors produced by L*#Ltrans or R*#Rtrans are \n\
      ordered such that the dimensions with the highest correlations come first. \n\
      That is, after applying the transforms produced by cca() to a set of vectors \n\
      you will find that dimension 0 has the highest correlation, then dimension 1 \n\
      has the next highest, and so on.  This also means that the list of estimated \n\
      correlations returned from cca() will always be listed in decreasing order. \n\
    - This function performs the ridge regression version of Canonical Correlation    \n\
      Analysis when regularization is set to a value > 0.  In particular, larger    \n\
      values indicate the solution should be more heavily regularized.  This can be    \n\
      useful when the dimensionality of the data is larger than the number of    \n\
      samples.    \n\
    - A good discussion of CCA can be found in the paper \"Canonical Correlation    \n\
      Analysis\" by David Weenink.  In particular, this function is implemented    \n\
      using equations 29 and 30 from his paper.  We also use the idea of doing CCA    \n\
      on a reduced rank approximation of L and R as suggested by Paramveer S.    \n\
      Dhillon in his paper \"Two Step CCA: A new spectral method for estimating    \n\
      vector models of words\".   "

        );
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "NumericalAlgorithms/Spectral/SwshInterpolation.hpp"

#include <array>
#include <boost/math/special_functions/binomial.hpp>
#include <cmath>
#include <complex>
#include <cstddef>

#include "DataStructures/ComplexDataVector.hpp"
#include "DataStructures/ComplexModalVector.hpp"
#include "DataStructures/DataVector.hpp"
#include "DataStructures/SpinWeighted.hpp"
#include "NumericalAlgorithms/Spectral/SwshCoefficients.hpp"
#include "NumericalAlgorithms/Spectral/SwshCollocation.hpp"
#include "NumericalAlgorithms/Spectral/SwshTransform.hpp"
#include "Utilities/GenerateInstantiations.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/StaticCache.hpp"
#include "Utilities/TMPL.hpp"

/// \cond

namespace Spectral {
namespace Swsh {

SpinWeightedSphericalHarmonic::SpinWeightedSphericalHarmonic(
    const int spin, const size_t l, const int m) noexcept
    : spin_{spin}, l_{l}, m_{m} {
  overall_prefactor_ = 1.0;
  const double double_l = l;
  const double double_m = m;
  const double double_spin = spin;
  if (std::abs(m) > std::abs(spin)) {
    for (size_t i = 0; i < static_cast<size_t>(std::abs(m) - std::abs(spin));
         ++i) {
      const double double_i = i;
      overall_prefactor_ *= (double_l + std::abs(double_m) - double_i) /
                            (double_l - (std::abs(double_spin) + double_i));
    }
  } else if (std::abs(spin) > std::abs(m)) {
    for (size_t i = 0; i < static_cast<size_t>(std::abs(spin) - std::abs(m));
         ++i) {
      const double double_i = i;
      overall_prefactor_ *= (double_l - (std::abs(double_m) + double_i)) /
                            (double_l + std::abs(double_spin) - double_i);
    }
  }
  // if neither is greater (they are equal), then the prefactor is 1.0
  overall_prefactor_ *= (2.0 * l + 1.0) / (4.0 * M_PI);
  overall_prefactor_ = sqrt(overall_prefactor_);
  overall_prefactor_ *= (m % 2) == 0 ? 1.0 : -1.0;

  // gcc warns about the casts in ways that are impossible to satisfy
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
  if (static_cast<int>(l) < std::abs(spin)) {
    if (spin < 0) {
      r_prefactors_ =
          std::vector<double>(l + static_cast<size_t>(std::abs(spin)) + 1, 0.0);
    }
  } else {
    // the casts in the reserve are in correct order, but clang-format
    // erroneously requests a change
    // NOLINTNEXTLINE(misc-misplaced-widening-cast)
    r_prefactors_.reserve(static_cast<size_t>(static_cast<int>(l) - spin + 1));
    for (int r = 0; r <= (static_cast<int>(l) - spin); ++r) {
      if (r + spin - m >= 0 and static_cast<int>(l) - r + m >= 0) {
        r_prefactors_.push_back(
            boost::math::binomial_coefficient<double>(
                static_cast<size_t>(static_cast<int>(l) - spin),
                static_cast<size_t>(r)) *
            boost::math::binomial_coefficient<double>(
                static_cast<size_t>(static_cast<int>(l) + spin),
                static_cast<size_t>(spin - m + r)) *
            (((static_cast<int>(l) - r - spin) % 2) == 0 ? 1.0 : -1.0));
      } else {
        r_prefactors_.push_back(0.0);
      }
    }
  }
#pragma GCC diagnostic pop
}

void SpinWeightedSphericalHarmonic::evaluate(
    const gsl::not_null<ComplexDataVector*> result, const DataVector& theta,
    const DataVector& phi, const DataVector& sin_theta_over_2,
    const DataVector& cos_theta_over_2) const noexcept {
  result->destructive_resize(theta.size());
  *result = 0.0;
  DataVector theta_factor{theta.size()};
  for (int r = 0; r <= (static_cast<int>(l_) - spin_); ++r) {
    if (2 * static_cast<int>(l_) > 2 * r + spin_ - m_) {
      theta_factor = pow(cos_theta_over_2, 2 * r + spin_ - m_) *
                     pow(sin_theta_over_2,
                         2 * static_cast<int>(l_) - (2 * r + spin_ - m_));
    } else if (2 * static_cast<int>(l_) < 2 * r + spin_ - m_) {
      theta_factor = pow(cos_theta_over_2 / sin_theta_over_2,
                         2 * r + spin_ - m_ - 2 * static_cast<int>(l_)) *
                     pow(cos_theta_over_2, 2 * l_);
    } else {
      theta_factor = pow(cos_theta_over_2, 2 * l_);
    }
    *result += gsl::at(r_prefactors_, r) * theta_factor;
  }
  // optimization note: this has not been compared with a complex `exp`
  // function, and it is not obvious which should be faster in practice.
  *result *=
      overall_prefactor_ *
      (std::complex<double>(1.0, 0.0) * cos(static_cast<double>(m_) * phi) +
       std::complex<double>(0.0, 1.0) * sin(static_cast<double>(m_) * phi));
}

ComplexDataVector SpinWeightedSphericalHarmonic::evaluate(
    const DataVector& theta, const DataVector& phi,
    const DataVector& sin_theta_over_2,
    const DataVector& cos_theta_over_2) const noexcept {
  ComplexDataVector result{theta.size(), 0.0};
  evaluate(make_not_null(&result), theta, phi, sin_theta_over_2,
           cos_theta_over_2);
  return result;
}

std::complex<double> SpinWeightedSphericalHarmonic::evaluate(
    const double theta, const double phi) const noexcept {
  std::complex<double> accumulator = 0.0;
  const double cos_theta_over_two = cos(0.5 * theta);
  const double sin_theta_over_two = sin(0.5 * theta);
  double theta_factor;
  for (int r = 0; r <= (static_cast<int>(l_) - spin_); ++r) {
    if (2 * static_cast<int>(l_) > 2 * r + spin_ - m_) {
      theta_factor = pow(cos_theta_over_two, 2 * r + spin_ - m_) *
                     pow(sin_theta_over_two,
                         2 * static_cast<int>(l_) - (2 * r + spin_ - m_));
    } else if (2 * static_cast<int>(l_) < 2 * r + spin_ - m_) {
      theta_factor = pow(cos_theta_over_two / sin_theta_over_two,
                         2 * r + spin_ - m_ - 2 * static_cast<int>(l_)) *
                     pow(cos_theta_over_two, 2 * l_);
    } else {
      theta_factor = pow(cos_theta_over_two, 2 * l_);
    }
    accumulator += gsl::at(r_prefactors_, r) *
                   std::complex<double>(cos(static_cast<double>(m_) * phi),
                                        sin(static_cast<double>(m_) * phi)) *
                   theta_factor;
  }
  accumulator *= overall_prefactor_;
  return accumulator;
}

void SpinWeightedSphericalHarmonic::pup(PUP::er& p) noexcept {
  p | spin_;
  p | l_;
  p | m_;
  p | overall_prefactor_;
  p | r_prefactors_;
}

// A function for indexing a desired element in one of the caches stored
// in `ClenshawRecurrenceConstants`.
// Useful for accessing the `beta_constant`, `alpha_constant`, or
// `alpha_prefactor` recurrence constants.
size_t clenshaw_cache_index(const size_t l_max, const int spin, const int l,
                            const int m) noexcept {
  return goldberg_mode_index(l_max - 2, static_cast<size_t>(l - 2), m) -
         static_cast<size_t>(square(spin));
}

// see the detailed doxygen for `SwshInterpolator` for full mathematical details
// of the recurrence constant computations
template <int Spin>
struct ClenshawRecurrenceConstants {
  ClenshawRecurrenceConstants() = default;

  explicit ClenshawRecurrenceConstants(size_t l_max) noexcept
      : alpha_prefactor{square(l_max + 1) -
                        square(static_cast<size_t>(std::abs(Spin)))},
        alpha_constant{square(l_max + 1) -
                       square(static_cast<size_t>(std::abs(Spin)))},
        beta_constant{square(l_max + 1) -
                      square(static_cast<size_t>(std::abs(Spin)))},
        harmonic_at_l_min_prefactors{2 * l_max + 1},
        harmonic_at_l_min_plus_one_recurrence_prefactors{2 * l_max + 1},
        harmonic_m_recurrence_prefactors{2 * l_max + 1} {
    ASSERT(static_cast<int>(l_max) > Spin,
           "l_max must be greater than the spin-weight when computing "
           "ClenshawRecurrenceConstants");
    double l_plus_k;
    double l_min_plus_k;
    double a;
    double b;
    int l_min;
    double prefactor_accumulator;
    lambda.reserve(2 * l_max + 1);
    for (int m = -static_cast<int>(l_max); m <= static_cast<int>(l_max); ++m) {
      a = static_cast<double>(std::abs(Spin + m));
      b = static_cast<double>(std::abs(Spin - m));
      l_min = std::max(std::abs(m), std::abs(Spin));

      // gcc warns about an optimization that doesn't work if we overflow. None
      // of this will overflow provided l_max is not unreasonably high (less
      // than ~10^5 will not overflow).
      for (int l = l_min + 2; l <= static_cast<int>(l_max); ++l) {
        // start caching at 2 greater than the l_min for a given m. Those are
        // the last terms needed by (descending) Clenshaw sum.
        l_plus_k = static_cast<double>(l) - 0.5 * (a + b);
        alpha_prefactor[clenshaw_cache_index(l_max, Spin, l, m)] =
            0.5 * sqrt((2.0 * l + 1.0) * (2.0 * l - 1.0) /
                       (l_plus_k * (l_plus_k + a + b) * (l_plus_k + a) *
                        (l_plus_k + b)));
        alpha_constant[clenshaw_cache_index(l_max, Spin, l, m)] =
            alpha_prefactor[clenshaw_cache_index(l_max, Spin, l, m)] *
            ((square(a) - square(b)) / (2.0 * l - 2.0));
        alpha_prefactor[clenshaw_cache_index(l_max, Spin, l, m)] *= (2.0 * l);
        beta_constant[clenshaw_cache_index(l_max, Spin, l, m)] =
            -sqrt((2.0 * l + 1.0) * (l_plus_k + a - 1.0) *
                  (l_plus_k + b - 1.0) * (l_plus_k - 1.0) *
                  (l_plus_k + a + b - 1.0) /
                  ((2.0 * l - 3.0) * l_plus_k * (l_plus_k + a + b) *
                   (l_plus_k + a) * (l_plus_k + b))) *
            (2.0 * l) / (2.0 * l - 2.0);
      }
      lambda.push_back(Spin >= -m ? 0 : Spin + m);

      // pre-compute the prefactors for the lowest order harmonics for each m
      prefactor_accumulator = 1.0;
      l_min_plus_k = -0.5 * (std::abs(Spin + m) + b) + l_min;
      for (int i = 1; i <= b; ++i) {
        if (l_min_plus_k + a + i > 0.0) {
          prefactor_accumulator *= static_cast<double>(l_min_plus_k + a + i);
        }
        if (l_min_plus_k + i > 0.0) {
          prefactor_accumulator /= static_cast<double>(l_min_plus_k + i);
        }
      }
      prefactor_accumulator =
          sqrt(prefactor_accumulator * (2.0 * l_min + 1.0) / (4.0 * M_PI));
      prefactor_accumulator *=
          ((m + gsl::at(lambda, m + static_cast<int>(l_max))) % 2) == 0 ? 1.0
                                                                        : -1.0;
      // this is the right order of the casts, other orders give the wrong
      // answer NOLINTNEXTLINE(misc-misplaced-widening-cast)
      harmonic_at_l_min_prefactors[static_cast<size_t>(
          m + static_cast<int>(l_max))] = prefactor_accumulator;

      // pre-compute the prefactors for bootstrapping the second-to-lowest order
      // harmonics for each m

      // this is the right order of the casts, other orders give the wrong
      // answer NOLINTNEXTLINE(misc-misplaced-widening-cast)
      harmonic_at_l_min_plus_one_recurrence_prefactors[static_cast<size_t>(
          m + static_cast<int>(l_max))] =
          sqrt((2.0 * (l_min) + 3.0) * (l_min_plus_k + 1.0) *
               (l_min_plus_k + a + b + 1.0) /
               ((2.0 * (l_min) + 1.0) * (l_min_plus_k + a + 1.0) *
                (l_min_plus_k + b + 1.0)));
    }
    // separate loop because we'll need the lambdas entirely populated for this
    // set of prefactors
    int lambda_difference;
    for (int m = -static_cast<int>(l_max); m <= static_cast<int>(l_max); ++m) {
      if (std::abs(m) > std::abs(Spin)) {
        l_min = std::max(std::abs(m), std::abs(Spin));
        a = std::abs(Spin + m);
        b = std::abs(Spin - m);
        l_min_plus_k = -0.5 * (std::abs(Spin + m) + b) + l_min;

        prefactor_accumulator =
            sqrt((2.0 * std::abs(m) + 1.0) * (l_min_plus_k + a + b - 1.0) *
                 (l_min_plus_k + a + b) /
                 ((2.0 * std::abs(m) - 1.0) * (l_min_plus_k + a) *
                  (l_min_plus_k + b)));
        // there is an extra `1` in these expressions to account for the -1 out
        // front of the recurrence relations.
        lambda_difference = gsl::at(lambda, m + static_cast<int>(l_max)) + 1;
        if (m > 0) {
          lambda_difference -= gsl::at(lambda, m - 1 + static_cast<int>(l_max));
        } else {
          lambda_difference -= gsl::at(lambda, m + 1 + static_cast<int>(l_max));
        }
        prefactor_accumulator *= lambda_difference % 2 == 0 ? 1.0 : -1.0;
        // this is the right order of the casts, other orders give the wrong
        // answer NOLINTNEXTLINE(misc-misplaced-widening-cast)
        harmonic_m_recurrence_prefactors[static_cast<size_t>(
            m + static_cast<int>(l_max))] = prefactor_accumulator;
      }
    }
  }

  /// Serialization for Charm++.
  void pup(PUP::er& p) noexcept {  // NOLINT
    p | alpha_prefactor;
    p | alpha_constant;
    p | beta_constant;
    p | lambda;
    p | harmonic_at_l_min_prefactors;
    p | harmonic_at_l_min_plus_one_recurrence_prefactors;
    p | harmonic_m_recurrence_prefactors;
  }

  // Tables are stored in a triangular Goldberg style
  DataVector alpha_prefactor;
  DataVector alpha_constant;
  DataVector beta_constant;
  std::vector<int> lambda;
  DataVector harmonic_at_l_min_prefactors;
  DataVector harmonic_at_l_min_plus_one_recurrence_prefactors;
  DataVector harmonic_m_recurrence_prefactors;
};

// A lazy static cache interface for retrieving `ClenshawRecurrenceConstants`.
template <int Spin>
const ClenshawRecurrenceConstants<Spin>& cached_clenshaw_factors(
    const size_t l_max) noexcept {
  const static auto lazy_clenshaw_cache =
      make_static_cache<CacheRange<0, collocation_maximum_l_max>>(
          [](const size_t local_l_max) noexcept {
            return ClenshawRecurrenceConstants<Spin>{local_l_max};
          });
  return lazy_clenshaw_cache(l_max);
}

SwshInterpolator::SwshInterpolator(const DataVector& theta,
                                   const DataVector& phi,
                                   const size_t l_max) noexcept
    : l_max_{l_max},
      raw_libsharp_coefficient_buffer_{
          size_of_libsharp_coefficient_vector(l_max)},
      raw_goldberg_coefficient_buffer_{square(l_max + 1)} {
  cos_m_phi_ = std::vector<DataVector>(l_max + 1);
  sin_m_phi_ = std::vector<DataVector>(l_max + 1);
  cos_theta_ = cos(theta);
  sin_theta_ = sin(theta);
  cos_theta_over_two_ = cos(0.5 * theta);
  sin_theta_over_two_ = sin(0.5 * theta);
  // evaluate cos(m phi) and sin(m phi) via recurrence
  cos_m_phi_[0] = DataVector{phi.size(), 1.0};
  sin_m_phi_[0] = DataVector{phi.size(), 0.0};
  const DataVector m_phi_beta = sin(phi);
  const DataVector m_phi_alpha = 2.0 * square(sin(0.5 * phi));
  for (size_t m = 1; m <= l_max; ++m) {
    cos_m_phi_[m] = cos_m_phi_[m - 1] - (m_phi_alpha * cos_m_phi_[m - 1] +
                                         m_phi_beta * sin_m_phi_[m - 1]);
    sin_m_phi_[m] = sin_m_phi_[m - 1] - (m_phi_alpha * sin_m_phi_[m - 1] -
                                         m_phi_beta * cos_m_phi_[m - 1]);
  }
}

template <int Spin>
void SwshInterpolator::interpolate(
    const gsl::not_null<SpinWeighted<ComplexDataVector, Spin>*> interpolated,
    const SpinWeighted<ComplexModalVector, Spin>& goldberg_modes) const
    noexcept {
  interpolated->destructive_resize(cos_theta_.size());
  interpolated->data() = 0.0;

  // used only if s=0;
  SpinWeighted<ComplexDataVector, Spin> cached_base_harmonic;

  // used during both recurrence legs
  SpinWeighted<ComplexDataVector, Spin> current_cached_harmonic;
  SpinWeighted<ComplexDataVector, Spin> current_cached_harmonic_l_plus_one;

  // perform the Clenshaw sums over positive m >= 0.
  for (int m = 0; m <= static_cast<int>(l_max_); ++m) {
    if (std::abs(Spin) >= std::abs(m)) {
      direct_evaluation_swsh_at_l_min(make_not_null(&current_cached_harmonic),
                                      m);
      evaluate_swsh_at_l_min_plus_one(
          make_not_null(&current_cached_harmonic_l_plus_one),
          current_cached_harmonic, m);
    } else {
      evaluate_swsh_m_recurrence_at_l_min(
          make_not_null(&current_cached_harmonic), m);
      evaluate_swsh_at_l_min_plus_one(
          make_not_null(&current_cached_harmonic_l_plus_one),
          current_cached_harmonic, m);
    }
    if (Spin == 0 and m == 0) {
      cached_base_harmonic = current_cached_harmonic;
    }
    clenshaw_sum(interpolated, current_cached_harmonic,
                 current_cached_harmonic_l_plus_one, goldberg_modes, m);
  }
  // perform the Clenshaw sums over m < 0.
  for (int m = -1; m >= -static_cast<int>(l_max_); --m) {
    // initialize the recurrence for negative m
    if (m == -1 and Spin == 0) {
      current_cached_harmonic = cached_base_harmonic;
    }
    if (std::abs(Spin) >= std::abs(m)) {
      direct_evaluation_swsh_at_l_min(make_not_null(&current_cached_harmonic),
                                      m);
      evaluate_swsh_at_l_min_plus_one(
          make_not_null(&current_cached_harmonic_l_plus_one),
          current_cached_harmonic, m);
    } else {
      evaluate_swsh_m_recurrence_at_l_min(
          make_not_null(&current_cached_harmonic), m);
      evaluate_swsh_at_l_min_plus_one(
          make_not_null(&current_cached_harmonic_l_plus_one),
          current_cached_harmonic, m);
    }
    clenshaw_sum(interpolated, current_cached_harmonic,
                 current_cached_harmonic_l_plus_one, goldberg_modes, m);
  }
}

template <int Spin>
void SwshInterpolator::interpolate(
    const gsl::not_null<SpinWeighted<ComplexDataVector, Spin>*> interpolated,
    const SpinWeighted<ComplexDataVector, Spin>& libsharp_collocation) const
    noexcept {
  SpinWeighted<ComplexModalVector, Spin> libsharp_modes;
  // this function is 'const', but modifies the internal buffer. The reason to
  // allow it to be 'const' anyways is that no interface makes any assumption
  // about the starting state of the internal buffer; it is kept exclusively to
  // save allocations.
  libsharp_modes.set_data_ref(raw_libsharp_coefficient_buffer_.data(),
                              raw_libsharp_coefficient_buffer_.size());
  swsh_transform(l_max_, 1, make_not_null(&libsharp_modes),
                 libsharp_collocation);
  SpinWeighted<ComplexModalVector, Spin> goldberg_modes;
  libsharp_to_goldberg_modes(make_not_null(&goldberg_modes), libsharp_modes,
                             l_max_);
  interpolate(interpolated, goldberg_modes);
}

template <int Spin>
void SwshInterpolator::direct_evaluation_swsh_at_l_min(
    const gsl::not_null<SpinWeighted<ComplexDataVector, Spin>*> harmonic,
    const int m) const noexcept {
  const auto& clenshaw_factors = cached_clenshaw_factors<Spin>(l_max_);
  // for this evaluation, we don't worry about recurrence because it will only
  // be called for m between -s and +s, and s should always be small. In
  // principle, it is probably true that a more complicated recurrence exists
  // for this case, but would require a bit of derivation work
  harmonic->data() =
      // clang-tidy: this is the right order of the casts, other orders give the
      // wrong answer
      // NOLINTNEXTLINE(misc-misplaced-widening-cast)
      clenshaw_factors.harmonic_at_l_min_prefactors[static_cast<size_t>(
          m + static_cast<int>(l_max_))] *
      (std::complex<double>(1.0, 0.0) * gsl::at(cos_m_phi_, std::abs(m)) +
       std::complex<double>(0.0, 1.0) * (m >= 0 ? 1.0 : -1.0) *
           gsl::at(sin_m_phi_, std::abs(m))) *
      pow(sin_theta_over_two_, static_cast<size_t>(std::abs(Spin + m))) *
      pow(cos_theta_over_two_, static_cast<size_t>(std::abs(Spin - m)));
}

template <int Spin>
void SwshInterpolator::evaluate_swsh_at_l_min_plus_one(
    const gsl::not_null<SpinWeighted<ComplexDataVector, Spin>*> harmonic,
    const SpinWeighted<ComplexDataVector, Spin>& harmonic_at_l_min,
    const int m) const noexcept {
  const auto& clenshaw_factors = cached_clenshaw_factors<Spin>(l_max_);
  const double a = std::abs(Spin + m);
  const double b = std::abs(Spin - m);
  harmonic->data() =
      clenshaw_factors
          // clang-tidy: this is the right order of the casts, other orders give
          // the wrong answer
          // NOLINTNEXTLINE(misc-misplaced-widening-cast)
          .harmonic_at_l_min_plus_one_recurrence_prefactors[static_cast<size_t>(
              m + static_cast<int>(l_max_))] *
      harmonic_at_l_min.data() *
      (a + 1.0 + 0.5 * (a + b + 2.0) * (cos_theta_ - 1.0));
}

template <int Spin>
void SwshInterpolator::evaluate_swsh_m_recurrence_at_l_min(
    const gsl::not_null<SpinWeighted<ComplexDataVector, Spin>*> harmonic,
    const int m) const noexcept {
  const auto& clenshaw_factors = cached_clenshaw_factors<Spin>(l_max_);
  harmonic->data() =
      // this is the right order of the casts, other orders give the wrong
      // answer
      // NOLINTNEXTLINE(misc-misplaced-widening-cast)
      clenshaw_factors.harmonic_m_recurrence_prefactors[static_cast<size_t>(
          m + static_cast<int>(l_max_))] *
      (sin_theta_ / 2.0) * harmonic->data();
  if (m > 0) {
    harmonic->data() *= (std::complex<double>(1.0, 0.0) * cos_m_phi_[1] +
                         std::complex<double>(0.0, 1.0) * sin_m_phi_[1]);
  } else {
    harmonic->data() *= (std::complex<double>(1.0, 0.0) * cos_m_phi_[1] -
                         std::complex<double>(0.0, 1.0) * sin_m_phi_[1]);
  }
}

template <int Spin>
void SwshInterpolator::clenshaw_sum(
    const gsl::not_null<SpinWeighted<ComplexDataVector, Spin>*> interpolation,
    const SpinWeighted<ComplexDataVector, Spin>& l_min_harmonic,
    const SpinWeighted<ComplexDataVector, Spin>& l_min_plus_one_harmonic,
    const SpinWeighted<ComplexModalVector, Spin>& goldberg_modes,
    const int m) const noexcept {
  // Since we need various combinations of the three-term recurrence constants
  // up to two orders higher, we write recurrence results to a cyclic
  // three-element cache
  std::array<ComplexDataVector, 3> recurrence_cache;
  recurrence_cache[2] = ComplexDataVector{interpolation->size(), 0.0};
  recurrence_cache[1] = ComplexDataVector{interpolation->size(), 0.0};
  recurrence_cache[0] = ComplexDataVector{interpolation->size(), 0.0};
  const auto& clenshaw_factors = cached_clenshaw_factors<Spin>(l_max_);

  for (auto l = static_cast<int>(l_max_);
       l > std::max(std::abs(Spin), std::abs(m)); l--) {
    // We want to define some cache_offset so that we can index the three
    // elements of recurrence_cache with indices cache_offset%3,
    // (cache_offset+1)%3, and (cache_offset+2)%3, and so that cache_offset
    // decreases by one on each iteration. The "obvious" way to do this is to
    // choose cache_offset = l - l_max, so that cache_offset starts at zero and
    // then decreases each iteration. However, this gives negative values of
    // cache_offset, and C++ modular arithmetic doesn't behave the way we'd want
    // for that process at negative values. But note that adding any multiple of
    // 3 to the "obvious" value of cache_offset will give identical indexing,
    // and choosing this multiple of 3 large enough (i.e. larger than l_max)
    // guarantees that the new cache_offset is positive for all l. So we choose
    // to add 3*l_max to the "obvious" value of cache_offset; in other words we
    // define cache_offset = l + 2 l_max.
    // In future, if this trick needs to be re-implemented in another use-case,
    // it should instead be factored out into a separate rotating cache utility.
    const int cache_offset = (l + 2 * static_cast<int>(l_max_));
    // gcc warns about the casts in ways that are impossible to satisfy
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
    gsl::at(recurrence_cache, (cache_offset) % 3) =
        goldberg_modes.data()[square(static_cast<size_t>(l)) +
                              static_cast<size_t>(l + m)];
    if (l < static_cast<int>(l_max_)) {
      gsl::at(recurrence_cache, (cache_offset) % 3) +=
          (clenshaw_factors
               .alpha_constant[clenshaw_cache_index(l_max_, Spin, l + 1, m)] +
           cos_theta_ * clenshaw_factors.alpha_prefactor[clenshaw_cache_index(
                            l_max_, Spin, l + 1, m)]) *
          gsl::at(recurrence_cache, (cache_offset + 1) % 3);
    }
    if (l < static_cast<int>(l_max_) - 1) {
      gsl::at(recurrence_cache, (cache_offset) % 3) +=
          clenshaw_factors
              .beta_constant[clenshaw_cache_index(l_max_, Spin, l + 2, m)] *
          gsl::at(recurrence_cache, (cache_offset + 2) % 3);
    }
  }
  const int l_min = std::max(std::abs(Spin), std::abs(m));
  const int cache_offset = (l_min + 2 * static_cast<int>(l_max_));

  if (l_max_ >=
      static_cast<size_t>(std::max(std::abs(Spin), std::abs(m))) + 2) {
    *interpolation +=
        l_min_harmonic *
            goldberg_modes.data()[square(static_cast<size_t>(l_min)) +
                                  static_cast<size_t>(l_min + m)] +
        l_min_plus_one_harmonic *
            gsl::at(recurrence_cache, (cache_offset + 1) % 3) +
        l_min_harmonic * gsl::at(recurrence_cache, (cache_offset + 2) % 3) *
            clenshaw_factors.beta_constant[clenshaw_cache_index(
                l_max_, Spin, std::max(std::abs(Spin), std::abs(m)) + 2, m)];
  } else {
    *interpolation +=
        l_min_harmonic *
            goldberg_modes.data()[square(static_cast<size_t>(l_min)) +
                                  static_cast<size_t>(l_min + m)] +
        l_min_plus_one_harmonic *
            gsl::at(recurrence_cache, (cache_offset + 1) % 3);
  }
#pragma GCC diagnostic pop
}

void SwshInterpolator::pup(PUP::er& p) noexcept {
  p | l_max_;
  p | cos_theta_;
  p | sin_theta_;
  p | cos_theta_over_two_;
  p | sin_theta_over_two_;
  p | sin_m_phi_;
  p | cos_m_phi_;
  p | raw_libsharp_coefficient_buffer_;
  p | raw_goldberg_coefficient_buffer_;
}

#define GET_SPIN(data) BOOST_PP_TUPLE_ELEM(0, data)

#define INTERPOLATION_INSTANTIATION(r, data)                                  \
  template struct ClenshawRecurrenceConstants<GET_SPIN(data)>;                \
  template const ClenshawRecurrenceConstants<GET_SPIN(data)>&                 \
  cached_clenshaw_factors<GET_SPIN(data)>(const size_t l_max) noexcept;       \
  template void SwshInterpolator::interpolate<GET_SPIN(data)>(                \
      const gsl::not_null<SpinWeighted<ComplexDataVector, GET_SPIN(data)>*>   \
          interpolated,                                                       \
      const SpinWeighted<ComplexModalVector, GET_SPIN(data)>& goldberg_modes) \
      const noexcept;                                                         \
  template void SwshInterpolator::interpolate<GET_SPIN(data)>(                \
      const gsl::not_null<SpinWeighted<ComplexDataVector, GET_SPIN(data)>*>   \
          interpolated,                                                       \
      const SpinWeighted<ComplexDataVector, GET_SPIN(data)>&                  \
          libsharp_collocation) const noexcept;                               \
  template void                                                               \
  SwshInterpolator::direct_evaluation_swsh_at_l_min<GET_SPIN(data)>(          \
      const gsl::not_null<SpinWeighted<ComplexDataVector, GET_SPIN(data)>*>   \
          harmonic,                                                           \
      const int m) const noexcept;                                            \
  template void                                                               \
  SwshInterpolator::evaluate_swsh_at_l_min_plus_one<GET_SPIN(data)>(          \
      const gsl::not_null<SpinWeighted<ComplexDataVector, GET_SPIN(data)>*>   \
          harmonic,                                                           \
      const SpinWeighted<ComplexDataVector, GET_SPIN(data)>&                  \
          harmonic_at_l_min,                                                  \
      const int m) const noexcept;                                            \
  template void                                                               \
  SwshInterpolator::evaluate_swsh_m_recurrence_at_l_min<GET_SPIN(data)>(      \
      const gsl::not_null<SpinWeighted<ComplexDataVector, GET_SPIN(data)>*>   \
          harmonic,                                                           \
      const int m) const noexcept;                                            \
  template void SwshInterpolator::clenshaw_sum<GET_SPIN(data)>(               \
      const gsl::not_null<SpinWeighted<ComplexDataVector, GET_SPIN(data)>*>   \
          interpolation,                                                      \
      const SpinWeighted<ComplexDataVector, GET_SPIN(data)>& l_min_harmonic,  \
      const SpinWeighted<ComplexDataVector, GET_SPIN(data)>&                  \
          l_min_plus_one_harmonic,                                            \
      const SpinWeighted<ComplexModalVector, GET_SPIN(data)>& goldberg_modes, \
      const int m) const noexcept;

GENERATE_INSTANTIATIONS(INTERPOLATION_INSTANTIATION, (-2, -1, 0, 1, 2))

#undef INTERPOLATION_INSTANTIATION
#undef GET_SPIN

}  // namespace Swsh
}  // namespace Spectral
/// \endcond


#include "multi_cg/multi_cg.hpp"

#include <Eigen/Core>

#include <iostream>

using namespace Eigen;

struct BlockVector;

struct BlockVector {
    MatrixXcd vec;

    typedef std::complex<double> value_type;

    void block_axpy(std::vector<std::complex<double>> alphas, BlockVector const &X, size_t num) {
        DiagonalMatrix<std::complex<double>,Dynamic,Dynamic> D = Map<VectorXcd>(alphas.data(), num).asDiagonal();
        vec.leftCols(num) += X.vec.leftCols(num) * D;
    }

    void block_axpy_scatter(std::vector<std::complex<double>> alphas, BlockVector const &X, std::vector<size_t> ids) {
        for (size_t i = 0; i < ids.size(); ++i) {
            vec.col(ids[i]) += alphas[i] * X.vec.col(i);
        }
    }

    // rhos[i] = dot(X[i], Y[i])
    void block_dot(BlockVector const &Y, std::vector<std::complex<double>> &rhos, size_t num) {
        VectorXcd result = (vec.leftCols(num).adjoint() * Y.vec.leftCols(num)).diagonal();
        VectorXcd::Map(rhos.data(), result.size()) = result;
    }

    // X[:, i] = Z[:, i] + alpha[i] * X[:, i] for i < num_unconverged
    void block_xpby(BlockVector const &Z, std::vector<std::complex<double>> alphas, size_t num) {
        DiagonalMatrix<std::complex<double>,Dynamic,Dynamic> D = Map<VectorXcd>(alphas.data(), num).asDiagonal();
        vec.leftCols(num) = Z.vec.leftCols(num) + vec.leftCols(num) * D;
    }

    void copy(BlockVector const &X, size_t num) {
        vec.leftCols(num) = X.vec.leftCols(num);
    }

    void fill(std::complex<double> val) {
        vec.fill(val);
    }

    auto cols() {
        return vec.cols();
    }

    void repack(std::vector<size_t> const &ids) {
        for (size_t i = 0; i < ids.size(); ++i) {
            auto j = ids[i];
            if (j != i) {
                vec.col(i) = vec.col(j);
            }
        }
    }
};

// This is a linear but special operator A(X)
// producing AX + XD where D_ii = shifts[i] is a diagonal matrix.
// So column-wise it performs (A + shift[i])X[:, i]
// the multiply function basically does a gemv on every column with a different shift
// so alpha * A(X) + beta * Y.
struct PosDefMatrixShifted {
    DiagonalMatrix<double, Dynamic, Dynamic> A;
    VectorXd shifts;

    void multiply(double alpha, BlockVector const &u, double beta, BlockVector &v, size_t num) {
        v.vec.leftCols(num) = alpha * A * u.vec.leftCols(num) + alpha * u.vec.leftCols(num) * shifts.head(num).asDiagonal() + beta * v.vec.leftCols(num);
    }

    void repack(std::vector<size_t> const &ids) {
        for (size_t i = 0; i < ids.size(); ++i) {
            auto j = ids[i];
            if (j != i) {
                shifts[i] = shifts[j];
            }
        }
    }
};

struct IdentityPreconditioner {
    void apply(BlockVector &C, BlockVector const &B) {
        C = B;
    }
    void repack(std::vector<size_t> const &ids) {
        // nothing to do;
    }
};

int main() {
    size_t m = 40;
    size_t n = 10;

    auto A_shifts = VectorXd::LinSpaced(n, 1, n);
    auto A_diag = VectorXd::LinSpaced(m, 1, m);

    auto A = PosDefMatrixShifted{
        A_diag.asDiagonal(),
        A_shifts
    };

    auto P = IdentityPreconditioner{};

    auto U = BlockVector{MatrixXcd::Zero(m, n)};
    auto C = BlockVector{MatrixXcd::Zero(m, n)};
    auto X = BlockVector{MatrixXcd::Random(m, n)};
    auto B = BlockVector{MatrixXcd::Random(m, n)};
    auto R = B;

    auto tol = 1e-10;

    auto resnorms = sirius::cg::multi_cg(
        A, P,
        X, R, U, C,
        100, tol, false
    );

    // check the residual norms according to the algorithm
    for (size_t i = 0; i < resnorms.size(); ++i) {
        std::cout << "shift " << i << " needed " << resnorms[i].size() << " iterations " << std::abs(resnorms[i].back()) << "\n";

        if (std::abs(resnorms[i].back()) > tol) {
            return 1;
        }
    }

    // True residual norms might be different! because of rounding errors.
    VectorXd true_resnorms = (A_diag.asDiagonal() * X.vec + X.vec * A_shifts.asDiagonal() - B.vec).colwise().norm();

    for (Eigen::Index i = 0; i < true_resnorms.size(); ++i) {
        std::cout << "true resnorm " << i << ": " << true_resnorms[i] << '\n';
        if (true_resnorms[i] > tol * 100) {
            return 2;
        }
    }
}


///////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

//[eigen_eg
#include <iostream>
#include <boost/multiprecision/cpp_complex.hpp>
#include <boost/multiprecision/eigen.hpp>
#include <Eigen/Dense>

int main()
{
   using namespace Eigen;
   typedef boost::multiprecision::cpp_complex_quad complex_type;
   //
   // We want to solve Ax = b for x,
   // define A and b first:
   //
   Matrix<complex_type, 2, 2> A, b;
   A << complex_type(2, 3), complex_type(-1, -2), complex_type(-1, -4), complex_type(3, 6);
   b << 1, 2, 3, 1;
   std::cout << "Here is the matrix A:\n" << A << std::endl;
   std::cout << "Here is the right hand side b:\n" << b << std::endl;
   //
   // Solve for x:
   //
   Matrix<complex_type, 2, 2> x = A.fullPivHouseholderQr().solve(b);
   std::cout << "The solution is:\n" << x << std::endl;
   //
   // Compute the error in the solution by using the norms of Ax - b and b:
   //
   complex_type::value_type relative_error = (A*x - b).norm() / b.norm();
   std::cout << "The relative error is: " << relative_error << std::endl;
   return 0;
}
//]

/*
//[eigen_out
Here is the matrix A:
(2,3) (-1,-2)
(-1,-4)   (3,6)
Here is the right hand side b:
1 2
3 1
The solution is:
(0.6,-0.6)   (0.7,-0.7)
(0.64,-0.68) (0.58,-0.46)
The relative error is: 2.63132e-34
//]
*/


#include "graycode.h"
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <boost/math/special_functions/round.hpp>

bool CalculateGP(cv::Mat& absPhase, std::vector<cv::Mat> &images, int startGray, int endGray, int startPhase)
{
    cv::Mat gray_code(absPhase.rows, absPhase.cols, CV_8UC1);
    cv::Mat phase(absPhase.rows, absPhase.cols, CV_8UC1);
    cv::Mat mask(absPhase.rows, absPhase.cols, CV_8UC1);

    if(!CalculateGrayCodeImg(gray_code, images, startGray, endGray))
    {
        std::cout << "Error creating gray code image";
        return false;
    }
    cv::imwrite("graycode.jpg", gray_code);

    CalculateAbsolutePhase(phase, images, startPhase);

    cv::imwrite("phase.jpg", phase);

    MaskEvaluation(mask, images[startPhase], images[startPhase+1], images[startPhase+2], images[startPhase+3], 5, 255, 10); //defults
    BinaryAndOperation(phase, mask);
    BinaryAndOperation(gray_code, mask);

    cv::imwrite("phase2.jpg", phase);

    EvaluateAbsPhase(absPhase, phase, gray_code);

    return false;
}

bool CalculateGrayCodeImg( cv::Mat& code_img, std::vector<cv::Mat>& images, long StartIndex, long EndIndex )
{
    unsigned char *normal = NULL;
    unsigned char *invers = NULL;
    unsigned char *result = NULL;
    long           j, h, BitPlane;
    unsigned long  k;

    // Check if we have correct amount of images
    if( static_cast<long>(images.size()) <= EndIndex || (EndIndex-StartIndex+1)!=16 )
    {
        std::cout << "not enough images provided\n";
        return false;
    }

    for( int i=StartIndex; i<=EndIndex; i++ )
    {
        if( images[i].type() != CV_8UC1 )
        {
			cv::cvtColor(images[i], images[i], CV_BGR2GRAY);
            //std::cout << "Wrong image type\n";
            //return false;
        }
    }

    unsigned int width = images[0].cols;
    unsigned int height = images[0].rows;

    // Image size and format test
    if( code_img.type() != CV_8UC1 || code_img.cols != width || code_img.rows != height )
    {
        std::cout << "wrong format of gray image\n";
        return false;
    }

    for( unsigned int i=0; i < height; i++ )
    {
        result = (unsigned char*)code_img.row(i).data;
        memset(result, 0, code_img.cols );

        for( j=0; j<=7; j++ )
        {
            normal = (unsigned char*)images[2*j+StartIndex].row(i).data;
            invers = (unsigned char*)images[2*j+1+StartIndex].row(i).data;
            BitPlane = 1 << (7-j);

            for( k=0; k < width; k++ )
            {
                if (normal[k] > invers[k]){
                    result[k] = (unsigned char)( result[k] | BitPlane );
                }
            }
        }

        for( k=0; k < width; k++ )
        {
            h = result[k];
            // inverse graycode calculation
            result[k] = (unsigned char)( LinearCode( h & 255 ) );
        }
    }
    return true;
}

bool CalculateAbsolutePhase(cv::Mat& phase_img, std::vector<cv::Mat>& images, int offset)
{
    unsigned char *P;
    unsigned char *Q;
    unsigned char *S;
    unsigned char *T;
    unsigned char *result;

    long	i,j;
    long	A,B;
    double	Scale;
    long	ImgWidth, ImgHeight;

    ImgWidth = images[0].cols;
    ImgHeight = images[0].rows;

    if( phase_img.type() != CV_8UC1 || phase_img.cols != ImgWidth || phase_img.rows != ImgHeight )
    {
        std::cout << "wrong format of phase image";
        return false;
    }

    Scale = 128.0/M_PI;

    // all rows
    for( i=0; i<ImgHeight; i++ )
    {
        P = images[offset+0].row( i ).data;
        Q = images[offset+1].row( i ).data;
        S = images[offset+2].row( i ).data;
        T = images[offset+3].row( i ).data;
        result = (unsigned char*)phase_img.row( i ).data;
        // all columns
        for( j=0; j<ImgWidth; j++ ) {
            A = S[j] - P[j];
            B = T[j] - Q[j];
            if ((A == 0) && (B == 0)) result[j] = (unsigned char)(0);
            else result[j] = (unsigned char)boost::math::lround(atan2((double)A, (double)B)*Scale);
        }
    }
    return true;
}

void MaskEvaluation(cv::Mat& Mask, const cv::Mat& Phase1, const cv::Mat& Phase2, const cv::Mat& Phase3, const cv::Mat& Phase4,
                           int DynamicThreshold, int MaximumThreshold, int SinusThreshold )
{
    unsigned char* P;
    unsigned char* Q;
    unsigned char* S;
    unsigned char* T;
    unsigned char* R;
    long W,H,I,J;
    long Min,Max;

    W = (long)(Mask.cols);
    H = (long)(Mask.rows);

    for (I = 0; I < H; I++)
    {
        P = (unsigned char*) Phase1.row(I).data;
        Q = (unsigned char*) Phase2.row(I).data;
        S = (unsigned char*) Phase3.row(I).data;
        T = (unsigned char*) Phase4.row(I).data;
        R = (unsigned char*) Mask.row(I).data;

        for (J = 0; J < W; J++)
        {
            Min = (*P);
            Max = (*P);
            if ((*Q) < Min) Min = (*Q); else if ((*Q) > Max) Max = (*Q);
            if ((*S) < Min) Min = (*S); else if ((*S) > Max) Max = (*S);
            if ((*T) < Min) Min = (*T); else if ((*T) > Max) Max = (*T);

            if( (Max-Min)>=DynamicThreshold )
            {
                if( MaximumThreshold==255 ) {
                    if( (*P) <MaximumThreshold && (*Q)<MaximumThreshold && (*S)<MaximumThreshold && (*T)<MaximumThreshold )
                        *R = (unsigned char)(255);
                    else if( abs( ( (*P) + (*S) ) - ( (*Q) + (*T) ) ) <= SinusThreshold )	//ist die Phase sinusf\F6rmig?
                        *R = (unsigned char)(255);
                    else
                        *R = (unsigned char)(0);
                }
                else {
                    if( (*P)<=MaximumThreshold && (*Q)<=MaximumThreshold && (*S)<=MaximumThreshold && (*T)<=MaximumThreshold )
                        *R = (unsigned char)(255);
                    else
                        *R = (unsigned char)(0);
                }
            }
            else *R = (unsigned char)(0);

            P++; Q++; S++; T++; R++;
        }
    }
}

unsigned int BinaryAND(cv::Mat& dst, const cv::Mat& src)
{
    unsigned char *S;
    unsigned char *D;
    long i;
    long j;
    long H, W;
    unsigned int errval;

    W = (long)( dst.cols );
    H = (long)( dst.rows );

    // rows
    for( i = 0; i < H; i++)
    {
        D = (unsigned char*) dst.row(i).data;
        S = (unsigned char*) src.row(i).data;
        // cols
        for (j = 0; j < W; j++)
        {
            unsigned char dval = (D[j]);
            unsigned char sval = (S[j]);
            D[j] = dval & sval;
            //D[j] &= S[j];
        }
    }
    return 0;
}

void EvaluateAbsPhase(cv::Mat& AbsPhase, cv::Mat& Phase, cv::Mat& GCode)
{
    unsigned char* GC;
    unsigned char* Ph;
    unsigned short* APh;
    long I,j;
    long c,p,PRI;
    long W,H;

    W = (long)(GCode.cols);
    H = (long)(GCode.rows);

    PRI = 64;

    for(j=0; j < H  ; j++)
    {
        GC = (unsigned char*)GCode.row(j).data;
        Ph = (unsigned char*)Phase.row(j).data;
        APh = (unsigned short*)AbsPhase.row(j).data;

        for (I = 0; I < W; I++)
        {
            c = (*GC);
            p = (char)(*Ph);
            if ((c < 1)||(c > 254))
            {
                *APh = 0;
            }
            else
            {
                if (p < -PRI)
                {
                    if (c & 1)
                    {
                        c++;
                    }
                }
                else
                {
                    if (p > PRI)
                    {
                        if (!(c & 1))
                        {
                            c--;
                        }
                    }
                }
                *APh = (unsigned short)(((c >> 1) << 8) + p);
            }
            GC++; Ph++; APh++;
        }// endfor
    }// end for j
}

unsigned long LinearCode(unsigned long n)
{
    unsigned long idiv;
    int ish = 1;
    unsigned long ans = n;

    for(;;)
    {
        ans ^= ( idiv = ans >> ish );
        if( idiv <= 1 || ish == 16 )
            return ans;
        ish <<= 1;
    }
}

double BLInterpolate(double x, double y, const cv::Mat &Phase)
{
    //bilinear interpolation
    double dx = x-(int)x;
    double dy = y-(int)y;

    short ptl = Phase.at<short>((int)y , (int)x);
    short ptr = Phase.at<short>((int)y, (int)x+1);
    short pbl = Phase.at<short>((int)y+1, (int)x);
    short pbr = Phase.at<short>((int)y+1, (int)x+1);

    double weight_tl = (1.0 - dx) * (dy);
    double weight_tr = (dx)       * (dy);
    double weight_bl = (1.0 - dx) * (1.0 - dy);
    double weight_br = (dx)       * (1.0 - dy);

    return (ptl*weight_tl)+(ptr*weight_tr)+(pbl*weight_bl)+(pbr*weight_br);
}

unsigned int BinaryAndOperation(cv::Mat& dst, const cv::Mat& src)
{
    unsigned char *S;
    unsigned char *D;
    long i;
    long j;
    long H, W;
    unsigned int errval;

    // check image compatibility
    //if( (errval = CompatImages( dst, src, PIX_8BIT ) ) != OK ) return errval;

    W = (long)( dst.cols );
    H = (long)( dst.rows );

    // rows
    for( i = 0; i < H; i++)
    {
        D = (unsigned char*) dst.row(i).data;
        S = (unsigned char*) src.row(i).data;
        // cols
        for (j = 0; j < W; j++)
        {
            unsigned char dval = (D[j]);
            unsigned char sval = (S[j]);
            D[j] = dval & sval;
            //D[j] &= S[j];
        }
    }
    return 0;
}


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/estimators/estimate_uncalibrated_absolute_pose.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ceres/rotation.h>
#include <memory>
#include <vector>

#include "theia/sfm/camera/projection_matrix_utils.h"
#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/estimators/feature_correspondence_2d_3d.h"
#include "theia/sfm/pose/four_point_focal_length.h"
#include "theia/sfm/types.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

using Eigen::Matrix3d;
using Eigen::Vector3d;

// An estimator for computing the uncalibrated absolute pose from 4 feature
// correspondences. The feature correspondences should be normalized such that
// the principal point is at (0, 0).
class UncalibratedAbsolutePoseEstimator
    : public Estimator<FeatureCorrespondence2D3D, Matrix3x4d> {
 public:
  UncalibratedAbsolutePoseEstimator() {}

  // 3 correspondences are needed to determine the absolute pose.
  double SampleSize() const { return 4; }

  // Estimates candidate absolute poses from correspondences.
  bool EstimateModel(
      const std::vector<FeatureCorrespondence2D3D>& correspondences,
      std::vector<Matrix3x4d>* absolute_poses) const {
    const std::vector<Eigen::Vector2d> features = {correspondences[0].feature,
                                                   correspondences[1].feature,
                                                   correspondences[2].feature,
                                                   correspondences[3].feature};
    const std::vector<Eigen::Vector3d> world_points = {
        correspondences[0].world_point,
        correspondences[1].world_point,
        correspondences[2].world_point,
        correspondences[3].world_point};

    const int num_solutions =
        FourPointPoseAndFocalLength(features, world_points, absolute_poses);
    return num_solutions > 0;
  }

  // The error for a correspondences given an absolute pose. This is the squared
  // reprojection error.
  double Error(const FeatureCorrespondence2D3D& correspondence,
               const Matrix3x4d& absolute_pose) const {
    // The reprojected point is computed as R * (X - c) where R is the camera
    // rotation, c is the position, and X is the 3D point.
    const Eigen::Vector2d reprojected_feature =
        (absolute_pose * correspondence.world_point.homogeneous())
            .eval()
            .hnormalized();
    return (reprojected_feature - correspondence.feature).squaredNorm();
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(UncalibratedAbsolutePoseEstimator);
};

}  // namespace

bool EstimateUncalibratedAbsolutePose(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<FeatureCorrespondence2D3D>& normalized_correspondences,
    UncalibratedAbsolutePose* absolute_pose,
    RansacSummary* ransac_summary) {
  UncalibratedAbsolutePoseEstimator absolute_pose_estimator;
  std::unique_ptr<SampleConsensusEstimator<UncalibratedAbsolutePoseEstimator> >
      ransac = CreateAndInitializeRansacVariant(
          ransac_type, ransac_params, absolute_pose_estimator);
  // Estimate the absolute pose.
  Matrix3x4d projection_matrix;
  const bool success = ransac->Estimate(
      normalized_correspondences, &projection_matrix, ransac_summary);

  // Recover the focal length and pose.
  Eigen::Matrix3d calibration_matrix;
  Eigen::Vector3d rotation;
  DecomposeProjectionMatrix(projection_matrix,
                            &calibration_matrix,
                            &rotation,
                            &absolute_pose->position);

  // Convert angle-axis rotation to rotation matrix.
  ceres::AngleAxisToRotationMatrix(
      rotation.data(),
      ceres::ColumnMajorAdapter3x3(absolute_pose->rotation.data()));

  absolute_pose->focal_length =
      calibration_matrix(0, 0) / calibration_matrix(2, 2);

  return success;
}

}  // namespace theia


#include <vector>

#include <glm/glm.hpp>

#include <Eigen/Sparse>
#include <Eigen/SparseQR>

#include "Types.h"
#include "Dynamic.h"

namespace{
    using namespace BalloonFEM;
    const Vec3 v[4] = {Vec3(0), Vec3(1, 0, 0), Vec3(0, 1, 0), Vec3(0, 0, 1)};
    const Mat3x2 m[3][3] = {
        { Mat3x2(v[1], v[0]), Mat3x2(v[2], v[0]), Mat3x2(v[3], v[0])},
        { Mat3x2(v[0], v[1]), Mat3x2(v[0], v[2]), Mat3x2(v[0], v[3])},
        { -Mat3x2(v[1], v[1]), -Mat3x2(v[2], v[2]), -Mat3x2(v[3], v[3])}
    };
}

namespace BalloonFEM
{
    void Engine::computeFilmForces(ObjState &state, Vvec3 &f_sum)
    {
        Vvec3 &pos = state.world_space_pos;

		/* compute film elastic force */

		for (MIter f = m_tetra->films.begin(); f != m_tetra->films.end(); f++)
		{
			for (PIter p = f->pieces.begin(); p != f->pieces.end(); p++)
			{
				iVec3 &id = p->v_id;
				Vec3 &v0 = pos[id[0]];
				Vec3 &v1 = pos[id[1]];
				Vec3 &v2 = pos[id[2]];

				/* calculate deformation in world space */
				Mat3x2 Ds = Mat3x2(v0 - v2, v1 - v2);

				/* calculate deformation gradient */
				Mat3x2 F = Ds * p->Bm;

				/* calculate Piola for this tetra */
				Mat3x2 P = m_film_model->Piola(F);

				/* calculate forces contributed from this tetra */
				Mat3x2 H = - p->volume() * P * transpose(p->Bm);

				f_sum[id[0]] += H[0];
				f_sum[id[1]] += H[1];
				f_sum[id[2]] -= H[0] + H[1];
			}
		}

    }

    SpMat Engine::computeFilmDiffMat(ObjState &state)
    {
       	printf("building film force differential matrix \n");
		/* project from constrained freedom state to world space */
		Vvec3 &pos = state.world_space_pos;

	    std::vector<T> coefficients;
		coefficients.clear();
		size_t count_film = 0;
		for (size_t i = 0; i < m_tetra->films.size(); i++)
			count_film += m_tetra->films[i].pieces.size();
		coefficients.reserve( 9 * 9 * count_film);

		for (MIter f = m_tetra->films.begin(); f != m_tetra->films.end(); f++)
        {
			for (PIter p = f->pieces.begin(); p != f->pieces.end(); p++)
			{
				/* assgin world space position */
				iVec3 &id = p->v_id;
				Vec3 &v0 = pos[id[0]];
				Vec3 &v1 = pos[id[1]];
				Vec3 &v2 = pos[id[2]];

				/* calculate deformation in world space */
				Mat3x2 Ds = Mat3x2(v0 - v2, v1 - v2);

				/* calculate deformation gradient */
				Mat3x2 F = Ds * p->Bm;

				/* i is index of vertex, j is index of dimention */
				for (size_t i = 0; i < 3; i++)
				for (size_t j = 0; j < 3; j++)
				{
					/* calculate delta deformation in world space */
					Mat3x2 dDs = m[i][j];

					/* calculate delta deformation gradient */
					Mat3x2 dF = dDs * p->Bm;

					/* calculate delta Piola */
					Mat3x2 dP = m_film_model->StressDiff(F, dF);

					/* calculate forces contributed from this tetra */
					Mat3x2 dH = - p->volume() * dP * transpose(p->Bm);

					for (size_t w = 0; w < 2; w++)
					for (size_t l = 0; l < 3; l++)
						coefficients.push_back(T(3 * id[w] + l, 3 * id[i] + j, dH[w][l]));

					Vec3 df_3 = - dH[0] - dH[1];
					for (size_t l = 0; l < 3; l++)
						coefficients.push_back(T(3 * id[2] + l, 3 * id[i] + j, df_3[l]));
				}
			}
		}

		SpMat E( 3 * pos.size(), 3 * pos.size());
		E.setFromTriplets(coefficients.begin(), coefficients.end());

        return E;
    }
}


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iostream>
#include <limits>
#include <list>
#include <numeric>
#include <queue>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) (a).begin(), (a).end()
#define AALL(a, n) (a), ((a) + (n))
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define MODNUM (INT(1e9 + 7))
#define MOD(x) ((x) % MODNUM)

using namespace std;

using ll = long long;
using VI = vector<int>;
using VI2D = vector<vector<int>>;

const int INF = 2e9;
const double EPS = 1e-10;
const double PI = acos(-1.0);

const int dx[] = {-1, 0, 1, 0};
const int dy[] = {0, -1, 0, 1};

template <typename T>
int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T>
void chmax(T& m, T x) {
	m = max(m, x);
}

template <typename T>
void chmin(T& m, T x) {
	m = min(m, x);
}

template <typename T>
T square(T x) {
	return x * x;
}

inline int toInt(string s) {
	int v;
	istringstream sin(s);
	sin >> v;
	return v;
}

// modを取りつつ二項係数を計算する関数を返す
auto make_mod_comb(long long mod) {
	const int COMB_MAX = 1100000;
	vector<long long> fact(COMB_MAX);
	vector<long long> fact_inv(COMB_MAX);
	vector<long long> inv(COMB_MAX);

	fact[0] = fact[1] = 1;
	fact_inv[0] = fact_inv[1] = 1;
	inv[1] = 1;

	for(int i = 2; i < COMB_MAX; i++) {
		fact[i] = (fact[i - 1] * i) % mod;
		inv[i] = mod - (inv[mod % i] * (mod / i)) % mod;
		fact_inv[i] = (fact_inv[i - 1] * inv[i]) % mod;
	}

	return [mod = mod, fact = move(fact), fact_inv = move(fact_inv)](
			   const long long n, const long long r) {
		if(n < r || n < 0 || r < 0) {
			return 0LL;
		}
		return (fact[n] * ((fact_inv[r] * fact_inv[n - r]) % mod)) % mod;
	};
}

int main() {
	int r1, c1, r2, c2;
	scanf("%d %d %d %d", &r1, &c1, &r2, &c2);
	auto mod_comb = make_mod_comb(MODNUM);
	ll result = 0;
	RANGE(i, r1, r2 + 1) {
		RANGE(j, c1, c2 + 1) { result = MOD(result + mod_comb(i + j, i)); }
	}
	printf("%lld\n", result);
	return 0;
}


#include "sargparse/File.h"
#include "sargparse/Parameter.h"

#include "stl/STLParser.h"
#include "global_parameters.h"

#include "tetrahedron.h"
#include "transformer.h"

#include <iostream>
#include <vector>
#include <armadillo>

void print_mass_properties();
sargp::Command cmd { "mass_properties", "show mass properties", print_mass_properties };

auto totalMass = cmd.Parameter<std::optional<double>>({}, "total_mass", "total mass of the object (used to calculate mass properties)");
auto density = cmd.Parameter<std::optional<double>>({}, "density", "density of the object (used to calculate mass properties)");
auto tensorPerspectiveOrigin = cmd.Flag("tensor_from_origin", "print the tensor from the perspective of the origin frame");

auto skew(arma::colvec3 const& v)
{
    return arma::mat33 {
        { 0, -v(2), v(1) },
        { v(2), 0, -v(0) },
        { -v(1), v(0), 0 },
    };
}

void print_mass_properties()
{
    if (not inFiles) {
        throw std::runtime_error("in has to be specified!");
    }

    kinematicTree::visual::stl::STLParser parser;
    kinematicTree::visual::mesh::Mesh mesh;
    for (auto const& file : *inFiles) {
        std::cout << "loading : " << file << "\n";
        auto subMesh = parser.parse(file);
		for (auto const& facet : subMesh.getFacets()) {
			mesh.addFacet(facet);
		}
    }

	auto transform = getTransform();
    transform.print("applying transform");
    mesh.applyTransform(transform);

    arma::mat33 inertia_tensor = arma::zeros(3, 3);
    double totalVolume {};
    arma::colvec3 com {};

    for (auto const& facet : mesh.getFacets()) {
        if (facet.mVertices.size() < 3) {
            continue;
        }
        auto const& base = facet.mVertices[0];
        for (auto i { 1 }; i < facet.mVertices.size() - 1; ++i) {
            auto tetrahedron = Tetrahedron { { base, facet.mVertices[i], facet.mVertices[i + 1] } };

            totalVolume += tetrahedron.volume;
            com += tetrahedron.com * tetrahedron.volume;
            inertia_tensor += tetrahedron.normed_inertia_tensor;
        }
    }

    com = com / totalVolume;

    std::cout << "volume: " << totalVolume << "\n";
    com.print("COM");
    std::cout << "\n\n";

    if (*density) {
        auto totMass = totalVolume * **density;
        std::cout << "properties by given density: (" << **density << ")\n\n";
        std::cout << "total mass: " << totMass << "\n";
        arma::mat33 I = inertia_tensor * **density;

        if (not *tensorPerspectiveOrigin) {
            // move the inertia_tensor to the COM
            I += totMass * skew(com) * skew(com);
        }

        I.print("inertia tensor");
    }

    if (*totalMass) {
        auto density = **totalMass / totalVolume;
        std::cout << "properties by given total mass: (" << **totalMass << ")\n\n";
        std::cout << "density: " << density << "\n";
        arma::mat33 I = inertia_tensor * density;

        // move the inertia_tensor to the COM
        if (not *tensorPerspectiveOrigin) {
            I += **totalMass * skew(com) * skew(com);
        }

        I.print("inertia tensor");
    }
}


#include <dirent.h>
#include <iostream>
#include <vector>
#include <string>
#include <utility>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Geometry>

#include <opencv2/opencv.hpp>
#define pi acos(-1)

const int G_land_num = 74;
const int G_train_pic_id_num = 3300;
const int G_nShape = 47;
const int G_nVerts = 11510;
const int G_nFaces = 11540;
const int G_test_num = 77;
const int G_iden_num = 77;
const int G_inner_land_num = 59;
const int G_line_num = 50;
const int G_jaw_land_num = 20;
#define normalization
struct Target_type {
	Eigen::VectorXf exp;
	Eigen::RowVector3f tslt;
	Eigen::Matrix3f rot;
	Eigen::MatrixX2f dis;

};

struct DataPoint
{
	cv::Mat image;
	cv::Rect face_rect;
	std::vector<cv::Point2d> landmarks;
	//std::vector<cv::Point2d> init_shape;
	Target_type shape, init_shape;
	Eigen::VectorXf user;
	Eigen::RowVector2f center;
	Eigen::MatrixX2f land_2d;
#ifdef posit
	float f;
#endif // posit
#ifdef normalization
	Eigen::MatrixX3f s;
#endif

	Eigen::VectorXi land_cor;
};

void load_lv(std::string name, DataPoint &temp) {
	std::cout << "load coefficients...file:" << name << "\n";
	FILE *fp;
	fopen_s(&fp, name.c_str(), "rb");

	temp.user.resize(G_iden_num);
	for (int j = 0; j < G_iden_num; j++)
		fread(&temp.user(j), sizeof(float), 1, fp);
	std::cout << temp.user << "\n";
	system("pause");
	temp.land_2d.resize(G_land_num, 2);
	for (int i_v = 0; i_v < G_land_num; i_v++) {
		fread(&temp.land_2d(i_v, 0), sizeof(float), 1, fp);
		fread(&temp.land_2d(i_v, 1), sizeof(float), 1, fp);
	}

	fread(&temp.center(0), sizeof(float), 1, fp);
	fread(&temp.center(1), sizeof(float), 1, fp);

	temp.shape.exp.resize(G_nShape);
	for (int i_shape = 0; i_shape < G_nShape; i_shape++)
		fread(&temp.shape.exp(i_shape), sizeof(float), 1, fp);

	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
		fread(&temp.shape.rot(i, j), sizeof(float), 1, fp);

	for (int i = 0; i < 3; i++) fread(&temp.shape.tslt(i), sizeof(float), 1, fp);

	temp.land_cor.resize(G_land_num);
	for (int i_v = 0; i_v < G_land_num; i_v++) fread(&temp.land_cor(i_v), sizeof(int), 1, fp);

	temp.s.resize(2, 3);
	for (int i = 0; i < 2; i++) for (int j = 0; j < 3; j++)
		fread(&temp.s(i, j), sizeof(float), 1, fp);

	temp.shape.dis.resize(G_land_num, 2);
	for (int i_v = 0; i_v < G_land_num; i_v++) {
		fread(&temp.shape.dis(i_v, 0), sizeof(float), 1, fp);
		fread(&temp.shape.dis(i_v, 1), sizeof(float), 1, fp);
	}
	std::cout << temp.shape.dis << "\n";
	system("pause");
	fclose(fp);
	puts("load successful!");
}

//assume the be could not be more than 90
void cal_uler_angle(Eigen::Matrix3f R) {
	Eigen::Vector3f x, y, z,t;
	x = R.row(0).transpose();
	y = R.row(1).transpose();
	z = R.row(2).transpose();
	float al, be, ga, gaw;
	if (fabs(1 - z(2)*z(2)) < 1e-3) {
		ga=gaw=be = 0;
		al = acos(x(0));
		if (y(0) < 0) al = 2 * pi - al;
	}
	else {

		be = acos(z(2));
		al = acos(std::max(std::min(float(1.0),z(1) / sqrt(1 - z(2)*z(2))),float(-1.0)));

		if (z(0) < 0) al = 2 * pi - al;//according to the sin(al)

		t(0) = cos(al), t(1) = sin(al), t(2) = 0;
		t.normalize();
		x.normalize();
		//t.normalized();
		ga = acos(t.dot(x));
		gaw = acos(std::max(std::min(float(1.0), -y(2) / sqrt(1 - z(2)*z(2))), float(-1.0)));

		printf("%.10f %.10f %.10f\n", -y(2), sqrt(1 - z(2)*z(2)), -y(2) / sqrt(1 - z(2)*z(2)));
		if (x(2) < 0) ga = 2 * pi - ga, gaw = 2 * pi - gaw;//according to the sin(ga)
	}
	std::cout << R << "\n----------------------\n";
	printf("%.10f %.10f %.10f %.10f %.10f\n",z(2), al/pi*180, be / pi * 180, ga / pi * 180, gaw / pi * 180);
	system("pause");
}

Eigen::Matrix3f get_r_from_angle(float angle, int axis) {
	Eigen::Matrix3f ans;
	ans.setZero();
	ans(axis, axis) = 1;
	int idx_x = 0, idx_y = 1;
	if (axis == 0)
		idx_x = 1, idx_y = 2;
	else
		if (axis == 2)
			idx_x = 0, idx_y = 1;
		else
			idx_x = 0, idx_y = 2;
	ans(idx_x, idx_x) = cos(angle), ans(idx_x, idx_y) = -sin(angle), ans(idx_y, idx_x) = sin(angle), ans(idx_y, idx_y) = cos(angle);
	return ans;
}

Eigen::Matrix3f get_r_from_angle(const Eigen::Vector3f &angle) {
	Eigen::Matrix3f ans;
	float Sa = sin(angle(0)), Ca = cos(angle(0)), Sb = sin(angle(1)),
		Cb = cos(angle(1)), Sc = sin(angle(2)), Cc = cos(angle(2));

	ans(0, 0) = Ca * Cc - Sa * Cb*Sc;
	ans(0, 1) = -Sa * Cc - Ca * Cb*Sc;
	ans(0, 2) = Sb * Sc;
	ans(1, 0) = Ca * Sc + Sa * Cb*Cc;
	ans(1, 1) = -Sa * Sc + Ca * Cb*Cc;
	ans(1, 2) = -Sb * Cc;
	ans(2, 0) = Sa * Sb;
	ans(2, 1) = Ca * Sb;
	ans(2, 2) = Cb;
	return ans;
}
void test_r(DataPoint data) {
	Eigen::Matrix3f rot;
	rot = get_r_from_angle(data.shape.tslt(2), 2)*get_r_from_angle(data.shape.tslt(1), 0)*get_r_from_angle(data.shape.tslt(0), 2);
	std::cout << rot << "\n";
	std::cout << get_r_from_angle(data.shape.tslt) << "\n";
	system("pause");
}

int main() {

	DataPoint data;
	data.shape.tslt << 1, 20, 0.5;
	test_r(data);
	//load_lv("./test/pose_4_t108.lv",data);//data/test_debug_lv_005_04_03_051_05
	//cal_uler_angle(data.shape.rot);

	return 0;
}
//g++ -Wall -std=c++11 `pkg-config --cflags opencv` -o deal deal_falut.cpp `pkg-config --libs opencv`


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXcf A = MatrixXcf::Random(4,4);
HessenbergDecomposition<MatrixXcf> hd(4);
hd.compute(A);
cout << "The matrix H in the decomposition of A is:" << endl << hd.matrixH() << endl;
hd.compute(2*A); // re-use hd to compute and store decomposition of 2A
cout << "The matrix H in the decomposition of 2A is:" << endl << hd.matrixH() << endl;

  return 0;
}


/*
 * Copyright (c) 2013-2014 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef MATRIX_INVERSION_HPP
#define MATRIX_INVERSION_HPP

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#if defined(BOOST_UBLAS_TYPE_CHECK)
#define MATRIX_INVERSION_SAVE BOOST_UBLAS_TYPE_CHECK
#undef BOOST_UBLAS_TYPE_CHECK
#define BOOST_UBLAS_TYPE_CHECK 0
#include <boost/numeric/ublas/lu.hpp>
#undef BOOST_UBLAS_TYPE_CHECK
#define BOOST_UBLAS_TYPE_CHECK MATRIX_INVERSION_SAVE
#else
#define BOOST_UBLAS_TYPE_CHECK 0
#include <boost/numeric/ublas/lu.hpp>
#undef BOOST_UBLAS_TYPE_CHECK
#endif

#ifdef USE_LAPACK
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/lapack/gesv.hpp>
#include <boost/numeric/bindings/blas/blas3.hpp>
namespace bnb = boost::numeric::bindings;
#endif

#ifdef USE_ATLAS
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/atlas/clapack.hpp>
#include <boost/numeric/bindings/atlas/cblas3.hpp>
namespace bnb = boost::numeric::bindings;
#endif

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T>
bool invert(const ub::matrix<T>& a, ub::matrix<T>& b) {
	ub::matrix<T> tmp(a);
	ub::permutation_matrix<> pm(tmp.size1());

	if (ub::lu_factorize(tmp, pm) != 0) return false;

	b = ub::identity_matrix<T>(tmp.size1());

	try {
		ub::lu_substitute(tmp, pm, b);
	}
	catch (...) {
		return false;
	}

	return true;
}

// special version for double
#if defined(USE_LAPACK) || defined(USE_ATLAS)
template <>
bool invert(const ub::matrix<double>& a, ub::matrix<double>& b) {
	ub::matrix<double, ub::column_major> tmp(a);
	ub::permutation_matrix<int> pm(tmp.size1());

	#ifdef USE_LAPACK
	if (bnb::lapack::getrf(tmp, pm) != 0) return false;
	if (bnb::lapack::getri(tmp, pm) != 0) return false;
	#endif
	#ifdef USE_ATLAS
	if (bnb::atlas::getrf(tmp, pm) != 0) return false;
	if (bnb::atlas::getri(tmp, pm) != 0) return false;
	#endif

	b = tmp;

	return true;
}
#endif // defined(USE_LAPACK) || defined(USE_ATLAS)

template <class T>
bool linear_equation(const ub::matrix<T>& a, const ub::vector<T>& b, ub::vector<T>& x) {
	ub::matrix<T> tmp(a);
	ub::permutation_matrix<> pm(tmp.size1());

	if (ub::lu_factorize(tmp, pm) != 0) return false;

	x = b;

	ub::lu_substitute(tmp, pm, x);

	return true;
}

// special version for double
#if defined(USE_LAPACK) || defined(USE_ATLAS)
template <>
bool linear_equation(const ub::matrix<double>& a, const ub::vector<double>& b, ub::vector<double>& x) {
	ub::matrix<double, ub::column_major> tmp(a);
	// int i;
	// int size = tmp.size1();
	ub::matrix<double, ub::column_major> tmp2(tmp.size1(), 1);

	// for (i=0; i<size; i++) tmp2(i, 0) = b(i);
	// ub::column(tmp2, 0).assign(b);
	ub::column(tmp2, 0) = b;

	#ifdef USE_LAPACK
	if (bnb::lapack::gesv(tmp, tmp2) != 0) return false;
	#endif
	#ifdef USE_ATLAS
	if (bnb::atlas::gesv(tmp, tmp2) != 0) return false;
	#endif

	// for (i=0; i<size; i++) x(i) = tmp2(i, 0);
	x = ub::column(tmp2, 0);

	return true;
}
#endif // defined(USE_LAPACK) || defined(USE_ATLAS)

template <class T>
void mm_mult(const ub::matrix<T>& a, const ub::matrix<T>& b, ub::matrix<T>& c) {
	c = ub::prod(a, b);
}

// special version for double
#if defined(USE_LAPACK) || defined(USE_ATLAS)
template <>
void mm_mult(const ub::matrix<double>& a, const ub::matrix<double>& b, ub::matrix<double>& c) {
	ub::matrix<double, ub::column_major> ca(a);
	ub::matrix<double, ub::column_major> cb(a);
	ub::matrix<double, ub::column_major> cc(c);

	#ifdef USE_LAPACK
		bnb::blas::gemm(ca, cb, cc);
	#endif
	#ifdef USE_ATLAS
		bnb::atlas::gemm(ca, cb, cc);
	#endif

	c = cc;
}
#endif // defined(USE_LAPACK) || defined(USE_ATLAS)

} // namespace kv

#endif // MATRIX_INVERSION_HPP


/// @file  linalg.hpp
/// @brief Linear algebra routines.

#pragma once
#ifndef ORDGEO_LINALG_LINALG_HPP
#define ORDGEO_LINALG_LINALG_HPP

#include <ordgeo/config.hpp>
#include <Eigen/Eigen>
#include <exception>
#include <memory>
#include <string>
#include <vector>

namespace ORDGEO_NAMESPACE {
namespace linalg {

/// An exception to throw in case of mathematical error in some linear algebra
/// routine.
struct LinAlgErr : public std::runtime_error {
	virtual ~LinAlgErr() = default;

	/// Build an LinAlgErr with the specified message.
	LinAlgErr(std::string message)
		: std::runtime_error("ordgeo::LinAlgErr: " + message) {
	}
};

/// The eigenvalues and eigenvectors of a matrix
struct EigResult {
	virtual ~EigResult() = default;

	/// The eigenvalues
	Eigen::ArrayXcd eig;

	/// The eigenvectors
	Eigen::EigenSolver<Eigen::MatrixXd>::EigenvectorsType vec;
};

/// Iterates over nonzero entries in a dense vector.
template<typename T>
struct DenseNZIterator {
	DenseNZIterator(const T& vector) : _vec(vector), _idx(-1) { ++(*this); }
	virtual ~DenseNZIterator() = default;
	void operator++() {
		++_idx;
		while (_idx < _vec.size() && _vec[_idx] == 0) {
			++_idx;
		}
	}
	operator bool() const {
		return _idx < _vec.size();
	}
	size_t index() const {
		return static_cast<size_t>(_idx);
	}

	const T& _vec;
	int _idx;
};

/// Compute the eigenvalues and eigenvectors of a matrix.
/// Throws LinAlgErr on failure.
EigResult eigendecomposition(const Eigen::MatrixXd& matrix,
	bool assumeSelfAdjoint = false);

/// Ask whether a matrix is positive semidefinite.
/// This tests whether the eigenvalues are non-negative, to within eps
/// precision.
/// Any PSD matrix is a valid n x n distance matrix for some Euclidean space
/// R^d, with d <= n - 1.
bool isPSD(const Eigen::MatrixXd& matrix, double eps = 1e-12);

/// Attempt to project a matrix onto the PSD cone.
/// Throws LinAlgErr in case of failure.
/// This will produce a PSD matrix which is as close as possible to the input
/// matrix. This is often used with methods which compute matrices to satisfy
/// some loss minimization objective without constraining those matrices to be
/// distance matrices. The output from such a method can be projected onto the
/// PSD cone to obtain the PSD solution which is as close as possible to the
/// solution with minimal loss. This is sometimes faster than constraining the
/// optimization problem to stay within the PSD cone.
Eigen::MatrixXd projectOntoPSDCone(const Eigen::MatrixXd& matrix,
	bool assumeSelfAdjoint);

/// Project a matrix onto the nearest matrix of the specified rank.
/// This works by setting the smallest eigenvalues to zero.
Eigen::MatrixXd projectOntoLowRank(const Eigen::MatrixXd& matrix, size_t rank);

/// Project a matrix onto the nuclear norm ball.
/// This has the effect of reducing its rank.
///
/// [1] Efficient Projections onto the .1-Ball for Learning in High Dimensions.
///     John Duchi, Shai Shalev-Shwartz, Yoram Singer, and Tushar Chandra.
///     International Conference on Machine Learning (ICML 2008)
///     http://www.cs.berkeley.edu/~jduchi/projects/DuchiSiShCh08.pdf
Eigen::MatrixXd projectOntoNuclearNorm(const Eigen::MatrixXd& matrix,
	double lambda);

/// Project a matrix onto the unit sphere.
/// All vectors are scaled to unit length.
Eigen::MatrixXd projectOntoUnitSphere(const Eigen::MatrixXd& matrix);

/// A position match is a rotation/reflection, translation, and scaling of a
/// position matrix so that its points are as close as possible to the positions
/// of the corresponding rows in some target matrix.
/// This is also known as a Procrustes transformation.
struct PositionMatch {
	virtual ~PositionMatch() = default;

	/// Finds a Procrustes transformation which minimizes the sum of squared
	/// distances between corresponding rows of target and testee.
	///
	/// [1] I. Borg & P. Groenen (1997): Modern multidimensional scaling: theory
	///     and applications. Springer.
	static std::shared_ptr<PositionMatch> Create(const Eigen::MatrixXd& target,
		const Eigen::MatrixXd& testee);

	/// Transforms a matrix in-place into the target space.
	virtual void transform(Eigen::MatrixXd& matrix) = 0;

	/// Return the rotation matrix.
	virtual Eigen::MatrixXd rotationMatrix() const = 0;

	/// Return the translation vector.
	virtual Eigen::VectorXd translationVector() const = 0;

	/// Return the scaling factor.
	virtual double scalingFactor() const = 0;
};

/// Find the distance scaling which minimizes the difference between two
/// distance matrices, in a least-squares sense.
double distScalingFactor(const Eigen::MatrixXd& dhat,
	const Eigen::MatrixXd& dtrue);

/// Select the specified rows of a matrix.
template<typename mat_type>
mat_type selectRows(const mat_type& X, std::vector<size_t> rows) {
	mat_type result(rows.size(), X.cols());
	for (size_t ii = 0; ii < rows.size(); ii++) {
		result.row(ii) = X.row(rows[ii]);
	}
	return result;
}
template<>
Eigen::SparseMatrix<double, Eigen::RowMajor> selectRows(
	const Eigen::SparseMatrix<double, Eigen::RowMajor>& X,
	std::vector<size_t> rows);

/// Convert a position matrix into a Euclidean distance matrix.
template<typename mat_type>
Eigen::MatrixXd posToDist(const mat_type& X) {
	Eigen::MatrixXd B = X * X.transpose();
	Eigen::VectorXd c = B.diagonal();
	Eigen::VectorXd one = Eigen::VectorXd::Ones(X.rows());
	return (c * one.transpose() + one * c.transpose() - 2 * B).cwiseSqrt();
}

/// Convert a position matrix into a Cosine distance matrix.
template<typename mat_type>
Eigen::MatrixXd posToCosineDist(const mat_type& X) {
	Eigen::MatrixXd dists(X.rows(), X.rows());
	Eigen::VectorXd norm(X.rows());
	for (Eigen::Index ii = 0; ii < X.rows(); ii++) {
		norm(ii) = X.row(ii).norm();
	}
	for (Eigen::Index ii = 0; ii < X.rows(); ii++) {
		auto a = X.row(ii);
		for (Eigen::Index jj = 0; jj < ii; jj++) {
			auto b = X.row(jj);
			dists(ii,jj) = 1.0 - (a.dot(b) / (norm(ii) + norm(jj)));
			dists(jj,ii) = dists(ii,jj);
		}
	}
	return dists;
}

/// Convert a position matrix into a Jaccard distance matrix.
template<typename M, typename V, typename IT>
Eigen::MatrixXd posToJaccardDist(const M& X) {
	Eigen::MatrixXd dists(X.rows(), X.rows());
	for (Eigen::Index ii = 0; ii < X.rows(); ii++) {
		V a = X.row(ii);
		for (Eigen::Index jj = 0; jj < ii; jj++) {
			V b = X.row(jj);

			IT ita(a), itb(b);
			double inBoth = 0, total = 0;
			while (ita && itb) {
				total++;
				if (ita.index() < itb.index()) {
					++ita;
				} else if (ita.index() > itb.index()) {
					++itb;
				} else {
					inBoth++;
					++ita;
					++itb;
				}
			}
			for (; ita; ++ita) {
				total++;
			}
			for (; itb; ++itb) {
				total++;
			}

			dists(ii,jj) = total ? (total - inBoth) / total : INFINITY;
			dists(jj,ii) = dists(ii,jj);
		}
	}
	return dists;
}

/// Center a matrix
Eigen::MatrixXd centerMatrix(const Eigen::MatrixXd& X);

/// Center a position matrix about the origin, and scale it to unit diameter
Eigen::MatrixXd normalizePos(const Eigen::MatrixXd& X);

/// Transform a vector by isotonic regression so its values are changed by the
/// smallest amount possible to appear in the specified order.
///
/// Implements the PAVA algorithm with uniform weights, as described here:
/// http://stat.wikia.com/wiki/Isotonic_regression
Eigen::VectorXd isotonicRegression(const Eigen::VectorXd& V,
	const Eigen::VectorXd& weights, const std::vector<Eigen::Index>& order);

/// Calculate the Euclidean distance between two points.
inline double dist(const Eigen::MatrixXd& X, Eigen::Index a, Eigen::Index b) {
	assert(a < X.rows());
	assert(b < X.rows());
	return (X.row(a) - X.row(b)).norm();
}

/// Calculate the squared Euclidean distance between two points.
inline double sqDist(const Eigen::MatrixXd& X, Eigen::Index a, Eigen::Index b) {
	assert(a < X.rows());
	assert(b < X.rows());
	return (X.row(a) - X.row(b)).squaredNorm();
}

/// Get pairwise squared Euclidean distances from a dissimilarity kernel.
Eigen::MatrixXd sqDistsFromKernel(const Eigen::MatrixXd& K);

/// Calculate the squared Euclidean distance between two points from a kernel.
inline double sqDistFromKernel(const Eigen::MatrixXd K, Eigen::Index a,
	Eigen::Index b) {
	assert(K.rows() == K.cols());
	assert(a < K.rows());
	assert(b < K.rows());
	return K(a,a) + K(b,b) - 2 * K(a,b);
}

/// Get an embedding from a dissimilarity kernel via SVD
Eigen::MatrixXd embeddingFromKernelSVD(const Eigen::MatrixXd& K, size_t nDim);

/// Compute a kernel matrix from a given position/feature matrix.
template<typename mat_type>
Eigen::MatrixXd kernelForFeatures(const mat_type& mat) {
	Eigen::Index nObj = mat.rows();
	Eigen::MatrixXd kernel = Eigen::MatrixXd::Zero(nObj, nObj);
	for (Eigen::Index ii = 0; ii < nObj; ii++) {
		for (Eigen::Index jj = 0; jj <= ii; jj++) {
			kernel(ii,jj) = mat.row(ii).dot(mat.row(jj));
			if (ii != jj) {
				kernel(jj,ii) = kernel(ii,jj);
			}
		}
	}
	return kernel;
}

/// Find the intersection of spheres with k-dimensional center coordinates.
/// The sphere centers are given as the columns of centers.
/// The quality variable is negative if the spheres do not intersect,
/// zero if they intersect in R^k, and positive if they intersect in R^(k+1).
/// Citation: Thm. 3.3 from
///     H.X. Huang, Z.-A. Liang, and P. M. Pardalos,
///     "Some Properties for the Euclidean Distance Matrix and Positive
///     Semidefinite Matrix Completion Problems,"
///     J Glob Optim, vol. 25, no. 1, pp. 3–21, 2003.
Eigen::VectorXd sphereIntersection(const Eigen::MatrixXd& centers,
	const Eigen::VectorXd& radii, double& quality);

} // end namespace linalg
} // end namespace ORDGEO_NAMESPACE
#endif /* ORDGEO_LINALG_LINALG_HPP */


#include <iostream>
#include <nbsimMyFunctions.h>
#include <nbsimExceptionMacro.h>
#include "nbsimParticle.h"
#include "nbsimMassiveParticle.h"
#include "nbsimSolarSystemData.ipp"
#include <Eigen/Dense>
#include <CLI/CLI.hpp>
#include <chrono>
#include <omp.h>

int main(int argc, char** argv){
    CLI::App app{"solar system simulator"};
    double step_size, total_time;
    app.add_option("-s,--timestep", step_size,  "step size, unit:year");
    app.add_option("-t,--totaltime", total_time,  "duration of simulation, unit:year");

    std::string planet_name[9];
    Eigen::Vector3d init_position, init_velocity, r_com(0,0,0), p_total(0,0,0);
    double mu,mu_total=0;
    std::shared_ptr<nbsim::MassiveParticle> planet_ptr[9];
    CLI11_PARSE(app, argc, argv);

    std::clock_t c_start = std::clock();
    auto t_start = std::chrono::high_resolution_clock::now();
    omp_set_num_threads(16);

    #pragma omp parallel for
    for (int i=0; i<9; i++) {
		planet_name[i]=nbsim::solarSystemData[i].name;
		init_position=nbsim::solarSystemData[i].position;
		init_velocity=nbsim::solarSystemData[i].velocity;
        mu=nbsim::solarSystemData[i].mu;
        mu_total+=mu;
        std::shared_ptr<nbsim::MassiveParticle> ptr_particle_i(new nbsim::MassiveParticle(init_position, init_velocity, mu/6.67408e-11));
		planet_ptr[i]=ptr_particle_i;
	}

    for (int i=0; i<9; i++){
		for (int j=0; j<9; j++){
			planet_ptr[i]->addAttractor(planet_ptr[j]);
            if (planet_ptr[i]==planet_ptr[j]){
                planet_ptr[i]->removeAttractor(planet_ptr[j]);
            }
		}
	}
    #pragma omp parallel
    for (double test_time=0; test_time<total_time; test_time+=step_size){

        #pragma omp for
		for (int i=0;i<9;i++){
			planet_ptr[i]->calculateAcceleration();
		}
        #pragma omp for nowait
		for (int i=0;i<9;i++){
			planet_ptr[i]->integrateTimestep(step_size);
		}
	}
    #pragma omp parallel for
    for (int i=0;i<9;i++){
        r_com+=nbsim::solarSystemData[i].mu*(planet_ptr[i]->getPosition());
        p_total+=nbsim::solarSystemData[i].mu*(planet_ptr[i]->getVelocity());
    }
    std::clock_t c_end = std::clock();
    auto t_end = std::chrono::high_resolution_clock::now();

    r_com=r_com/mu_total;

    for (int i=0;i<9;i++){
		std::cout<<planet_name[i]<<"\n original position:"<<nbsim::solarSystemData[i].position<<"\n current position:"<<planet_ptr[i]->getPosition()<<std::endl;

    }
    std::cout<<"\n r_com is:"<<r_com<<std::endl;
    std::cout<<"\n p_total is:"<<p_total<<std::endl;
    std::cout << std::fixed << std::setprecision(2) << "CPU time used: "
              << 1000.0 * (c_end - c_start) / CLOCKS_PER_SEC << " ms\n"
              << "Wall clock time passed: "
              << std::chrono::duration<double, std::milli>(t_end-t_start).count()
              << " ms\n";
    return 0;
}