#ifndef TRIUMF_BNMR_SRF_LOCAL_HPP
#define TRIUMF_BNMR_SRF_LOCAL_HPP

// C++ standard library headers
#include <algorithm>
#include <cmath>
#include <limits>
#include <numeric>
#include <vector>

// Boost headers
#include <boost/math/interpolators/pchip.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>

// triumf++ headers
#include <triumf/bnmr/nuclei.hpp>
#include <triumf/math/pdf.hpp>
#include <triumf/nmr/dipole_dipole.hpp>
#include <triumf/nmr/nuclei.hpp>
#include <triumf/numpy.hpp>
#include <triumf/superconductivity/bcs.hpp>
#include <triumf/superconductivity/phenomenology.hpp>
#include <triumf/superconductivity/pippard.hpp>

// ROOT headers
#include <ROOT/RCsvDS.hxx>
#include <ROOT/RDF/RInterface.hxx>
#include <ROOT/RDataFrame.hxx>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// β-detected nuclear magnetic resonance (β-NMR)
namespace bnmr {

// superconducting radio-frequency (SRF) materials
namespace srf {

// local electrodynamics
namespace local {

/// Model SLR rate.
/// The surface "dead layer" has its own distict SLR rate.
template <typename T = double>
T slr_rate_z(T z, T temperature, T critical_temperature, T lambda_0, T exponent,
             T applied_field, T dipole_field, T correlation_rate,
             T slr_constant, T slr_exponent, T surface_thickness,
             T surface_rate) {
  // correct for the field-dependence to the critical temperature
  // https://doi.org/10.1103/PhysRevB.2.3545
  // https://doi.org/10.1016/j.nima.2004.09.003
  // https://doi.org/10.1088/0953-2048/25/6/065014
  constexpr T Nb_B_c2 = 0.425; // T
  T corrected_critical_temperature =
      triumf::superconductivity::phenomenology::critical_temperature<T>(
          applied_field, critical_temperature, Nb_B_c2, 0.5);
  // correct depth for the surface layer
  T _z_ = z - surface_thickness;
  if (_z_ < 0.0) {
    return surface_rate;
  } else {
    // calculate the local field from the screening profile
    T lambda = triumf::superconductivity::phenomenology::penetration_depth(
        temperature, corrected_critical_temperature, exponent, lambda_0);
    T screened_field = temperature > corrected_critical_temperature
                           ? applied_field
                           : applied_field * std::exp(-1.0 * _z_ / lambda);
    // calculate the dipole-dipole SLR rate in the superconducting state
    T dd_rate = triumf::nmr::dipole_dipole::slr_rate<T>(
        screened_field, dipole_field, correlation_rate,
        triumf::bnmr::nuclei::lithium_8<T>::gyromagnetic_ratio(),
        triumf::nmr::nuclei::niobium_93<T>::gyromagnetic_ratio());
    // calculate the SLR rate in the normal state
    T ns_rate = slr_constant * std::pow(temperature, slr_exponent);
    // return the "surface" contribution at shallow depths
    return dd_rate + ns_rate;
  }
}

/// Model SLR rate.
/// The surface "dead layer" has the same SLR rate as the normal state.
template <typename T = double>
T slr_rate_nss_z(T z, T temperature, T critical_temperature, T lambda_0,
                 T exponent, T applied_field, T dipole_field,
                 T correlation_rate, T slr_constant, T slr_exponent,
                 T surface_thickness) {
  // correct for the field-dependence to the critical temperature
  // https://doi.org/10.1103/PhysRevB.2.3545
  // https://doi.org/10.1016/j.nima.2004.09.003
  // https://doi.org/10.1088/0953-2048/25/6/065014
  constexpr T Nb_B_c2 = 0.425; // T
  T corrected_critical_temperature =
      triumf::superconductivity::phenomenology::critical_temperature<T>(
          applied_field, critical_temperature, Nb_B_c2, 0.5);
  // correct depth for the surface layer
  T _z_ = z - surface_thickness;
  if (_z_ < 0.0) {
    // calculate the dipole-dipole SLR rate in the normal state
    T dd_rate_surf = triumf::nmr::dipole_dipole::slr_rate<T>(
        applied_field, dipole_field, correlation_rate,
        triumf::bnmr::nuclei::lithium_8<T>::gyromagnetic_ratio(),
        triumf::nmr::nuclei::niobium_93<T>::gyromagnetic_ratio());
    // calculate the SLR rate in the normal state
    T ns_rate_surf = slr_constant * std::pow(temperature, slr_exponent);
    // return the "surface" contribution at shallow depths
    return dd_rate_surf + ns_rate_surf;
  } else {
    // calculate the local field from the screening profile
    T lambda = triumf::superconductivity::phenomenology::penetration_depth(
        temperature, corrected_critical_temperature, exponent, lambda_0);
    T screened_field = temperature > corrected_critical_temperature
                           ? applied_field
                           : applied_field * std::exp(-1.0 * _z_ / lambda);
    // calculate the dipole-dipole SLR rate in the superconducting state
    T dd_rate = triumf::nmr::dipole_dipole::slr_rate<T>(
        screened_field, dipole_field, correlation_rate,
        triumf::bnmr::nuclei::lithium_8<T>::gyromagnetic_ratio(),
        triumf::nmr::nuclei::niobium_93<T>::gyromagnetic_ratio());
    // calculate the SLR rate in the normal state
    T ns_rate = slr_constant * std::pow(temperature, slr_exponent);
    // return the "surface" contribution at shallow depths
    return dd_rate + ns_rate;
  }
}

/// Model SLR rate for a thin film.
/// The surface "dead layer" has its own distict SLR rate.
template <typename T = double>
T slr_rate_film_z(T z, T temperature, T critical_temperature, T lambda_0,
                  T exponent, T applied_field, T dipole_field,
                  T correlation_rate, T slr_constant, T slr_exponent,
                  T surface_thickness, T surface_rate, T film_thickness) {
  // correct for the field-dependence to the critical temperature
  // https://doi.org/10.1103/PhysRevB.2.3545
  // https://doi.org/10.1016/j.nima.2004.09.003
  // https://doi.org/10.1088/0953-2048/25/6/065014
  constexpr T Nb_B_c2 = 0.425; // T
  T corrected_critical_temperature =
      triumf::superconductivity::phenomenology::critical_temperature<T>(
          applied_field, critical_temperature, Nb_B_c2, 0.5);
  // correct depth for the surface layer
  T _z_ = z - surface_thickness;
  // correct film thickness for the surface layer
  T _d_ = 0.5 * (film_thickness - surface_thickness);
  if (_z_ < 0.0) {
    return surface_rate;
  } else {
    // calculate the local field from the screening profile
    T lambda = triumf::superconductivity::phenomenology::penetration_depth(
        temperature, corrected_critical_temperature, exponent, lambda_0);
    T screened_field = temperature > corrected_critical_temperature
                           ? applied_field
                           : applied_field *
                                 std::cosh((0.5 * _d_ - _z_) / lambda) /
                                 std::cosh(0.5 * _d_ / lambda);
    // calculate the dipole-dipole SLR rate in the superconducting state
    T dd_rate = triumf::nmr::dipole_dipole::slr_rate<T>(
        screened_field, dipole_field, correlation_rate,
        triumf::bnmr::nuclei::lithium_8<T>::gyromagnetic_ratio(),
        triumf::nmr::nuclei::niobium_93<T>::gyromagnetic_ratio());
    // calculate the SLR rate in the normal state
    T ns_rate = slr_constant * std::pow(temperature, slr_exponent);
    // return the "surface" contribution at shallow depths
    return dd_rate + ns_rate;
  }
}

/// Model SLR rate for a thin film.
/// The surface "dead layer" has the same SLR rate as the normal state.
template <typename T = double>
T slr_rate_film_nss_z(T z, T temperature, T critical_temperature, T lambda_0,
                      T exponent, T applied_field, T dipole_field,
                      T correlation_rate, T slr_constant, T slr_exponent,
                      T surface_thickness, T film_thickness) {
  // correct for the field-dependence to the critical temperature
  // https://doi.org/10.1103/PhysRevB.2.3545
  // https://doi.org/10.1016/j.nima.2004.09.003
  // https://doi.org/10.1088/0953-2048/25/6/065014
  constexpr T Nb_B_c2 = 0.425; // T
  T corrected_critical_temperature =
      triumf::superconductivity::phenomenology::critical_temperature<T>(
          applied_field, critical_temperature, Nb_B_c2, 0.5);
  // correct depth for the surface layer
  T _z_ = z - surface_thickness;
  // correct film thickness for the surface layer
  T _d_ = 0.5 * (film_thickness - surface_thickness);
  if (_z_ < 0.0) {
    // calculate the dipole-dipole SLR rate in the normal state
    T dd_rate_surf = triumf::nmr::dipole_dipole::slr_rate<T>(
        applied_field, dipole_field, correlation_rate,
        triumf::bnmr::nuclei::lithium_8<T>::gyromagnetic_ratio(),
        triumf::nmr::nuclei::niobium_93<T>::gyromagnetic_ratio());
    // calculate the SLR rate in the normal state
    T ns_rate_surf = slr_constant * std::pow(temperature, slr_exponent);
    // return the "surface" contribution at shallow depths
    return dd_rate_surf + ns_rate_surf;
  } else {
    // calculate the local field from the screening profile
    T lambda = triumf::superconductivity::phenomenology::penetration_depth(
        temperature, corrected_critical_temperature, exponent, lambda_0);
    T screened_field = temperature > corrected_critical_temperature
                           ? applied_field
                           : applied_field *
                                 std::cosh((0.5 * _d_ - _z_) / lambda) /
                                 std::cosh(0.5 * _d_ / lambda);
    // calculate the dipole-dipole SLR rate in the superconducting state
    T dd_rate = triumf::nmr::dipole_dipole::slr_rate<T>(
        screened_field, dipole_field, correlation_rate,
        triumf::bnmr::nuclei::lithium_8<T>::gyromagnetic_ratio(),
        triumf::nmr::nuclei::niobium_93<T>::gyromagnetic_ratio());
    // calculate the SLR rate in the normal state
    T ns_rate = slr_constant * std::pow(temperature, slr_exponent);
    // return the "surface" contribution at shallow depths
    return dd_rate + ns_rate;
  }
}

/// Depth-resolved analyzer.
/// For implantation averaging over the SLR model (independent surface rate).
template <typename T = double> class DepthResolvedAnalyzer {
public:
  /// constructor.
  DepthResolvedAnalyzer(const std::string &csv_filename) {
    // read in the data for the stopping profiles
    read_csv_data(csv_filename);

    // default initialized values
    temperature = 2.5;
    critical_temperature = 9.25;
    lambda_0 = 40.0;
    exponent = 4.0;
    applied_field = 0.02;
    dipole_field = 1e-5;
    correlation_rate = 1.0 / 23.8e-6;
    slr_constant = 0.75;
    slr_exponent = 1.0;
    surface_thickness = 5.0;
    surface_rate = 10.0;
  };

  /// Read the CSV data
  void read_csv_data(const std::string &csv_filename) {
    // read the data into a ROOT DataFrame...
    auto df = ROOT::RDF::MakeCsvDataFrame(csv_filename);
    // ...and extract the values
    _energy = df.Take<T>("Energy (keV)").GetValue();
    _alpha_1 = df.Take<T>("alpha_1").GetValue();
    _alpha_1_error = df.Take<T>("alpha_1_error").GetValue();
    _beta_1 = df.Take<T>("beta_1").GetValue();
    _beta_1_error = df.Take<T>("beta_1_error").GetValue();
    _z_max_1 = df.Take<T>("z_max_1").GetValue();
    _z_max_1_error = df.Take<T>("z_max_1_error").GetValue();
    _fraction_1 = df.Take<T>("fraction_1").GetValue();
    _fraction_1_error = df.Take<T>("fraction_1_error").GetValue();
    _alpha_2 = df.Take<T>("alpha_2").GetValue();
    _alpha_2_error = df.Take<T>("alpha_2_error").GetValue();
    _beta_2 = df.Take<T>("beta_2").GetValue();
    _beta_2_error = df.Take<T>("beta_2_error").GetValue();
    _z_max_2 = df.Take<T>("z_max_2").GetValue();
    _z_max_2_error = df.Take<T>("z_max_2_error").GetValue();
  };

  /// Return the the minium energy available for interpolation.
  T energy_min() {
    return *std::min_element(_energy.begin(), _energy.end()) +
           std::sqrt(std::numeric_limits<T>::epsilon());
  };

  /// Return the the maximum energy available for interpolation.
  T energy_max() {
    return *std::max_element(_energy.begin(), _energy.end()) -
           std::sqrt(std::numeric_limits<T>::epsilon());
  };

  /// Return an interpolated alpha_1 value.
  T alpha_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        alpha_interpolator_1(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_alpha_1)));
    return alpha_interpolator_1(energy_keV);
  };

  /// Return an interpolated alpha_2 value.
  T alpha_2(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        alpha_interpolator_2(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_alpha_2)));
    return alpha_interpolator_2(energy_keV);
  };

  /// Return an interpolated beta_1 value.
  T beta_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        beta_interpolator_1(std::move(std::vector<T>(_energy)),
                            std::move(std::vector<T>(_beta_1)));
    return beta_interpolator_1(energy_keV);
  };

  /// Return an interpolated beta_2 value.
  T beta_2(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        beta_interpolator_2(std::move(std::vector<T>(_energy)),
                            std::move(std::vector<T>(_beta_2)));
    return beta_interpolator_2(energy_keV);
  };

  /// Return an interpolated z_max_1 value.
  T z_max_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        z_max_interpolator_1(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_z_max_1)));
    return z_max_interpolator_1(energy_keV);
  };

  /// Return an interpolated z_max_2 value.
  T z_max_2(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        z_max_interpolator_2(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_z_max_2)));
    return z_max_interpolator_2(energy_keV);
  };

  /// Return an interpolated fraction_1 value.
  T fraction_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        fraction_interpolator_1(std::move(std::vector<T>(_energy)),
                                std::move(std::vector<T>(_fraction_1)));
    return fraction_interpolator_1(energy_keV);
  };

  /// Return the average implantation depth.
  T z_average(T energy_keV) {
    T a_1 = alpha_1(energy_keV);
    T b_1 = beta_1(energy_keV);
    T zm_1 = z_max_1(energy_keV);
    T f_1 = fraction_1(energy_keV);
    T a_2 = alpha_2(energy_keV);
    T b_2 = beta_2(energy_keV);
    T zm_2 = z_max_2(energy_keV);
    return f_1 * zm_1 * a_1 / (a_1 + b_1) +
           (1.0 - f_1) * zm_2 * a_2 / (a_2 + b_2);
  };

  /// depth-averaging using numeric integration
  T operator()(T energy_keV) {
    static boost::math::quadrature::tanh_sinh<T> integrator;
    auto integrand = [&](T z) {
      return slr_rate_z<T>(z, temperature, critical_temperature, lambda_0,
                           exponent, applied_field, dipole_field,
                           correlation_rate, slr_constant, slr_exponent,
                           surface_thickness, surface_rate) *
             triumf::math::pdf::two_modified_beta<T>(
                 z, alpha_1(energy_keV), beta_1(energy_keV),
                 z_max_1(energy_keV), fraction_1(energy_keV),
                 alpha_2(energy_keV), beta_2(energy_keV), z_max_2(energy_keV));
    };
    T z_max = std::max(z_max_1(energy_keV), z_max_2(energy_keV));
    T Q = integrator.integrate(integrand, 0.0, z_max);
    return Q;
  };

  /// model parameters
  T temperature;
  T critical_temperature;
  T lambda_0;
  T exponent;
  T applied_field;
  T dipole_field;
  T correlation_rate;
  T slr_constant;
  T slr_exponent;
  T surface_thickness;
  T surface_rate;
  T electron_phonon_coupling;

private:
  /// vectors of data from csv file
  std::vector<T> _energy;
  std::vector<T> _alpha_1;
  std::vector<T> _alpha_1_error;
  std::vector<T> _beta_1;
  std::vector<T> _beta_1_error;
  std::vector<T> _z_max_1;
  std::vector<T> _z_max_1_error;
  std::vector<T> _fraction_1;
  std::vector<T> _fraction_1_error;
  std::vector<T> _alpha_2;
  std::vector<T> _alpha_2_error;
  std::vector<T> _beta_2;
  std::vector<T> _beta_2_error;
  std::vector<T> _z_max_2;
  std::vector<T> _z_max_2_error;
};

/// Depth-resolved analyzer.
/// For implantation averaging over the SLR model (normal state surface rate).
template <typename T = double> class DepthResolvedAnalyzerNSS {
public:
  /// constructor.
  DepthResolvedAnalyzerNSS(const std::string &csv_filename) {
    // read in the data for the stopping profiles
    read_csv_data(csv_filename);

    // default initialized values
    temperature = 2.5;
    critical_temperature = 9.25;
    lambda_0 = 40.0;
    exponent = 4.0;
    applied_field = 0.02;
    dipole_field = 1e-5;
    correlation_rate = 1.0 / 23.8e-6;
    slr_constant = 0.75;
    slr_exponent = 1.0;
    surface_thickness = 5.0;
    surface_rate = 10.0;
  };

  /// Read the CSV data
  void read_csv_data(const std::string &csv_filename) {
    // read the data into a ROOT DataFrame...
    auto df = ROOT::RDF::MakeCsvDataFrame(csv_filename);
    // ...and extract the values
    _energy = df.Take<T>("Energy (keV)").GetValue();
    _alpha_1 = df.Take<T>("alpha_1").GetValue();
    _alpha_1_error = df.Take<T>("alpha_1_error").GetValue();
    _beta_1 = df.Take<T>("beta_1").GetValue();
    _beta_1_error = df.Take<T>("beta_1_error").GetValue();
    _z_max_1 = df.Take<T>("z_max_1").GetValue();
    _z_max_1_error = df.Take<T>("z_max_1_error").GetValue();
    _fraction_1 = df.Take<T>("fraction_1").GetValue();
    _fraction_1_error = df.Take<T>("fraction_1_error").GetValue();
    _alpha_2 = df.Take<T>("alpha_2").GetValue();
    _alpha_2_error = df.Take<T>("alpha_2_error").GetValue();
    _beta_2 = df.Take<T>("beta_2").GetValue();
    _beta_2_error = df.Take<T>("beta_2_error").GetValue();
    _z_max_2 = df.Take<T>("z_max_2").GetValue();
    _z_max_2_error = df.Take<T>("z_max_2_error").GetValue();
  };

  /// Return the the minium energy available for interpolation.
  T energy_min() {
    return *std::min_element(_energy.begin(), _energy.end()) +
           std::sqrt(std::numeric_limits<T>::epsilon());
  };

  /// Return the the maximum energy available for interpolation.
  T energy_max() {
    return *std::max_element(_energy.begin(), _energy.end()) -
           std::sqrt(std::numeric_limits<T>::epsilon());
  };

  /// Return an interpolated alpha_1 value.
  T alpha_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        alpha_interpolator_1(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_alpha_1)));
    return alpha_interpolator_1(energy_keV);
  };

  /// Return an interpolated alpha_2 value.
  T alpha_2(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        alpha_interpolator_2(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_alpha_2)));
    return alpha_interpolator_2(energy_keV);
  };

  /// Return an interpolated beta_1 value.
  T beta_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        beta_interpolator_1(std::move(std::vector<T>(_energy)),
                            std::move(std::vector<T>(_beta_1)));
    return beta_interpolator_1(energy_keV);
  };

  /// Return an interpolated beta_2 value.
  T beta_2(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        beta_interpolator_2(std::move(std::vector<T>(_energy)),
                            std::move(std::vector<T>(_beta_2)));
    return beta_interpolator_2(energy_keV);
  };

  /// Return an interpolated z_max_1 value.
  T z_max_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        z_max_interpolator_1(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_z_max_1)));
    return z_max_interpolator_1(energy_keV);
  };

  /// Return an interpolated z_max_2 value.
  T z_max_2(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        z_max_interpolator_2(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_z_max_2)));
    return z_max_interpolator_2(energy_keV);
  };

  /// Return an interpolated fraction_1 value.
  T fraction_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        fraction_interpolator_1(std::move(std::vector<T>(_energy)),
                                std::move(std::vector<T>(_fraction_1)));
    return fraction_interpolator_1(energy_keV);
  };

  /// Return the average implantation depth.
  T z_average(T energy_keV) {
    T a_1 = alpha_1(energy_keV);
    T b_1 = beta_1(energy_keV);
    T zm_1 = z_max_1(energy_keV);
    T f_1 = fraction_1(energy_keV);
    T a_2 = alpha_2(energy_keV);
    T b_2 = beta_2(energy_keV);
    T zm_2 = z_max_2(energy_keV);
    return f_1 * zm_1 * a_1 / (a_1 + b_1) +
           (1.0 - f_1) * zm_2 * a_2 / (a_2 + b_2);
  };

  /// depth-averaging using numeric integration
  T operator()(T energy_keV) {
    static boost::math::quadrature::tanh_sinh<T> integrator;
    auto integrand = [&](T z) {
      return slr_rate_nss_z<T>(z, temperature, critical_temperature, lambda_0,
                               exponent, applied_field, dipole_field,
                               correlation_rate, slr_constant, slr_exponent,
                               surface_thickness) *
             triumf::math::pdf::two_modified_beta<T>(
                 z, alpha_1(energy_keV), beta_1(energy_keV),
                 z_max_1(energy_keV), fraction_1(energy_keV),
                 alpha_2(energy_keV), beta_2(energy_keV), z_max_2(energy_keV));
    };
    T z_max = std::max(z_max_1(energy_keV), z_max_2(energy_keV));
    T Q = integrator.integrate(integrand, 0.0, z_max);
    return Q;
  };

  /// model parameters
  T temperature;
  T critical_temperature;
  T lambda_0;
  T exponent;
  T applied_field;
  T dipole_field;
  T correlation_rate;
  T slr_constant;
  T slr_exponent;
  T surface_thickness;
  T surface_rate;
  T electron_phonon_coupling;

private:
  /// vectors of data from csv file
  std::vector<T> _energy;
  std::vector<T> _alpha_1;
  std::vector<T> _alpha_1_error;
  std::vector<T> _beta_1;
  std::vector<T> _beta_1_error;
  std::vector<T> _z_max_1;
  std::vector<T> _z_max_1_error;
  std::vector<T> _fraction_1;
  std::vector<T> _fraction_1_error;
  std::vector<T> _alpha_2;
  std::vector<T> _alpha_2_error;
  std::vector<T> _beta_2;
  std::vector<T> _beta_2_error;
  std::vector<T> _z_max_2;
  std::vector<T> _z_max_2_error;
};

/// Depth-resolved analyzer for thin films.
/// For implantation averaging over the SLR model (independent surface rate).
template <typename T = double> class DepthResolvedFilmAnalyzer {
public:
  /// constructor.
  DepthResolvedFilmAnalyzer(const std::string &csv_filename) {
    // read in the data for the stopping profiles
    read_csv_data(csv_filename);

    // default initialized values
    temperature = 2.5;
    critical_temperature = 9.25;
    lambda_0 = 40.0;
    exponent = 4.0;
    applied_field = 0.02;
    dipole_field = 1e-5;
    correlation_rate = 1.0 / 23.8e-6;
    slr_constant = 0.75;
    slr_exponent = 1.0;
    surface_thickness = 5.0;
    surface_rate = 10.0;
    film_thickness = 300.0;
  };

  /// Read the CSV data
  void read_csv_data(const std::string &csv_filename) {
    // read the data into a ROOT DataFrame...
    auto df = ROOT::RDF::MakeCsvDataFrame(csv_filename);
    // ...and extract the values
    _energy = df.Take<T>("Energy (keV)").GetValue();
    _alpha_1 = df.Take<T>("alpha_1").GetValue();
    _alpha_1_error = df.Take<T>("alpha_1_error").GetValue();
    _beta_1 = df.Take<T>("beta_1").GetValue();
    _beta_1_error = df.Take<T>("beta_1_error").GetValue();
    _z_max_1 = df.Take<T>("z_max_1").GetValue();
    _z_max_1_error = df.Take<T>("z_max_1_error").GetValue();
    _fraction_1 = df.Take<T>("fraction_1").GetValue();
    _fraction_1_error = df.Take<T>("fraction_1_error").GetValue();
    _alpha_2 = df.Take<T>("alpha_2").GetValue();
    _alpha_2_error = df.Take<T>("alpha_2_error").GetValue();
    _beta_2 = df.Take<T>("beta_2").GetValue();
    _beta_2_error = df.Take<T>("beta_2_error").GetValue();
    _z_max_2 = df.Take<T>("z_max_2").GetValue();
    _z_max_2_error = df.Take<T>("z_max_2_error").GetValue();
  };

  /// Return the the minium energy available for interpolation.
  T energy_min() {
    return *std::min_element(_energy.begin(), _energy.end()) +
           std::sqrt(std::numeric_limits<T>::epsilon());
  };

  /// Return the the maximum energy available for interpolation.
  T energy_max() {
    return *std::max_element(_energy.begin(), _energy.end()) -
           std::sqrt(std::numeric_limits<T>::epsilon());
  };

  /// Return an interpolated alpha_1 value.
  T alpha_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        alpha_interpolator_1(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_alpha_1)));
    return alpha_interpolator_1(energy_keV);
  };

  /// Return an interpolated alpha_2 value.
  T alpha_2(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        alpha_interpolator_2(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_alpha_2)));
    return alpha_interpolator_2(energy_keV);
  };

  /// Return an interpolated beta_1 value.
  T beta_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        beta_interpolator_1(std::move(std::vector<T>(_energy)),
                            std::move(std::vector<T>(_beta_1)));
    return beta_interpolator_1(energy_keV);
  };

  /// Return an interpolated beta_2 value.
  T beta_2(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        beta_interpolator_2(std::move(std::vector<T>(_energy)),
                            std::move(std::vector<T>(_beta_2)));
    return beta_interpolator_2(energy_keV);
  };

  /// Return an interpolated z_max_1 value.
  T z_max_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        z_max_interpolator_1(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_z_max_1)));
    return z_max_interpolator_1(energy_keV);
  };

  /// Return an interpolated z_max_2 value.
  T z_max_2(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        z_max_interpolator_2(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_z_max_2)));
    return z_max_interpolator_2(energy_keV);
  };

  /// Return an interpolated fraction_1 value.
  T fraction_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        fraction_interpolator_1(std::move(std::vector<T>(_energy)),
                                std::move(std::vector<T>(_fraction_1)));
    return fraction_interpolator_1(energy_keV);
  };

  /// Return the average implantation depth.
  T z_average(T energy_keV) {
    T a_1 = alpha_1(energy_keV);
    T b_1 = beta_1(energy_keV);
    T zm_1 = z_max_1(energy_keV);
    T f_1 = fraction_1(energy_keV);
    T a_2 = alpha_2(energy_keV);
    T b_2 = beta_2(energy_keV);
    T zm_2 = z_max_2(energy_keV);
    return f_1 * zm_1 * a_1 / (a_1 + b_1) +
           (1.0 - f_1) * zm_2 * a_2 / (a_2 + b_2);
  };

  /// depth-averaging using numeric integration
  T operator()(T energy_keV) {
    static boost::math::quadrature::tanh_sinh<T> integrator;
    auto integrand = [&](T z) {
      return slr_rate_film_z<T>(z, temperature, critical_temperature, lambda_0,
                                exponent, applied_field, dipole_field,
                                correlation_rate, slr_constant, slr_exponent,
                                surface_thickness, surface_rate,
                                film_thickness) *
             triumf::math::pdf::two_modified_beta<T>(
                 z, alpha_1(energy_keV), beta_1(energy_keV),
                 z_max_1(energy_keV), fraction_1(energy_keV),
                 alpha_2(energy_keV), beta_2(energy_keV), z_max_2(energy_keV));
    };
    T z_max = std::max(z_max_1(energy_keV), z_max_2(energy_keV));
    T Q = integrator.integrate(integrand, 0.0, z_max);
    return Q;
  };

  /// model parameters
  T temperature;
  T critical_temperature;
  T lambda_0;
  T exponent;
  T applied_field;
  T dipole_field;
  T correlation_rate;
  T slr_constant;
  T slr_exponent;
  T surface_thickness;
  T surface_rate;
  T electron_phonon_coupling;
  T film_thickness;

private:
  /// vectors of data from csv file
  std::vector<T> _energy;
  std::vector<T> _alpha_1;
  std::vector<T> _alpha_1_error;
  std::vector<T> _beta_1;
  std::vector<T> _beta_1_error;
  std::vector<T> _z_max_1;
  std::vector<T> _z_max_1_error;
  std::vector<T> _fraction_1;
  std::vector<T> _fraction_1_error;
  std::vector<T> _alpha_2;
  std::vector<T> _alpha_2_error;
  std::vector<T> _beta_2;
  std::vector<T> _beta_2_error;
  std::vector<T> _z_max_2;
  std::vector<T> _z_max_2_error;
};

/// Depth-resolved analyzer for thin films.
/// For implantation averaging over the SLR model (normal state surface rate).
template <typename T = double> class DepthResolvedFilmAnalyzerNSS {
public:
  /// constructor.
  DepthResolvedFilmAnalyzerNSS(const std::string &csv_filename) {
    // read in the data for the stopping profiles
    read_csv_data(csv_filename);

    // default initialized values
    temperature = 2.5;
    critical_temperature = 9.25;
    lambda_0 = 40.0;
    exponent = 4.0;
    applied_field = 0.02;
    dipole_field = 1e-5;
    correlation_rate = 1.0 / 23.8e-6;
    slr_constant = 0.75;
    slr_exponent = 1.0;
    surface_thickness = 5.0;
    surface_rate = 10.0;
    film_thickness = 300.0;
  };

  /// Read the CSV data
  void read_csv_data(const std::string &csv_filename) {
    // read the data into a ROOT DataFrame...
    auto df = ROOT::RDF::MakeCsvDataFrame(csv_filename);
    // ...and extract the values
    _energy = df.Take<T>("Energy (keV)").GetValue();
    _alpha_1 = df.Take<T>("alpha_1").GetValue();
    _alpha_1_error = df.Take<T>("alpha_1_error").GetValue();
    _beta_1 = df.Take<T>("beta_1").GetValue();
    _beta_1_error = df.Take<T>("beta_1_error").GetValue();
    _z_max_1 = df.Take<T>("z_max_1").GetValue();
    _z_max_1_error = df.Take<T>("z_max_1_error").GetValue();
    _fraction_1 = df.Take<T>("fraction_1").GetValue();
    _fraction_1_error = df.Take<T>("fraction_1_error").GetValue();
    _alpha_2 = df.Take<T>("alpha_2").GetValue();
    _alpha_2_error = df.Take<T>("alpha_2_error").GetValue();
    _beta_2 = df.Take<T>("beta_2").GetValue();
    _beta_2_error = df.Take<T>("beta_2_error").GetValue();
    _z_max_2 = df.Take<T>("z_max_2").GetValue();
    _z_max_2_error = df.Take<T>("z_max_2_error").GetValue();
  };

  /// Return the the minium energy available for interpolation.
  T energy_min() {
    return *std::min_element(_energy.begin(), _energy.end()) +
           std::sqrt(std::numeric_limits<T>::epsilon());
  };

  /// Return the the maximum energy available for interpolation.
  T energy_max() {
    return *std::max_element(_energy.begin(), _energy.end()) -
           std::sqrt(std::numeric_limits<T>::epsilon());
  };

  /// Return an interpolated alpha_1 value.
  T alpha_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        alpha_interpolator_1(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_alpha_1)));
    return alpha_interpolator_1(energy_keV);
  };

  /// Return an interpolated alpha_2 value.
  T alpha_2(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        alpha_interpolator_2(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_alpha_2)));
    return alpha_interpolator_2(energy_keV);
  };

  /// Return an interpolated beta_1 value.
  T beta_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        beta_interpolator_1(std::move(std::vector<T>(_energy)),
                            std::move(std::vector<T>(_beta_1)));
    return beta_interpolator_1(energy_keV);
  };

  /// Return an interpolated beta_2 value.
  T beta_2(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        beta_interpolator_2(std::move(std::vector<T>(_energy)),
                            std::move(std::vector<T>(_beta_2)));
    return beta_interpolator_2(energy_keV);
  };

  /// Return an interpolated z_max_1 value.
  T z_max_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        z_max_interpolator_1(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_z_max_1)));
    return z_max_interpolator_1(energy_keV);
  };

  /// Return an interpolated z_max_2 value.
  T z_max_2(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        z_max_interpolator_2(std::move(std::vector<T>(_energy)),
                             std::move(std::vector<T>(_z_max_2)));
    return z_max_interpolator_2(energy_keV);
  };

  /// Return an interpolated fraction_1 value.
  T fraction_1(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>>
        fraction_interpolator_1(std::move(std::vector<T>(_energy)),
                                std::move(std::vector<T>(_fraction_1)));
    return fraction_interpolator_1(energy_keV);
  };

  /// Return the average implantation depth.
  T z_average(T energy_keV) {
    T a_1 = alpha_1(energy_keV);
    T b_1 = beta_1(energy_keV);
    T zm_1 = z_max_1(energy_keV);
    T f_1 = fraction_1(energy_keV);
    T a_2 = alpha_2(energy_keV);
    T b_2 = beta_2(energy_keV);
    T zm_2 = z_max_2(energy_keV);
    return f_1 * zm_1 * a_1 / (a_1 + b_1) +
           (1.0 - f_1) * zm_2 * a_2 / (a_2 + b_2);
  };

  /// depth-averaging using numeric integration
  T operator()(T energy_keV) {
    static boost::math::quadrature::tanh_sinh<T> integrator;
    auto integrand = [&](T z) {
      return slr_rate_film_nss_z<T>(
                 z, temperature, critical_temperature, lambda_0, exponent,
                 applied_field, dipole_field, correlation_rate, slr_constant,
                 slr_exponent, surface_thickness, film_thickness) *
             triumf::math::pdf::two_modified_beta<T>(
                 z, alpha_1(energy_keV), beta_1(energy_keV),
                 z_max_1(energy_keV), fraction_1(energy_keV),
                 alpha_2(energy_keV), beta_2(energy_keV), z_max_2(energy_keV));
    };
    T z_max = std::max(z_max_1(energy_keV), z_max_2(energy_keV));
    T Q = integrator.integrate(integrand, 0.0, z_max);
    return Q;
  };

  /// model parameters
  T temperature;
  T critical_temperature;
  T lambda_0;
  T exponent;
  T applied_field;
  T dipole_field;
  T correlation_rate;
  T slr_constant;
  T slr_exponent;
  T surface_thickness;
  T surface_rate;
  T electron_phonon_coupling;
  T film_thickness;

private:
  /// vectors of data from csv file
  std::vector<T> _energy;
  std::vector<T> _alpha_1;
  std::vector<T> _alpha_1_error;
  std::vector<T> _beta_1;
  std::vector<T> _beta_1_error;
  std::vector<T> _z_max_1;
  std::vector<T> _z_max_1_error;
  std::vector<T> _fraction_1;
  std::vector<T> _fraction_1_error;
  std::vector<T> _alpha_2;
  std::vector<T> _alpha_2_error;
  std::vector<T> _beta_2;
  std::vector<T> _beta_2_error;
  std::vector<T> _z_max_2;
  std::vector<T> _z_max_2_error;
};

} // namespace local

} // namespace srf

} // namespace bnmr

} // namespace triumf

#endif // TRIUMF_BNMR_SRF_LOCAL_HPP


#pragma once

#include <Eigen/Core>
#include <autodiff/autodiff_types.hpp>

namespace ipc::rigid {
namespace autogen {

    template <typename T> using Vector2T = Eigen::Matrix<T, 2, 1>;

    template <typename T>
    void time_of_impact_coeff(
        const Eigen::Vector2d& Vi,
        const Eigen::Vector2d& Vj,
        const Eigen::Vector2d& Vk,
        const Vector2T<T>& Ui,
        const Vector2T<T>& Uj,
        const Vector2T<T>& Uk,
        T& a,
        T& b,
        T& c);

    template <>
    inline void time_of_impact_coeff<double>(
        const Eigen::Vector2d& Vi,
        const Eigen::Vector2d& Vj,
        const Eigen::Vector2d& Vk,
        const Vector2T<double>& Ui,
        const Vector2T<double>& Uj,
        const Vector2T<double>& Uk,
        double& a,
        double& b,
        double& c)
    {
        a = -Ui[0] * Uj[1] + Ui[0] * Uk[1] + Ui[1] * Uj[0] - Ui[1] * Uk[0]
            - Uj[0] * Uk[1] + Uj[1] * Uk[0];
        b = -Ui[0] * Vj[1] + Ui[0] * Vk[1] + Ui[1] * Vj[0] - Ui[1] * Vk[0]
            + Uj[0] * Vi[1] - Uj[0] * Vk[1] - Uj[1] * Vi[0] + Uj[1] * Vk[0]
            - Uk[0] * Vi[1] + Uk[0] * Vj[1] + Uk[1] * Vi[0] - Uk[1] * Vj[0];
        c = -Vi[0] * Vj[1] + Vi[0] * Vk[1] + Vi[1] * Vj[0] - Vi[1] * Vk[0]
            - Vj[0] * Vk[1] + Vj[1] * Vk[0];

        double max_coeff = std::max(a, std::max(b, c));
        a /= max_coeff;
        b /= max_coeff;
        c /= max_coeff;
    }

} // namespace autogen
} // namespace ipc::rigid

#include "auto_time_of_impact_coeff.ipp"


#ifndef SPATHTPP
#define SPATHTPP

// -------------------------------------------------------
//
//   Spatially-regularized Levenberg Marquardt algorithm
//   Coded by J. de la Cruz Rodriguez (ISP-SU, 2020)
//
//   Reference: de la Cruz Rodriguez (2019):
//   https://ui.adsabs.harvard.edu/abs/2019A%26A...631A.153D/abstract
//
//   -------------------------------------------------------

#include <cmath>

#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

namespace spa{

  // ************************************************************** //

  template<class T> inline T SQ(T const &var){return var*var;}

  // ************************************************************** //

  template <class T, typename U > U ksum(const size_t n, const T* const  __restrict__ arr){

    U sum = 0, c = 0;

    for(size_t kk = 0; kk<n; ++kk){
      U const y = static_cast<U>(arr[kk]) - c;
      U const t = sum + y;
      c = (t - sum) - y;
      sum = t;
    }

    return sum;
  }

  // ************************************************************** //

  template <class T, typename U > U ksum2(const size_t n, const T* const __restrict__ arr){

    U sum = 0, c = 0;

    for(size_t kk = 0; kk<n; ++kk){
      U const y = SQ<U>(static_cast<U>(arr[kk])) - c;
      U const t = sum + y;
      c = (t - sum) - y;
      sum = t;
    }

    return sum;
  }
  // ************************************************************** //

  template <class T, typename U > U ksumMult(const size_t n, const T* const __restrict__ arr, const T* const __restrict__ arr1){

    U sum = 0, c = 0;

    for(size_t kk = 0; kk<n; ++kk){
      U const y = static_cast<U>(arr[kk]*arr1[kk]) - c;
      U const t = sum + y;
      c = (t - sum) - y;
      sum = t;
    }

    return sum;
  }
  // ************************************************************** //

  template<typename T>
  struct Par{
    bool isCyclic;
    bool limited;
    T scale;
    T limits[2];
    T alpha;

    Par(): isCyclic(false), limited(false), scale(1.0), limits{0,0}, alpha(0){};
    Par(bool const cyclic, bool const ilimited, T const scal, T const mi, T const ma, T const alp):
      isCyclic(cyclic), limited(ilimited), scale(scal), limits{mi,ma}, alpha(alp){};

    Par(Par<T> const& in): isCyclic(in.isCyclic) ,limited(in.limited), scale(in.scale), limits{in.limits[0], in.limits[1]}, alpha(in.alpha){};

    Par<T> &operator=(Par<T> const& in)
    {
      isCyclic = in.isCyclic, limited = in.limited, scale=in.scale, limits[0]=in.limits[0], limits[1]=in.limits[1], alpha = in.alpha;
      return *this;
    }

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

    inline void Normalize(T &val)const{val /= scale;};

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

    inline void Scale(T &val)const{val *= scale;};

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

    inline void Check(T &val)const{
      if(!limited) return;
      if(isCyclic){
	if(val > limits[1]) val -= 3.1415926f;
	if(val < limits[0]) val += 3.1416026f;
      }
      val = std::max<T>(std::min<T>(val, limits[1]),limits[0]);
    }

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

    inline void CheckNormalized(T &val)const{
      if(!limited) return;
      Scale(val);
      Check(val);
      Normalize(val);
    }

  };

  // ************************************************************** //

  template<typename T> struct Chi2{
    T chi2;
    T pen2;

    Chi2(T const ichi2, T const ipen2): chi2(ichi2), pen2(ipen2){};

    Chi2(): chi2(0), pen2(0){};

    Chi2(Chi2<T> const& in): chi2(in.chi2), pen2(in.pen2){};

    Chi2<T> &operator=(Chi2<T> const& in){chi2 = in.chi2; pen2 = in.pen2; return *this;}

    inline T value()const{return chi2 + pen2;}

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

    std::string formatted()const{
      char bla[50];
      sprintf(bla, "%13.5f (%13.5f + %13.5f)", value(), chi2, pen2);
      return std::string(bla);
    }

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

    T operator()()const{return value();}

  };

  // ************************************************************** //

  template<typename T> struct container{
    int nDat, ny, nx, Nreal;
    T mu;
    Eigen::TensorMap<Eigen::Tensor<T,3, Eigen::RowMajor>> obs;
    Eigen::Matrix<T,Eigen::Dynamic,1> sig;

    std::vector<Par<T>> Pinfo;
    std::vector<const ml::Milne<T>*> Me;

    int getNreal()const{return Nreal;}

    container(): nDat(0), ny(0), nx(0), Nreal(1), mu(1),   sig(), Pinfo(), Me(){};

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

    container(int const iny, int const inx, T const iMu, int const inDat, T* const __restrict__ iobs,
	      const T* const __restrict__ isig,
	      std::vector<Par<T>> const& Pinfo_in, std::vector<ml::Milne<T>> const& iMe):
      nDat(inDat), ny(iny), nx(inx), mu(iMu), obs(iobs, iny, inx, inDat), sig(nDat), Pinfo(Pinfo_in)
    {

      // --- References to Me class ---//
      int const nthreads = (int)iMe.size();
      for(int ii=0; ii<nthreads; ++ii) Me.emplace_back(static_cast<const ml::Milne<T>*>(&iMe[ii]));

      // --- sigma --- //
      Nreal = 0;
      for(int ii=0; ii<nDat; ++ii){
	sig[ii] = 1/isig[ii];

	if(isig[ii] < 1.e10)
	  ++Nreal;

      }
    }

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

    int getNthreads()const{return int(Me.size());}

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

    void NormalizePars(int const nPar,  T* const __restrict__ par)const
    {
      int const ny1 = ny;
      int const nx1 = nx;

      for(int yy=0; yy<ny1; ++yy)
	for(int xx=0; xx<nx1; ++xx)
	  for(int pp=0; pp<nPar; ++pp)
	    par[yy*nx*nPar + xx*nPar + pp] /= Pinfo[pp].scale;
    }

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

    void ScalePars(int const nPar,  T* const __restrict__ par)const
    {
      int const ny1 = ny;
      int const nx1 = nx;

      for(int yy=0; yy<ny1; ++yy)
	for(int xx=0; xx<nx1; ++xx)
	  for(int pp=0; pp<nPar; ++pp)
	    par[yy*nx*nPar + xx*nPar + pp] *= Pinfo[pp].scale;
    }

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

    void checkPars(int const nPar,  T* const __restrict__ par)const
    {
      int const ny1 = ny;
      int const nx1 = nx;

      for(int pp=0; pp<nPar; ++pp){

	if(!Pinfo[pp].limited) continue;

	T const imin = Pinfo[pp].limits[0];
	T const imax = Pinfo[pp].limits[1];

	for(int yy=0; yy<ny1; ++yy)
	  for(int xx=0; xx<nx1; ++xx){
	    T &iPar = par[yy*nx*nPar + xx*nPar + pp];
	    iPar = std::min<T>(std::max<T>(iPar, imin), imax);
	  } //xx
      } // pp
    }

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

    void synthesize(int const nPar,  T* const __restrict__ par,  T* const __restrict__ syn,  T* const __restrict__ r)const
    {
      int const nthreads = Me.size(); int const npix = nx*ny;
      int ipix = 0, tid = 0, ww= 0;
      int const ndat = nDat;
      T const scl = 1.0 / sqrt(T(Nreal*npix));
      const T* const __restrict__ o = static_cast<const T* const>(&obs(0,0,0));

#pragma omp parallel default(shared) firstprivate(ipix, tid, ww) num_threads(nthreads)

      {
	tid = omp_get_thread_num();
#pragma omp for
	for(ipix = 0; ipix<npix; ++ipix){

	  // --- scale parameters --- //
	  for(ww=0; ww<nPar; ++ww){
	    Pinfo[ww].Scale(par[ipix*nPar + ww]);
	    Pinfo[ww].Check(par[ipix*nPar + ww]);
	  }

	  Me[tid]->synthesize(&par[ipix*nPar], &syn[nDat*ipix], mu);

	  for(ww=0; ww<ndat; ++ww)
	    r[ipix*ndat + ww] = (o[ipix*ndat + ww] - syn[ipix*ndat + ww]) * sig[ww] * scl;

	  for(ww=0; ww<nPar; ++ww)
	    Pinfo[ww].Normalize(par[ipix*nPar + ww]);

	}
      } // parallel block

    }

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

    T fx(int const nPar, T* const __restrict__ par,  T* const __restrict__ syn_in, T* const __restrict__ r)const
    {

      // --- Synthesize without derivatives --- //
      synthesize(nPar, par, syn_in, r);

      long const nEl = long(nDat)*long(nx*ny);
      return ksum2<T,double>(nEl, r);
    }

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

    T getChi2( T* const __restrict__ r)const
    {
      int const ndat = nDat;
      int const npix = nx*ny;
      int const nthreads = Me.size();

      std::vector<double> chi2(nthreads,T(0));

      // --- split the work in threads, each stores its own count --- //
      int ipix=0, tid=0;
#pragma omp parallel default(shared) firstprivate(ipix, tid) num_threads(nthreads)
      {
	tid = omp_get_thread_num();
#pragma omp for
	for(ipix=0; ipix<npix; ++ipix){

	  chi2[tid] += ksum2<T,double>(ndat, r[ipix*ndat]);

	} // ipix
      }// parallel block

      // --- add chi2 from all threads --- //
      T chitot = chi2[tid];

      for(int ii=1; ii<nthreads; ++ii)
	chitot += chi2[ii];

      return chitot;
    }

    // ------------------------------------------------------------ //
    template<typename iType = long>
    Eigen::Matrix<T,Eigen::Dynamic,1> getGamma(int const npar, T* const __restrict__ par)const
    {
      int const nthreads = Me.size();
      iType const npix = nx*ny;
      iType const ndat = nDat;

      // --- how many penalty functions do we need? Npixels-1 have penalties, (0,0) doesn't ---//

      int const nPen = 2*npix*npar; //
      T const sqr_nPen = sqrt(T(nPen));
      Eigen::Matrix<T,Eigen::Dynamic,1> Gam(nPen); Gam.setZero();
      Eigen::TensorMap<Eigen::Tensor<T,3,Eigen::RowMajor>> m(par, ny, nx, npar);
      T const normAzi = 3.1415926 / Pinfo[2].scale;

      // --- Scaling factors are sqrt-ed so when squared we get the right number --- //

      T* const  __restrict__ sq_alpha = new T [npar]();
      for(int ii=0; ii<npar; ++ii) sq_alpha[ii] = sqrt(Pinfo[ii].alpha) / sqr_nPen;

      int ipix=0, tid=0, xx=0, yy=0, pp=0;
#pragma omp parallel default(shared) firstprivate(ipix, tid, xx, yy, pp) num_threads(nthreads)
      {
	tid = omp_get_thread_num();
#pragma omp for
for(ipix=1; ipix<npix; ++ipix){

	  yy = ipix / nx;
	  xx = ipix - yy*nx;

	  if((yy-1) >= 0){
	    for(pp=0; pp<npar; ++pp)
	      Gam[ipix*npar*2 + pp*2] += sq_alpha[pp] * (m(yy,xx,pp) - m(yy-1,xx,pp));

	    // --- check azimuth --- //
	    pp = 2;
	    T const azi = (m(yy,xx,pp) - m(yy-1,xx,pp));
	    if     (fabs(azi-normAzi) < fabs(azi)) Gam[ipix*npar*2 + pp*2] =  sq_alpha[pp]*(azi-normAzi);
	    else if(fabs(azi+normAzi) < fabs(azi)) Gam[ipix*npar*2 + pp*2] =  sq_alpha[pp]*(azi+normAzi);
	  }
	  if((xx-1) >= 0){
	    for(pp=0; pp<npar; ++pp)
	      Gam[ipix*npar*2 + pp*2 + 1] += sq_alpha[pp] * (m(yy,xx,pp) - m(yy,xx-1,pp));

	    // --- check azimuth --- //
	    pp = 2;
	    T const azi = (m(yy,xx,pp) - m(yy,xx-1,pp));
	    if     (fabs(azi-normAzi) < fabs(azi)) Gam[ipix*npar*2 + pp*2 + 1] =  sq_alpha[pp]*(azi-normAzi);
	    else if(fabs(azi+normAzi) < fabs(azi)) Gam[ipix*npar*2 + pp*2 + 1] =  sq_alpha[pp]*(azi+normAzi);
	  }

	}// ipix
      }// parallel

      delete [] sq_alpha;

      return Gam;
    }

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

    void synthesize_der_one(int const npar, T* __restrict__ par, T* __restrict__ r,
			    T* __restrict__ J, int const tid, int const ipix)const
    {

      T const scl = sqrt(T(Nreal)*T(nx*ny));
      T iScl = 0;
      int const ndat = nDat;

      const T* const __restrict__ o = static_cast<const T* const>(&obs(0,0,0));

      for(int pp=0; pp<npar; ++pp){
	Pinfo[pp].Scale(par[pp]);
	//Pinfo[pp].Check(par[pp]);
      }

      Me[tid]->synthesize_rf(par, r, J, mu);

      for(int ww=0; ww<ndat; ++ww)
	r[ww] = (o[ipix*ndat+ww] - r[ww]) * sig[ww] / scl;

      // --- scale J and compute r--- //
      for(int pp=0; pp<npar; ++pp){
	iScl = Pinfo[pp].scale / scl;

	for(int ww=0; ww<ndat; ++ww)
	  J[pp*ndat+ww] *= iScl * sig[ww];

	Pinfo[pp].Normalize(par[pp]);

      } // pp

    }

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

    T fx_dx(int const nPar,  T* const __restrict__ par,  T* const __restrict__ syn_in,  T* const __restrict__ r,
	    T* const __restrict__ J)const
    {

      synthesize_rf(nPar, par, syn_in, r, J);

      long const nEl = long(nDat)*long(nx*ny);
      return ksum2<T,double>(nEl, r);
    }

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

    template<typename iType = long>
    Eigen::SparseMatrix<T, Eigen::RowMajor, iType> get_L(int const npar,  T* const __restrict__ par)const
    {

      iType const npix = nx*ny;
      iType const ndat = nDat;
      iType const Nx = nx;
      iType const Ny = ny;
      iType const nthreads = Me.size();

      iType const nPen = npix*2*npar;
      T const sqr_nPen = sqrt(T(nPen));
      std::vector<T> iAlpha(npar, T(0));
      for(int ii=0;ii<npar; ++ii) iAlpha[ii] = sqrt(Pinfo[ii].alpha) / sqr_nPen;

      // --- get matrix dimensions --- //

      int const nrows = 2*npar*npix;
      int const ncols = npix*npar;

      Eigen::SparseMatrix<T,Eigen::RowMajor, iType> L(nrows, ncols);

      // --- Get number of elements per row --- //

      int const Elements_per_row = 2;
      Eigen::VectorXi nElements_per_row = Eigen::VectorXi::Constant(2*npix*npar, Elements_per_row); // 1D vector of integers

      // --- correct numbers for first column and first row --- //

      for(int pp=0; pp<npar; ++pp){

	for(int yy = 0; yy<Ny; ++yy){
	  int const iPix = (yy*nx + 0);
	  nElements_per_row[2*iPix * npar + 2*pp+1] = 0;
	} // yy

	for(int xx = 0; xx<Nx; ++xx){
	  int const iPix = (0*Nx + xx);
	  nElements_per_row[2*iPix * npar + 2*pp ] = 0;
	} // xx

      }// pp

      // --- reserve elements in the sparse matrix --- //

      L.reserve(nElements_per_row);

      // --- Fill Matrix in parallel --- //

      iType ipix=0, tid=0, xx=0, yy=0, pp=0;
#pragma omp parallel default(shared) firstprivate(ipix, tid, xx, yy, pp) num_threads(nthreads)
      {
	tid = omp_get_thread_num();
#pragma omp for
	for(ipix=1; ipix<npix; ++ipix){
	  yy = ipix / nx;
	  xx = ipix - yy*nx;

	  // --- Each thread fills all regularization derivatives for one pixel (all parameters) --- //

	  if((yy-1) >= 0)
 	    for(pp = 0; pp<npar; ++pp){
	      L.insert(2*npar*ipix + 2*pp    , ipix*npar + pp - npar*nx) = -iAlpha[pp]; // One pixel below
	      L.insert(2*npar*ipix + 2*pp    , ipix*npar + pp)           =  iAlpha[pp]; // the pixel itself.
	    }

	  if((xx-1) >= 0)
	    for(pp = 0; pp<npar; ++pp){
	      L.insert(2*npar*ipix + 2*pp+1  , ipix*npar + pp - npar)    = -iAlpha[pp]; // One pixel to the left
	      L.insert(2*npar*ipix + 2*pp+1  , ipix*npar + pp)           =  iAlpha[pp]; // the pixel itself.
	    }
	} //ipix
      }// parallel

      return L;
    }

  };
}

#endif


#pragma once
/* This code is a stripped down version of 'google/spherical-harmonics' on
 * github to focus on spherical harmonics rotations using Eigen. This part
 * of the code is release using the Apache license V2.0 (see [here][license])
 *
 * Modifications:
 *  + I added a new Apply interface to the Rotation class that takes Eigen
 *    vectors as input and outputs to perform efficient Matrix products.
 *    See Rotation::Apply(const Eigen::VectorXf&, Eigen::VectorXf&) const.
 *  + I changed all Eigen type to be 32b float instead of 64b.
 *
 * [license]: https://github.com/google/spherical-harmonics/blob/master/LICENSE
 */

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

// Include STL
#include <vector>
#include <memory>

template <class T>
using VectorX = Eigen::Matrix<T, Eigen::Dynamic, 1>;

// Get the total number of coefficients for a function represented by
// all spherical harmonic basis of degree <= @order (it is a point of
// confusion that the order of an SH refers to its degree and not the order).
constexpr int GetCoefficientCount(int order) {
  return (order + 1) * (order + 1);
}

// Get the one dimensional index associated with a particular degree @l
// and order @m. This is the index that can be used to access the Coeffs
// returned by SHSolver.
constexpr int GetIndex(int l, int m) {
  return l * (l + 1) + m;
}

// Usage: CHECK(bool, string message);
// Note that it must end a semi-colon, making it look like a
// valid C++ statement (hence the awkward do() while(false)).
#ifndef NDEBUG
# define CHECK(condition, message) \
  do { \
    if (!(condition)) { \
      std::cerr << "Check failed (" #condition ") in " << __FILE__ \
        << ":" << __LINE__ << ", message: " << message << std::endl; \
      std::exit(EXIT_FAILURE); \
    } \
  } while(false)
#else
# define ASSERT(condition, message) do {} while(false)
#endif

// Return true if the first value is within epsilon of the second value.
bool NearByMargin(double actual, double expected) {
  double diff = actual - expected;
  if (diff < 0.0) {
    diff = -diff;
  }
  // 5 bits of error in mantissa (source of '32 *')
  return diff < 32 * std::numeric_limits<double>::epsilon();
}

// ---- The following functions are used to implement SH rotation computations
//      based on the recursive approach described in [1, 4]. The names of the
//      functions correspond with the notation used in [1, 4].

// See http://en.wikipedia.org/wiki/Kronecker_delta
double KroneckerDelta(int i, int j) {
  if (i == j) {
    return 1.0;
  } else {
    return 0.0;
  }
}

// [4] uses an odd convention of referring to the rows and columns using
// centered indices, so the middle row and column are (0, 0) and the upper
// left would have negative coordinates.
//
// This is a convenience function to allow us to access an Eigen::MatrixXf
// in the same manner, assuming r is a (2l+1)x(2l+1) matrix.
double GetCenteredElement(const Eigen::MatrixXf& r, int i, int j) {
  // The shift to go from [-l, l] to [0, 2l] is (rows - 1) / 2 = l,
  // (since the matrix is assumed to be square, rows == cols).
  int offset = (r.rows() - 1) / 2;
  return r(i + offset, j + offset);
}

// P is a helper function defined in [4] that is used by the functions U, V, W.
// This should not be called on its own, as U, V, and W (and their coefficients)
// select the appropriate matrix elements to access (arguments @a and @b).
double P(int i, int a, int b, int l, const std::vector<Eigen::MatrixXf>& r) {
  if (b == l) {
    return GetCenteredElement(r[1], i, 1) *
        GetCenteredElement(r[l - 1], a, l - 1) -
        GetCenteredElement(r[1], i, -1) *
        GetCenteredElement(r[l - 1], a, -l + 1);
  } else if (b == -l) {
    return GetCenteredElement(r[1], i, 1) *
        GetCenteredElement(r[l - 1], a, -l + 1) +
        GetCenteredElement(r[1], i, -1) *
        GetCenteredElement(r[l - 1], a, l - 1);
  } else {
    return GetCenteredElement(r[1], i, 0) * GetCenteredElement(r[l - 1], a, b);
  }
}

// The functions U, V, and W should only be called if the correspondingly
// named coefficient u, v, w from the function ComputeUVWCoeff() is non-zero.
// When the coefficient is 0, these would attempt to access matrix elements that
// are out of bounds. The list of rotations, @r, must have the @l - 1
// previously completed band rotations. These functions are valid for l >= 2.

double U(int m, int n, int l, const std::vector<Eigen::MatrixXf>& r) {
  // Although [1, 4] split U into three cases for m == 0, m < 0, m > 0
  // the actual values are the same for all three cases
  return P(0, m, n, l, r);
}

double V(int m, int n, int l, const std::vector<Eigen::MatrixXf>& r) {
  if (m == 0) {
    return P(1, 1, n, l, r) + P(-1, -1, n, l, r);
  } else if (m > 0) {
    return P(1, m - 1, n, l, r) * sqrt(1 + KroneckerDelta(m, 1)) -
        P(-1, -m + 1, n, l, r) * (1 - KroneckerDelta(m, 1));
  } else {
    // Note there is apparent errata in [1,4,4b] dealing with this particular
    // case. [4b] writes it should be P*(1-d)+P*(1-d)^0.5
    // [1] writes it as P*(1+d)+P*(1-d)^0.5, but going through the math by hand,
    // you must have it as P*(1-d)+P*(1+d)^0.5 to form a 2^.5 term, which
    // parallels the case where m > 0.
    return P(1, m + 1, n, l, r) * (1 - KroneckerDelta(m, -1)) +
        P(-1, -m - 1, n, l, r) * sqrt(1 + KroneckerDelta(m, -1));
  }
}

double W(int m, int n, int l, const std::vector<Eigen::MatrixXf>& r) {
  if (m == 0) {
    // whenever this happens, w is also 0 so W can be anything
    return 0.0;
  } else if (m > 0) {
    return P(1, m + 1, n, l, r) + P(-1, -m - 1, n, l, r);
  } else {
    return P(1, m - 1, n, l, r) - P(-1, -m + 1, n, l, r);
  }
}

// Calculate the coefficients applied to the U, V, and W functions. Because
// their equations share many common terms they are computed simultaneously.
void ComputeUVWCoeff(int m, int n, int l, double* u, double* v, double* w) {
  double d = KroneckerDelta(m, 0);
  double denom = (abs(n) == l ? 2.0 * l * (2.0 * l - 1) : (l + n) * (l - n));

  *u = sqrt((l + m) * (l - m) / denom);
  *v = 0.5 * sqrt((1 + d) * (l + abs(m) - 1.0) * (l + abs(m)) / denom)
      * (1 - 2 * d);
  *w = -0.5 * sqrt((l - abs(m) - 1) * (l - abs(m)) / denom) * (1 - d);
}

// Calculate the (2l+1)x(2l+1) rotation matrix for the band @l.
// This uses the matrices computed for band 1 and band l-1 to compute the
// matrix for band l. @rotations must contain the previously computed l-1
// rotation matrices, and the new matrix for band l will be appended to it.
//
// This implementation comes from p. 5 (6346), Table 1 and 2 in [4] taking
// into account the corrections from [4b].
void ComputeBandRotation(int l, std::vector<Eigen::MatrixXf>* rotations) {
  // The band's rotation matrix has rows and columns equal to the number of
  // coefficients within that band (-l <= m <= l implies 2l + 1 coefficients).
  Eigen::MatrixXf rotation(2 * l + 1, 2 * l + 1);
  for (int m = -l; m <= l; m++) {
    for (int n = -l; n <= l; n++) {
      double u, v, w;
      ComputeUVWCoeff(m, n, l, &u, &v, &w);

      // The functions U, V, W are only safe to call if the coefficients
      // u, v, w are not zero
      if (!NearByMargin(u, 0.0))
          u *= U(m, n, l, *rotations);
      if (!NearByMargin(v, 0.0))
          v *= V(m, n, l, *rotations);
      if (!NearByMargin(w, 0.0))
          w *= W(m, n, l, *rotations);

      rotation(m + l, n + l) = (u + v + w);
    }
  }

  rotations->push_back(rotation);
}

class Rotation {
 public:
/*
  // Create a new Rotation that can applies @rotation to sets of coefficients
  // for the given @order. @order must be at least 0.
  static std::unique_ptr<Rotation> Create(int order,
                                          const Eigen::Quaternionf& rotation);

  // Create a new Rotation that applies the same rotation as @rotation. This
  // can be used to efficiently calculate the matrices for the same 3x3
  // transform when a new order is necessary.
  static std::unique_ptr<Rotation> Create(int order, const Rotation& rotation);
*/
  // Transform the SH basis coefficients in @coeff by this rotation and store
  // them into @result. These may be the same vector. The @result vector will
  // be resized if necessary, but @coeffs must have its size equal to
  // GetCoefficientCount(order()).
  //
  // This rotation transformation produces a set of coefficients that are equal
  // to the coefficients found by projecting the original function rotated by
  // the same rotation matrix.
  //
  // There are explicit instantiations for double, float, and Array3f.
  template <typename T>
  void Apply(const std::vector<T>& coeffs,  std::vector<T>* result) const;
  void Apply(const Eigen::MatrixXf& coeffs, Eigen::MatrixXf& result) const;
  void Apply(const Eigen::VectorXf& coeffs, Eigen::VectorXf& result) const;

  // The order (0-based) that the rotation was constructed with. It can only
  // transform coefficient vectors that were fit using the same order.
  int order() const;

  // Return the rotation that is effectively applied to the inputs of the
  // original function.
  Eigen::Quaternionf rotation() const;

  // Return the (2l+1)x(2l+1) matrix for transforming the coefficients within
  // band @l by the rotation. @l must be at least 0 and less than or equal to
  // the order this rotation was initially constructed with.
  const Eigen::MatrixXf& band_rotation(int l) const;

  Rotation(int order, const Eigen::Quaternionf& rotation);

 private:
  const int order_;
  const Eigen::Quaternionf rotation_;

  std::vector<Eigen::MatrixXf> band_rotations_;
};

Rotation::Rotation(int order, const Eigen::Quaternionf& rotation)
    : order_(order), rotation_(rotation) {
  band_rotations_.reserve(GetCoefficientCount(order));

  // Order 0 (first band) is simply the 1x1 identity since the SH basis
  // function is a simple sphere.
  Eigen::MatrixXf r(1, 1);
  r(0, 0) = 1.0;
  band_rotations_.push_back(r);

  r.resize(3, 3);
  // The second band's transformation is simply a permutation of the
  // rotation matrix's elements, provided in Appendix 1 of [1], updated to
  // include the Condon-Shortely phase. The recursive method in
  // ComputeBandRotation preserves the proper phases as high bands are computed.
  Eigen::Matrix3f rotation_mat = rotation.toRotationMatrix();
  r(0, 0) = rotation_mat(1, 1);
  r(0, 1) = -rotation_mat(1, 2);
  r(0, 2) = rotation_mat(1, 0);
  r(1, 0) = -rotation_mat(2, 1);
  r(1, 1) = rotation_mat(2, 2);
  r(1, 2) = -rotation_mat(2, 0);
  r(2, 0) = rotation_mat(0, 1);
  r(2, 1) = -rotation_mat(0, 2);
  r(2, 2) = rotation_mat(0, 0);
  band_rotations_.push_back(r);

  // Recursively build the remaining band rotations, using the equations
  // provided in [4, 4b].
  for (int l = 2; l <= order; l++) {
    ComputeBandRotation(l, &band_rotations_);
  }
}

/*
std::unique_ptr<Rotation> Rotation::Create(
    int order, const Eigen::Quaternionf& rotation) {
#ifndef NDEBUG
  CHECK(order >= 0, "Order must be at least 0.");
  CHECK(NearByMargin(rotation.squaredNorm(), 1.0),
        "Rotation must be normalized.");
#endif

  std::unique_ptr<Rotation> sh_rot(new Rotation(order, rotation));

  // Order 0 (first band) is simply the 1x1 identity since the SH basis
  // function is a simple sphere.
  Eigen::MatrixXf r(1, 1);
  r(0, 0) = 1.0;
  sh_rot->band_rotations_.push_back(r);

  r.resize(3, 3);
  // The second band's transformation is simply a permutation of the
  // rotation matrix's elements, provided in Appendix 1 of [1], updated to
  // include the Condon-Shortely phase. The recursive method in
  // ComputeBandRotation preserves the proper phases as high bands are computed.
  Eigen::Matrix3f rotation_mat = rotation.toRotationMatrix();
  r(0, 0) = rotation_mat(1, 1);
  r(0, 1) = -rotation_mat(1, 2);
  r(0, 2) = rotation_mat(1, 0);
  r(1, 0) = -rotation_mat(2, 1);
  r(1, 1) = rotation_mat(2, 2);
  r(1, 2) = -rotation_mat(2, 0);
  r(2, 0) = rotation_mat(0, 1);
  r(2, 1) = -rotation_mat(0, 2);
  r(2, 2) = rotation_mat(0, 0);
  sh_rot->band_rotations_.push_back(r);

  // Recursively build the remaining band rotations, using the equations
  // provided in [4, 4b].
  for (int l = 2; l <= order; l++) {
    ComputeBandRotation(l, &(sh_rot->band_rotations_));
  }

  return sh_rot;
}

std::unique_ptr<Rotation> Rotation::Create(int order,
                                           const Rotation& rotation) {
#ifndef NDEBUG
  CHECK(order >= 0, "Order must be at least 0.");
#endif

  std::unique_ptr<Rotation> sh_rot(new Rotation(order, rotation.rotation_));

  // Copy up to min(order, rotation.order_) band rotations into the new
  // SHRotation. For shared orders, they are the same. If the new order is
  // higher than already calculated then the remainder will be computed next.
  for (int l = 0; l <= std::min(order, rotation.order_); l++) {
    sh_rot->band_rotations_.push_back(rotation.band_rotations_[l]);
  }

  // Calculate remaining bands (automatically skipped if there are no more).
  for (int l = rotation.order_ + 1; l <= order; l++) {
    ComputeBandRotation(l, &(sh_rot->band_rotations_));
  }

  return sh_rot;
}
*/

int Rotation::order() const { return order_; }

Eigen::Quaternionf Rotation::rotation() const { return rotation_; }

const Eigen::MatrixXf& Rotation::band_rotation(int l) const {
  return band_rotations_[l];
}

template <typename T>
void Rotation::Apply(const std::vector<T>& coeff,
                     std::vector<T>* result) const {
#ifndef NDEBUG
  CHECK(coeff.size() == GetCoefficientCount(order_),
        "Incorrect number of coefficients provided.");
#endif

  // Resize to the required number of coefficients.
  // If result is already the same size as coeff, there's no need to zero out
  // its values since each index will be written explicitly later.
  if (result->size() != coeff.size()) {
    result->assign(coeff.size(), T());
  }

  // Because of orthogonality, the coefficients outside of each band do not
  // interact with one another. By separating them into band-specific matrices,
  // we take advantage of that sparsity.

  for (int l = 0; l <= order_; l++) {
    VectorX<T> band_coeff(2 * l + 1);

    // Fill band_coeff from the subset of @coeff that's relevant.
    for (int m = -l; m <= l; m++) {
      // Offset by l to get the appropiate vector component (0-based instead
      // of starting at -l).
      band_coeff(m + l) = coeff[GetIndex(l, m)];
    }

    band_coeff = band_rotations_[l].cast<T>() * band_coeff;

    // Copy rotated coefficients back into the appropriate subset into @result.
    for (int m = -l; m <= l; m++) {
      (*result)[GetIndex(l, m)] = band_coeff(m + l);
    }
  }
}

void Rotation::Apply(const Eigen::MatrixXf& coeffs,
                     Eigen::MatrixXf& result) const {
   const int rows = coeffs.cols();
   for(int l=0; l<=order_; ++l) {
      const int i = l*l;
      const int n = 2*l+1;
      result.block(i, 0, n, rows) = band_rotations_[l] * coeffs.block(i, 0, n, rows);
   }
}

void Rotation::Apply(const Eigen::VectorXf& coeffs,
                     Eigen::VectorXf& result) const {
   for(int l=0; l<=order_; ++l) {
      const int i = l*l;
      const int n = 2*l+1;
      result.segment(i, n) = band_rotations_[l] * coeffs.segment(i, n);
   }
}


#include "tasktorrent/tasktorrent.hpp"
#ifdef USE_MKL
#include <mkl_cblas.h>
#include <mkl_lapacke.h>
#else
#include <cblas.h>
#include <lapacke.h>
#endif
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <fstream>
#include <array>
#include <random>
#include <mutex>
#include <iostream>
#include <map>
#include <memory>
#include <set>
#include <mpi.h>
#include <string>
#include <cxxopts.hpp>

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

typedef array<int, 2> int2;
typedef array<int, 3> int3;

/*
 * Parametrized priorities for cholesky:
 * 0. No priority, only enforces potrf>trsm>gemm
 * 1. Row-based priority, prioritize tasks with smaller row number in addition to priority 0.
 * 2. Critical path priority, prioritize tasks with longest distance to the exit task. For references, check out the paper
    Beaumont, Olivier, et al. "A Makespan Lower Bound for the Scheduling of the Tiled Cholesky Factorization based on ALAP Schedule." (2020).
 * 3. Critical path and row priority, prioritize tasks with smaller row number in addition to priority 2. We also enforces potrf>trsm>gemm
 */

enum PrioKind { no = 0, row = 1, cp = 2, cp_row = 3};

/*
* 3D Cholesky based on the algorithm from the paper
    Kalluri Eswar, et al. "On Mapping Data and Computation for Parallel Sparse Cholesky Factorization." (1995)
* However, this implementation only uses 3D mapping for gemm tasks. 1D mapping is used for potrf tasks and 2D mapping is used for trsm tasks.
*/

void cholesky3d(int n_threads, int verb, int block_size, int num_blocks, int npcols, int nprows, PrioKind prio, int log, int debug)
{
    const int rank = comm_rank();
    const int n_ranks = comm_size();
    std::atomic<long long int> potrf_us_t(0);
    std::atomic<long long int> trsm_us_t(0);
    std::atomic<long long int> gemm_us_t(0);
    std::atomic<long long int> accu_us_t(0);
    assert(npcols * nprows == n_ranks);
    int q = static_cast<int>(cbrt(n_ranks));
    if (q * q * q != n_ranks)
    {
        if (rank == 0)
        {
            cerr << "Number of processes must be a perfect cube." << endl;
        }
        MPI_Finalize();
        exit(1);
    }

    int3 rank_3d;
    int2 rank_2d;
    rank_3d[0] = rank / (q * q);
    rank_3d[1] = (rank % (q * q)) / q;
    rank_3d[2] = (rank % (q * q)) % q;
    rank_2d[0] = rank % nprows;
    rank_2d[1] = rank / nprows;

    // Number of tasks
    int n_tasks_per_rank = 2;
    struct acc_data {
        vector<std::unique_ptr<MatrixXd>> to_accumulate; // to_accumulate[k] holds matrix result of gemm(k,i,j)
    };

    auto potf_block_2_prio = [&](int j) {
        if (prio == PrioKind::cp_row) {
            return (double)(9 * (num_blocks - j) - 1) + 18 * num_blocks * num_blocks;
        }
        else if(prio == PrioKind::cp) {
            return (double)(9 * (num_blocks - j) - 1);
        }
        else if(prio == PrioKind::row) {
            return 3.0 * (double)(num_blocks - j);
        }
        else {
            return 3.0;
        }
    };

    auto trsm_block_2_prio = [&](int2 ij) {
        if (prio == PrioKind::cp_row) {
            return (double)((num_blocks - ij[0]) + num_blocks * (9.0 * num_blocks - 9.0 * ij[1] - 2.0) + 9 * num_blocks * num_blocks);
        }
        else if(prio == PrioKind::cp) {
            return (double)(9 * (num_blocks - ij[1]) - 2);
        }
        else if(prio == PrioKind::row) {
            return 2.0 * (double)(num_blocks - ij[0]);
        }
        else {
            return 2.0;
        }
    };

    auto gemm_block_2_prio = [&](int3 kij) {
        if (prio == PrioKind::cp_row) {
            return (double)(num_blocks - kij[1]) + num_blocks * (9.0 * num_blocks - 3.0 * kij[2] - 6.0 * (kij[0] / q) - 2.0);
        }
        else if(prio == PrioKind::cp) {
            return (double)(9 * num_blocks - 9 * kij[2] - 2);
        }
        else if(prio == PrioKind::row) {
            return (double)(num_blocks - kij[1]);
        }
        else {
            return 1.0;
        }
    };

    std::vector<acc_data> gemm_results(num_blocks*num_blocks);
    auto val = [&](int i, int j) { return 1.0/(double)((i + j)*(i + j) + 1) + ((i == j) ? 1.0 * block_size * num_blocks : 0.0); };
    auto rank3d21 = [&](int i, int j, int k) { return ((j % q) * q + k % q) + (i % q) * q * q;};
    auto rank2d21 = [&](int i, int j) { return (j % npcols) * nprows + (i % nprows);};
    auto rank1d21 = [&](int j) { return j % n_ranks; };
    vector<unique_ptr<MatrixXd>> blocks(num_blocks*num_blocks);

    auto bloc_2_rank = [&](int i, int j) {
        int r = (j % npcols) * nprows + (i % nprows);
        assert(r >= 0 && r < n_ranks);
        return r;
    };

    auto block_2_thread = [&](int i, int j) {
        int ii = i / nprows;
        int jj = j / npcols;
        int num_blocksit = num_blocks / nprows;
        return (ii + jj * num_blocksit) % n_threads;
    };
    {
        Eigen::MatrixXd A = Eigen::MatrixXd::Identity(256,256);
        Eigen::MatrixXd B = Eigen::MatrixXd::Identity(256,256);
        Eigen::MatrixXd C = Eigen::MatrixXd::Identity(256,256);
        for(int i = 0; i < 10; i++) {
            cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, 256, 256, 256, 1.0, A.data(), 256, B.data(), 256, 1.0, C.data(), 256);
        }
    }
    for (int ii=0; ii<num_blocks; ii++) {
        for (int jj=0; jj<num_blocks; jj++) {
            auto val_loc = [&](int i, int j) { return val(ii*block_size+i,jj*block_size+j); };
            int dest = (ii == jj) ? rank1d21(ii) : rank2d21(ii,jj);
            if(dest == rank) {
                blocks[ii+jj*num_blocks]=make_unique<MatrixXd>(block_size, block_size);
                *blocks[ii+jj*num_blocks]=MatrixXd::NullaryExpr(block_size, block_size, val_loc);
                gemm_results[ii+jj*num_blocks].to_accumulate= vector<std::unique_ptr<MatrixXd>>(q);
                for (int ll=0; ll<q; ll++) {
                    gemm_results[ii+jj*num_blocks].to_accumulate[ll]=make_unique<MatrixXd>(block_size, block_size);
                    *(gemm_results[ii+jj*num_blocks].to_accumulate[ll])=MatrixXd::Zero(block_size, block_size);
                }
            }
            else if (((ii % q) == rank_3d[0]) && ((jj % q) == rank_3d[1])) {
                blocks[ii+jj*num_blocks]=make_unique<MatrixXd>(block_size, block_size);
                *blocks[ii+jj*num_blocks]=MatrixXd::Zero(block_size, block_size);
            }
            else {
                blocks[ii+jj*num_blocks]=make_unique<MatrixXd>(block_size, block_size);
            }
        }
    }
    // Initialize the communicator structure
    Communicator comm(MPI_COMM_WORLD, verb);
    // Initialize the runtime structures
    Threadpool tp(n_threads, &comm, verb, "WkTuto_" + to_string(rank) + "_");
    Taskflow<int> potrf(&tp, verb);
    Taskflow<int2> trsm(&tp, verb);
    Taskflow<int3> gemm(&tp, verb);
    Taskflow<int3> accu(&tp, verb);
    DepsLogger dlog(1000000);
    Logger ttorlog(1000000);
    if (log)  {
        tp.set_logger(&ttorlog);
        comm.set_logger(&ttorlog);
    }
    // Create active message
    auto am_trsm = comm.make_large_active_msg(
        [&](int& j) {
                int offset = ((j + 1) / nprows + (((j + 1) % nprows) > rank_2d[0])) * nprows + rank_2d[0];
                for(int i = offset; i < num_blocks; i = i + nprows) {
                    if (debug) printf("Fulfilling trsm (%d, %d) on rank (%d, %d)\n", i, j, rank_2d[0], rank_2d[1]);
                    assert(rank2d21(i, j) == rank);
                    trsm.fulfill_promise({i,j});
                }
            },
            [&](int& j){
                return blocks[j+j*num_blocks]->data();
            },
            [&](int& j){
                return;
            });

    auto am_gemm = comm.make_large_active_msg(
        [&](int& i, int& k) {
            assert(k % q == rank_3d[2]);
            int offset_c = ((k + 1) / q + (((k + 1) % q) > rank_3d[1])) * q + rank_3d[1];
            if (i % q == rank_3d[0]) {
                for(int j = offset_c; j < i; j = j + q) {
                    if (debug) printf("TRSM (%d, %d) Fulfilling gemm (%d, %d, %d) on rank (%d, %d, %d)\n", i, k, k, i, j, rank_3d[2], rank_3d[0], rank_3d[1]);
                    assert(rank3d21(i,j,k) == rank);
                    gemm.fulfill_promise({k,i,j});
                }
            }
            int offset_r = (i / q + ((i % q) > rank_3d[0])) * q + rank_3d[0];
            if (i % q == rank_3d[1]) {
                for(int j = offset_r; j < num_blocks; j = j + q) {
                    if (debug) printf("TRSM (%d, %d) Fulfilling gemm (%d, %d, %d) on rank (%d, %d, %d)\n", i,k, k, j, i, rank_3d[2], rank_3d[0], rank_3d[1]);
                    assert(rank3d21(j,i,k) == rank);
                    gemm.fulfill_promise({k,j,i});
                }
            }
        },
        [&](int& i, int& k) {
            return blocks[i+k*num_blocks]->data();
        },
        [&](int& i, int& k) {
            return;
        });

    potrf.set_task([&](int j) {
            assert(rank1d21(j) == rank);
            timer t1 = wctime();
            int info = LAPACKE_dpotrf(LAPACK_COL_MAJOR, 'L', block_size, blocks[j+j*num_blocks]->data(), block_size);
            timer t2 = wctime();
            potrf_us_t += 1e6 * elapsed(t1, t2);
            assert(info == 0);
            if (debug) printf("Running POTRF %d on rank %d\n", j, rank);
        })
        .set_fulfill([&](int j) {
            for (int p = 0; p < nprows; p++)
            {
                int r = rank2d21(p,j);
                if (rank == r) {
                    int offset = ((j + 1) / nprows + ((j + 1) % nprows) / (rank_2d[0] + 1)) * nprows + rank_2d[0];
                    for(int i = offset; i < num_blocks; i = i + nprows) {
                        trsm.fulfill_promise({i,j});
                    }
                }
                else {
                    auto Ljjv = view<double>(blocks[j+j*num_blocks]->data(), block_size*block_size);
                    am_trsm->send_large(r, Ljjv, j);
                }
            }
        })
        .set_indegree([&](int j) {
            if (j==0) {
                return 1;
            }
            else if (j < q) {
                return j;
            }
            else {
                return q;
            }
        })
        .set_mapping([&](int j) {
            return block_2_thread(j,j);
        })
        .set_binding([&](int j) {
            return false;

        })
        .set_priority(potf_block_2_prio)
        .set_name([&](int j) {
            return "POTRF" + to_string(j) + "_" + to_string(rank);
        });

    trsm.set_task([&](int2 ij) {
            int i=ij[0];
            int j=ij[1];
            assert(rank2d21(i,j) == rank);
            timer t1 = wctime();
            cblas_dtrsm(CblasColMajor, CblasRight, CblasLower, CblasTrans, CblasNonUnit, block_size, block_size, 1.0, blocks[j + j * num_blocks]->data(),block_size, blocks[i + j * num_blocks]->data(), block_size);
            timer t2 = wctime();
            trsm_us_t += 1e6 * elapsed(t1, t2);
            if (debug) printf("Running trsm (%d, %d) on rank %d, %d\n", i, j, rank_2d[0], rank_2d[1]);
        })
        .set_fulfill([&](int2 ij) {
            int i=ij[0];
            int j=ij[1];
            for (int ri = 0; ri < q; ri++)  {
                for (int rj = 0; rj < q; rj++) {
                    int r = rank3d21(ri, rj, j);
                    if (r == rank) {
                        int offset_c = ((j + 1) / q + (((j + 1) % q) > rank_3d[1])) * q + rank_3d[1];
                        if (i % q == rank_3d[0]) {
                            for(int k = offset_c; k < i; k = k + q) {
                                gemm.fulfill_promise({j,i,k});
                            }
                        }
                        int offset_r = (i / q + ((i % q) > rank_3d[0])) * q + rank_3d[0];
                        if (i % q == rank_3d[1]) {
                            for(int k = offset_r; k < num_blocks; k = k + q) {
                                gemm.fulfill_promise({j,k,i});
                            }
                        }
                    }
                    else {
                        auto Lijv = view<double>(blocks[i + j * num_blocks]->data(), block_size*block_size);
                        am_gemm->send_large(r, Lijv, i, j);
                    }
                }
            }
        })
        .set_indegree([&](int2 ij) {
            int i=ij[0];
            int j=ij[1];
            return (j < q) ? j + 1 : q + 1;
        })
        .set_mapping([&](int2 ij) {
            int i=ij[0];
            int j=ij[1];
            return block_2_thread(i,j);
        })
        .set_binding([&](int2 ij) {
            int i=ij[0];
            int j=ij[1];
            return false;

        })
        .set_priority(trsm_block_2_prio)
        .set_name([&](int2 ij) {
            int i=ij[0];
            int j=ij[1];
            return "TRSM" + to_string(j) + "_" + to_string(i) + "_" +to_string(rank);
        });

    auto am_accu = comm.make_large_active_msg(
        [&](int& i, int& j, int& from) {
            accu.fulfill_promise({from, i, j});
        },
        [&](int& i, int& j, int& from){
            return gemm_results[i+j*num_blocks].to_accumulate[from]->data();
        },
        [&](int& i, int& j, int& from){
            return;
        });

    gemm.set_task([&](int3 kij) {
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            assert(rank3d21(i,j,k) == rank);
            timer t1 = wctime();
            if (i==j) {
                cblas_dsyrk(CblasColMajor, CblasLower, CblasNoTrans, block_size, block_size, -1.0, blocks[i+k*num_blocks]->data(), block_size, 1.0, blocks[i+j*num_blocks]->data(), block_size);
            }
            else {
                cblas_dgemm(CblasColMajor, CblasNoTrans, CblasTrans, block_size, block_size, block_size, -1.0,blocks[i+k*num_blocks]->data(), block_size, blocks[j+k*num_blocks]->data(), block_size, 1.0, blocks[i+j*num_blocks]->data(), block_size);
            }
            timer t2 = wctime();
            if (debug) printf("Running gemm (%d, %d, %d) on rank %d, %d, %d\n", k, i, j, rank_3d[2], rank_3d[0], rank_3d[1]);
            gemm_us_t += 1e6 * elapsed(t1, t2);
        })
        .set_fulfill([&](int3 kij) {
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            if (k+q<=j-1) {
                gemm.fulfill_promise({k+q, i, j});
            }
            else {
                int dest = (i == j) ? rank1d21(i) : rank2d21(i, j);
                if (dest == rank) {
                    if (debug) printf("gemm (%d, %d, %d) fulfilling accumu (%d, %d, %d) on rank %d, %d, %d\n", k, i, j, rank_3d[2], i, j, rank_3d[2], rank_3d[0], rank_3d[1]);
                    accu.fulfill_promise({rank_3d[2], i, j});
                }
                else {
                    int kk = rank_3d[2];
                    auto Lij = view<double>(blocks[i+j*num_blocks]->data(), block_size*block_size);
                    if (debug) printf("gemm (%d, %d, %d) Sending accumu (%d, %d, %d) to rank %d, %d\n", k, i, j, rank_3d[2], i, j, dest % nprows, dest / nprows);
                    am_accu->send_large(dest, Lij, i, j, kk);
                }
            }
        })
        .set_indegree([&](int3 kij) {
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            return 3 - (k/q == 0) - (i == j);
        })
        .set_mapping([&](int3 kij) {
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            return block_2_thread(i,j);
        })
        .set_binding([&](int3 kij) {
            return false;

        })
        .set_priority(gemm_block_2_prio)
        .set_name([&](int3 kij) { // This is just for debugging and profiling
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            return "gemm" + to_string(k) + "_" + to_string(i)+"_"+to_string(j)+"_"+to_string(comm_rank());
        });

    accu.set_task([&](int3 kij) {
            int k=kij[0]; // Step (gemm's pivot)
            int i=kij[1]; // Row
            int j=kij[2]; // Col
            int dest = (i == j) ? rank1d21(i) : rank2d21(i,j);
            assert(dest == rank);
            assert(j <= i);
            if (debug) printf("Running accumu (%d, %d, %d) on rank %d, %d\n", k, i, j, rank % nprows, rank / nprows);
            {
                timer t_ = wctime();
                *blocks[i+j*num_blocks] += (*gemm_results[i+j*num_blocks].to_accumulate[k]);
                timer t__ = wctime();
                accu_us_t += 1e6 * elapsed(t_, t__);
            }
        })
        .set_fulfill([&](int3 kij) {
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            assert(j <= i);
            if(i == j) {
                potrf.fulfill_promise(i);
            } else {
                trsm.fulfill_promise({i,j});
            }
        })
        .set_indegree([&](int3 kij) {
            return 1;
        })
        .set_mapping([&](int3 kij) {
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            return block_2_thread(i,j);
        })
        .set_priority(gemm_block_2_prio)
        .set_binding([&](int3 kij) {
            return true; // IMPORTANT
        })
        .set_name([&](int3 kij) { // This is just for debugging and profiling
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            return "accumu" + to_string(k) + "_" + to_string(i)+"_"+to_string(j)+"_"+to_string(comm_rank());
        });

    MPI_Barrier(MPI_COMM_WORLD);
    timer t0 = wctime();
    if (rank == 0){
        potrf.fulfill_promise(0);
    }
    tp.join();
    MPI_Barrier(MPI_COMM_WORLD);
    timer t1 = wctime();
    MPI_Status status;
    if (rank==0) {
        cout<<"3D, Number of ranks "<<n_ranks<<", block_size "<<block_size<<", num_blocks "<<num_blocks<<", n_threads "<<n_threads<<", Priority "<<prio<<", Elapsed time: "<<elapsed(t0,t1)<<endl;
    }
    MatrixXd A;
    A = MatrixXd::NullaryExpr(block_size*num_blocks,block_size*num_blocks, val);
    MatrixXd L = A;
    for (int ii=0; ii<num_blocks; ii++) {
        for (int jj=0; jj<num_blocks; jj++) {
            if (jj<=ii)  {
                int dest = (ii == jj) ? rank1d21(ii) : rank2d21(ii,jj);
                if (rank==0 && rank!=dest) {
                    blocks[ii+jj*num_blocks]=make_unique<MatrixXd>(block_size,block_size);
                    MPI_Recv(blocks[ii+jj*num_blocks]->data(), block_size*block_size, MPI_DOUBLE, dest, 0, MPI_COMM_WORLD, &status);
                }
                else if (rank==dest && rank != 0) {
                    MPI_Send(blocks[ii+jj*num_blocks]->data(), block_size*block_size, MPI_DOUBLE, 0, 0, MPI_COMM_WORLD);
                }
            }
        }
    }
    if (rank == 0)  {
        for (int ii=0; ii<num_blocks; ii++) {
            for (int jj=0; jj<num_blocks; jj++) {
                if (jj<=ii)  {
                    L.block(ii*block_size,jj*block_size,block_size,block_size)=*blocks[ii+jj*num_blocks];
                }
            }
        }
        auto L1=L.triangularView<Lower>();
        VectorXd x = VectorXd::Random(block_size * num_blocks);
        VectorXd b = A*x;
        L1.solveInPlace(b);
        L1.transpose().solveInPlace(b);
        double error = (b - x).norm() / x.norm();
        cout << "Error solve: " << error << endl;
    }
    if (log)  {
        std::ofstream logfile;
        string filename = "ttor_3Dcholesky_Priority_"+to_string(block_size)+"_"+to_string(num_blocks)+"_"+ to_string(n_threads)+"_"+ to_string(n_ranks)+"_"+ to_string(prio)+".log."+to_string(rank);
        logfile.open(filename);
        logfile << ttorlog;
        logfile.close();
    }
}

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

    int req = MPI_THREAD_FUNNELED;
    int prov = -1;

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

    assert(prov == req);

    std::stringstream sstr;
    sstr << comm_size();
    const std::string comm_size_str = sstr.str();

    cxxopts::Options options("2d_cholesky", "2D dense cholesky using TaskTorrent");
    options.add_options()
        ("help", "Print help")
        ("n_threads", "Number of threads", cxxopts::value<int>()->default_value("2"))
        ("verb", "Verbosity level", cxxopts::value<int>()->default_value("0"))
        ("block_size", "Block size", cxxopts::value<int>()->default_value("5"))
        ("num_blocks", "Number of blocks", cxxopts::value<int>()->default_value("10"))
        ("nprows", "Number of processors accross rows", cxxopts::value<int>()->default_value("1"))
        ("npcols", "Number of processors accross columns", cxxopts::value<int>()->default_value(comm_size_str.c_str()))
        ("kind", "Priority kind", cxxopts::value<int>()->default_value("0"))
        ("log", "Enable logging", cxxopts::value<bool>()->default_value("false"))
        ("debug", "Debug or not", cxxopts::value<bool>()->default_value("false"));
    auto result = options.parse(argc, argv);

    const int n_threads = result["n_threads"].as<int>();
    const int verb = result["verb"].as<int>();
    const int block_size = result["block_size"].as<int>();
    const int num_blocks = result["num_blocks"].as<int>();
    const int nprows = result["nprows"].as<int>();
    const int npcols = result["npcols"].as<int>();
    const PrioKind prio = (PrioKind)(result["kind"].as<int>());
    const bool log = result["log"].as<bool>();
    const bool debug = result["debug"].as<bool>();

    assert(block_size > 0);
    assert(num_blocks > 0);
    assert(n_threads > 0);
    assert(verb >= 0);
    assert(nprows >= 0);
    assert(npcols >= 0);

    if (result.count("help")) {
        std::cout << options.help({"", "Group"}) << endl;
        exit(0);
    }
    if(comm_rank() == 0) printf("Arguments: block_size (size of blocks) %d\nnum_blocks (# of blocks) %d\nn_threads %d\nverb %d\nnprows %d\nnpcols %d\nprio %d\nlog %d\ndebug %d\n",
        block_size, num_blocks, n_threads, verb, nprows, npcols, (int)prio, log, debug);

    cholesky3d(n_threads, verb, block_size, num_blocks, npcols, nprows, prio, log, debug);
    MPI_Finalize();
}


#ifndef CPP_EVALUATOR_UTILS_HH
#define CPP_EVALUATOR_UTILS_HH

#include "Evaluator.hh"

#include <array>
#include <iterator>

#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/algorithm/max_element.hpp>
#include <boost/range/algorithm/transform.hpp>
#include <boost/range/numeric.hpp>

namespace tl
{

/**
 * Linear tensor field expressed in barycentric coordinates
 */
using TensorInterp = TensorProductBezierTriangle<Mat3d, double, 1>;

/**
 * Compute the derivatives of a TensorProductBezierTriangle along two
 * orthogonal directions on the triangle in the space indicated by @a D.
 *
 * @param poly The polynomial.
 * @tparam D The space (0-sizeof...(Degrees)) in which to perform the derivative
 */
template <std::size_t D,
          typename TPBT,
          typename T,
          typename C,
          std::size_t... Degrees>
auto derivatives(
        const TensorProductBezierTriangleBase<TPBT, T, C, Degrees...>& poly)
        -> std::array<
                typename TensorProductDerivativeType<D, T, C, Degrees...>::type,
                2>
{
    static_assert(D < sizeof...(Degrees), "Invalid index");
    auto d0 = poly.template derivative<D>(0);
    auto d1 = poly.template derivative<D>(1);
    auto d2 = poly.template derivative<D>(2);

    auto da = (d1 - d0) / std::sqrt(2);

    auto db = (2 * d2 - d0 - d1) / std::sqrt(6);

    return {da, db};
}

/**
 * @brief Compute an upper bound for the magnitude of the function value.
 * @details Finds the coefficient with the maximum absolute value.
 *
 * @param poly The polynomial
 * @return An upper bound for std::abs(poly(x)) on the triangles
 */
template <typename TPBT,
          typename T,
          typename C,
          std::size_t... Degrees>
double abs_upper_bound(
        const TensorProductBezierTriangleBase<TPBT, T, C, Degrees...>& poly)
{
    return boost::accumulate(poly.coefficients(), 0., MaxAbs{});
}

/**
 * @brief Compute an upper bound for the magnitude of a number of polynomials.
 * @details Finds the coefficient with the maximum absolute value of all
 *     polynomials.
 *
 * @param polys A sequence of polynomials
 * @return An upper bound for std::abs(poly(x)) over all polynomials in the
 *     sequence
 */
template <typename TPBTSeq>
double abs_max_upper_bound(const TPBTSeq& polys)
{
    using namespace boost;
    using namespace boost::adaptors;
    return accumulate(polys | transformed([](const auto& f) {
                          return abs_upper_bound(f);
                      }),
                      0.,
                      Max{});
}

/**
 * @brief Compute an upper bound for the gradient magnitude of the polynomial in
 *      the space indicated by @a D.
 *
 * @param poly The polynomial
 * @tparam D The space (0-sizeof...(Degrees)) in which to perform the derivative
 * @return an estimate for the upper bound of the gradient magnitude of poly(x)
 *      on the triangles
 */
template <std::size_t D,
          typename TPBT,
          typename T,
          typename C,
          std::size_t... Degrees>
double derivatives_upper_bound(
        const TensorProductBezierTriangleBase<TPBT, T, C, Degrees...>& poly)
{
    static_assert(D < sizeof...(Degrees), "Invalid index");
    auto upper_bound = typename TPBT::Coeffs{};
    // Estimate upper bound of gradient magnitude by L1 norm of control points
    // todo: Can we do better? (Represent gradient magnitude as polynomial)
    auto abssum = [](double v1, double v2) {
        return std::abs(v1) + std::abs(v2);
    };
    auto derivs = derivatives<D>(poly);
    boost::transform(derivs[0].coefficients(),
                     derivs[1].coefficients(),
                     std::begin(upper_bound),
                     abssum);
    return *boost::max_element(upper_bound);
}

/**
 * @brief Compute an upper bound for the gradient magnitude of a sequence of
 *      polynomials in the space indicated by @a D.
 *
 * @param polys A sequence (range) of polynomials
 * @tparam D The space in which to perform the derivatives
 * @return Upper bound for the gradient magnitude on the triangles
 */
template <std::size_t D, typename TPBTSeq>
double derivatives_max_upper_bound(const TPBTSeq& polys)
{
    using namespace boost;
    using namespace boost::adaptors;
    return accumulate(polys | transformed([](const auto& f) {
                          return derivatives_upper_bound<D>(f);
                      }),
                      0.,
                      Max{});
}

/**
 * @brief Compute an upper bound for the norm of a vector of values from
 *     multiple polynomials.
 * @details Finds an upper bound for sqrt(poly_1(x)^2 + poly_2(x)^2 + ...)
 *      where poly_i are the elements of the sequence @a polys.
 *
 * @param polys A sequence (range) of polynomials
 * @return Upper bound for the norm of the vector od polynomials on the
 *     triangles
 */
template <typename TPBTSeq>
double upper_bound_norm(const TPBTSeq& polys)
{
    using namespace boost;
    using namespace boost::adaptors;
    return std::sqrt(accumulate(polys | transformed([](const auto& f) {
                                    return std::pow(
                                            *max_element(f.coefficients()), 2);
                                }),
                                0.));
}

} // namespace tl

#endif