#include<iostream>
#include <Eigen/Eigenvalues>
#include"numerics.hpp"
#include"reddm.hpp"
#include"tpoperators.hpp"
#include "files.hpp"

using namespace Many_Body;
int main(int argc, char *argv[])
{

  size_t M{};
  size_t L{};
  double t0{};
  double omega{};
  double gamma{};
  try
  {
    options_description desc{"Options"};
    desc.add_options()
      ("help,h", "Help screen")
      ("L", value(&L)->default_value(4), "L")
      ("M", value(&M)->default_value(2), "M")
      ("t", value(&t0)->default_value(1.), "t0")
      ("gam", value(&gamma)->default_value(1.), "gamma")
      ("omg", value(&omega)->default_value(1.), "omega");

    variables_map vm;
    store(parse_command_line(argc, argv, desc), vm);
    notify(vm);

    if (vm.count("help"))
      {std::cout << desc << '\n'; return 0;}
    else{
      if (vm.count("L"))
      {      std::cout << "L: " << vm["L"].as<size_t>() << '\n';

      }
     if (vm.count("M,m"))
      {
	std::cout << "M: " << vm["M"].as<size_t>() << '\n';

      }
      if (vm.count("t"))
      {
	std::cout << "t0: " << vm["t"].as<double>() << '\n';
      }
       if (vm.count("omg"))
      {
	std::cout << "omega: " << vm["omg"].as<double>() << '\n';
      }
       if (vm.count("gam"))
      {
	std::cout << "gamma: " << vm["gam"].as<double>() << '\n';
      }
    }
  }
  catch (const error &ex)
  {
    std::cerr << ex.what() << '\n';
    return 0;
  }

    using Mat= Operators::Mat;

   using HolsteinBasis= TensorProduct<ElectronBasis, PhononBasis>;

  ElectronBasis e( L, 1);
    std::cout<< e<<std::endl;

  PhononBasis ph(L, M);
  std::cout<< ph<<std::endl;
  HolsteinBasis TP(e, ph);
    std::cout<< TP << std::endl;
    std::cout<< TP.dim << std::endl;
        Mat E1=Operators::EKinOperatorL(TP, e, t0, true);
      Mat Ebdag=Operators::NBosonCOperator(TP, ph, gamma, true);
      Mat Eb=Operators::NBosonDOperator(TP, ph, gamma, true);
      Mat Eph=Operators::NumberOperator(TP, ph, omega,  true);

      //Mat E=Operators::NumberOperatore(TP, e, 1, false);
      //    std::cout<< HH << std::endl;
      Eigen::VectorXd eigenVals(TP.dim);
      Mat H=E1+Eph  +Ebdag + Eb;

      // state
//         Eigen::VectorXcd evec=Eigen::VectorXcd::Zero(H.rows());
// 	evec(3)=1./std::sqrt(3*2);
// 	evec(7)=std::exp(-im*static_cast<std::complex<double>>(2*pi/3))*1./std::sqrt(3*2);
// 	   evec(11)=std::exp(-im*static_cast<std::complex<double>>(4*pi/3))*1./std::sqrt(3*2);
// 	   evec(21)=1./std::sqrt(3*2);
// 	evec(22)=std::exp(-im*static_cast<std::complex<double>>(2*pi/3))*1./std::sqrt(3*2);
// 	evec(23)=std::exp(-im*static_cast<std::complex<double>>(4*pi/3))*1./std::sqrt(3*2);
// 	std::cout<< " E in "<<(evec.adjoint()*H*evec)(0)<<std::endl;
// 	std::cout<< " norm "<<(evec.adjoint()*evec)(0)<<std::endl;
// auto ov=makeRedDMTP(TP, ph,  0, evec);
//  for(auto g : ov)
//    {
//      std::cout<< g << std::endl;
//      std::cout<< std::endl<<std::endl;
//    }

      Eigen::MatrixXd HH=Eigen::MatrixXd(H);
	  Eigen::MatrixXd HH2=Eigen::MatrixXd(H);
	  Eigen::VectorXd ev=Eigen::VectorXd(TP.dim);
	  bool isDiag=false;
	 //  std::vector<double> Tr={0.0100, 0.1000, 1.000, 2.0000, 5.0000, 10.0000};
	 //  for(auto t: Tr){
	 //      std::string sT=std::string(std::to_string(t)).substr(0,6);
 auto t=T;
	       auto optModes=makeThermalRDMTP(HH,ev,  TP, t, isDiag, 0);
	 //  //	  std::cout << "sum of all eigenvalues "<< optModes.sum()<< std::endl;
	 //      isDiag=true;
	  int n=0;
	  double ent{0};
	  for(auto& l : optModes)
	    {
	      	      std::string sn=std::string(std::to_string(n)).substr(0,1);
		      //std::string filename="OML"+std::to_string(L)+"M"+std::to_string(M)+"t0_"+"1.0"+"gam"+sgam+"omg"+ somg+"T"+ sT+ "esec" +sn  + ".bin";
		 std::cout << n << std::endl;
		 for(int i=0; i<l.rows(); i++)
		   {
		     ent-=l(i)*std::log(l(i));
		   }
		 std::cout<< "value "<< " fot T = "<< t <<std::endl;
		 	      std::cout<< l<<std::endl;
			      std::cout << " and sum "<< l.sum()<< std::endl;
			      //bin_write(filename, l);
		 n++;
	    }

	  std::cout<< "ENTROPY "<< ent << std::endl;
  return 0;
}


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

/*
 Copyright (C) 2007, 2008 Klaus Spanderen

 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 matrix.hpp
    \brief matrix used in linear algebra.
*/

#include <ql/math/matrix.hpp>
#if defined(QL_PATCH_MSVC)
#pragma warning(push)
#pragma warning(disable:4180)
#pragma warning(disable:4127)
#endif

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif

#if defined(__clang__)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wunused-function"
#endif

#if !defined(QL_NO_UBLAS_SUPPORT)
#if BOOST_VERSION > 106300
#include <boost/serialization/array_wrapper.hpp>
#endif
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#endif

#if defined(QL_PATCH_MSVC)
#pragma warning(pop)
#endif

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

#if defined(__clang__)
#pragma clang diagnostic pop
#endif

namespace QuantLib {

    Disposable<Matrix> inverse(const Matrix& m) {
        #if !defined(QL_NO_UBLAS_SUPPORT)

        QL_REQUIRE(m.rows() == m.columns(), "matrix is not square");

        boost::numeric::ublas::matrix<Real> a(m.rows(), m.columns());

        std::copy(m.begin(), m.end(), a.data().begin());

        boost::numeric::ublas::permutation_matrix<Size> pert(m.rows());

        // lu decomposition
        Size singular = 1;
        try {
            singular = lu_factorize(a, pert);
        } catch (const boost::numeric::ublas::internal_logic& e) {
            QL_FAIL("lu_factorize error: " << e.what());
        } catch (const boost::numeric::ublas::external_logic& e) {
            QL_FAIL("lu_factorize error: " << e.what());
        }
        QL_REQUIRE(singular == 0, "singular matrix given");

        boost::numeric::ublas::matrix<Real>
            inverse = boost::numeric::ublas::identity_matrix<Real>(m.rows());

        // backsubstitution
        try {
            boost::numeric::ublas::lu_substitute(a, pert, inverse);
        } catch (const boost::numeric::ublas::internal_logic& e) {
            QL_FAIL("lu_substitute error: " << e.what());
        }

        Matrix retVal(m.rows(), m.columns());
        std::copy(inverse.data().begin(), inverse.data().end(),
                  retVal.begin());

        return retVal;

        #else
        QL_FAIL("this version of gcc does not support "
                "the Boost uBLAS library");
        #endif
    }

    Real determinant(const Matrix& m) {
        #if !defined(QL_NO_UBLAS_SUPPORT)
        QL_REQUIRE(m.rows() == m.columns(), "matrix is not square");

        boost::numeric::ublas::matrix<Real> a(m.rows(), m.columns());
        std::copy(m.begin(), m.end(), a.data().begin());

        // lu decomposition
        boost::numeric::ublas::permutation_matrix<Size> pert(m.rows());
        /* const Size singular = */ lu_factorize(a, pert);

        Real retVal = 1.0;

        for (Size i=0; i < m.rows(); ++i) {
            if (pert[i] != i)
                retVal *= -a(i,i);
            else
                retVal *=  a(i,i);
        }
        return retVal;

        #else
        QL_FAIL("this version of gcc does not support "
                "the Boost uBLAS library");
        #endif
    }
}


#include <vector>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <unsupported/Eigen/MatrixFunctions>

#include "ihgp/InfiniteHorizonGP.hpp"

const double InfiniteHorizonGP::DARE_EPS = 1e-10;
const int InfiniteHorizonGP::DARE_MAXIT = 100;

InfiniteHorizonGP::InfiniteHorizonGP(const double dt, const Eigen::MatrixXd &F, const Eigen::MatrixXd &HH, const Eigen::MatrixXd &Pinf, const double &R, const std::vector<Eigen::MatrixXd> &dF, const std::vector<Eigen::MatrixXd> &dPinf, const std::vector<double> &dR)
{
    // Solve A and Q (stationary systems)
    A = (F*dt).exp();
    Q = Pinf - A*Pinf*A.transpose();
    Rc = R;

    // Assign measurement model
    H = HH;

    // Solve the discrete algebraic Riccati equation
    Eigen::MatrixXd PP = InfiniteHorizonGP::DARE(A,H,Q,R);
    PP_update = PP;

    // Stationary innovation variance
    S = (H*PP*H.transpose())(0) + R;

    // Stationary gain
    K = PP*H.transpose()/S;

    // State covariance
    PF = PP - K*H*PP;

    // Pre-calculate
    HA = (H*A).transpose();
    AKHA = A-K*H*A;

    // Number of parameters
    nparam = (int)dF.size();

    // State dimensionality
    int dim = (int)F.rows();

    // Initialize mean and helpers
    m.setZero(dim);

    // Prepare
    Eigen::MatrixXd AK = A*K;
    Eigen::MatrixXd FF(2*dim,2*dim);
    FF.setZero();
    FF.topLeftCorner(dim,dim) = F;
    FF.bottomRightCorner(dim,dim) = F;

    // Allocate the arrays
    HdA  = new Eigen::VectorXd[nparam];
    dK = new Eigen::VectorXd[nparam];
    dAKHA = new Eigen::MatrixXd[nparam];
    dS = new double[nparam];
    dm = new Eigen::VectorXd[nparam];

    // Pre-calculate the needed derivative matrices
    for (int j=0; j<nparam; j++)
    {
        // Assign derivative of drift model matrix
        FF.bottomLeftCorner(dim,dim) = dF[j];

        // Solve the matrix exponential (see ...)
        Eigen::MatrixXd AA = (FF*dt).exp();

        // Extract the derivative of the discrete-time dynamic model
        Eigen::MatrixXd dA = AA.bottomLeftCorner(dim,dim);
        Eigen::MatrixXd dQ = dPinf[j] - dA*Pinf*A.transpose() - A*dPinf[j]*A.transpose() - A*Pinf*dA.transpose();
        dQ = .5*(dQ+dQ.transpose()).eval();

        // Precalculate C
        Eigen::MatrixXd C = dA*PP*A.transpose() + A*PP*dA.transpose() - dA*PP*H.transpose()*AK.transpose() - AK*H*PP*dA.transpose() + AK*dR[j]*AK.transpose() + dQ;
        C = .5*(C+C.transpose()).eval();

        // Solve DARE
        Eigen::MatrixXd dPP = InfiniteHorizonGP::DARE(A-AK*H,Eigen::MatrixXd::Zero(dim,dim),C,0.0);

        // Evaluate dS and dK
        dS[j] = (H*dPP*H.transpose())(0) + dR[j];
        dK[j] = dPP*H.transpose()/S - PP*H.transpose()*(((H*dPP*H.transpose())(0)+dR[j])/S/S);
        dAKHA[j] = dA - dK[j]*H*A - K*H*dA;
        HdA[j] = (H*dA).transpose();

        // Initial dm
        dm[j] = Eigen::VectorXd::Zero(dim);

    }

    // Initialize log likelihood and its gradient
    edata = 0;
    gdata = Eigen::VectorXd::Zero(nparam);
}

InfiniteHorizonGP::~InfiniteHorizonGP()
{
    delete[] HdA;
    delete[] dK;
    delete[] dAKHA;
    delete[] dS;
    delete[] dm;
}

void InfiniteHorizonGP::init_step()
{
    // Initialize log likelihood and its gradient
    edata = 0;
    gdata = Eigen::VectorXd::Zero(nparam);

    // Update
    // Stationary innovation variance
    S = (H*PP_update*H.transpose())(0) + Rc;

    // Stationary gain
    K = PP_update*H.transpose()/S;

    // State covariance
    PF = PP_update - K*H*PP_update;

    // Pre-calculate
    HA = (H*A).transpose();
    AKHA = A-K*H*A;

    //
    MF.clear();
}

void InfiniteHorizonGP::update(const double &y)
{
    // Define constants
    const double PI = 3.141592654;

    // Innovation mean
    double v = y - HA.dot(m);

    // Update marginal likelihood
    edata += .5*v*v/S + .5*log(2*PI) + .5*log(S);

    // Update derivatives
    for (int j=0; j<nparam; j++)
    {
        // Derivatives of innovation mean
        double dv = -HdA[j].dot(m) - HA.dot(dm[j]);

        // Derivatives of marginal likelihood
        gdata(j) += v*dv/S - .5*v*v*dS[j]/S/S + .5*dS[j]/S;

        // Derivatives of state mean
        dm[j] = dAKHA[j]*m + AKHA*dm[j] + dK[j]*y;
    }

    // Recursion for the state mean
    m = AKHA*m + K*y;

    // Store state mean for possible backward pass
    MF.push_back(m);

}

std::vector<double> InfiniteHorizonGP::getEft()
{
    // Solve backward smoother gain
    int dim = (int)A.rows();
    Eigen::MatrixXd PP = A*PF*A.transpose()+Q;
    Eigen::LDLT<Eigen::MatrixXd> PPldl = PP.ldlt();
    Eigen::MatrixXd G = PPldl.solve(A*PF).transpose();

    // Solve smoother state covariance
    // Eigen::MatrixXd QQ = PF-G*PP*G.transpose();
    // QQ = .5*(QQ+QQ.transpose()).eval();
    // P = InfiniteHorizonGP::DARE(G,Eigen::MatrixXd::Zero(dim,dim),QQ,0.0);

    // Output vector
    std::vector<double> Eft;

    // Initialize with last element
    m = MF[MF.size()-1];
    Eft.push_back((H*m)(0));

    // Run backward pass
    for (int k=(int)MF.size()-2; k>=0; k--)
    {
        m = MF[k] + G*(m-A*MF[k]);
        Eft.push_back((H*m)(0));
    }

    // Reverse
    std::reverse(Eft.begin(),Eft.end());

    return Eft;

}

double InfiniteHorizonGP::getVarft()
{
    return (H*P*H.transpose())(0);
}

double InfiniteHorizonGP::getLik()
{
    return edata;
}

Eigen::VectorXd InfiniteHorizonGP::getLikDeriv()
{
    return gdata;
}

Eigen::MatrixXd InfiniteHorizonGP::DARE(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &Q, const double &R)
{

    // Initial guess
    int dim = (int)A.rows();
    Eigen::MatrixXd X(dim,dim);
    Eigen::MatrixXd X_prev(dim,dim);
    Eigen::MatrixXd K(dim,B.rows());
    X.setIdentity();

    // Number of loops
    int n = 0;

    // Iterate a maximum of 100 iterations
    while (n < DARE_MAXIT)
    {
        // Step
        n++;
        X_prev = X;

        // Gain (NB: Does not work in the general case, but for scalar R and possibly zero B)
        if (abs(R) < 1e-15)
        {
            K.setZero();
        } else {
            K = A*(X*B.transpose() / ((B*X*B.transpose())(0)+R));
        }

        // Recursion
        X = (A - K*B)*X*(A - K*B).transpose() + K*R*K.transpose() + Q;

        // Check if we should break (use the Frobenius norm)
        if ((X-X_prev).norm() < DARE_EPS)
        {
            break;
        }
    }

    return X;
}


// Copyright (c) 2021 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <pybind11/numpy.h>
#include <pybind11/stl.h>

#include <Eigen/Core>
#include <boost/geometry.hpp>
#include <iostream>
#include <optional>

#include "pyinterp/axis.hpp"
#include "pyinterp/detail/broadcast.hpp"
#include "pyinterp/detail/isviewstream.hpp"
#include "pyinterp/detail/math/binning.hpp"
#include "pyinterp/detail/math/streaming_histogram.hpp"
#include "pyinterp/eigen.hpp"

namespace pyinterp {

/// Group a number of more or less continuous values into a smaller number of
/// "bins" located on a grid.
template <typename T>
class Histogram2D {
 public:
  /// Statistics handled by this object.
  using StreamingHistogram = detail::math::StreamingHistogram<T>;

  /// Default constructor
  ///
  /// @param x Definition of the bin centers for the X axis of the grid.
  /// @param y Definition of the bin centers for the Y axis of the grid.
  Histogram2D(std::shared_ptr<Axis<double>> x, std::shared_ptr<Axis<double>> y,
              const std::optional<size_t>& bin_count)
      : x_(std::move(x)), y_(std::move(y)), histogram_(x_->size(), y_->size()) {
    if (bin_count) {
      for (int ix = 0; ix < histogram_.rows(); ++ix) {
        for (int jx = 0; jx < histogram_.cols(); ++jx) {
          histogram_(ix, jx).resize(*bin_count);
        }
      }
    }
  }

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

  /// Copy constructor
  ///
  /// @param rhs right value
  Histogram2D(const Histogram2D& rhs) = delete;

  /// Move constructor
  ///
  /// @param rhs right value
  Histogram2D(Histogram2D&& rhs) noexcept = delete;

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

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

  /// Inserts new values in the grid from Z values for X, Y data
  /// coordinates.
  void push(const pybind11::array_t<T>& x, const pybind11::array_t<T>& y,
            const pybind11::array_t<T>& z) {
    detail::check_array_ndim("x", 1, x, "y", 1, y, "z", 1, z);
    detail::check_ndarray_shape("x", x, "y", y, "z", z);

    auto _x = x.template unchecked<1>();
    auto _y = y.template unchecked<1>();
    auto _z = z.template unchecked<1>();

    {
      pybind11::gil_scoped_release release;

      const auto& x_axis = static_cast<pyinterp::detail::Axis<double>&>(*x_);
      const auto& y_axis = static_cast<pyinterp::detail::Axis<double>&>(*y_);

      for (pybind11::ssize_t idx = 0; idx < x.size(); ++idx) {
        auto value = _z(idx);

        if (!std::isnan(value)) {
          auto ix = x_axis.find_index(_x(idx), true);
          auto iy = y_axis.find_index(_y(idx), true);

          if (ix != -1 && iy != -1) {
            histogram_(ix, iy)(value);
          }
        }
      }
    }
  }

  /// Reset the statistics.
  void clear() {
    for (Eigen::Index ix = 0; ix < histogram_.rows(); ++ix) {
      for (Eigen::Index jx = 0; jx < histogram_.cols(); ++jx) {
        histogram_(ix, jx).clear();
      }
    }
  }

  /// Compute the count of points within each bin.
  [[nodiscard]] auto count() const -> pybind11::array_t<uint64_t> {
    return calculate_statistics<decltype(&StreamingHistogram::count),
                                uint64_t>(&StreamingHistogram::count);
  }

  /// Compute the minimum of values for points within each bin.
  [[nodiscard]] auto min() const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::min);
  }

  /// Compute the maximum of values for points within each bin.
  [[nodiscard]] auto max() const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::max);
  }

  /// Compute the mean of values for points within each bin.
  [[nodiscard]] auto mean() const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::mean);
  }

  /// Compute the variance of values for points within each bin.
  [[nodiscard]] auto variance() const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::variance);
  }

  /// Compute the kurtosis of values for points within each bin.
  [[nodiscard]] auto kurtosis() const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::kurtosis);
  }

  /// Compute the quantile of values for points within each bin.
  [[nodiscard]] auto quantile(const T& q) const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::quantile, q);
  }

  /// Compute the skewness of values for points within each bin.
  [[nodiscard]] auto skewness() const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::skewness);
  }
  /// Compute the sum of weights within each bin.
  [[nodiscard]] auto sum_of_weights() const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::sum_of_weights);
  }

  /// Gets the X-Axis
  [[nodiscard]] inline auto x() const -> std::shared_ptr<Axis<double>> {
    return x_;
  }

  /// Gets the Y-Axis
  [[nodiscard]] inline auto y() const -> std::shared_ptr<Axis<double>> {
    return y_;
  }

  /// Pickle support: get state of this instance
  [[nodiscard]] auto getstate() const -> pybind11::tuple {
    return pybind11::make_tuple(x_->getstate(), y_->getstate(), marshal());
  }

  /// Pickle support: set state of this instance
  static auto setstate(const pybind11::tuple& state)
      -> std::unique_ptr<Histogram2D<T>> {
    if (state.size() != 3) {
      throw std::invalid_argument("invalid state");
    }

    // Unmarshalling X-Axis
    auto x = std::make_shared<Axis<double>>();
    *x = Axis<double>::setstate(state[0].cast<pybind11::tuple>());

    // Unmarshalling Y-Axis
    auto y = std::make_shared<Axis<double>>();
    *y = Axis<double>::setstate(state[1].cast<pybind11::tuple>());

    // Unmarshalling instance
    auto result = std::make_unique<Histogram2D<T>>(x, y, 40);
    auto marshal_data = state[2].cast<pybind11::bytes>();
    Histogram2D::unmarshal(marshal_data.cast<std::string_view>(),
                           result->histogram_);
    return result;
  }

  /// Aggregation of statistics
  auto operator+=(const Histogram2D& other) -> Histogram2D& {
    if (*x_ != *(other.x_) || *y_ != *(other.y_)) {
      throw std::invalid_argument("Unable to combine different grids");
    }
    for (Eigen::Index ix = 0; ix < histogram_.rows(); ++ix) {
      for (Eigen::Index iy = 0; iy < histogram_.cols(); ++iy) {
        auto& lhs = histogram_(ix, iy);
        auto& rhs = other.histogram_(ix, iy);

        // Statistics are defined only in the other instance.
        if (lhs.size() == 0 && rhs.size() != 0) {
          lhs = rhs;
          // If the statistics are defined in both instances they can be
          // combined.
        } else if (lhs.size() != 0 && rhs.size() != 0) {
          lhs += rhs;
        }
      }
    }
    return *this;
  }

  /// Returns the histogram for each bin.
  auto histograms() const -> pybind11::array_t<detail::math::Bin<T>> {
    auto bins_count = size_t(0);
    for (Eigen::Index ix = 0; ix < histogram_.rows(); ++ix) {
      for (Eigen::Index iy = 0; iy < histogram_.cols(); ++iy) {
        bins_count = std::max(
            bins_count, histogram_(ix, iy).size());
      }
    }
    auto result =
        pybind11::array_t<detail::math::Bin<T>>(pybind11::array::ShapeContainer(
            {x_->size(), y_->size(),
             static_cast<pybind11::ssize_t>(bins_count)}));
    auto _result = result.template mutable_unchecked<3>();
    {
      auto gil = pybind11::gil_scoped_release();

      for (Eigen::Index ix = 0; ix < histogram_.rows(); ++ix) {
        for (Eigen::Index iy = 0; iy < histogram_.cols(); ++iy) {
          auto iz = size_t(0);
          auto& bins = histogram_(ix, iy).bins();
          for (iz = 0; iz < bins.size(); ++iz) {
            _result(ix, iy, iz) = bins[iz];
          }
          for (; iz < bins_count; ++iz) {
            _result(ix, iy, iz) =
                detail::math::Bin<T>{std::numeric_limits<T>::quiet_NaN(), T(0)};
          }
        }
      }
    }
    return result;
  }

 private:
  /// Grid axis
  std::shared_ptr<Axis<double>> x_;
  std::shared_ptr<Axis<double>> y_;

  /// Statistics grid
  Matrix<StreamingHistogram> histogram_;

  /// Calculation of a given statistical variable.
  template <typename Func, typename Type = T, typename... Args>
  [[nodiscard]] auto calculate_statistics(const Func& func, Args... args) const
      -> pybind11::array_t<Type> {
    pybind11::array_t<Type> z({x_->size(), y_->size()});
    auto _z = z.template mutable_unchecked<2>();
    {
      pybind11::gil_scoped_release release;

      for (Eigen::Index ix = 0; ix < histogram_.rows(); ++ix) {
        for (Eigen::Index iy = 0; iy < histogram_.cols(); ++iy) {
          _z(ix, iy) = (histogram_(ix, iy).*func)(args...);
        }
      }
    }
    return z;
  }

  [[nodiscard]] auto marshal() const -> pybind11::bytes {
    auto gil = pybind11::gil_scoped_release();
    auto ss = std::stringstream();
    ss.exceptions(std::stringstream::failbit);
    auto rows = histogram_.rows();
    ss.write(reinterpret_cast<const char*>(&rows), sizeof(rows));
    auto cols = histogram_.cols();
    ss.write(reinterpret_cast<const char*>(&cols), sizeof(cols));
    for (int ix = 0; ix < histogram_.rows(); ++ix) {
      for (int jx = 0; jx < histogram_.cols(); ++jx) {
        auto marshal_hist = static_cast<std::string>(histogram_(ix, jx));
        auto size = marshal_hist.size();
        ss.write(reinterpret_cast<const char*>(&size), sizeof(size));
        ss.write(marshal_hist.c_str(), size);
      }
    }
    return ss.str();
  }

  static auto unmarshal(const std::string_view& data,
                        Matrix<StreamingHistogram>& histogram) -> void {
    auto gil = pybind11::gil_scoped_release();
    auto ss = detail::isviewstream(data);
    ss.exceptions(std::stringstream::failbit);

    try {
      auto rows = Eigen::Index(0);
      auto cols = Eigen::Index(0);
      ss.read(reinterpret_cast<char*>(&rows), sizeof(rows));
      ss.read(reinterpret_cast<char*>(&cols), sizeof(cols));
      if (rows != histogram.rows() || cols != histogram.cols()) {
        throw std::invalid_argument("invalid state");
      }
      for (int ix = 0; ix < rows; ++ix) {
        for (int jx = 0; jx < cols; ++jx) {
          auto size = size_t(0);
          ss.read(reinterpret_cast<char*>(&size), sizeof(size));
          auto marshal_hist = std::string(size, '\0');
          ss.read(marshal_hist.data(), size);
          histogram(ix, jx) = std::move(StreamingHistogram(marshal_hist));
        }
      }
    } catch (std::ios_base::failure&) {
      throw std::invalid_argument("invalid state");
    }
  }
};

}  // namespace pyinterp


#ifndef __DYNAUTODIFF_DISTRIBUTIONS__
#define __DYNAUTODIFF_DISTRIBUTIONS__
#include "Common.hpp"
#include "Var.hpp"
#include "boost/math/special_functions/gamma.hpp"
#include <boost/math/special_functions/math_fwd.hpp>
#include <numbers>

#define UNWRAP(...) __VA_ARGS__

namespace DynAutoDiff {
namespace bm = boost::math;
#define DISTFUNCTIONTEMPLATE(functionname, structname, functionargs, funinput, assertstmt,         \
                             requires_grad_stmt)                                                   \
    template <Reduction R = Sum, typename T = double>                                              \
    std::shared_ptr<Var<T>> functionname(std::shared_ptr<Var<T>> X functionargs) {                 \
        assertstmt std::vector<std::shared_ptr<Var<T>>> input_nodes{funinput};                     \
        if constexpr (R == Reduction::None) {                                                      \
            return std::make_shared<Var<T>>(X->rows(), 1, requires_grad_stmt, input_nodes,         \
                                            std::make_unique<structname##EvalGrad<T>>(R));         \
        } else {                                                                                \
            return std::make_shared<Var<T>>(1, 1, requires_grad_stmt, input_nodes,                 \
                                            std::make_unique<structname##EvalGrad<T>>(R));         \
        }                                                                                \
    };

template <typename T = double> struct LnMVNormalDenEvalGrad : EvalGradFunctionBase<T> {
    TMat<T> iS;
    TMat<T> Xmu;
    int N, d;
    int R;
    LnMVNormalDenEvalGrad(int R) : R(R){};
    double Sd; // Sigma determinant
    std::string get_name() const override { return "LnMVNormalDenEvalGrad"; };
    boost::json::object to_json() const override {
        boost::json::object res;
        res["name"] = "ln_mvnormal_den";
        res["Reduction"] = R;
        return res;
    };
    void eval(TMap<T> &dest, const std::vector<TMap<T>> &inputs) override {
        N = inputs[0].rows();
        d = inputs[0].cols();
        iS = inputs[2].inverse();
        Sd = inputs[2].determinant();

        Xmu.resize(N, d);
        TMat<T> res(N, 1);

        for (int i = 0; i < N; ++i) {
            Xmu.row(i) = (inputs[0].row(i).transpose() - inputs[1]).transpose();
            res.coeffRef(i, 0) = -Xmu.row(i) * iS * Xmu.row(i).transpose();
        };

        if (R == Reduction::None) {
            dest = 0.5 * (res.array() - std::log(Sd) - d * log(2 * std::numbers::pi));
        } else {
            dest.coeffRef(0, 0) =
                0.5 * (res.sum() - N * std::log(Sd) - N * d * log(2 * std::numbers::pi));

            if (R == Reduction::Mean) {
                dest = dest.array() / N;
            }
        }
    };
    std::vector<TMat<T>> grad(const std::shared_ptr<Var<T>> &current) override {
        const auto &X = current->input_node(0)->val();
        const auto &mu = current->input_node(1)->val();
        const auto &Sigma = current->input_node(2)->val();

        std::vector<TMat<T>> res;

        // X
        if (current->input_node(0)->requires_grad()) {
            TMat<T> Xg(N, d);
            if (R == Reduction::None) {
                for (int i = 0; i < N; ++i) {
                    Xg.row(i) =
                        -(iS * Xmu.row(i).transpose()).transpose() * current->grad().coeff(i, 0);
                }
            } else {
                Xg = -(iS * Xmu.transpose()).transpose() * current->grad().coeff(0, 0);
            }
            // std::cout << Xg << std::endl;
            if (R == Reduction::Mean) {
                Xg = Xg / N;
            }
            res.emplace_back(Xg);
        } else {
            res.emplace_back(TMat<T>());
        };
        // mu
        if (current->input_node(1)->requires_grad()) {
            TMat<T> mug(d, 1);
            // mu's gradient is negation of X's.
            if (current->input_node(0)->requires_grad()) {
                mug = -res[0].colwise().sum().transpose();
            } else {
                if (R == Reduction::None) {
                    TMat<T> Xg(N, d);
                    for (int i = 0; i < N; ++i) {
                        Xg.row(i) =
                            (iS * Xmu.row(i).transpose()).transpose() * current->grad().coeff(i, 0);
                    }
                    mug = Xg.colwise().sum().transpose();
                } else {
                    mug = (iS * Xmu.transpose()).rowwise().sum() * current->g();
                }
            }
            if (R == Reduction::Mean) {
                mug = mug / N;
            }
            res.emplace_back(mug);

        } else {
            res.emplace_back(TMat<T>());
        }
        // Sigma
        if (current->input_node(2)->requires_grad()) {
            TMat<T> Sg(d, d);

            if (R == Reduction::None) {
                Sg.setZero();
                for (int i = 0; i < N; ++i) {
                    Sg = Sg + iS.transpose() * Xmu.row(i).transpose() * Xmu.row(i) *
                                  iS.transpose() * current->grad().coeff(i, 0);
                }
                Sg = Sg - iS.transpose() * current->grad().sum();
            } else {
                Sg = iS.transpose() * Xmu.transpose() * Xmu * iS.transpose() *
                     current->grad().coeff(0, 0);
                Sg = Sg - iS.transpose() * N;
            }

            Sg = Sg * 0.5;

            if (R == Reduction::Mean) {
                Sg = Sg / N;
            }
            res.emplace_back(Sg);
        } else {
            res.emplace_back(TMat<T>());
        }

        return res;
    }
};

template <typename T = double> struct LnNormalDenEvalGrad : EvalGradFunctionBase<T> {
    TMat<T> Xmu, Xmu2;
    int N;
    int R;
    LnNormalDenEvalGrad(int R) : R(R){};
    std::string get_name() const override { return "LnNormalDenEvalGrad"; };
    boost::json::object to_json() const override {
        boost::json::object res;
        res["name"] = "ln_normal_den";
        res["Reduction"] = R;
        return res;
    };
    void eval(TMap<T> &dest, const std::vector<TMap<T>> &inputs) override {
        N = inputs[0].rows();
        T mu = inputs[1].coeff(0, 0);
        T sigma = inputs[2].coeff(0, 0);
        const auto &X = inputs[0];

        TMat<T> res(N, 1);
        Xmu = X.array() - mu;
        Xmu2 = Xmu.array().pow(2);

        if (R == Reduction::None) {
            dest = -0.5 * std::log(2 * std::numbers::pi) - std::log(sigma) -
                   0.5 * Xmu2.array() / (sigma * sigma);
        } else {
            dest.coeffRef(0, 0) = -0.5 * N * std::log(2 * std::numbers::pi) - N * std::log(sigma) -
                                  0.5 * Xmu2.sum() / (sigma * sigma);
            if (R == Reduction::Mean) {
                dest = dest.array() / N;
            }
        }
    };
    std::vector<TMat<T>> grad(const std::shared_ptr<Var<T>> &current) override {
        const auto &X = current->input_node(0)->val();
        T mu = current->input_node(1)->val().coeff(0, 0);
        T sigma = current->input_node(2)->val().coeff(0, 0);

        std::vector<TMat<T>> res;

        // X
        if (current->input_node(0)->requires_grad()) {
            TMat<T> Xg(N, 1);
            if (R == Reduction::None) {
                Xg = -Xmu.array() / (sigma * sigma) * current->grad().array();
            } else {
                Xg = -Xmu.array() / (sigma * sigma) * current->grad().coeff(0, 0);

                if (R == Reduction::Mean) {
                    Xg = Xg / N;
                }
            }
            // std::cout << Xg << std::endl;
            res.emplace_back(Xg);
        } else {
            res.emplace_back(TMat<T>());
        };
        // mu
        if (current->input_node(1)->requires_grad()) {
            TMat<T> mug(1, 1);
            // mu's gradient is negation of X's.
            if (current->input_node(0)->requires_grad()) {
                mug.coeffRef(0, 0) = -res[0].sum();
            } else {
                if (R == Reduction::None) {
                    mug.coeffRef(0, 0) = (Xmu.array() * current->grad().array()).sum() / (sigma * sigma);
                } else {
                    mug.coeffRef(0, 0) = Xmu.sum() / (sigma * sigma) * current->grad().coeff(0, 0);
                    if (R == Reduction::Mean) {
                        mug = mug / N;
                    }
                }
            }
            res.emplace_back(mug);
        } else {
            res.emplace_back(TMat<T>());
        }
        // Sigma
        if (current->input_node(2)->requires_grad()) {
            TMat<T> sg(1, 1);

            if (R == Reduction::None) {
                sg.coeffRef(0, 0) =
                    ((-1.0 / sigma + Xmu2.array() / (std::pow(sigma, 3))) * current->grad().array()).sum();
            } else {
                sg.coeffRef(0, 0) = (-N / sigma + Xmu2.sum() / (std::pow(sigma, 3))) *
                                    current->grad().coeff(0, 0);
                if (R == Reduction::Mean) {
                    sg = sg / N;
                }
            }
            res.emplace_back(sg);
        } else {
            res.emplace_back(TMat<T>());
        }

        return res;
    }
};

template <typename T = double> struct LnTDenEvalGrad : EvalGradFunctionBase<T> {
    TMat<T> Xmu;
    int N;
    int R;
    TMat<T> xm, xm2, fx, lfx;
    LnTDenEvalGrad(int R) : R(R){};
    std::string get_name() const override { return "LnTDenEvalGrad"; };
    boost::json::object to_json() const override {
        boost::json::object res;
        res["name"] = "ln_t_den";
        res["Reduction"] = R;
        return res;
    };
    void eval(TMap<T> &dest, const std::vector<TMap<T>> &inputs) override {
        N = inputs[0].rows();
        fx.resize(N, 1);
        xm.resize(N, 1);
        xm2.resize(N, 1);
        lfx.resize(N, 1);
        T mu = inputs[1].coeff(0, 0);
        T sigma = inputs[2].coeff(0, 0);
        T nu = inputs[3].coeff(0, 0);
        const auto &X = inputs[0];

        xm = X.array() - mu;
        xm2 = xm.array().pow(2);
        fx = xm2.array() / (sigma * sigma) / nu + 1;
        lfx = fx.array().log();

        if (R == Reduction::None) {
            dest = bm::lgamma(nu / 2 + 0.5) - bm::lgamma(nu / 2) -
                   0.5 * std::log(nu * std::numbers::pi) - std::log(sigma) -
                   (nu + 1) / 2 * lfx.array();
        } else {
            dest.coeffRef(0, 0) = N * (bm::lgamma(nu / 2 + 0.5) - bm::lgamma(nu / 2) -
                                       0.5 * std::log(nu * std::numbers::pi) - std::log(sigma)) -
                                  (nu + 1) / 2 * lfx.array().sum();
            if (R == Reduction::Mean) {
                dest = dest.array() / N;
            }
        }
    };
    std::vector<TMat<T>> grad(const std::shared_ptr<Var<T>> &current) override {
        const auto &X = current->input_node(0)->val();
        T mu = current->input_node(1)->val().coeff(0, 0);
        T sigma = current->input_node(2)->val().coeff(0, 0);
        T nu = current->input_node(3)->val().coeff(0, 0);

        std::vector<TMat<T>> res;

        // X
        if (current->input_node(0)->requires_grad()) {
            TMat<T> Xg(N, 1);
            if (R == Reduction::None) {
                Xg = -(1 + 1.0 / nu) / (sigma * sigma) * xm.array() / fx.array() *
                     current->grad().array();
            } else {
                Xg = -(1 + 1.0 / nu) / (sigma * sigma) * (xm.array() / fx.array()) *
                     current->grad().coeff(0, 0);

                if (R == Reduction::Mean) {
                    Xg = Xg / N;
                }
            }
            // std::cout << Xg << std::endl;
            res.emplace_back(Xg);
        } else {
            res.emplace_back(TMat<T>());
        };
        // mu
        if (current->input_node(1)->requires_grad()) {
            TMat<T> mug(1, 1);
            // mu's gradient is negation of X's.
            if (current->input_node(0)->requires_grad()) {
                mug.coeffRef(0, 0) = -res[0].sum();
            } else {
                if (R == Reduction::None) {
                    mug.coeffRef(0, 0) = (1 + 1.0 / nu) / (sigma * sigma) *
                                         (xm.array() / fx.array() * current->grad().array()).sum();
                } else {
                    mug.coeffRef(0, 0) = (1 + 1.0 / nu) / (sigma * sigma) *
                                         (xm.array() / fx.array()).sum() *
                                         current->grad().coeff(0, 0);
                    if (R == Reduction::Mean) {
                        mug = mug / N;
                    }
                }
            }
            res.emplace_back(mug);
        } else {
            res.emplace_back(TMat<T>());
        }
        // sigma
        if (current->input_node(2)->requires_grad()) {
            TMat<T> sg(1, 1);

            if (R == Reduction::None) {
                sg.coeffRef(0, 0) = ((-1.0 / sigma + (1 + 1.0 / nu) / (sigma * sigma * sigma) *
                                                         xm2.array() / fx.array()) *
                                     current->grad().array())
                                        .sum();
            } else {
                sg.coeffRef(0, 0) = (-N / sigma + (1 + 1.0 / nu) / (sigma * sigma * sigma) *
                                                      (xm2.array() / fx.array()).sum()) *
                                    current->grad().coeff(0, 0);
                if (R == Reduction::Mean) {
                    sg = sg / N;
                }
            }
            res.emplace_back(sg);
        } else {
            res.emplace_back(TMat<T>());
        }
        // nu
        if (current->input_node(3)->requires_grad()) {
            TMat<T> nug(1, 1);

            if (R == Reduction::None) {
                nug.coeffRef(0, 0) =
                    0.5 * ((bm::digamma((nu + 1) / 2) - bm::digamma(nu / 2) - 1.0 / nu -
                            (lfx.array() -
                             (nu + 1) / (nu * nu) / (sigma * sigma) * xm2.array() / fx.array())) *
                           current->grad().array())
                              .sum();
            } else {
                nug.coeffRef(0, 0) =
                    0.5 *
                    (N * (bm::digamma((nu + 1) / 2) - bm::digamma(nu / 2) - 1.0 / nu) -
                     (lfx.array() -
                      (nu + 1) / (nu * nu) / (sigma * sigma) * xm2.array() / fx.array())
                         .sum()) *
                    current->grad().coeff(0, 0);
                if (R == Reduction::Mean) {
                    nug = nug / N;
                }
            }
            res.emplace_back(nug);
        } else {
            res.emplace_back(TMat<T>());
        }
        return res;
    }
};

DISTFUNCTIONTEMPLATE(
    ln_mvnormal_den, LnMVNormalDen,
    UNWRAP(, std::shared_ptr<Var<T>> mu, std::shared_ptr<Var<T>> Sigma), UNWRAP(X, mu, Sigma),
    UNWRAP(assert(((mu->cols() == 1) && (Sigma->rows() == Sigma->cols())) &&
                  "mu should be a column vector. Sigma should be a square matrix.");),
    X->requires_grad() || mu->requires_grad() || Sigma->requires_grad())
DISTFUNCTIONTEMPLATE(
    ln_normal_den, LnNormalDen, UNWRAP(, std::shared_ptr<Var<T>> mu, std::shared_ptr<Var<T>> sigma),
    UNWRAP(X, mu, sigma),
    UNWRAP(assert(((X->cols() == 1) && (mu->size() == 1) && (sigma->size() == 1)) &&
                  "X must be a column vector. mu and sigma must be a scalar.");),
    X->requires_grad() || mu->requires_grad() || sigma->requires_grad())
DISTFUNCTIONTEMPLATE(
    ln_t_den, LnTDen,
    UNWRAP(, std::shared_ptr<Var<T>> mu, std::shared_ptr<Var<T>> sigma, std::shared_ptr<Var<T>> nu),
    UNWRAP(X, mu, sigma, nu),
    UNWRAP(assert(((X->cols() == 1) && (mu->size() == 1) && (sigma->size() == 1) &&
                   (nu->size() == 1)) &&
                  "X must be a column vector. mu, sigma and nu must be scalars.");),
    X->requires_grad() || mu->requires_grad() || sigma->requires_grad() || nu->requires_grad())
#undef DISTFUNCTIONTEMPLATE
#undef UNWRAP
};     // namespace DynAutoDiff
#endif // !


/*!
 * @file
 * Defines the @ref Integer datatype.
 *
 *
 * @copyright Louis Dionne 2014
 * Distributed under the Boost Software License, Version 1.0.
 *         (See accompanying file LICENSE.md or copy at
 *             http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_MPL11_INTEGER_HPP
#define BOOST_MPL11_INTEGER_HPP

#include <boost/mpl11/fwd/integer.hpp>

#include <boost/mpl11/bitwise.hpp>
#include <boost/mpl11/comparable.hpp>
#include <boost/mpl11/detail/logical_or/strict.hpp>
#include <boost/mpl11/enumerable.hpp>
#include <boost/mpl11/group.hpp>
#include <boost/mpl11/integral_domain.hpp>
#include <boost/mpl11/logical.hpp>
#include <boost/mpl11/monoid.hpp>
#include <boost/mpl11/orderable.hpp>
#include <boost/mpl11/ring.hpp>

namespace boost { namespace mpl11 {
    namespace integral_detail {
        template <typename ...T>
        struct or_impl : true_ { };

        template <typename ...T>
        struct or_impl<integer_c<T, false>...> : false_ { };

        template <typename ...T>
        struct and_impl : false_ { };

        template <typename ...T>
        struct and_impl<integer_c<T, true>...> : true_ { };
    } // end namespace integral_detail

    template <typename T, T ...v>
    struct or_<integer_c<T, v>...>
        : detail::logical_or::strict<bool_<(bool)v>...>
    { };

    template <typename T, T ...v>
    struct and_<integer_c<T, v>...> // DeMorgan
        : bool_<!detail::logical_or::strict<bool_<!v>...>::value>
    { };

    template <>
    struct Monoid<Integer> : instantiate<Monoid>::with<Integer> {
        template <typename x, typename y>
        using plus_impl = integer_c<
            decltype(x::value + y::value),
            x::value + y::value
        >;

        template <typename ...>
        using zero_impl = int_<0>;
    };

    template <>
    struct Group<Integer> : instantiate<Group>::with<Integer> {
        template <typename x, typename y>
        using minus_impl = integer_c<
            decltype(x::value - y::value),
            x::value - y::value
        >;

        template <typename x>
        using negate_impl = integer_c<
            decltype(-x::value),
            -x::value
        >;
    };

    template <>
    struct Ring<Integer> : instantiate<Ring>::with<Integer> {
        template <typename x, typename y>
        using mult_impl = integer_c<
            decltype(x::value * y::value),
            x::value * y::value
        >;

        template <typename ...>
        using one_impl = int_<1>;
    };

    template <>
    struct IntegralDomain<Integer>
        : instantiate<IntegralDomain>::with<Integer>
    {
        template <typename x, typename y>
        using div_impl = integer_c<
            decltype(x::value / y::value),
            x::value / y::value
        >;

        template <typename x, typename y>
        using mod_impl = integer_c<
            decltype(x::value % y::value),
            x::value % y::value
        >;
    };

    template <>
    struct Enumerable<Integer> : instantiate<Enumerable>::with<Integer> {
        template <typename x>
        using succ_impl = integer_c<
            decltype(x::value + 1),
            x::value + 1
        >;

        template <typename x>
        using pred_impl = integer_c<
            decltype(x::value - 1),
            x::value - 1
        >;
    };

    template <>
    struct Comparable<Integer> : instantiate<Comparable>::with<Integer> {
        template <typename x, typename y>
        using equal_impl = bool_<x::value == y::value>;

        template <typename x, typename y>
        using not_equal_impl = bool_<x::value != y::value>;
    };

    template <>
    struct Orderable<Integer> : instantiate<Orderable>::with<Integer> {
        template <typename x, typename y>
        using less_impl = bool_<(x::value < y::value)>;

        template <typename x, typename y>
        using less_equal_impl = bool_<(x::value <= y::value)>;

        template <typename x, typename y>
        using greater_impl = bool_<(x::value > y::value)>;

        template <typename x, typename y>
        using greater_equal_impl = bool_<(x::value >= y::value)>;

        template <typename x, typename y>
        using max_impl = if_c<(x::value < y::value), y, x>;

        template <typename x, typename y>
        using min_impl = if_c<(x::value < y::value), x, y>;
    };

    template <>
    struct Bitwise<Integer> : instantiate<Bitwise>::with<Integer> {
        template <typename x, typename y>
        using bitand_impl = integer_c<
            decltype(x::value & y::value),
            x::value & y::value
        >;

        template <typename x, typename y>
        using bitor_impl = integer_c<
            decltype(x::value | y::value),
            x::value | y::value
        >;

        template <typename x, typename y>
        using bitxor_impl = integer_c<
            decltype(x::value ^ y::value),
            x::value ^ y::value
        >;

        template <typename x, typename n>
        using shift_left_impl = integer_c<
            decltype(x::value << n::value),
            (x::value << n::value)
        >;

        template <typename x, typename n>
        using shift_right_impl = integer_c<
            decltype(x::value >> n::value),
            (x::value >> n::value)
        >;

        template <typename x>
        using compl_impl = integer_c<
            decltype(~x::value),
            ~x::value
        >;
    };
}} // end namespace boost::mpl11

#endif // !BOOST_MPL11_INTEGER_HPP


#include "per_face_prin_curvature.h"
#include <igl/per_vertex_normals.h>
#include <igl/principal_curvature.h>
#include <igl/avg_edge_length.h>
#include <igl/massmatrix.h>
#include <igl/adjacency_list.h>
#include <igl/per_vertex_normals.h>
#include <igl/unique_edge_map.h>
#include <igl/edge_flaps.h>
#include <igl/barycenter.h>
#include <igl/parallel_for.h>
#include <igl/pinv.h>
#include <Eigen/SparseCore>
#include <iostream>
using namespace std;

void per_vertex_signed_prin_curvature(Eigen::MatrixXd & V,Eigen::MatrixXi & F, Eigen::MatrixXd & PD1, Eigen::MatrixXd & PD2, Eigen::VectorXd & PC1, Eigen::VectorXd & PC2){

    int n = V.rows();
    int m = F.rows();
    Eigen::MatrixXi E,uE,EF,EI;
    std::vector<std::vector<int>> A,uE2E;
    Eigen::VectorXi EMAP;
  Eigen::MatrixXd N;
    std::vector<int> b;
    Eigen::MatrixXd bary;
    igl::barycenter(V,F,bary);
    igl::adjacency_list(F,A);
   igl::per_vertex_normals(V,F,N);

   igl::unique_edge_map(F,E,uE,EMAP,uE2E);
   igl::edge_flaps(F,uE,EMAP,EF,EI);

    PC1.resize(n);
    PC2.resize(n);
    PD1.resize(n,3);
    PD2.resize(n,3);

    //std::cout << "Starting iteration" << std::endl;

    //for (int i=0; i<m; i++) { // For each face
    igl::parallel_for(V.rows(),[&] (const int i){

        // Step 1: Find unique combined indeces of all vertices of face
        // uu = V(unique(u{F(i,1)},u{F(i,2)},u{F(i,3)}),:)
        // vv = uu-bary(i,:) what is this notation in c++...
        // Triangle i has edges E.row(i), E.row(i+m), E.row(i+2m)
        // So incident (repeated) faces are EF(i,0), EF(i,1), EF(i+m,0),
        // EF(i+m,1), EF(i+2m,0) and EF(i+2m,1);
        // std::cout << "entered loop" << std::endl;
        std::vector<int> vertex_indeces,v1,v2,v3;
        std::vector<std::vector<int>> v123;
        // v123.resize(3);
        //for(int j = 0; j < 3; j++){
        //    v123[j].resize(3);
            // std::cout << "entered loop" << std::endl;
            //std::cout << EF(EMAP(i+(j*m)),0) << std::endl;
           // std::cout << EF(EMAP(i+(j*m)),1) << std::endl;

         //   for(int orient = 0; orient < 2; orient++){
         //       if(EF(EMAP(i+(j*m)),orient) != i){
                 //   std::cout << "found one" << std::endl;
         //       v123[j].push_back(F(EF(EMAP(i+(j*m)),orient),0));
         //       v123[j].push_back(F(EF(EMAP(i+(j*m)),orient),1));
         //       v123[j].push_back(F(EF(EMAP(i+(j*m)),orient),2));
         //       }
         //       }
          //  }
        // std::cout << "exited loop" << std::endl;
        //v1 = v123[0];
        //v2 = v123[1];
        //v3 = v123[2];
        //assert(v1.size() == 3);
        //assert(v2.size() == 3);
        //assert(v3.size() == 3);
       //  std::cout << "survived assertions" << std::endl;

        Eigen::MatrixXd P;
        Eigen::Vector3d w,uu,vv;
        Eigen::VectorXd u,v,b,a;

        double E,FF,G,e,f,g,det;
        v1 = A[i];
        vertex_indeces.insert(vertex_indeces.end(),v1.begin(),v1.end());
        for (int j = 0; j < v1.size(); j++) {
            v2 = A[v1[j]];
            vertex_indeces.insert(vertex_indeces.end(),v2.begin(),v2.end());
        }
        sort( vertex_indeces.begin(), vertex_indeces.end() );
        vertex_indeces.erase( unique( vertex_indeces.begin(), vertex_indeces.end() ), vertex_indeces.end() );
        int k = vertex_indeces.size();
        for (int j=0; j<k; j++) {
            P.conservativeResize(j+1,3);
            P.row(j) = V.row(vertex_indeces[j]) - bary.row(i);
        }
        // END OF STEP 1

        // Step 2: Build orthonormal basis from N
        w = N.row(i);

        if (w(2)==-1) {
            vv(0) = 0;
            vv(1) = -1;
            vv(2) = 0;
            uu(0) = -1;
            uu(1) = 0;
            uu(2) = 0;
        }
        else{
            vv(0) = 1-(pow(w(0),2)/(1+w(2)));
            vv(1) = -w(0)*w(1)/(1+w(2));
            vv(2) = -w(0);
            uu(0) = -w(0)*w(1)/(1+w(2));
            uu(1) = 1-(pow(w(1),2)/(1+w(2)));
            uu(2) = -w(1);
        }
        // END OF STEP 2
        // STEP 3: PROJECT
        Eigen::MatrixXd S(3,2);
        S.col(0) = uu;
        S.col(1) = vv;
        u = P*uu;
        v = P*vv;
        b = P*w;
        // END OF STEP 3
        // STEP 4: FIT QUADRIC
        Eigen::MatrixXd A2(k,6);
        Eigen::MatrixXd A2_pseudo(6,k);
        A2.col(0) = u;
        A2.col(1) = v;
        A2.col(2) = u.cwiseProduct(u);
        A2.col(3) = u.cwiseProduct(v);
        A2.col(4) = v.cwiseProduct(v);
        A2.col(5).setOnes();
        igl::pinv(A2,A2_pseudo);
        a = A2_pseudo*b;
        // END OF STEP 4
        // STEP 5: BUILD SS
        E = 1+(pow(a(0),2));
        FF = a(0)*a(1);
        G = 1+(pow(a(1),2));
        e = (2*a(2))/sqrt((pow(a(0),2))+1+(pow(a(1),2)));
        f = (a(3))/sqrt((pow(a(0),2))+1+(pow(a(1),2)));
        g = (2*a(4))/sqrt((pow(a(0),2))+1+(pow(a(1),2)));
        Eigen::Matrix2d S1,S2,SS;
        S1 << e,f,f,g;
        det = (G*E)-(pow(FF,2));
        if (det==0) {
            Eigen::Matrix2d S2_pseudo;
            S2 << E,FF,FF,G;
            igl::pinv(S2,S2_pseudo);
            SS = S1*S2_pseudo;
            //std::cout << "Det zero" << std::endl;
        }else{
            S2 << G,-FF,-FF,E;
            S2 /= det;
            SS = S1*S2;
        }
        // END OF STEP 5
        // STEP 6: EIGENALALYSIS OF SS
        // std::cout << "Before eigensolver" << std::endl;
        Eigen::EigenSolver<Eigen::MatrixXd> es(SS);
        PC1(i) = - es.eigenvalues().real().coeff(0);
        PC2(i) = - es.eigenvalues().real().coeff(1);
        if (PC1(i)>PC2(i)) {
            std::swap(PC1(i),PC2(i));
            PD1.row(i) = es.eigenvectors().real().coeff(0,1)*uu + es.eigenvectors().real().coeff(1,1)*vv;
            PD2.row(i) = es.eigenvectors().real().coeff(0,0)*uu + es.eigenvectors().real().coeff(1,0)*vv;
        }else{
            PD1.row(i) = es.eigenvectors().real().coeff(0,0)*uu + es.eigenvectors().real().coeff(1,0)*vv;
            PD2.row(i) = es.eigenvectors().real().coeff(0,1)*uu + es.eigenvectors().real().coeff(1,1)*vv;
        }
       // PD1.row(i) = uu;
       // PD2.row(i) = w;
        //std::cout << "Ending iteration" << std::endl;
    },0);
        //
    //}

}

// g++ -I/usr/local/libigl/external/eigen -I/usr/local/libigl/include -std=c++11 -framework Accelerate main.cpp principal_curvatures_silvia.cpp -o main


// Copyright (C) 2019 David Harmon and Artificial Necessity
// This code distributed under zlib, see LICENSE.txt for terms.

#pragma once

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

#include "energy.hpp"

class ModifiedConjugateGradient {
public:
    bool compute(const SparseMatrixd& A) {
        if (A.rows() != A.cols()) {
            std::cerr << "Non-square matrix A!" << std::endl;
            return false;
        }

        if (P_.size() != A.rows() / 3) {
            P_.resize(A.rows() / 3);
        }

        for (int i=0; i<A.rows(); i+=3) {
            Mat3d Ai = A.block(i,i,3,3);
            P_[i/3] = Ai.inverse();
        }

        return true;
    }

    void resize(int nbr) {
        S_.resize(nbr, Eigen::Matrix3d::Identity());
    }

    void reset() {
        for (Eigen::Matrix3d& M : S_) {
            M.setIdentity();
        }
    }

    void setFilter(int idx, const Eigen::Matrix3d& C) {
        S_[idx] = C;
    }

    void filterInPlace(Eigen::VectorXd& v) {
        #pragma omp parallel for
        for (size_t i=0; i<S_.size(); i++) {
            v.segment<3>(3*i) = S_[i] * v.segment<3>(3*i);
        }
    }

    Eigen::VectorXd filter(const Eigen::VectorXd& v) {
        Eigen::VectorXd out(v.size());
        #pragma omp parallel for
        for (size_t i=0; i<S_.size(); i++) {
            out.segment<3>(3*i) = S_[i] * v.segment<3>(3*i);
        }
        return out;
    }

    template <typename F, typename Derived>
    void solve(F multiply, const Eigen::MatrixBase<Derived>& b, Eigen::VectorXd& x) {
        static double eps = 2.5e-3;
        static const size_t maxNbrItrs = 1000;

        VecXd r = b - multiply(x); filterInPlace(r);
        VecXd c(b.size());

        for (int i=0; i<b.size()/3; i++) {
            c.segment<3>(3*i) = S_[i] * P_[i] * r.segment<3>(3*i);
        }

        double dNew = r.dot(c);
        double d0 = dNew;

        double tol = eps * eps * d0;

        size_t nbrItrs = 0;
        size_t minNbr = 10;
        while ((dNew > tol || nbrItrs < minNbr) && nbrItrs < maxNbrItrs)
        {
            VecXd q = multiply(c); filterInPlace(q);
            double a = dNew / c.dot(q);

            x.noalias() += a * c;
            r.noalias() -= a * q;

            VecXd s(r.size());
            for (int i=0; i<b.size()/3; i++) {
                s.segment<3>(3*i) = P_[i] * r.segment<3>(3*i);
            }

            double dOld = dNew;
            dNew = r.dot(s);

            c *= dNew / dOld;
            c.noalias() += s;
	    filterInPlace(c);

            nbrItrs++;
        }

        std::cout << "CG finished in " << nbrItrs
                  << " iterations with error " << dNew << " (" << tol << ")" << std::endl;
    }

    const Eigen::Matrix3d& S(int idx) {
        return S_[idx];
    }

protected:
    std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> P_;
    std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> S_;
};


/*
This file is a part of Raman-Scattering-Code-Conversion.
<https://github.com/Kirbologist/Raman-Scattering-Code-Conversion>

Written by Siwan Li for the UQ School of Maths and Physics.
Based on the SMARTIES MATLAB package by W.R.C. Somerville, B. Auguié, E.C. Le Ru
Copyright (C) 2021-2022 Siwan Li

This source code form is subject to the terms of the MIT License.
If a copy of the MIT License was not distributed with this file,
you can obtain one at <https://opensource.org/licenses/MIT>.

This code contains all 'rvh' SMARTIES functions that are used in Raman scattering calculations,
i.e. T-matrix related functions specific to particles with mirror-reflection symmetry.
Henceforth, such symmetry shall be called rvh symmetry.
*/

#ifndef RVH_HPP
#define RVH_HPP

#include "core.hpp"
#include "sph.hpp"
#include "vsh.hpp"
#include "misc.hpp"
#include <Eigen/SVD>

using namespace Eigen;
using namespace std;

namespace Smarties {

  /* Struct containing vectors of expansion coefficients of the multipole expansions of a set of electric fields */
  template <class Real>
  struct stAbcdnm {
    // Scattering field coefficients
    ArrayXc<Real> p_nm;
    ArrayXc<Real> q_nm;
    // Incident field coefficients
    ArrayXc<Real> a_nm;
    ArrayXc<Real> b_nm;
    // Internal field coefficients
    ArrayXc<Real> c_nm;
    ArrayXc<Real> d_nm;
  };

  /* Struct containing orientation-averaged cross-sections for various wavelengths*/
  template <class Real>
  struct stCrossSection {
    ArrayXr<Real> C_ext; // Wavelength-dependent extinction coefficients
    ArrayXr<Real> C_sca; // Wavelength-dependent scattering coefficients
    ArrayXr<Real> C_abs; // Wavelength-dependent absorption coefficients
  };

  /*
  Calculate T (and possibly R) matrices from P, Q matrices, for scatterers with a plane of symmetry.
  This makes use of block inversion. See sec. 4.5 and eq. 70 in JQSRT2013 for details.
  Inputs:
    st_PQ_list - std::vector of size M, each element of which contains a unique pointer to an stPQ struct
                 (one entry for each m in abs_m_vec), and which makes use of the rvh symmetry
    get_R - if true, R is computed and returned
  Output:
    A std::vector of size M of unique pointers to stTR structs.
  Dependencies:
    InvertLUcol
  */
  template <class Real>
  vector<unique_ptr<stTR<Real>>> rvhGetTRfromPQ(vector<unique_ptr<stPQ<Real>>>& st_PQ_list, bool get_R = false) {
    // Note that all matrix inversions uses LU decomposition with partial pivoting of the columns
    int num_entries = st_PQ_list.size();
    vector<unique_ptr<stTR<Real>>> output(num_entries);
    for (int i = 0; i < num_entries; i++) {
      output[i] = make_unique<stTR<Real>>();
      // Get blocks of P and Q matrices
      unique_ptr<stPQ<Real>>& st_PQ = st_PQ_list[i];
      unique_ptr<stTR<Real>>& st_TR = output[i];
      // Get blocks of P_eo and Q_eo matrices
      MatrixXc<Real> P11_ee = st_PQ->st_4M_P_eo().M11;
      MatrixXc<Real> P12_eo = st_PQ->st_4M_P_eo().M12;
      MatrixXc<Real> P21_oe = st_PQ->st_4M_P_eo().M21;
      MatrixXc<Real> P22_oo = st_PQ->st_4M_P_eo().M22;
      MatrixXc<Real> Q11_ee = st_PQ->st_4M_Q_eo().M11;
      MatrixXc<Real> Q12_eo = st_PQ->st_4M_Q_eo().M12;
      MatrixXc<Real> Q21_oe = st_PQ->st_4M_Q_eo().M21;
      MatrixXc<Real> Q22_oo = st_PQ->st_4M_Q_eo().M22;

      // Get blocks of P_oe and Q_oe matrices
      MatrixXc<Real> P11_oo = st_PQ->st_4M_P_oe().M11;
      MatrixXc<Real> P12_oe = st_PQ->st_4M_P_oe().M12;
      MatrixXc<Real> P21_eo = st_PQ->st_4M_P_oe().M21;
      MatrixXc<Real> P22_ee = st_PQ->st_4M_P_oe().M22;
      MatrixXc<Real> Q11_oo = st_PQ->st_4M_Q_oe().M11;
      MatrixXc<Real> Q12_oe = st_PQ->st_4M_Q_oe().M12;
      MatrixXc<Real> Q21_eo = st_PQ->st_4M_Q_oe().M21;
      MatrixXc<Real> Q22_ee = st_PQ->st_4M_Q_oe().M22;

      int num_even = Q11_ee.rows();
      int num_odd = Q11_oo.rows();
      ArrayXi ind1_eo = st_PQ->st_4M_Q_eo().ind1;
      ArrayXi ind2_eo = st_PQ->st_4M_Q_eo().ind2;
      ArrayXi ind1_oe = st_PQ->st_4M_Q_oe().ind1;
      ArrayXi ind2_oe = st_PQ->st_4M_Q_oe().ind2;

      int m = st_PQ->st_4M_P_eo().m;

      // m == 0 case is done separately, since M12 and M21 are empty.
      if (!m) {
        st_TR->st_4M_R_oe().M11 = InvertLUcol(Q11_oo);
        st_TR->st_4M_T_oe().M11 = -P11_oo * st_TR->st_4M_R_oe().M11.matrix();
        st_TR->st_4M_R_oe().M22 = InvertLUcol(Q22_ee);
        st_TR->st_4M_T_oe().M22 = -P22_ee * st_TR->st_4M_R_oe().M22.matrix();
        st_TR->st_4M_R_eo().M11 = InvertLUcol(Q11_ee);
        st_TR->st_4M_T_eo().M11 = -P11_ee * st_TR->st_4M_R_eo().M11.matrix();
        st_TR->st_4M_R_eo().M22 = InvertLUcol(Q22_oo);
        st_TR->st_4M_T_eo().M22 = -P22_oo * st_TR->st_4M_R_eo().M22.matrix();

        st_TR->st_4M_T_eo().M12 = ArrayXXc<Real>::Zero(num_even, num_odd);
        st_TR->st_4M_T_eo().M21 = ArrayXXc<Real>::Zero(num_odd, num_even);
        st_TR->st_4M_T_oe().M12 = ArrayXXc<Real>::Zero(num_odd, num_even);
        st_TR->st_4M_T_oe().M21 = ArrayXXc<Real>::Zero(num_even, num_odd);

        if (get_R) {
          st_TR->st_4M_R_eo().M12 = ArrayXXc<Real>::Zero(num_even, num_odd);
          st_TR->st_4M_R_eo().M21 = ArrayXXc<Real>::Zero(num_odd, num_even);
          st_TR->st_4M_R_oe().M12 = ArrayXXc<Real>::Zero(num_odd, num_even);
          st_TR->st_4M_R_oe().M21 = ArrayXXc<Real>::Zero(num_even, num_odd);
        }
      } else { // Do inversion using JQSRT2013 eq. 70

        // For M_eo matrices
        MatrixXc<Real> Q11_inv = InvertLUcol(Q11_ee);

        MatrixXc<Real> G1 = P11_ee * Q11_inv;
        MatrixXc<Real> G3 = P21_oe * Q11_inv;
        MatrixXc<Real> G5 = Q21_oe * Q11_inv;
        MatrixXc<Real> F2_m1 = Q22_oo - G5 * Q12_eo;
        MatrixXc<Real> F2 = InvertLUcol(F2_m1);

        MatrixXc<Real> G2 = P22_oo * F2;
        MatrixXc<Real> G4 = P12_eo * F2;
        MatrixXc<Real> G6 = Q12_eo * F2;

        st_TR->st_4M_T_eo().M12 = G1 * G6 - G4;
        st_TR->st_4M_T_eo().M22 = G3 * G6 - G2;
        st_TR->st_4M_T_eo().M11 = -G1 - st_TR->st_4M_T_eo().M12.matrix() * G5;
        st_TR->st_4M_T_eo().M21 = -G3 - st_TR->st_4M_T_eo().M22.matrix() * G5;

        if (get_R) {
          st_TR->st_4M_R_eo().M12 = -Q11_inv * G6;
          st_TR->st_4M_R_eo().M22 = F2;
          st_TR->st_4M_R_eo().M11 = Q11_inv - st_TR->st_4M_R_eo().M12.matrix() * G5;
          st_TR->st_4M_R_eo().M21 = -st_TR->st_4M_R_eo().M22.matrix() * G5;
        }

        // For M_oe matrices
        Q11_inv = InvertLUcol(Q11_oo);

        G1 = P11_oo * Q11_inv;
        G3 = P21_eo * Q11_inv;
        G5 = Q21_eo * Q11_inv;
        F2_m1 = Q22_ee - G5 * Q12_oe;
        F2 = InvertLUcol(F2_m1);

        G2 = P22_ee * F2;
        G4 = P12_oe * F2;
        G6 = Q12_oe * F2;

        st_TR->st_4M_T_oe().M12 = G1 * G6 - G4;
        st_TR->st_4M_T_oe().M22 = G3 * G6 - G2;
        st_TR->st_4M_T_oe().M11 = -G1 - st_TR->st_4M_T_oe().M12.matrix() * G5;
        st_TR->st_4M_T_oe().M21 = -G3 - st_TR->st_4M_T_oe().M22.matrix() * G5;

        if (get_R) {
          st_TR->st_4M_R_oe().M12 = -Q11_inv * G6;
          st_TR->st_4M_R_oe().M22 = F2;
          st_TR->st_4M_R_oe().M11 = Q11_inv - st_TR->st_4M_R_oe().M12.matrix() * G5;
          st_TR->st_4M_R_oe().M21 = -st_TR->st_4M_R_oe().M22.matrix() * G5;
        }
      }

      st_TR->st_4M_T_eo().m = st_TR->st_4M_T_oe().m = m;
      st_TR->st_4M_T_eo().ind1 = ind1_eo;
      st_TR->st_4M_T_eo().ind2 = ind2_eo;
      st_TR->st_4M_T_oe().ind1 = ind1_oe;
      st_TR->st_4M_T_oe().ind2 = ind2_oe;
      st_TR->mat_list.push_back("st_4M_T");

      if (get_R) {
        st_TR->st_4M_R_eo().m = st_TR->st_4M_R_oe().m = m;
        st_TR->st_4M_R_eo().ind1 = ind1_eo;
        st_TR->st_4M_R_eo().ind2 = ind2_eo;
        st_TR->st_4M_R_oe().ind1 = ind1_oe;
        st_TR->st_4M_R_oe().ind2 = ind2_oe;
        st_TR->mat_list.push_back("st_4M_R");
      }
    }
    return output;
  }

  /*
  Truncate P and Q matrices in st_mat_list such that the maximum number of multipoles N is now N_max.
  This also removes values of m that are larger than N_max.
  This function works on st_mat_list which makes use of rvh symmetry.
  Inputs:
    st_mat_list - a vector of unique pointers to stPQ structs, with one entry for each m of abs_m_vec.
    N_max - The maximum value of N (and m) that is desired in the output.
  Output:
    returns a deep copy of the same st_mat_list vector, but where all M11, M12, M21 and M22 members
    have been truncated to N = N_max and values of m > N_max are removed.
  Dependencies:
    LogicalIndices
  */
  template <class Real>
  vector<unique_ptr<stPQ<Real>>> rvhTruncateMatrices(const vector<unique_ptr<stPQ<Real>>>& st_mat_list, int N_max) {
    vector<unique_ptr<stPQ<Real>>> output;
    int num_entries = st_mat_list.size();
    for (int i = 0 ; i < num_entries; i++) {
      int num_st_4M = st_mat_list[i]->mat_list.size() * 2; // Count how many matrices in the struct are defined
      int m;
      if (num_st_4M > 0 && (m = st_mat_list[i]->st_4M_list[0].m) <= N_max) {
        auto output_st_TR = make_unique<stPQ<Real>>();
        output_st_TR->mat_list = st_mat_list[i]->mat_list;
        for (int j = 0; j < num_st_4M; j++) {
          int new_size = N_max - max(1, m);
          ArrayXb ind1_valid = st_mat_list[i]->st_4M_list[j].ind1 <= new_size;
          ArrayXb ind2_valid = st_mat_list[i]->st_4M_list[j].ind2 <= new_size;

          ArrayXi new_ind1 = LogicalIndices(ind1_valid);
          ArrayXi new_ind2 = LogicalIndices(ind2_valid);

          output_st_TR->st_4M_list[j].ind1 = st_mat_list[i]->st_4M_list[j].ind1(new_ind1);
          output_st_TR->st_4M_list[j].ind2 = st_mat_list[i]->st_4M_list[j].ind2(new_ind2);
          output_st_TR->st_4M_list[j].m = m;

          ArrayXXc<Real> current_matrix = st_mat_list[i]->st_4M_list[j].M11;
          output_st_TR->st_4M_list[j].M11 = current_matrix(new_ind1, new_ind1);
          current_matrix = st_mat_list[i]->st_4M_list[j].M12;
          output_st_TR->st_4M_list[j].M12 = current_matrix(new_ind1, new_ind2);
          current_matrix = st_mat_list[i]->st_4M_list[j].M21;
          output_st_TR->st_4M_list[j].M21 = current_matrix(new_ind2, new_ind1);
          current_matrix = st_mat_list[i]->st_4M_list[j].M22;
          output_st_TR->st_4M_list[j].M22 = current_matrix(new_ind2, new_ind2);
        }
        output.push_back(move(output_st_TR));
      }
    }
    return output;
  }

  /*
  Truncate T and R matrices in st_mat_list such that the maximum number of multipoles N is now N_max.
  This also removes values of m that are larger than N_max.
  This function works on st_mat_list which makes use of rvh symmetry.
  Inputs:
    st_mat_list - a vector of unique pointers to stTR structs, with one entry for each m of abs_m_vec.
    N_max - The maximum value of N (and m) that is desired in the output.
  Output:
    returns a deep copy of the same st_mat_list vector, but where all M11, M12, M21 and M22 members
    have been truncated to N = N_max and values of m > N_max are removed.
  Dependencies:
    LogicalIndices
  */
  template <class Real>
  vector<unique_ptr<stTR<Real>>> rvhTruncateMatrices(const vector<unique_ptr<stTR<Real>>>& st_mat_list, int N_max) {
    vector<unique_ptr<stTR<Real>>> output;
    int num_entries = st_mat_list.size();
    for (int i = 0 ; i < num_entries; i++) {
      int num_st_4M = st_mat_list[i]->mat_list.size() * 2; // Count how many matrices in the struct are defined
      int m;
      if (num_st_4M > 0 && (m = st_mat_list[i]->st_4M_list[0].m) <= N_max) {
        auto output_st_TR = make_unique<stTR<Real>>();
        output_st_TR->mat_list = st_mat_list[i]->mat_list;
        for (int j = 0; j < num_st_4M; j++) {
          int new_size = N_max - max(1, m);
          ArrayXb ind1_valid = st_mat_list[i]->st_4M_list[j].ind1 <= new_size;
          ArrayXb ind2_valid = st_mat_list[i]->st_4M_list[j].ind2 <= new_size;

          ArrayXi new_ind1 = LogicalIndices(ind1_valid);
          ArrayXi new_ind2 = LogicalIndices(ind2_valid);

          output_st_TR->st_4M_list[j].ind1 = st_mat_list[i]->st_4M_list[j].ind1(new_ind1);
          output_st_TR->st_4M_list[j].ind2 = st_mat_list[i]->st_4M_list[j].ind2(new_ind2);
          output_st_TR->st_4M_list[j].m = m;

          ArrayXXc<Real> current_matrix = st_mat_list[i]->st_4M_list[j].M11;
          output_st_TR->st_4M_list[j].M11 = current_matrix(new_ind1, new_ind1);
          current_matrix = st_mat_list[i]->st_4M_list[j].M12;
          output_st_TR->st_4M_list[j].M12 = current_matrix(new_ind1, new_ind2);
          current_matrix = st_mat_list[i]->st_4M_list[j].M21;
          output_st_TR->st_4M_list[j].M21 = current_matrix(new_ind2, new_ind1);
          current_matrix = st_mat_list[i]->st_4M_list[j].M22;
          output_st_TR->st_4M_list[j].M22 = current_matrix(new_ind2, new_ind2);
        }
        output.push_back(move(output_st_TR));
      }
    }
    return output;
  }

  /*
  Symmetrises the matrices given in mat_list, and leaves other matrices present in st_mat_list unchanged.
  Uses the upper triangular matrices to deduce the lower triangular parts from the symmetry relations
  of the T-matrix obtained from eqs. 5.34 and 5.37 of [Mishchenko 2002], i.e.
  T11 = T11.transpose(), T22 = T22.transpose(), T12 = -T21.transpose(), T21 = -T12.transpose()
  This function assumes rvh symmetry. While the original MATLAB code could theoretically work with P and Q matrices,
  the implementation here only works with T and R matrices, although it's intended only for T and R matrices anyway.
  Inputs:
    st_mat_list - a std::vector containing unique pointers to stTR structs containing
                  T (and possibly R) matrices to symmetrise
    mat_list - a std::vector of strings specifying which structs are to be symmetrised
  Outputs:
    A deep copy of st_mat_list, but with all the desired matrices symmetrised.
  */
  template <class Real>
  vector<unique_ptr<stTR<Real>>> rvhGetSymmetricMat(const vector<unique_ptr<stTR<Real>>>& st_mat_list,
      vector<string> mat_list = {"st_4M_T"}) {
    enum parity {EO, OE, END};
    int num_entries = st_mat_list.size();
    vector<unique_ptr<stTR<Real>>> output(num_entries);

    for (int i = 0; i < num_entries; i++) {
      output[i] = make_unique<stTR<Real>>(*(st_mat_list[i])); // Make a deep copy at the ith entry of st_mat_list
      int num_st_4M = mat_list.size();
      for (int j = 0; j < num_st_4M; j++) {
        for (int k = EO; k != END; k++) { // Symmetrise eo matrix, then oe matrix
          st4M<Real>* st_4M;
          if (mat_list[j] == "st_4M_T") {
            if (k == EO)
              st_4M = &(output[i]->st_4M_T_eo());
            else if (k == OE)
              st_4M = &(output[i]->st_4M_T_oe());
          } else if (mat_list[j] == "st_4M_R") {
            if (k == EO)
              st_4M = &(output[i]->st_4M_R_eo());
            else if (k == OE)
              st_4M = &(output[i]->st_4M_R_oe());
          } else
            continue;
          ArrayXi ind1 = st_4M->ind1;
          ArrayXi ind2 = st_4M->ind2;

          if (ind1.size() && ind2.size()) { // Check that neither two are empty
            int offset12 = (ind2(0) - ind1(0) + 1)/2;
            // int offset21 = 1 - offset12;

            // Upper triangular without diagonal
            MatrixXc<Real> upper = st_4M->M11.matrix().template triangularView<StrictlyUpper>();
            // Diagonal only
            MatrixXc<Real> diag = static_cast<VectorXc<Real>>(st_4M->M11.matrix().diagonal()).asDiagonal();
            st_4M->M11 = (upper + diag + upper.transpose()).array(); // Symmtrised matrix

            upper = st_4M->M22.matrix().template triangularView<StrictlyUpper>();
            diag = static_cast<VectorXc<Real>>(st_4M->M22.matrix().diagonal()).asDiagonal();
            st_4M->M22 = (upper + diag + upper.transpose()).array();

            // In either case, the transpose of upper2 complements upper1
            MatrixXc<Real> upper1, upper2;
            if (offset12) {
              upper1 = st_4M->M12.matrix().template triangularView<StrictlyUpper>();
              upper2 = st_4M->M21.matrix().template triangularView<Upper>();
            } else {
              upper1 = st_4M->M12.matrix().template triangularView<Upper>();
              upper2 = st_4M->M21.matrix().template triangularView<StrictlyUpper>();
            }
            st_4M->M12 = (upper1 - upper2.transpose()).array(); // Symmetrised matrix
            st_4M->M21 = -st_4M->M12.transpose(); // Symmetrised matrix
          }
        }
      }
    }

    return output;
  }

  /*
  Calculates the field expansion coefficients from the T/R-matrices for a given incident plane wave
  (for one wavelength only). If R is not defined in st_TR_list, then the internal fields are not computed.
  If stIncEabnm is given, the expansion coefficients for the incident wave are not recalculated.
  This method is valid for scatterers with rvh symmetry.

  If st_TR_list contains m values that are not required, then they are ignored.
  Inputs:
    N_max - the maximum multipole order to use in the expansions
            (should be less than or equal to the T-matrix size)
    st_TR_list - std::vector of size M containing unique pointers to stTR structs for each m.
                 This should contain all m present in st_inc_par.abs_m_vec, i.e. all m where |m| <= N,
                 and should make use of the rvh symmetry.
    st_inc_par - A struct containing information about the incident field, as from vshMakeIncidentParams.
    st_inc_E_abmm - A struct containing the expansion coefficients a_nm and b_nm of the incident wave.
                    These are recalculated if not provided.
  Output:
    returns a stAbcdnm struct containing all the calculated expansion coefficients.
    The case where n = 0 and m = 0 is included for padding, so every vector in the struct has size
    P = (N_max + 1)^2. The padding exists to simplify the indexing.
    c_nm and d_nm are only defined if R is defined in st_TR_list.
  Dependencies:
    Seq2Array, LogicalSlice, vshGetIncidentCoefficients
  */
  template <class Real>
  unique_ptr<stAbcdnm<Real>> rvhGetFieldCoefficients(int N_max,
      const vector<unique_ptr<stTR<Real>>>& st_TR_list, const unique_ptr<stIncPar<Real>>& st_inc_par,
      unique_ptr<stIncEabnm<Real>> st_inc_E_abnm = unique_ptr<stIncEabnm<Real>>()) {

    // Coeff of incident wave
    if (!st_inc_E_abnm)
      st_inc_E_abnm = vshGetIncidentCoeffs(N_max, st_inc_par);

    bool get_R = false;
    if (find(st_TR_list[0]->mat_list.begin(), st_TR_list[0]->mat_list.end(), "st_4M_R")
        != st_TR_list[0]->mat_list.end())
      get_R = true;

    // Truncate to relevant m's
    ArrayXb abs_m_vec_valid = st_inc_par->abs_m_vec <= N_max;
    ArrayXi abs_m_vec = LogicalSlice(st_inc_par->abs_m_vec, abs_m_vec_valid);

    int num_M = st_TR_list.size(); // number of m-values in T, not all of which may be needed
    int M = abs_m_vec.size();
    int P_max = (N_max + 1)*(N_max + 1); // number of (n,m) coupled indices (with (0,0) included as padding)

    ArrayXc<Real> a_nm = st_inc_E_abnm->a_nm; // [P X 1]
    ArrayXc<Real> b_nm = st_inc_E_abnm->b_nm; // [P X 1]
    ArrayXc<Real> p_nm = ArrayXc<Real>::Zero(P_max); // [P X 1]
    ArrayXc<Real> q_nm = ArrayXc<Real>::Zero(P_max); // [P X 1]
    ArrayXc<Real> c_nm = ArrayXc<Real>::Zero(get_R ? P_max : 1); // if get_R, [P X 1]
    ArrayXc<Real> d_nm = ArrayXc<Real>::Zero(get_R ? P_max : 1); // if get_R, [P X 1]
    ArrayXi m_ind_for_T(M);

    // Find the corresponding m-indices in st_TR_list (in case they are not in standard order)
    for (int m_ind11 = 0; m_ind11 < M; m_ind11++) {
      for (int m_ind = 0; m_ind < num_M; m_ind++) {
        if (st_TR_list[m_ind]->st_4M_T_eo().m == abs_m_vec(m_ind11)) {
          m_ind_for_T(m_ind11) = m_ind;
          break;
        }
      }
    }

    for (int m_ind11 = 0; m_ind11 < M; m_ind11++) {
      int m_ind = m_ind_for_T(m_ind11);
      int m = abs_m_vec(m_ind11);
      int N_min = max(m, 1);

      ArrayXi n_vec_e = Seq2Array(N_min + (N_min % 2), N_max, 2); // indices for even n [N_e X 1]
      ArrayXi n_vec_o = Seq2Array(N_min + 1 - (N_min % 2), N_max, 2); // indices for odd n [N_o X 1]

      ArrayXi p_vec_e = n_vec_e * (n_vec_e + 1) + m; // [N_e X 1]
      ArrayXi p_vec_o = n_vec_o * (n_vec_o + 1) + m; // [N_o X 1]
      ArrayXi n_vec_T_e = ArrayXi::LinSpaced(n_vec_e.size(), 0, n_vec_e.size() - 1);
      ArrayXi n_vec_T_o = ArrayXi::LinSpaced(n_vec_o.size(), 0, n_vec_o.size() - 1);

      VectorXc<Real> p_nm_e = st_TR_list[m_ind]->st_4M_T_eo().M11(n_vec_T_e, n_vec_T_e).matrix() *
          a_nm(p_vec_e).matrix() + st_TR_list[m_ind]->st_4M_T_eo().M12(n_vec_T_e, n_vec_T_o).matrix() *
          b_nm(p_vec_o).matrix();
      VectorXc<Real> q_nm_o = st_TR_list[m_ind]->st_4M_T_eo().M21(n_vec_T_o, n_vec_T_e).matrix() *
          a_nm(p_vec_e).matrix() + st_TR_list[m_ind]->st_4M_T_eo().M22(n_vec_T_o, n_vec_T_o).matrix() *
          b_nm(p_vec_o).matrix();
      VectorXc<Real> p_nm_o = st_TR_list[m_ind]->st_4M_T_oe().M11(n_vec_T_o, n_vec_T_o).matrix() *
          a_nm(p_vec_o).matrix() + st_TR_list[m_ind]->st_4M_T_oe().M12(n_vec_T_o, n_vec_T_e).matrix() *
          b_nm(p_vec_e).matrix();
      VectorXc<Real> q_nm_e = st_TR_list[m_ind]->st_4M_T_oe().M21(n_vec_T_e, n_vec_T_o).matrix() *
          a_nm(p_vec_o).matrix() + st_TR_list[m_ind]->st_4M_T_oe().M22(n_vec_T_e, n_vec_T_e).matrix() *
          b_nm(p_vec_e).matrix();

      for (int i = 0; i < n_vec_e.size(); i++) {
        p_nm(p_vec_e(i)) = p_nm_e(i);
        q_nm(p_vec_e(i)) = q_nm_e(i);
      }
      for (int i = 0; i < n_vec_o.size(); i++) {
        p_nm(p_vec_o(i)) = p_nm_o(i);
        q_nm(p_vec_o(i)) = q_nm_o(i);
      }

      if (get_R) {
        VectorXc<Real> c_nm_e = st_TR_list[m_ind]->st_4M_R_eo().M11(n_vec_T_e, n_vec_T_e).matrix() *
            a_nm(p_vec_e).matrix() + st_TR_list[m_ind]->st_4M_R_eo().M12(n_vec_T_e, n_vec_T_o).matrix() *
            b_nm(p_vec_o).matrix();
        VectorXc<Real> d_nm_o = st_TR_list[m_ind]->st_4M_R_eo().M21(n_vec_T_o, n_vec_T_e).matrix() *
            a_nm(p_vec_e).matrix() + st_TR_list[m_ind]->st_4M_R_eo().M22(n_vec_T_o, n_vec_T_o).matrix() *
            b_nm(p_vec_o).matrix();
        VectorXc<Real> c_nm_o = st_TR_list[m_ind]->st_4M_R_oe().M11(n_vec_T_o, n_vec_T_o).matrix() *
            a_nm(p_vec_o).matrix() + st_TR_list[m_ind]->st_4M_R_oe().M12(n_vec_T_o, n_vec_T_e).matrix() *
            b_nm(p_vec_e).matrix();
        VectorXc<Real> d_nm_e = st_TR_list[m_ind]->st_4M_R_oe().M21(n_vec_T_e, n_vec_T_o).matrix() *
            a_nm(p_vec_o).matrix() + st_TR_list[m_ind]->st_4M_R_oe().M22(n_vec_T_e, n_vec_T_e).matrix() *
            b_nm(p_vec_e).matrix();

        for (int i = 0; i < n_vec_e.size(); i++) {
          c_nm(p_vec_e(i)) = c_nm_e(i);
          d_nm(p_vec_e(i)) = d_nm_e(i);
        }
        for (int i = 0; i < n_vec_o.size(); i++) {
          c_nm(p_vec_o(i)) = c_nm_o(i);
          d_nm(p_vec_o(i)) = d_nm_o(i);
        }
      }

      // Calculate for m < 0
      if (m) {
        ArrayXi p_vec_n_e = n_vec_e * (n_vec_e + 1) - m; // [N_e X 1]
        ArrayXi p_vec_n_o = n_vec_o * (n_vec_o + 1) - m; // [N_e X 1]

        // For negative m, using eq. 5.37 of [Mishchenko 2002]
        VectorXc<Real> p_nm_n_e = st_TR_list[m_ind]->st_4M_T_eo().M11(n_vec_T_e, n_vec_T_e).matrix() *
            a_nm(p_vec_n_e).matrix() - st_TR_list[m_ind]->st_4M_T_eo().M12(n_vec_T_e, n_vec_T_o).matrix() *
            b_nm(p_vec_n_o).matrix();
        VectorXc<Real> q_nm_n_o = -st_TR_list[m_ind]->st_4M_T_eo().M21(n_vec_T_o, n_vec_T_e).matrix() *
            a_nm(p_vec_n_e).matrix() + st_TR_list[m_ind]->st_4M_T_eo().M22(n_vec_T_o, n_vec_T_o).matrix() *
            b_nm(p_vec_n_o).matrix();
        VectorXc<Real> p_nm_n_o = st_TR_list[m_ind]->st_4M_T_oe().M11(n_vec_T_o, n_vec_T_o).matrix() *
            a_nm(p_vec_n_o).matrix() - st_TR_list[m_ind]->st_4M_T_oe().M12(n_vec_T_o, n_vec_T_e).matrix() *
            b_nm(p_vec_n_e).matrix();
        VectorXc<Real> q_nm_n_e = -st_TR_list[m_ind]->st_4M_T_oe().M21(n_vec_T_e, n_vec_T_o).matrix() *
            a_nm(p_vec_n_o).matrix() + st_TR_list[m_ind]->st_4M_T_oe().M22(n_vec_T_e, n_vec_T_e).matrix() *
            b_nm(p_vec_n_e).matrix();

        for (int i = 0; i < n_vec_e.size(); i++) {
          p_nm(p_vec_n_e(i)) = p_nm_n_e(i);
          q_nm(p_vec_n_e(i)) = q_nm_n_e(i);
        }
        for (int i = 0; i < n_vec_o.size(); i++) {
          p_nm(p_vec_n_o(i)) = p_nm_n_o(i);
          q_nm(p_vec_n_o(i)) = q_nm_n_o(i);
        }

        if (get_R) {
          VectorXc<Real> c_nm_n_e = st_TR_list[m_ind]->st_4M_R_eo().M11(n_vec_T_e, n_vec_T_e).matrix() *
              a_nm(p_vec_n_e).matrix() - st_TR_list[m_ind]->st_4M_R_eo().M12(n_vec_T_e, n_vec_T_o).matrix() *
              b_nm(p_vec_n_o).matrix();
          VectorXc<Real> d_nm_n_o = -st_TR_list[m_ind]->st_4M_R_eo().M21(n_vec_T_o, n_vec_T_e).matrix() *
              a_nm(p_vec_n_e).matrix() + st_TR_list[m_ind]->st_4M_R_eo().M22(n_vec_T_o, n_vec_T_o).matrix() *
              b_nm(p_vec_n_o).matrix();
          VectorXc<Real> c_nm_n_o = st_TR_list[m_ind]->st_4M_R_oe().M11(n_vec_T_o, n_vec_T_o).matrix() *
              a_nm(p_vec_n_o).matrix() - st_TR_list[m_ind]->st_4M_R_oe().M12(n_vec_T_o, n_vec_T_e).matrix() *
              b_nm(p_vec_n_e).matrix();
          VectorXc<Real> d_nm_n_e = -st_TR_list[m_ind]->st_4M_R_oe().M21(n_vec_T_e, n_vec_T_o).matrix() *
              a_nm(p_vec_n_o).matrix() + st_TR_list[m_ind]->st_4M_R_oe().M22(n_vec_T_e, n_vec_T_e).matrix() *
              b_nm(p_vec_n_e).matrix();

          for (int i = 0; i < n_vec_e.size(); i++) {
            c_nm(p_vec_n_e(i)) = c_nm_n_e(i);
            d_nm(p_vec_n_e(i)) = d_nm_n_e(i);
          }
          for (int i = 0; i < n_vec_o.size(); i++) {
            c_nm(p_vec_n_o(i)) = c_nm_n_o(i);
            d_nm(p_vec_n_o(i)) = d_nm_n_o(i);
          }
        }
      }
    }

    auto output = make_unique<stAbcdnm<Real>>();
    output->p_nm = p_nm; // [P X 1]
    output->q_nm = q_nm; // [P X 1]
    output->a_nm = a_nm; // [P X 1]
    output->b_nm = b_nm; // [P X 1]
    output->c_nm = c_nm; // if get_R, [P X 1]
    output->d_nm = d_nm; // if get_R, [P X 1]
    return output;
  }

  /*
  Calculate absorption, scattering and extinction cross sections, for the case of orientationally-averaged incidence.
  This is for matrices which make use of rvh symmetry.
  Inputs:
    k1 - wavevector [L X 1]
    st_TR_list - std::vector containing unique pointers to stTR structs (in rvh-block form).
                 Should be [L X M] and should contain all m where 0 <= m <= N
  Output:
    Returns a stCrossSection struct, where each member is of size [L X 1] and
    contains information for each wavelength.
  Dependencies:
    mp_pi
  */
  template <class Real>
  unique_ptr<stCrossSection<Real>> rvhGetAverageCrossSections(
      const ArrayXr<Real>& k1, const vector<vector<unique_ptr<stTR<Real>>>>& st_TR_list) {
    int L = st_TR_list.size();
    int M = st_TR_list[0].size();

    ArrayXc<Real> ext_sum = ArrayXc<Real>::Zero(L);
    ArrayXc<Real> sca_sum = ArrayXc<Real>::Zero(L);

    for (int l = 0; l < L; l++) { // Loop on lambda
      if (st_TR_list[l][0]->st_4M_T_eo().ind1.size() + st_TR_list[l][0]->st_4M_T_eo().ind2.size() + 1 != M)
        cout << "Warning in rvhGetAverageCrossSections: " <<
            "st_TR_list does not seem to contain T-matrices for all m" << endl;
      for (int m = 0; m < M; m++) {
        if (st_TR_list[l][m]->st_4M_T_eo().m != m || st_TR_list[l][m]->st_4M_T_oe().m != m)
          cout << "Warning in rvhGetAverageCrossSections: " <<
              "st_TR_list does not seem to contain T-matrices for all m" << endl;
        Real m_factor = m ? 1 : 0.5;

        // From eq. 5.107 of Mishchenko 2002
        ext_sum(l) += m_factor * (
            st_TR_list[l][m]->st_4M_T_eo().M11.matrix().diagonal().sum() +
            st_TR_list[l][m]->st_4M_T_oe().M11.matrix().diagonal().sum() +
            st_TR_list[l][m]->st_4M_T_eo().M22.matrix().diagonal().sum() +
            st_TR_list[l][m]->st_4M_T_oe().M22.matrix().diagonal().sum());

        // From eq. 5.141 of Mishchenko 2002
        sca_sum(l) += m_factor * (
            st_TR_list[l][m]->st_4M_T_eo().M11.abs().pow(2).sum() +
            st_TR_list[l][m]->st_4M_T_eo().M12.abs().pow(2).sum() +
            st_TR_list[l][m]->st_4M_T_eo().M21.abs().pow(2).sum() +
            st_TR_list[l][m]->st_4M_T_eo().M22.abs().pow(2).sum() +
            st_TR_list[l][m]->st_4M_T_oe().M11.abs().pow(2).sum() +
            st_TR_list[l][m]->st_4M_T_oe().M12.abs().pow(2).sum() +
            st_TR_list[l][m]->st_4M_T_oe().M21.abs().pow(2).sum() +
            st_TR_list[l][m]->st_4M_T_oe().M22.abs().pow(2).sum());
      }
    }

    auto output = make_unique<stCrossSection<Real>>();
    output->C_ext = -4*mp_pi<Real>()/k1.pow(2) * ext_sum.real(); // [L X 1]
    output->C_sca = 4*mp_pi<Real>()/k1.pow(2) * sca_sum.real(); // [L X 1]
    output->C_abs = -4*mp_pi<Real>()/k1.pow(2) * (ext_sum.real() + sca_sum.real()); // [L X 1]

    return output;
  }

  /*
  Finds an estimate for delta by studying the convergence of the T^{22,m=1}_{11} matrix element.
  Delta + 1 is the NQ for which this element has reached convergence.
  See JQSRT 160, 29 *2015) for further details. Also returns the estimated converged precision.
  Inputs:
    st_geometry - unique pointer to struct containing geometric information, as from sphMakeGeometry
    params - unique pointer to struct containing simulation parameters (only the first lambda is considered)
    NQ_max - maximum number of multipoles
  Output:
    Returns a struct containing an estimate for delta and estimated converged precision.
    If delta still could not be found, it returns output->delta = 0 and output->err = NaN.
  Dependencies:
    rvhGetTRfromPQ, rvhTruncateMatrices, sphCalculatePQ
  */
  template <class Real>
  stDelta<Real> sphEstimateDelta(const unique_ptr<stRtfunc<Real>>& st_geometry,
      const unique_ptr<stParams<Real>>& params, int NQ_max = 80) {
    stDelta<Real> output;
    Real min_acc = 1e-4;
    ArrayXi abs_m_vec = ArrayXi::Ones(1); // only one m

    // This works on only one wavelength, so we choose the largest k1 * s as representative of the worst case.
    // Find max and min relative refractive index
    int max_ind;
    (params->k1 * params->s).maxCoeff(&max_ind);
    auto param1 = make_unique<stParams<Real>>();
    param1->s = ArrayXr<Real>::Constant(1, params->s(max_ind));
    param1->k1 = ArrayXr<Real>::Constant(1, params->k1(max_ind));

    // Number of pointer used for linear fit to check if error has reached a plateau
    int N_for_conv = 5;
    MatrixXr<Real> A_fit(N_for_conv, 2);
    A_fit.col(0) = VectorXr<Real>::Ones(N_for_conv);
    A_fit.col(1) = VectorXr<Real>::LinSpaced(N_for_conv, 1, N_for_conv);
    // Store errors
    ArrayXr<Real> T_2211_err = ArrayXr<Real>::Zero((NQ_max + 1)/2);
    // Calculate P,Q matrices
    vector<unique_ptr<stPQ<Real>>> st_PQ_list = sphCalculatePQ(NQ_max, abs_m_vec, st_geometry, param1, NQ_max);
    complex<Real> T_2211 = complex<Real>(0.0, 0.0);

    for (int N = 1; N <= NQ_max; N += 2) { // Loop over truncation (only odd numbers)
      // Truncate P,Q to N and get corresponding T
      int n_count = (N - 1)/2; // Expression is always mathematically an integer
      vector<unique_ptr<stPQ<Real>>> trunc_st_PQ_list = rvhTruncateMatrices(st_PQ_list, N);
      vector<unique_ptr<stTR<Real>>> st_TR_list = rvhGetTRfromPQ(trunc_st_PQ_list, false);
      complex<Real> T_2211_new = st_TR_list[0]->st_4M_T_eo().M22(0, 0);

      // Relative error
      T_2211_err(n_count) = abs(T_2211/T_2211_new - static_cast<complex<Real>>(1));

      T_2211 = T_2211_new;
      // Minimum requirement for convergence
      if (n_count >= N_for_conv && T_2211_err(n_count) < min_acc) {
        // Then test if the last N_for_conv errors have flattened out by a
        // linear regression to the last N_for_conv points
        VectorXr<Real> b_vec = log10(abs(T_2211_err(seq(n_count - N_for_conv + 1, n_count))));
        MatrixXr<Real> coeff = A_fit.bdcSvd(ComputeThinU | ComputeThinV).solve(b_vec);
        Real slope = coeff(1);
        // if slope < 0, then still converging
        if (slope > -0.5) { // this means less than one digit better over 2*N_for_conv steps
          output.delta = N - 2*N_for_conv + 2; // N = 2*n_count - 1 > 2*N_for_conv - 1
          output.err = (T_2211_err(seq(n_count - N_for_conv + 1, n_count))).mean();
          return output;
        }
      }
    }
    output.delta = -1;
    output.err = 0;
    return output;
  }
}

#endif


/* type definitions for monte carlo simulation */

/*
 * HISTORY
 * 16-Oct-88 Jeffrey Trull (jt1j) at Carnegie-Mellon University
 *       Created
 */

/* note: in actual fact I am typing this in, 30 years later, from a paper copy,
 * and being a bit inexact about it
 */

#ifndef MONTE_HPP
#define MONTE_HPP

#include <boost/units/cmath.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/codata/universal_constants.hpp>
#include <boost/units/systems/si/codata/electron_constants.hpp>
#include <boost/units/systems/si/codata/electromagnetic_constants.hpp>

#include <boost/accumulators/numeric/functional.hpp>

// Accumulators don't like Boost.Units for various reasons
// but as usual there are customization points we can use to fix things
// I will do one as a training exercise but if writing this for others
// I would probably write a single-purpose custom accumulator of my own...

namespace boost { namespace numeric
{
// teach Accumulators how to produce a value of "one" for double/float times
// as the default weight for our time-weighted average velocity
// It shouldn't be necessary for the calculation since we will always supply a weight...

// specialize one<>
template<typename Float>
struct one<boost::units::quantity<boost::units::si::time, Float>>
{
    using type = one;
    using value_type = boost::units::quantity<boost::units::si::time, Float>;
    static value_type const value;

    operator value_type const & () const
    {
        throw std::runtime_error("we should never get here");
        return one::value;
    }
};

template<typename Float>
boost::units::quantity<boost::units::si::time, Float> const
one<boost::units::quantity<boost::units::si::time, Float>>::value =
    Float{1.0} * boost::units::si::second;

}}

namespace monte {

using namespace boost::units::si;

// constants
constexpr double two_pi   = boost::math::constants::two_pi<double>();
constexpr double     pi   = boost::math::constants::pi<double>();
constexpr auto   echarge  = constants::codata::e;
constexpr auto   dirac    = constants::codata::hbar;
constexpr auto   T        = 300.0 * kelvin;  // room temperature
constexpr auto   cm       = 0.01 * meter;
constexpr auto   angstrom = 1e-10 * meter;

/*
 * evaluate the constant of proportionality between the acoustic
 * scattering rate and the square root of the electron energy
 */
constexpr auto   eV = echarge * volt;
constexpr auto   g  = 10e-3 * kilogram;
constexpr auto   cm3 = cm*cm*cm;

// properties of GaAs
constexpr auto   E1 = 7.0 * eV;            // Acoustic deformation potential
constexpr auto   rho = 5.37 * g / cm3;     // Crystal density
constexpr auto   u = 5.2E5 * cm / second;  // speed of sound in GaAs
constexpr double meff = 0.063; // effective electron mass in gamma (000) valley (unitless)

// Shur 2-3-12 as modified by assignment (terms after gamma^1/2 removed)
constexpr auto scatter_const =
    // units for this initial constant not given in Shur :-/
    0.449E18 * ((g / cm3) * (cm / second) * (cm / second) /
                (kelvin * eV * eV * sqrt(eV) * second)) *
    std::pow(meff, 1.5) * T * (E1*E1) /
    (rho * (u*u)) ;

/*
 * evaluate the accelerations due to the constant electric field
 * between collisions
 */
constexpr auto Efield = 10000.0 * volt / cm;
constexpr auto accel_const = echarge * Efield / dirac ;

/*
 * evaluate k-to-velocity conversion constant
 */

// The instantaneous velocity (in real space) is the gradient of E with respect to k
// The Shur book seems to suggest calculating initial and final energies from a given timestep
// then assuming a straight line between. Here I am just using the instantaneous value,
// which is wrong.
constexpr auto   m_e       = constants::codata::m_e;   // electron mass
// If E(k) = (dirac^2 * k^2)/(2 * m_eff), and v = (1/dirac) * dE(k)/dt, then
// v(k) = dirac * k / m_eff
constexpr auto   vel_const = dirac/(meff * m_e) ;

// types

template<typename Float>
struct vector_str {
    vector_str() = default;
    boost::units::quantity<wavenumber, Float> x, y, z;

    // the energy associated with this k state
    boost::units::quantity<energy, Float> get_energy() const;

    // new set of k resulting from a collision
    vector_str
    collision_result(Float theta_r, Float phi_r) const;

};

template<typename Float>
using vector_ptr = vector_str<Float> *;

template<typename Float>
using vector_cptr = vector_str<Float> const *;

// utility functions

/* k to energy conversion */
template<typename Float>
boost::units::quantity<energy, Float>
vector_str<Float>::get_energy() const
/*
 * This appears to be very complicated and depends on which region
 * of reciprocal lattice space the electron currently occupies.
 * To make this tractable I'm assuming a parabolic relationship
 * given by E(k) = (dirac^2 * k^2)/(2 * m_eff)
 * See Shur p14.
 */
{
    constexpr auto C = (dirac * dirac) / (2.0 * meff * m_e) ;

    boost::units::quantity<energy> nrg = C * ((x * x) + (y * y) + (z * z));

    // returning above calculation directly gets a compile error
    // but we can explicitly convert it to another (possibly lower) precision *shrug*
    return boost::units::quantity<energy, Float>{nrg};

}

template<typename Float>
vector_str<Float>
vector_str<Float>::collision_result(Float theta_r, Float phi_r) const
{
    using namespace boost::units;

    // calculate our current spherical angles
    using angle = quantity<si::plane_angle, Float>;
    angle phi{atan(y/x)};
    angle theta{atan(sqrt(x*x + y*y)/z)};
    // and our current rho (radius)
    // total crystal momentum is preserved
    quantity<si::wavenumber, Float> total_k{sqrt(x*x + y*y + z*z)};

    // determine momentum components if original vector were Z axis aligned:
    vector_str<Float> k2prime;
    k2prime.x = total_k * sin(theta_r) * cos(phi_r);
    k2prime.y = total_k * sin(theta_r) * sin(phi_r);
    k2prime.z = total_k * cos(theta_r);

    // rotate to reflect actual initial angles and return
    vector_str<Float> lastk;
    lastk.x = cos(phi) * cos(theta) * k2prime.x
        + cos(phi) * sin(theta) * k2prime.z
        - sin(phi) * k2prime.y;

    lastk.y = sin(phi) * cos(theta) * k2prime.x
        + sin(phi) * sin(theta) * k2prime.z
        + cos(phi) * k2prime.y;

    lastk.z = cos(theta) * k2prime.z - sin(theta) * k2prime.x;

    return lastk;
}

}

#endif // MONTE_HPP