#include <iostream>
#include <Eigen/Dense>  // Dicht besetzte Matrizen ~ nicht volles Eigen3 Package, da sonst das Compilieren ewig dauert
						// Eigen3 liegt als rein header-basierte Bibliothek vor -> wird immer neu übersetzt!

using namespace Eigen;
using namespace std;

/*
 * Eigen3 lässt es zu, dynamisch Matrizen/Vektoren zu erstellen, ohne deren Dimensionen hart zu coden
 * Das ist ansonsten nur möglich, wenn man die Speicherverwaltung auf dem Heap selbst übernimmt
 * (arbeiten mit new)
 *
 * Schleifen über Komponenten zum Verrechnen nicht notwendig, alles im Hintergrund effizient implementiert
 */

Vector3d gib_ein_ergebnis_zurueck(Matrix3d const &mat,Vector3d const &vec){
  /*ü
   * Uebergeben wird eine Referenz auf eine Matrix (ein Alias innerhalb der Methode), die innerhalb der Methode nicht veränderlich sein sollen.
   * Vgl 'Constant Correctness'
   *
   * Rückgabe ist ein Objekt vom Typ Vector3d, dass sich aus der Verrechnung der beiden Übergabeparametern ergibt
   * Der *-Operator wurde überladen und gibt ein Objekt zurück, dass den Datentyp anhand des aufrufenden Objekts und des damit zu verrechnenden Objekts ableitet!
   * vgl Documentation (anschauen mit F3)
   */

  cout<<"Ausgabe Vektor:"<<vec<<endl;
  return mat*vec;
  // return vec*mat; // test ob er dann mäckert/was passiert wenn die Dimensionen nicht passen
  // Dann mäckert der Compiler, da Eigen über assertions checkt, ob die Dimensionen passen!

}

int main()
{
  Matrix3d m = Matrix3d::Random();  		// dies ist eine static Methode der Matrix3d Klasse (kommt noch)
  	  // Matrix3d erstellt eine 3x3 Matrix; diese soll initialisiert werden mit den Werten des (temporär eristierenden) Objekts Matrix3d::Random()
  	  /*
  	   * MatrixXd -> X x X große Matrix vom Type d = double
  	   * Konstruktor: Eigen::MatrixXd testMatrix(rows, columns);
  	   * Oder Zuweisung eines anderen, temprären Objekts
  	   * Eigen3::MatrixXd m = Eigen3::MatrixXd::Random(rows, cols); // Matrix mit Random Values zwischen -1 und 1 vom typ double
  	   * Eigen3::MatrixXd m2 = Eigen3::MatrixXd::Constant(rows, cols, value);
  	   */

  m = (m + Matrix3d::Constant(1.2)) * 50;
  	  // Rechnen mit gleichartigen Matrizen ist ähnlich wie in Mathematik möglich. Die +, -, *, /, ... Operatoren sind entsprechend überladen

  /*
  // debug
  Matrix3d testMatrixConst = Matrix3d::Constant(1.2);
  std::cout << "This is testMatrixConst: " << std::endl;
  std::cout << testMatrixConst << std::endl;
  // ::Constant(double) erstellt eine X dimensionale Matrix die in allen ihren Elementen die double-zahl stehen hat
  */

  cout << "m =" << endl << m << endl;

  // Gleiches gilt für Vektoren
  Vector3d V(1,2,3);  // Initialisierung der Komponeten mit (1,2,3)

  cout << "m * v =" << endl << m * V << endl;

  Vector3d vec;
  vec = gib_ein_ergebnis_zurueck(m,V);

  cout<<"vec:"<<vec<<endl;

  double val=vec.transpose()*vec;
  	  // Vetoren und Matrizen sind als Objekte implementiert!
  	  // transpose ist eine Methode von Vector3d und transponiert den Vector
  	  // Sie liefert ein (temporäres) Objekt vom selben Datentyp wie das Objekt mit dem die Methode aufgerufen wurde zurück

  cout<<"Skalarprodukt:"<<val<<" oder mit .dot Methode: "<<vec.dot(vec) <<endl;

  // aeusser Produkt:
  Matrix3d m2=vec*vec.transpose();

  cout<<"m2: "<<endl;
  cout<<m2<<endl;

  return 0;

}


/***
四：可变参模板
（2）折叠表达式:Fold Expressions，C++17
目的：计算某个值（表达式的结果当然是一个值）
该值得特殊性在于：它与所有可变参有关，而不是与单独某个可变参有关。 需要所有可变参都参与计算，才能求出该值。
折叠表达式有四种格式：一元左折、一元右折，二元左折、二元右折
注意，每种格式的折叠表达式都需要用圆括号括住。
左折：就是参数从左侧开始计算  ；
右折：参数从右侧开始计算

（2.1）一元左折（unary left fold）
格式： (... 运算符 一包参数)
计算方式：((( 参数1  运算符  参数2） 运算符  参数3）....运算符 参数N）
（2.2）一元右折（unary right fold）
格式： (一包参数 运算符 ...)
计算方式：( 参数1  运算符  ( ... (参数N-1  运算符  参数N)))
（2.3）二元左折（binary left fold）
格式：(init 运算符 ... 运算符 一包参数)
计算方式：(((init 运算符 参数1) 运算符 参数2) ... 运算符 参数N)
init表示 一个初始的东西，它可能是一个值，也可能是个其他东西。
（2.4）二元右折（binary right fold）
格式：(一包参数 运算符 ... 运算符 init)
计算方式：（参数1 运算符 （...（参数N 运算符 init )))

（3）可变参表达式

***/

#include <iostream>

//#include <boost/type_index.hpp>
using namespace std;
//#pragma warning(disable : 4996)

namespace _nmsp1
{
	template <typename... T>
	auto add_val(T... args)
	{
		//return (... + args); //圆括号不能省略，否则出现编译错误，一元左折
		return (args + ...); //一元右折

	}
	template <typename... T>
	auto sub_val_left(T... args)
	{
		return (... - args); //一元左折
	}
	template <typename... T>
	auto sub_val_right(T... args)
	{
		return (args - ...); //一元右折
	}

	template <typename... T>
	auto sub_val_left_b(T... args)
	{
		return (220 - ... - args); //二元左折：220就是init
	}

	template <typename... T>
	void print_val_left_b(T... args)
	{
		(cout << ... << args); //一元左折，但init是cout，运算符代表的是 <<
	}

	template <typename... T>
	auto sub_val_right_b(T... args)
	{
		return (args - ... - 220); //二元右折
	}
}
namespace _nmsp2
{
	template <typename... T>
	auto print_result(T const& ... args)
	{
		(cout << ... << args) << " 结束" << endl;
		return (... + args); //计算一下参数和值
	}

	template <typename... T>
	void  print_calc(T const& ... args)
	{
		cout << print_result(2 * args...) << endl;  //“2 * args...“就是可变参表达式
		                                   // print_result(2 * args...) 等价于 print_result(2 * 10, 2*20, 2*30,2*40)  ;

		//print_result(2 * args); //不行
		//print_result(args... * 2);  //不行
		//print_result(args * 2...); //不行
		//print_result(args * 2 ...); //可以
		//print_result((args * 2) ...); //可以
		print_result(args + args...); //可以，等价于print_result(10+10,20+20,30+30,40+40);

	}

}

int main()
{
	//cout << _nmsp1::add_val(10, 20, 30) << endl;

	//cout << _nmsp1::sub_val_left(10, 20, 30,40) << endl;  //-80：((10-20)-30)-40
	//cout << _nmsp1::sub_val_right(10, 20, 30, 40) << endl; //-20：10-(20-(30-40))

	//cout << _nmsp1::sub_val_left_b(10, 20, 30, 40) << endl; //120：(((220-10)-20)-30)-40
	//_nmsp1::print_val_left_b(10, "abc", 30, "def"); //10abc30def
	//cout << _nmsp1::sub_val_right_b(10, 20, 30, 40) << endl; //200：10-(20-（30-（40-220)))

	//cout << _nmsp2::print_result(10,20,30,40) << endl;
	_nmsp2::print_calc(10, 20, 30, 40);

	return 0;
}


#include <fmt/core.h>
#include <yaml-cpp/yaml.h>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Sparse>
#include <boost/math/quadrature/gauss.hpp>
#include <iostream>
#include <map>
#include <numeric>
#include <optional>
#include <vector>

#include "NumpySaver.hpp"
#include "Timer.hpp"
#include "bsplines.hpp"

using namespace Eigen;
using std::cout, std::endl, std::cerr;

const char* devider = "##########################################";

template <int num_points>
double matrix_B_element(int i, int j, std::vector<double>& knots, Index order) {
  using boost::math::quadrature::gauss;

  if (std::abs((int)i - (int)j) > order) return 0;

  int imax = std::max(i, j);
  int imin = std::min(i, j);

  Index n = knots.size();

  double sum = 0;

  for (int k = std::max(0, imax - 1); k < std::min(imin + order + 1, n - 1);
       k++) {
    auto f = [&](const double& t) {
      auto [values, idx] = bsplines::bsplines(t, knots, order);
      return values[i + order - idx] * values[j + order - idx];
    };
    sum += gauss<double, num_points>::integrate(f, knots[k], knots[k + 1]);
  }

  return sum;
}

template <int num_points>
double matrix_H_element(int i, int j, std::vector<double>& knots, Index order,
                        int l, int z,
                        std::function<double(double)>& additional_pot) {
  using boost::math::quadrature::gauss;
  if (std::abs((int)i - (int)j) > order) return 0;

  Index n = knots.size();
  int imax = std::max(i, j);
  int imin = std::min(i, j);

  double sum = 0;

  for (int k = std::max(0, imax - 1); k < std::min(imin + order + 1, n - 1);
       k++) {
    auto f = [&](const double& t) {
      auto [values, idx] = bsplines::bsplines(t, knots, order);
      auto [values_dd, idx_dd] = bsplines::ndx_bsplines(t, knots, order, 2);
      double h_val = -values_dd[j - (idx - order)] / 2. +
                     (l * (l + 1.) / (t * t * 2.) - z / t + additional_pot(t)) *
                         values[j - (idx - order)];
      return values[i - (idx - order)] * h_val;
      ;
    };

    sum += gauss<double, num_points>::integrate(f, knots[k], knots[k + 1]);
  }
  return sum;
}

template <int num_points>
MatrixXd matrix_B(std::vector<double>& knots, Index order) {
  Index n = knots.size();
  MatrixXd mat(n - 3 + order, n - 3 + order);

  for (int row = 0; row < mat.rows(); row++)
    for (int col = 0; col < mat.cols(); col++)
      mat(row, col) = matrix_B_element<num_points>(
          row - order + 1, col - order + 1, knots, order);

  return mat;
}

template <int num_points>
MatrixXd matrix_H(std::vector<double>& knots, Index order, int l, int z,
                  std::function<double(double)>& additional_pot) {
  Index n = knots.size();
  MatrixXd mat(n - 3 + order, n - 3 + order);

  for (int row = 0; row < mat.rows(); row++)
    for (int col = 0; col < mat.cols(); col++)
      mat(row, col) = matrix_H_element<num_points>(
          row - order + 1, col - order + 1, knots, order, l, z, additional_pot);

  return mat;
}

double evaluate_spline(const double& x, const std::vector<double>& knots,
                       const VectorXd& weights, std::size_t order) {
  auto n = knots.size();
  assert(int(n + order) - 3 == weights.size());

  auto [values, index] = bsplines::bsplines(x, knots, order);

  double sum = 0;
  for (std::size_t i = std::max(int(index) - 1, 0);
       i <= std::min(index - 1 + order, n + order - 4); i++)
    sum += values[i + 1 - index] * weights[i];

  return sum;
}

SparseMatrix<double> matrix_poisson(const std::vector<double>& knots,
                                    std::size_t order = 3) {
  auto n = knots.size();

  SparseMatrix<double> matrix(n + 1, n + order - 2);
  matrix.reserve(order * (n + 1) - 2);

  auto [deriv_0, _index] = bsplines::ndx_bsplines(knots[0], knots, order, 2);
  for (int col = 0; col < int(order) - 1; col++)
    matrix.insert(0, col) = deriv_0[col + 1];

  for (int row = 1; row < int(n); row++) {
    auto [deriv, index] = bsplines::ndx_bsplines(knots[row], knots, order, 2);
    for (int col = row - 1; col < row - 1 + int(order); col++)
      matrix.insert(row, col) = deriv[col - row + 2];
  }

  auto [deriv_last, __index] =
      bsplines::ndx_bsplines(knots[n - 1], knots, order, 1);
  matrix.insert(n, n + order - 3) = deriv_last[order];
  matrix.insert(n, n + order - 4) = deriv_last[order - 1];

  return matrix;
}

double rho_hydrogen_ground_state(double r, double e, double r_bohr = 1) {
  return e / M_PI / (r_bohr * r_bohr * r_bohr) * std::exp(-2 * r / r_bohr);
}

VectorXd rhs_poisson(const std::vector<double>& knots,
                     std::function<double(double)>& rho) {
  auto n = knots.size();
  VectorXd rhs(n + 1);
  for (std::size_t i = 0; i < n; i++) {
    rhs[i] = -knots[i] * 4 * M_PI * rho(knots[i]);
  }

  rhs[n] = 0;

  return rhs;
}

double evaluate_spline_poisson(const double& x,
                               const std::vector<double>& knots,
                               const VectorXd& weights, std::size_t order = 3) {
  assert(int(knots.size() + order) - 1 == weights.size());

  auto [values, index] = bsplines::bsplines(x, knots, order);

  double sum = 0;
  for (std::size_t i = 0; i <= order; i++)
    sum += values[i] * weights[index + i];

  return sum;
}

double test_norm_r2(std::function<double(double)> f, double xmin, double xmax) {
  using boost::math::quadrature::gauss;

  std::function<double(double)> integrant = [&](double x) {
    return f(x) * x * x;
  };

  double norm =
      4 * M_PI * gauss<double, 10000>::integrate(integrant, xmin, xmax);

  std::cout << "Norm is " << norm << endl;

  return norm;
}

double test_norm_sqrt(std::function<double(double)> f, double xmin,
                      double xmax) {
  using boost::math::quadrature::gauss;
  std::function<double(double)> integrant = [&](double x) {
    double c = f(x);
    return c * c;
  };

  double norm = gauss<double, 10000>::integrate(integrant, xmin, xmax);

  std::cout << "WVF Norm is " << norm << endl;

  return norm;
}

VectorXd evaluate_spline_poisson(const VectorXd& x,
                                 const std::vector<double>& knots,
                                 const VectorXd& weights,
                                 std::size_t order = 3) {
  auto n = x.size();
  VectorXd ret(n);
  for (Index i = 0; i < n; i++) {
    ret[i] = evaluate_spline(x[i], knots, weights, order);
  }
  return ret;
}

template <int order>
std::vector<double> normalization_constants_radial_wavefunctions(
    std::vector<double>& knots, MatrixXd& eigenvectors) {
  using boost::math::quadrature::gauss;

  std::vector<double> returner;
  returner.reserve(eigenvectors.cols());

  int col;
  std::function<double(double)> f = [&](double r) {
    double v = evaluate_spline(r, knots, eigenvectors.col(col), order);
    return v * v;
  };

  for (col = 0; col < eigenvectors.cols(); col++) {
    double norm = 0;
    for (std::size_t k = 0; k < knots.size() - 1; k++)
      norm += gauss<double, 2 * order>::integrate(f, knots[k], knots[k + 1]);
    returner.push_back(std::sqrt(norm));
  }

  return returner;
}

// int, int, int = N, n, l
template <int order>
std::tuple<double, std::size_t> solve(
    std::vector<double>& knots_atom, std::vector<double>& knots_poisson,
    std::vector<std::tuple<int, int, int>>& orbitals, int z,
    std::string& basefilename, double mixture = 0.4, double tol = 1e-6,
    std::size_t maxiter = 100) {
  using namespace std;
  using boost::math::quadrature::gauss;

  constexpr int gauss_points = double(order) * 1.5;

  size_t n_orbitals = orbitals.size();

  auto matrix_poisson_ = matrix_poisson(knots_poisson);
  // first do the (sparse) LU decomposition
  Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>
      solver_poisson;
  solver_poisson.analyzePattern(matrix_poisson_);
  solver_poisson.factorize(matrix_poisson_);

  auto B = matrix_B<gauss_points>(knots_atom, order);

  GeneralizedEigenSolver<MatrixXd> ges;

  VectorXd old_orbital_energies = VectorXd::Zero(n_orbitals);
  VectorXd orbital_energies = VectorXd::Ones(n_orbitals);

  VectorXd weights_poisson = VectorXd::Zero(matrix_poisson_.cols() + 1);
  VectorXd weights_poisson_old = VectorXd::Zero(matrix_poisson_.cols() + 1);

  std::map<int, std::pair<MatrixXd, VectorXd>> l_map, l_map_old;

  std::function rho = [&](double r) -> double {
    if (r == 0) return 0;
    double sum = 0;
    for (auto& [N, n, l] : orbitals) {
      if (!l_map.contains(l)) continue;
      double pnl_r =
          evaluate_spline(r, knots_atom, l_map[l].first.col(n - l - 1), order) /
          r;
      sum += N * pnl_r * pnl_r;
    }
    return sum / 4. / M_PI;
  };

  std::function rho_old = [&](double r) -> double {
    if (r == 0) return 0;
    double sum = 0;
    for (auto& [N, n, l] : orbitals) {
      if (!l_map_old.contains(l)) continue;
      double pnl_r = evaluate_spline(r, knots_atom,
                                     l_map_old[l].first.col(n - l - 1), order) /
                     r;
      sum += N * pnl_r * pnl_r;
    }
    return sum / 4. / M_PI;
  };

  std::function<double(double)> additional = [&](double r) -> double {
    if (r == 0.0) return 0;
    double old_exchange = -3. * pow(3. * rho_old(r) / 8. / M_PI, 1. / 3.);
    double new_exchange = -3. * pow(3. * rho(r) / 8. / M_PI, 1. / 3.);
    double old_direct =
        evaluate_spline_poisson(r, knots_poisson, weights_poisson_old, order) /
        r;
    double new_direct =
        evaluate_spline_poisson(r, knots_poisson, weights_poisson, order) / r;

    return (1. - mixture) * (new_direct + new_exchange) +
           mixture * (old_direct + old_exchange);
  };

  double energy = numeric_limits<double>::max();

  std::size_t iter = 0;

  while ((old_orbital_energies - orbital_energies).norm() > tol) {
    iter++;
    old_orbital_energies = orbital_energies;

    std::map<int, std::pair<MatrixXd, VectorXd>> l_map_new;

    for (size_t i = 0; i < n_orbitals; i++) {
      auto& [N, n, l] = orbitals[i];

      if (!l_map_new.contains(l)) {
        auto H = matrix_H<gauss_points>(knots_atom, order, l, z, additional);

        ges.compute(H, B);
        VectorXcd eigenvalues = ges.eigenvalues();

#ifndef NDEBUG
        if (eigenvalues.imag().cwiseAbs().sum() > 10e-3) {
          cerr << "Error: Complex eigenvalues!" << endl;
        }
#endif

        VectorXd real_eigen = eigenvalues.real();

        // sort the eigenvalues (and create an index list)
        // https://stackoverflow.com/questions/39693909/sort-eigen-matrix-column-values-by-ascending-order-of-column-1-values
        std::vector<int> indices(real_eigen.size());
        std::iota(indices.begin(), indices.end(), 0);
        std::sort(indices.begin(), indices.end(), [&](int A, int B) -> bool {
          return real_eigen[A] < real_eigen[B];
        });

        std::sort(real_eigen.data(), real_eigen.data() + real_eigen.size());

        MatrixXd unsorted_eigenvectors = ges.eigenvectors().real();
        l_map_new[l] = std::pair(MatrixXd(unsorted_eigenvectors.rows(),
                                          unsorted_eigenvectors.cols()),
                                 real_eigen);

        auto normalization_constants =
            normalization_constants_radial_wavefunctions<order>(
                knots_atom, unsorted_eigenvectors);

        for (Index row = 0; row < unsorted_eigenvectors.rows(); row++)
          for (Index col = 0; col < unsorted_eigenvectors.cols(); col++)
            l_map_new[l].first(row, col) =
                unsorted_eigenvectors(row, indices[col]) /
                normalization_constants[indices[col]];

        auto test = normalization_constants_radial_wavefunctions<order>(
            knots_atom, l_map_new[l].first);
      }
      orbital_energies[i] = l_map_new[l].second[n - l - 1];
    }

    l_map_old = l_map;
    l_map = l_map_new;

    auto rhs = rhs_poisson(knots_poisson, rho);

    weights_poisson_old = weights_poisson;
    weights_poisson << 0, solver_poisson.solve(rhs);

    if (iter >= maxiter) {
      cerr << "Reached maximum number of iterations!" << endl;
      break;
    }
  }
  energy = 0;

  function<double(double, int, int)> ee_interaction = [&](double x, int n,
                                                          int l) {
    double pnl_r =
        evaluate_spline(x, knots_atom, l_map[l].first.col(n - l - 1), order);
    return additional(x) * pnl_r * pnl_r;
  };

  for (size_t i = 0; i < n_orbitals; i++) {
    auto& [N, n, l] = orbitals[i];
    function<double(double)> integrant =
        bind(ee_interaction, std::placeholders::_1, n, l);

    double integral = 0;
    for (size_t k = 0; k < knots_poisson.size() - 1; k++) {
      integral += gauss<double, 2 * order>::integrate(
          integrant, knots_poisson[k], knots_poisson[k + 1]);
    }
    energy += N * (orbital_energies[i] - .5 * integral);
  }
  cout << "Orbital energies=" << orbital_energies.transpose() << endl;
  cout << "Total energy = " << energy << endl;

  // exporting/plotting
  VectorXd xx = VectorXd::LinSpaced(1000, 0, 10);
  VectorXd rho_x = VectorXd(xx.size());
  for (Index i = 0; i < xx.size(); i++) rho_x[i] = rho(xx[i]);
  NumpySaver(fmt::format("build/output/{}_rho.npy", basefilename))
      << xx << rho_x;

  return {energy, iter};
}

template <int order>
void solve_exp(int n_knots, double rmin, double rmax, double exp_fak,
               std::string& basefilename, std::optional<double> plot, int l,
               int z) {
  std::vector<double> knots(n_knots);
  for (int i = 0; i < n_knots; i++)
    knots[i] = (rmax - rmin) *
                   (std::exp(double(i) / double(n_knots - 1) * exp_fak) - 1.) /
                   (std::exp(exp_fak) - 1) +
               rmin;

  solve<order>(knots, basefilename, plot, l, z);
}

template <int order>
void solve_lin(int n_knots, double rmin, double rmax, std::string& basefilename,
               std::optional<double> plot, int l, int z) {
  std::vector<double> knots(n_knots);
  for (int i = 0; i < n_knots; i++)
    knots[i] = double(i) / double(n_knots - 1) * (rmax - rmin) + rmin;

  solve<order>(knots, basefilename, plot, l, z);
}

std::vector<std::tuple<int, int, int>> to_tuples(
    std::vector<std::vector<int>>& list) {
  // how i wish map was a thing here...

  std::vector<std::tuple<int, int, int>> orbitals;
  orbitals.reserve(list.size());
  for (auto& orbit : list)
    orbitals.push_back(std::make_tuple(orbit[0], orbit[1], orbit[2]));
  return orbitals;
}

int main() {
  int n_knots = 50;
  int n_knots_p = 1000;

  std::vector<double> knots_atom(n_knots);
  std::vector<double> knots_poisson(n_knots_p);

  for (int i = 0; i < n_knots; i++)
    knots_atom[i] = (1000. - 0.) *
                        (std::exp(double(i) / double(n_knots - 1) * 10.) - 1.) /
                        (std::exp(10.) - 1.) +
                    0.;
  for (int i = 0; i < n_knots_p; i++)
    knots_poisson[i] =
        (1000. - 0.) *
            (std::exp(double(i) / double(n_knots_p - 1) * 10.) - 1.) /
            (std::exp(10.) - 1.) +
        0.;

  Map<VectorXd> knots_map(knots_atom.data(), knots_atom.size());
  Map<VectorXd> knots_map_p(knots_poisson.data(), knots_poisson.size());
  NumpySaver("build/output/knots_poisson.npy") << knots_map_p;
  NumpySaver("build/output/knots_atom.npy") << knots_map;

  // Check the influence of the mixing factor
  std::vector<std::tuple<int, int, int>> orbitals_Ne = {
      {2, 1, 0}, {2, 2, 0}, {6, 2, 1}};  // N, n, l
  std::vector<std::tuple<int, int, int>> orbitals_Nep = {
      {2, 1, 0}, {2, 2, 0}, {5, 2, 1}};  // N, n, l

  VectorXd etas = VectorXd::LinSpaced(50, 0, 1);
  VectorXd iters(etas.size());
  for (Index i = 0; i < etas.size(); i++) {
    std::string name = fmt::format("eta_{}", etas[i]);
    auto [energy, iter] =
        solve<3>(knots_atom, knots_poisson, orbitals_Ne, 10, name, etas[i]);
    iters[i] = iter;
  }
  NumpySaver("build/output/etas.npy") << etas << iters;

  // iterate through ALL the elements
  YAML::Node elements = YAML::LoadFile("build/output/pse.yaml");

  std::size_t num_elements = elements.size();

  VectorXd zs(num_elements);
  VectorXd es(num_elements);
  VectorXd iterss(num_elements);
  VectorXd es_ion(num_elements);
  VectorXd iterss_ion(num_elements);

  for (std::size_t i = 0; i < elements.size(); i++) {
    YAML::Node element = elements[i];

    auto z = element["Z"].as<int>();
    auto name = element["name"].as<std::string>();
    auto symbol = element["symbol"].as<std::string>();
    auto orbitals_ = element["orbitals"].as<std::vector<std::vector<int>>>();
    auto orbitals_ionized_ =
        element["orbitals_ionized"].as<std::vector<std::vector<int>>>();

    auto orbitals = to_tuples(orbitals_);
    auto orbitals_ionized = to_tuples(orbitals_ionized_);

    cout << "Element " << name << "(" << i + 1 << "/" << elements.size() << ")"
         << endl;
    zs[i] = z;

    Timer t;
    auto [energy, iter] =
        solve<3>(knots_atom, knots_poisson, orbitals, z, symbol, .6);
    cout << "E=" << energy << " took " << iter << " iterations ("
         << t.elapsed_s() << "s)" << endl;

    es[i] = energy;
    iterss[i] = iter;

    std::string symbol_ionized = symbol + "+";

    t.reset();
    auto [energy_ion, iter_ion] = solve<3>(
        knots_atom, knots_poisson, orbitals_ionized, z, symbol_ionized, .4);
    cout << "E_{ion}=" << energy_ion << " took " << iter_ion << " iterations ("
         << t.elapsed_s() << "s)" << endl;

    es_ion[i] = energy_ion;
    iterss_ion[i] = iter_ion;
    cout << endl;
  }

  NumpySaver("build/output/elements.npy")
      << zs << es << iterss << es_ion << iterss_ion;
}


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

using namespace Eigen;

double categorical_cross_entropy(VectorXd, VectorXd);

int main(){
    using std::cout;
    using std::endl;

    VectorXd t = VectorXd::Zero(10);
    VectorXd y = VectorXd::Zero(10);
    double cross_entropy;

    t(2) = 1;
    y << 0.1, 0.05, 0.6, 0.0, 0.05, 0.1, 0.0, 0.1, 0.0, 0.0;

    cross_entropy = categorical_cross_entropy(t, y);
    cout << "cross entropy: " << cross_entropy << endl;

    return 0;
}

double categorical_cross_entropy(VectorXd t, VectorXd y){
    double delta = 1.0e-7;
    return -(t.array() * (y.array() + delta).log()).sum();
}


// Copyright (c) 2017 Evan S Weinberg
// A reference piece of code which computes matrix elements
// of a reference sparse gauged operator with both a central difference
// and a laplacian term component, to fill a dense
// Eigen matrix, then computes the spectrum and prints
// the Eigenvalues.

// This code is for complex, indefinite matrices.
// This code lives on github at github.com/weinbe2/eigens-with-eigen/

#include <iostream>
#include <iomanip>
#include <cmath>
#include <string>
#include <complex>
#include <random>

// Borrow dense matrix eigenvalue routines.
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

// This is just for convenience. By default, all matrices
// in Eigen are column major. You can replace "Dynamic"
// with a specific number to template just one size.
// You can also ust use "MatrixXcd".
typedef Matrix<complex<double>, Dynamic, Dynamic, ColMajor> cMatrix;

// Reference 1-D indefinite function.
void indefinite_1d_gauge(complex<double>* out, complex<double>* in, const int L, const double m2, complex<double>* field);

int main(int argc, char** argv)
{
  complex<double> *in_cplx;
  complex<double> *out_cplx;

  // Set output precision to be long.
  cout << setprecision(10);

  // Basic information about the lattice.
  const int length = 4;
  const double mass = 0.001;

  // Print the basic info.
  std::cout << "1D indefinite operator, length " << length << ", mass " << mass << ", periodic boundary conditions.\n";
  std::cout << "Change the length and mass by modifying the source.\n";

  // Allocate a random phase field.
  complex<double>* field = new complex<double>[length];
  std::mt19937 generator (1337u); // RNG, 1337u is the seed.
  std::uniform_real_distribution<> dist(-3.1415926535, 3.1415926535);
  for (int i = 0; i < length; i++)
  {
    // As a sanity check, you can set these all equal to 1.0.
    field[i] = exp(complex<double>(0.0, dist(generator)));
  }

  // Allocate.
  in_cplx = new complex<double>[length];
  out_cplx = new complex<double>[length];

  // Zero out.
  for (int i = 0; i < length; i++)
  {
    in_cplx[i] = out_cplx[i] = 0.0;
  }

  /////////////////////////////
  // COMPLEX, HERMITIAN CASE //
  /////////////////////////////

  std::cout << "Complex, Indefinite case.\n\n";

  // Allocate a sufficiently gigantic matrix.
  cMatrix mat_cplx = cMatrix::Zero(length, length);

  // Form matrix elements. This is where it's important that
  // dMatrix is column major.
  for (int i = 0; i < length; i++)
  {
    // Set a point on the rhs for a matrix element.
    // If appropriate, zero out the previous point.
    if (i > 0)
    {
      in_cplx[i-1] = 0.0;
    }
    in_cplx[i] = 1.0;

    // Zero out the "out" vector. I defined "laplace_1d_gauge" to
    // not require this, but I put this here for generality.
    for (int j = 0; j < length; j++)
      out_cplx[j] = 0.0;

    // PUT YOUR MAT-VEC HERE.
    indefinite_1d_gauge(out_cplx, in_cplx, length, mass, field);

    // Copy your output into the right memory location.
    // If your data layout supports it, you can also pass
    // "mptr" directly as your "output vector" when you call
    // your mat-vec.
    complex<double>* mptr = &(mat_cplx(i*length));

    for (int j = 0; j < length; j++)
    {
      mptr[j] = out_cplx[j];
    }
  }

  // We've now formed the dense matrix. We print it here
  // as a sanity check if it's small enough.
  if (length <= 16)
  {
    std::cout << mat_cplx << "\n";
  }

  // Get the eigenvalues and eigenvectors.
  ComplexEigenSolver< cMatrix > eigsolve_cplx_indef(length);
  eigsolve_cplx_indef.compute(mat_cplx);

  // Remark: if you only want the eigenvalues, you can call
  // eigsolve_cplx.compute(mat_cplx, EigenvaluesOnly);

  // Print the eigenvalues.
  cMatrix evals = eigsolve_cplx_indef.eigenvalues();
  std::cout << "The eigenvalues are:\n" << evals << "\n\n";
  // You can also index individual eigenvalues as "evals(i)", where
  // "i" zero-indexes the eigenvalues.

  // Print the eigenvectors if the matrix is small enough.
  // As a remark, this also shows you how to access the eigenvectors.
  if (length <= 16)
  {
    for (int i = 0; i < length; i++)
    {
      // You can use "VectorXd" as the type instead.
      cMatrix evec = eigsolve_cplx_indef.eigenvectors().col(i);

      // Print the eigenvector.
      std::cout << "Eigenvector " << i << " equals:\n" << evec << "\n\n";

      // You can also copy the eigenvector into another array as such:
      for (int j = 0; j < length; j++)
      {
        out_cplx[j] = evec(j);
      }
    }
  }

  // Clean up.
  delete[] field;
  delete[] in_cplx;
  delete[] out_cplx;

  return 0;
}

// Reference 1-D indefinite function with both a central
// difference and a Laplace-like term.
void indefinite_1d_gauge(complex<double>* out, complex<double>* in, const int L, const double m2, complex<double>* field)
{
  // Loop over all sites, applying fields.
  for (int i = 0; i < L; i++)
  {
    // 0.5's correspond to the central difference, 1.0's to the second derivative.
    out[i] = (2+m2)*in[i] + conj(field[(i-1+L)%L])*(-0.5-1.0)*in[(i-1+L)%L] + field[i]*(0.5-1.0)*in[(i+1)%L];
  }

  // Done.
  return;
}


#include <armadillo>

#include "evaluate_covariance.hpp"

using namespace arma;

/*
Get cross-covariance matrix between two location sets

Input:
    locationsX1: X coordinates of location set 1
    locationsY1: Y coordinates of location set 1
    locationsX2: X coordinates of location set 2
    locationsY2: Y coordinates of location set 2
    nLocations1: the number of locations in lcs1
    nLocations2: the number of locations in lcs2
    sill: sill parameter
    range: range parameter
    nugget: nugget parameter

Output:
    covarianceMatrix: obtained covariances, of dimension nLocations2 * nLocations1
*/
void evaluate_cross_covariance(double *covarianceMatrix, double *locationsX1, double *locationsY1, double *locationsX2, double *locationsY2, const unsigned long &nLocations1, const unsigned long &nLocations2, const double &sill, const double &range, const double &nugget)
{
    for(unsigned long index1 = 0; index1 < nLocations1; index1++)
        for(unsigned long index2 = 0; index2 < nLocations2; index2++)
        {
            double xDifference=locationsX1[index1]-locationsX2[index2];
            double yDifference=locationsY1[index1]-locationsY2[index2];

            double distance = sqrt( xDifference*xDifference + yDifference*yDifference );
            if (distance < 1e-14)
                covarianceMatrix[index1*nLocations2+index2] = sill + nugget;
            else
                covarianceMatrix[index1*nLocations2+index2] = sill*exp(-distance/range);
        }
}

/*
Get variance-covariance matrix for one location set

Input:
    locationsX: X coordinates
    locationsY: Y coordinates
    nLocations: the number of locations
    sill: sill parameter
    range: range parameter
    nugget: nugget parameter

Output:
    covarianceMatrix: obtained covariances, of dimension nLocations * nLocations
*/
void evaluate_variance_covariance(double *covarianceMatrix, double *locationsX, double *locationsY, const unsigned long &nLocations, const double &sill, const double &range, const double &nugget)
{
    double offDiagonal;
    double diagonal = sill + nugget;
    for(unsigned long index1 = 0; index1 < nLocations; index1++)
    {
        for(unsigned long index2 = 0; index2 < index1; index2++)
        {
            double xDifference=locationsX[index1]-locationsX[index2];
            double yDifference=locationsY[index1]-locationsY[index2];

            double distance = sqrt( xDifference*xDifference + yDifference*yDifference );
            if (distance < 1e-14)
                offDiagonal = sill + nugget;
            else
                offDiagonal = sill*exp(-distance/range);

            covarianceMatrix[index1*nLocations+index2] = offDiagonal;
            covarianceMatrix[index2*nLocations+index1] = offDiagonal;
        }
        covarianceMatrix[index1*nLocations+index1] = diagonal;
    }
}


#include <iostream>
#include <vector>
#include <limits>
#include <cmath>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

// For dijkstra
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
  boost::no_property, boost::property<boost::edge_weight_t, int> >      weighted_graph;
typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type weight_map;

// For max flow
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor>>>> graph;

class edge_adder {
  graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

using namespace std;

// Compute shortest distances
void dijkstra(const weighted_graph &G, int s, vector<int>& dist_map) {
  boost::dijkstra_shortest_paths(G, s,
    boost::distance_map(boost::make_iterator_property_map(
      dist_map.begin(), boost::get(boost::vertex_index, G))));
}

// Is it possible for all agents to seek shelter for a given tMax?
bool isFeasible(vector<vector<int>>& timeToShelter, int capacity, int timeToEnter, int tMax) {
  int a = timeToShelter.size();
  int s = timeToShelter[0].size();

  graph G(a + capacity * s + 2);
  edge_adder adder(G);
  int source = a + capacity * s;
  int target = source + 1;

  for (int i = 0; i < a; ++i) {
    adder.add_edge(source, i, 1);
  }

  for (int j = 0; j < s; ++j) {
    for (int c = 1; c <= capacity; ++c) {
      adder.add_edge(a + (c - 1) * s + j, target, 1);
    }
  }

  for (int i = 0; i < a; ++i) {
    for (int j = 0; j < s; ++j) {
      for (int c = 1; c <= capacity; ++c) {
        if (timeToShelter[i][j] <= tMax - c * timeToEnter) {
          adder.add_edge(i, a + (c - 1) * s + j, 1);
        }
      }
    }
  }

  long flow = boost::push_relabel_max_flow(G, source, target);
  return flow == a;
}

// Use binary search to find the smallest t such that (isFeasible(t) == True)
void solve() {

    int n, m, a, s, c, d;
    cin >> n >> m >> a >> s >> c >> d;

    // Read skiing area
    weighted_graph G(n);
    char w;
    int x, y, z;
    for (int i = 0; i < m; ++i) {
      cin >> w >> x >> y >> z;

      boost::add_edge(x, y, z, G);
      if (w == 'L') {
         boost::add_edge(y, x, z, G);
      }
    }

    // Read agent positions
    vector<int> agents(a);
    for (int i = 0; i < a; ++i) {
      cin >> agents[i];
    }

    // Read shelter positions
    vector<int> shelters(s);
    for (int j = 0; j < s; ++j) {
      cin >> shelters[j];
    }

    vector<vector<int>> distToShelter(a, vector<int>(s));
    for (int i = 0; i < a; ++i) {
      vector<int> dist_map(n);
      dijkstra(G, agents[i], dist_map);
      for (int j = 0; j < s; ++j) {
        distToShelter[i][j] = dist_map[shelters[j]];
      }
    }

    int low = 1;
    int high = numeric_limits<int>::max() - 1;

    while (low != high) {
      int t = low + (high - low) / 2;
      if (isFeasible(distToShelter, c, d, t)) {
        high = t;
      }
      else {
        low = t + 1;
      }
    }

    cout << low << endl;

}

int main()
{
  ios_base::sync_with_stdio(false);
  int t; cin >> t;
  while (t--) {
    solve();
  }
  return 0;
}


#ifndef RAI_CHOLINV_HPP
#define RAI_CHOLINV_HPP

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

namespace rai {

template<typename Derived, int n_>
static inline void cholInv(Eigen::Matrix<Derived, n_, n_> &A, Eigen::Matrix<Derived, n_, n_> &AInv) {
  unsigned short int i, j, k, n = (unsigned short int) (n_);
  Derived sum;
  Derived p[n];
  memcpy(AInv.data(), A.data(), n * n * sizeof(Derived));

  p[0] = std::sqrt(AInv(0, 0));

  for (j = 1; j < n; j++)
    AInv(j, 0) = AInv(0, j) / p[0];

  for (i = 1; i < n; i++) {
    sum = AInv(i, i);
    for (k = i - 1; k >= 1; k--)
      sum -= AInv(i, k) * AInv(i, k);
    sum -= AInv(i, 0) * AInv(i, 0);
    p[i] = std::sqrt(sum);
    for (j = i + 1; j < n; j++) {
      sum = AInv(i, j);
      for (k = i - 1; k >= 1; k--)
        sum -= AInv(i, k) * AInv(j, k);
      sum -= AInv(i, 0) * AInv(j, 0);
      AInv(j, i) = sum / p[i];
    }
  }

  for (i = 0; i < n; i++) {
    AInv(i, i) = 1.0 / p[i];
    for (j = i + 1; j < n; j++) {
      sum = 0.0;
      for (k = i; k < j; k++) {
        sum -= AInv(j, k) * AInv(k, i);
      }
      AInv(j, i) = sum / p[j];
    }
  }

  for (i = 0; i < n; i++) {
    AInv(i, i) *= AInv(i, i);
    for (k = i + 1; k < n; k++)
      AInv(i, i) += AInv(k, i) * AInv(k, i);

    for (j = i + 1; j < n; j++) {
      AInv(i, j) = AInv(j, i) * AInv(j, j);
      for (k = j + 1; k < n; k++)
        AInv(i, j) += AInv(k, i) * AInv(k, j);
    }
  }

  for (i = 1; i < n; i++)
    for (j = 0; j < i; j++)
      AInv(i, j) = AInv(j, i);

}
}

#endif //RAI_CHOLINV_HPP


#pragma once

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

#include "sampler.hpp"

namespace rek {

using namespace Eigen;
using RowVector = Eigen::Matrix<double, Eigen::Dynamic, 1>;

class Solver {

  /** Periodically check for convergence */
  int blockSize_ = 1000;

  constexpr static double tolerance_ = 10e-5;

  bool hasConverged(SparseMatrix<double, RowMajor> &A,
                    SparseMatrix<double, ColMajor> &AColMajor,
                    const RowVector &x, const RowVector &b, const RowVector &z,
                    double tolerance) const {

    const RowVector residual = A * x - b + z;
    const bool condOne = residual.norm() < tolerance;

    // Early return
    if (!condOne)
      return false;

    const RowVector zA = z.transpose() * AColMajor;
    return zA.norm() < tolerance;
  }

  // FIXME: Is this necessary? Duplicate with hasConverged()
  bool hasConvergedDense(Matrix<double, Dynamic, Dynamic, RowMajor> &A,
                         Matrix<double, Dynamic, Dynamic, ColMajor> &AColMajor,
                         const RowVector &x, const RowVector &b,
                         const RowVector &z, double tolerance) const {

    const RowVector residual = A * x - b + z;
    const bool condOne = residual.norm() < tolerance;

    // Early return
    if (!condOne)
      return false;

    const RowVector zA = z.transpose() * AColMajor;
    return zA.norm() < tolerance;
  }

  RowVector solve(SparseMatrix<double, RowMajor> &A,
                  SparseMatrix<double, ColMajor> &AColMajor, const RowVector &b,
                  long MaxIterations, double tolerance = tolerance_) const {
    const long m = A.rows(), n = AColMajor.cols();
    RowVector x(A.cols());
    RowVector z(b);
    RowVector rowNorms(A.rows());
    RowVector columnNorms(A.cols());

    x.setZero();

    for (long i = 0; i < m; i++) {
      rowNorms(i) = A.row(i).squaredNorm();
    }

    for (long j = 0; j < n; j++) {
      columnNorms(j) = AColMajor.col(j).squaredNorm();
    }

    sample::AliasSampler rowSampler(rowNorms);
    sample::AliasSampler colSampler(columnNorms);

    // Initialize Alias samplers, O(n)
    rowSampler.initSampler();
    colSampler.initSampler();

    for (long k = 0; k < MaxIterations; k++) {

      // Check for convergence every blockSize_ iterations
      if ((k + 1) % blockSize_ == 0 &&
          hasConverged(A, AColMajor, x, b, z, tolerance))
        break;

      const long i_k = rowSampler.walkerSample();
      const long j_k = colSampler.walkerSample();

      double val = -AColMajor.col(j_k).dot(z) / columnNorms(j_k);
      z += val * AColMajor.col(j_k);
      val = A.row(i_k).dot(x);
      val = (b(i_k) - z(i_k) - val) / rowNorms(i_k);

      // TODO: .toDense() seems to be required here!
      // FIXME: replace .toDense() with iteration over sparse i_k row
      x += val * A.row(i_k).toDense();
    }

    return x;
  }

  /**
   * Returns the solution to Ax=b using the Randomized Extended Kaczmarz method
   *
   * @param AColMajor Input dense matrix
   * @param b Right hand side vector
   * @param maxIterations Maximum number of iterations
   * @param tolerance Accuracy tolerance, default tolerance_
   * @return Returns an approximate solution to ||Ax - b||_2
   */
  RowVector solve(Matrix<double, Dynamic, Dynamic, ColMajor> &AColMajor,
                  Matrix<double, Dynamic, Dynamic, RowMajor> &ARowMajor,
                  const RowVector &b, long maxIterations,
                  double tolerance = tolerance_) const {
    const long m = ARowMajor.rows(), n = AColMajor.cols();
    RowVector x(AColMajor.cols());
    RowVector z(b);
    RowVector rowNorms(AColMajor.rows());
    RowVector columnNorms(AColMajor.cols());

    x.setZero();

    for (long i = 0; i < m; i++) {
      rowNorms(i) = ARowMajor.row(i).squaredNorm();
    }

    for (long j = 0; j < n; j++) {
      columnNorms(j) = AColMajor.col(j).squaredNorm();
    }

    for (long k = 0; k < maxIterations; k++) {

      // Check for convergence every blockSize_ iterations
      if ((k + 1) % blockSize_ == 0 &&
          hasConvergedDense(ARowMajor, AColMajor, x, b, z, tolerance)) {
        break;
      }

      // Extended Kaczmarz
      const long i_k = k % m;
      const long j_k = k % n;

      double val = -z.dot(AColMajor.col(j_k)) / columnNorms(j_k);
      z += val * AColMajor.col(j_k);
      val = x.dot(ARowMajor.row(i_k));
      val = (b(i_k) - z(i_k) - val) / rowNorms(i_k);

      x += val * ARowMajor.row(i_k);
    }
    return x;
  }

public:
  Solver() = default;

  RowVector solve(Matrix<double, Dynamic, Dynamic, ColMajor> &AColMajor,
                  const RowVector &b, long MaxIterations) const {
    // Duplicate input matrix to row major storage
    Matrix<double, Dynamic, Dynamic, RowMajor> ARowMajor(AColMajor);
    return solve(AColMajor, ARowMajor, b, MaxIterations);
  }

  /**
   * Returns the solution to Ax=b using the Randomized Extended Kaczmarz method
   *
   * @param A Input sparse matrix in row major format
   * @param b Right hand side vector
   * @param maxIterations Maximum number of iterations
   * @param tolerance Accuracy tolerance, default tolerance_
   * @return Returns an approximate solution to ||Ax - b||_2
   */
  RowVector solve(SparseMatrix<double, RowMajor> &A,
                  const Matrix<double, Dynamic, 1> &b, long maxIterations,
                  double tolerance = tolerance_) const {
    SparseMatrix<double, ColMajor> AColMajor(A.rows(), A.cols());

    // Copy row major to column major sparse matrix
    AColMajor.reserve(A.nonZeros());
    for (long k = 0; k < A.outerSize(); ++k)
      for (SparseMatrix<double, RowMajor>::InnerIterator it(A, k); it; ++it) {
        AColMajor.insert(it.row(), it.col()) = it.value();
      }

    return solve(A, AColMajor, b, maxIterations, tolerance);
  };

  /**
   * Returns the solution to Ax=b using the Randomized Extended Kaczmarz method
   *
   * @param A Input sparse matrix in column major format
   * @param b Right hand side vector
   * @param maxIterations Maximum number of iterations
   * @param tolerance Accuracy tolerance, default tolerance_
   * @return Returns an approximate solution to ||Ax - b||_2
   */
  RowVector solve(SparseMatrix<double, ColMajor> &AColMajor, const RowVector &b,
                  long maxIterations, double tolerance = tolerance_) const {

    SparseMatrix<double, RowMajor> A(AColMajor.rows(), AColMajor.cols());

    // Copy column major to row major sparse matrix
    A.reserve(AColMajor.nonZeros());
    for (long k = 0; k < AColMajor.outerSize(); ++k)
      for (SparseMatrix<double, ColMajor>::InnerIterator it(AColMajor, k); it;
           ++it)
        A.insert(it.row(), it.col()) = it.value();

    return solve(A, AColMajor, b, maxIterations, tolerance);
  };
};

} // namespace rek


/**
 * @file 1dwaveabsorbingbc.cc
 * @brief NPDE homework "1DWaveAbsorbingBC" code
 * @author Oliver Rietmann
 * @date 08.04.2019
 * @copyright Developed at ETH Zurich
 */

#include "1dwaveabsorbingbc.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <cmath>
#include <utility>
#include <vector>

namespace WaveAbsorbingBC1D {

constexpr double PI = 3.14159265358979323846;

// Boundary data on the right-hand side (x = 1)
double g(double t) { return 0 <= t && t <= PI ? std::sin(t) : 0.0; }

/**
 * @brief Get the full (--> including both boundary points) Galerkin matrix A
 * @param N number of spacial nodes, including x=0, but excluding x=1
 * @param c speed of propagation
 * @param h (spacial) meshwidth
 * @return Full Galerkin matrix A of shape (N+1)x(N+1)
 */
/* SAM_LISTING_BEGIN_7 */
Eigen::SparseMatrix<double> getA_full(unsigned int N, double c, double h) {
  std::vector<Eigen::Triplet<double>> triplets;
  triplets.reserve(3 * (N + 1) - 2);  // that many triplets needed
  const double scale = c * c / h;
  // store first row separately
  triplets.push_back(Eigen::Triplet<double>(0, 0, scale));
  triplets.push_back(Eigen::Triplet<double>(0, 1, -scale));
  // loop over interior rows
  for (unsigned i = 1; i < N; ++i) {
    triplets.push_back(Eigen::Triplet<double>(i, i - 1, -scale));
    triplets.push_back(Eigen::Triplet<double>(i, i, 2.0 * scale));
    triplets.push_back(Eigen::Triplet<double>(i, i + 1, -scale));
  }
  // store last row separately
  triplets.push_back(Eigen::Triplet<double>(N, N - 1, -scale));
  triplets.push_back(Eigen::Triplet<double>(N, N, scale));
  // Creat (N+1)x(N+1) sparse matrix in CRS format
  Eigen::SparseMatrix<double> A(N + 1, N + 1);
  A.setFromTriplets(triplets.begin(), triplets.end());
  return A;
}
/* SAM_LISTING_END_7 */

/**
 * @brief Get the full (--> including both boundary points) Galerkin matrix B
 * @param N number of spacial nodes, including x=0, but excluding x=1
 * @param c speed of propagation
 * @return Full Galerkin matrix B of size (N+1)x(N+1)
 */
/* SAM_LISTING_BEGIN_8 */
Eigen::SparseMatrix<double> getB_full(unsigned int N, double c) {
  Eigen::SparseMatrix<double> B(N + 1, N + 1);
  // Just a single non-zero entry; we can afford to sete it directly
  B.coeffRef(0, 0) = c;
  return B;
}
/* SAM_LISTING_END_8 */

/**
 * @brief Get the full (--> including both boundary points) Galerkin matrix M
 * @param N number of spacial nodes, including x=0, but excluding x=1
 * @param h (spacial) meshwidth
 * @return Full Galerkin matrix M of size (N+1)x(N+1)
 * Note that M is a diagonal matrix!
 */
/* SAM_LISTING_BEGIN_9 */
Eigen::SparseMatrix<double> getM_full(unsigned int N, double h) {
  Eigen::SparseMatrix<double> M(N + 1, N + 1);
  M.setIdentity();  // Supposed to be efficient
  M *= h;
  // Modify two entries; efficiency does not matter much
  M.coeffRef(0, 0) = h / 2;
  M.coeffRef(N, N) = h / 2;
  return M;
}
/* SAM_LISTING_END_9 */

/* SAM_LISTING_BEGIN_1 */
Eigen::MatrixXd waveLeapfrogABC(double c, double T, unsigned int N,
                                unsigned int m) {
  // N is also the number of cells of the mesh
  double h = 1.0 / N;
  // Obtain Galerkin matrices for truncated finite element space
  // Note that the functions get*_full return the matrices for the full finite
  // element space including the tent function located at x=1. Removing the last
  // row and column of that matrix amounts to dropping that basis function.
  // However, the efficiency of this block() operation in the case of sparse
  // matrices is in doubt, in particular, since the result is assigned to
  // another sparse matrix, which foils Eigen's expression template
  // optimization. The use of "auto" would be highly advisable here!
  Eigen::SparseMatrix<double> A = getA_full(N, c, h).block(0, 0, N, N);
  Eigen::SparseMatrix<double> B = getB_full(N, c).block(0, 0, N, N);
  Eigen::SparseMatrix<double> M = getM_full(N, h).block(0, 0, N, N);
  // Matrix for returning solution
  Eigen::MatrixXd R(m + 1, N + 1);
  //====================
  // Your code goes here
  //====================
  return R;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
std::pair<Eigen::VectorXd, Eigen::VectorXd> computeEnergies(
    const Eigen::MatrixXd &full_solution, double c, double tau) {
  int m = full_solution.rows() - 1;
  int N = full_solution.cols() - 1;
  double h = 1.0 / N;

  Eigen::SparseMatrix<double> A = getA_full(N, c, h);
  Eigen::SparseMatrix<double> M = getM_full(N, h);

  Eigen::VectorXd E_pot(m + 1);
  Eigen::VectorXd E_kin(m);

  //====================
  // Your code goes here
  //====================

  return std::make_pair(E_pot, E_kin);
}
/* SAM_LISTING_END_2 */

}  // namespace WaveAbsorbingBC1D


#include "enkf.hpp"
#include <Eigen/Core>
#include <vector>
#include <fstream>

using Vec = Eigen::VectorXd;
using Mat = Eigen::MatrixXd;

Vec Henon_Map(Vec& x, double& a, double& b){
  auto xn = x[0];
  auto yn = x[1];

  x[0] = 1 - a * xn * xn + yn;
  x[1] = b * xn;

  return x;
}

double obs_err(){
   std::random_device rd{};
   std::mt19937 gen{rd()};
   std::normal_distribution<> dis{0.0, std::sqrt(0.02)};

   return dis(gen);
}

int main(int argc, char** argv){
  using namespace fastmath;

  std::function<Vec(Vec&, double&, double&)> fmap = Henon_Map;
  int ensemble_size = 31;
  auto initial_state = Vec::Zero(2);

  Mat H(1,2);

  H << 1.0, 0.0;

  Vec true_state(2);
  true_state << 0.0, 0.0;

  Vec ensemble_mean = Vec::Zero(ensemble_size);

  Mat ensemble_cov = 0.01 * Mat::Identity(2,2);

  Mat obs_err_cov = 0.02 * Mat::Identity(1,1);

  int n_assim_cycles = 100;

  double a = 1.4, b = 0.3;
  Mat state_mat(2, n_assim_cycles);
  for(auto i = 0; i < n_assim_cycles; ++i){
    true_state = Henon_Map(true_state, a, b);
    state_mat.col(i) = true_state;
  }

  std::vector<double> filter_err, etkf_err;

  auto enkf_runner = VectorEnKF<double, double>(fmap, H, initial_state,
						obs_err_cov, initial_state,
						ensemble_cov, ensemble_size);

  auto etkf_runner = VectorEnKF<double, double>(fmap, H, initial_state,
						obs_err_cov, initial_state,
						ensemble_cov, ensemble_size);

  Vec y(1);
  double yval;
  Vec analysis_mean;
  for(auto i = 0; i < n_assim_cycles; ++i){
    true_state = state_mat.col(i);
    yval = true_state[0] + obs_err();
    y[0] = yval;
    enkf_runner.filter(y, a, b);
    etkf_runner.filter(y, a, b);
    analysis_mean = enkf_runner.state();
    Vec etkf_mean = etkf_runner.state();
    Vec err = true_state - analysis_mean;
    filter_err.push_back(err.norm());
    err = true_state - etkf_mean;
    etkf_err.push_back(err.norm());
  }

  std::ofstream fwriter("hw7.csv"), twriter("hw7etkf.csv");
  for(const auto& e : filter_err){
    fwriter << e << '\n';
  }

  for(const auto& e : etkf_err){
    twriter << e << '\n';
  }

  return 0;
}


#include <iostream>
#include <algorithm>
#include <cmath>
#include <chrono>
#include <Eigen/Dense>

using namespace std::chrono;

using namespace Eigen;

int main()
{

  auto start = high_resolution_clock::now();
  // C , Cn are the two price buffers used to propogate the Call value backwards in time
  // delta, gamma and theta are intermediate differential terms
  ArrayXd S(21), C(21), Cn(21), delta(19), gamma(19), theta(19);

  // set up spot grid
  S(0) = 0;
  double dx = 10;

  for (size_t i = 1; i < S.size(); i++)
  {
    S(i) = S(i-1) + dx;
  }

  // simulation parameters
  double K = 100;
  double s = 0.2;
  double r = 0.05;
  double T = 1;
  int nx = 20;
  int nt = (int)(T/(0.9/std::pow(s*nx,2))) + 1;
  double dt = T/nt;

  // Initialize C at the Maturity (boundary)
  for (size_t i = 0; i < C.size(); i++)
  {
    C(i) = std::max(S(i)-K, 0.0);
  }

  // Propagate system backwards in time,
  // apply descretized diff eqs and spatial boundary conditions
  for (size_t i = 0; i < 10; i++)
  {
    delta = (0.5/dx)*(C.tail<19>() - C.head<19>());
    gamma = (1/std::pow(dx,2))*(C.tail<19>() - 2*C.segment<19>(1) + C.head<19>());
    theta = -(0.5*std::pow(s,2))*square(S.segment<19>(1))*gamma - r*S.segment<19>(1)*delta + r*C.segment<19>(1);
    // Move C into Cn
    Cn = std::move(C);

    C.segment<19>(1) = Cn.segment<19>(1) - dt*theta;
    //spatial bc's
    C(0) = Cn(0)*(1 - r*dt);
    C(nx-1) = 2*C(nx-2) - C(nx-3);

  }

  // After function call
  auto stop = high_resolution_clock::now();

  auto duration = duration_cast<microseconds>(stop - start);

  std::cout << "ran in: " << duration.count() << " microsecs "<< std::endl;

  std::cout << "C: " << C << std::endl;

}


#include <cstdio>
using namespace std;
#include <armadillo>
#include "util.h"

double det(double* a, int n){
	int i, j, k, sign=0;
	double ret=1, t;
    double *b = new double[n*n];
	//if (a.n!=a.m)
	//	return 0;
	for (i=0; i<n; i++)
		for (j=0; j<n; j++)
			b[i*n+j] = a[i*n+j];

	for (i=0; i<n; i++){
		if (zero(b[i*n+i])){
			for (j=i+1; j<n; j++)
				if (!zero(b[j*n+i]))
					break;

            if (j == n) {
                delete[] b;
				return 0;
            }

            for (k=i; k<n; k++)
				t = b[i*n+k], b[i*n+k]=b[j*n+k], b[j*n+k] = t;
			sign++;
		}

		ret *= b[i*n+i];
		for (k=i+1; k<n; k++)
			b[i*n+k] /= b[i*n+i];
		for (j=i+1; j<n; j++)
			for (k=i+1; k<n; k++)
				b[j*n+k] -= b[j*n+i]*b[i*n+k];
	}

    delete[] b;

	if (sign&1)
		ret = -ret;
	return ret;
}

double det_xxx(double *s, int n)
{
    int z, j, k;
    double r, total = 0;
    //int b[N][N];
    double *b = new double[(n-1)*(n-1)];
    if(n > 2)
    {
        for(z = 0; z<n; z++)
        {
            for(j=0; j<n-1; j++)
            {
                for(k=0; k<n-1; k++)
                {
                    if(k>=z)
                        //b[j][k] = s[j+1][k+1];
                        b[j*(n-1)+k] = s[(j+1)*n+(k+1)];
                    else
                        //b[j][k] = s[j+1][k];
                        b[j*(n-1)+k] = s[(j+1)*n+k];
                }
            }

                if(z%2 == 0)
                    r = s[z] * det_xxx(b, n-1);
                else
                    r = (-1)*s[z]*det_xxx(b, n-1);
                total = total + r;

        }
    }
    else if(n == 2)
    {
        total = s[0]*s[n+1] - s[1]*s[n];
    }
    delete[] b;
    return total;
}

/*
 * c:为d+1个d维点，
 * d:维数
 * cc:输出外心坐标
 */
void circumcenter(double *c, int d, double *cc)
{
    //d+1阶行列式
    double *m = new double[(d+1)*(d+1)];
    //行列式第一列填充1
    for(int i=0; i<=d; i++)
    {
        m[i*(d+1)] = 1.0;
    }
    //其他填充输入的d+1个点坐标
    for(int i=0; i<=d; i++)
        for(int j=1; j<=d; j++)
            m[i*(d+1)+j] = c[i*d+j-1];

    //行列式m的值
    //使用armadillo库计算行列式值
    //double D = arma::det(arma::mat(m, d+1, d+1, false));
    //double D = det_xxx(m, d+1);
    double D = det(m, d+1);

    // Dj[j]为替换m的第j+1列后的行列式的值
    double *Dj = new double[d];
    // 用于替代j+1列
    double *tt = new double[d+1];

    //计算tt，详情请看参考文献
    for(int i=0; i<=d; i++)
    {
        tt[i] = 0;
    }

    for(int i=0; i<=d; i++)
    {
        for(int j=0; j<d; j++)
        {
            tt[i] += c[i*d+j]*c[i*d+j];
        }
    }

    for(int i=0; i<=d; i++)
    {
        tt[i] *= 0.5;
    }

    //Dj
    for(int j=0; j<d; j++)
    {
        //替换j+1列
        for(int i=0; i<=d; i++)
        {
            m[i*(d+1)+j+1] = tt[i];
        }

        //行列式值
        //使用armadillo库计算行列式值
        //Dj[j] = arma::det(arma::mat(m, d+1, d+1, false));
        //Dj[j] = det_xxx(m, d+1);
        Dj[j] = det(m, d+1);

        //恢复j+1列
        for(int i=0; i<=d; i++)
        {
            m[i*(d+1)+j+1] = c[i*d+j];
        }
    }

    //外心点，看参考文献
    for(int j=0; j<d; j++)
    {
        cc[j] = Dj[j] / D;

    }
    delete[] m;
    delete[] Dj;
    delete[] tt;
}

double sq_distance(const double *pa, const double *pb, int d)
{
    double sq_dis = 0;
    for(int i=0; i<d; i++)
    {
        sq_dis += (pa[i] - pb[i])*(pa[i] - pb[i]);
    }
    return sq_dis;
}

int sign(double v)
{
    if (v > 0) return 1;
    if (v < 0) return -1;
    return 0;
}


#pragma once

#include <Eigen/Core>
#include <Eigen/Cholesky>

#include <boost/random/random_device.hpp>
#include <boost/random/random_number_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/mersenne_twister.hpp>
#include "modprop/ModpropTypes.h"

namespace percepto

{

/*! \brief Simple multivariate normal sampling and PDF class. */
template <typename Engine = boost::mt19937, typename Scalar = double>
class MultivariateGaussian
{
public:

	typedef Scalar ScalarType;
	typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorType;
	typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType;

	typedef boost::normal_distribution<Scalar> UnivariateNormal;
	typedef boost::variate_generator<Engine&, UnivariateNormal> RandAdapter;

	/*! \brief Seeds the engine using a true random number. */
	MultivariateGaussian( const VectorType& u, const MatrixType& S )
	: _distribution( 0.0, 1.0 ),
	  _adapter( _generator, _distribution ),
	  _mean( u ), _covariance( S )
	{
		boost::random::random_device rng;
		_generator.seed( rng );
		Initialize();
	}

	/*! \brief Seeds the engine using a specified seed. */
	MultivariateGaussian( const VectorType& u, const MatrixType& S, unsigned long seed )
	: _distribution( 0.0, 1.0 ),
	  _adapter( _generator, _distribution ),
	  _mean( u ), _covariance( S )
	{
		_generator.seed( seed );
		Initialize();
	}

    void SetMean( const VectorType& u ) { _mean = u; }
    void SetCovariance( const MatrixType& S )
	{
		_covariance = S;
		Initialize();
	}

	const VectorType& GetMean() const { return _mean; }
	const MatrixType& GetCovariance() const { return _mean; }
	const MatrixType& GetCholesky() const { return _L; }

	/*! \brief Generate a sample truncated at a specified number of standard deviations. */
	VectorType Sample( double v = 3.0 )
	{
		VectorType samples( _mean.size() );
		for( unsigned int i = 0; i < _mean.size(); i++ )
		{
			double s;
			do
			{
				s = _adapter();
			}
			while( std::abs( s ) > v );
			samples(i) = s;
		}

		return _mean + _L*samples;
	}

	/*! \brief Evaluate the multivariate normal PDF for the specified sample. */
    double EvaluateProbability( const VectorType& x ) const
	{
		VectorType diff = x - _mean;
		Eigen::Matrix<Scalar, 1, 1> exponent = -0.5 * diff.dot( _LLT.solve( diff ) );
		return _z * std::exp( exponent(0) );
	}

protected:

	Engine _generator;
	UnivariateNormal _distribution;
	RandAdapter _adapter;

	VectorType _mean;
	MatrixType _covariance;

	MatrixType _L;
	Eigen::LLT<MatrixType> _LLT;

	double _z; // Normali_zation constant;

	void Initialize()
	{
		assert( _mean.rows() == _covariance.rows() );
		assert( _mean.rows() == _covariance.cols() );
		_LLT = Eigen::LLT<MatrixType>( _covariance );
		_L = _LLT.matrixL();
		_z = std::pow( 2*M_PI, -_mean.size()/2.0 ) * std::pow( _covariance.determinant(), -0.5 );
	}

};

}


//
// Created by Yohsuke Murase on 2020/06/04.
//

#include <iostream>
#include <vector>
#include <array>
#include <map>
#include <random>
#include <cassert>
#include <fstream>
#include <Eigen/Dense>
#include "StrategyN3M5.hpp"

// memory-1 species in strategy space discretized with `D`
// memory-1 strategy is characterized by tuple (p_{c0}, p_{c1}, p_{c2}, p_{d0}, p_{d1}, p_{d2}),
// where each of which denotes cooperation probability conditioned by the last Alice's action and the other players' defectors.
// Each of which can take discrete values [0,1/D,2/D,...,D/D].
// ID of a species is given by an integer with base-(D+1).
// ID can take values [0, (D+1)^6-1]
// ID=0 : AllD, ID=ID_MAX-1 : AllC
class Cprobs {
 public:
  Cprobs(size_t id, size_t DIS) {
    double dinv = 1.0 / DIS;
    const size_t B = DIS+1;
    c0 = (id % B) * dinv;
    c1 = ((id/B) % B) * dinv;
    c2 = ((id/B/B) % B) * dinv;
    d0 = ((id/B/B/B) % B) * dinv;
    d1 = ((id/B/B/B/B) % B) * dinv;
    d2 = ((id/B/B/B/B/B) % B) * dinv;
  }
  double c0, c1, c2, d0, d1, d2;

};

class Mem1Species {
 public:
  static size_t N_M1_Species(size_t DIS) { return (DIS+1)*(DIS+1)*(DIS+1)*(DIS+1)*(DIS+1)*(DIS+1); }
  Mem1Species(size_t _id, size_t _DIS) : id(_id), DIS(_DIS), prob(_id, _DIS) {
    assert(id <= N_M1_Species(DIS));
  }
  size_t id;
  size_t DIS;
  Cprobs prob;

  std::string ToString() const {
    const size_t B = DIS+1;
    size_t c0 = (id % B);
    size_t c1 = ((id/B) % B);
    size_t c2 = ((id/B/B) % B);
    size_t d0 = ((id/B/B/B) % B);
    size_t d1 = ((id/B/B/B/B) % B);
    size_t d2 = ((id/B/B/B/B/B) % B);
    std::ostringstream oss;
    oss << c0 << '-' << c1 << '-' << c2 << '-' << d0 << '-' << d1 << '-' << d2 << '/' << DIS;
    if (IsDefensible()) { oss << "_D"; }
    if (IsEfficient()) { oss << "_E"; }
    return oss.str();
  }

  std::vector<double> StationaryState(const Mem1Species& Bstr, const Mem1Species& Cstr, double error = 0.0) const {
    Eigen::Matrix<double,8,8> A;

    // state 0: ccc, state 1: ccd (last bit is A's history), ... 7: ddd
    // calculate transition probability from j to i

    for (size_t j = 0; j < 8; j++) {
      Action last_a = (j & 1ul) ? D : C;
      Action last_b = ((j>>1ul) & 1ul) ? D : C;
      Action last_c = ((j>>2ul) & 1ul) ? D : C;

      auto cooperation_prob = [](Action a, Action b, Action c, const Mem1Species& str) -> double {
        if (a == C) {
          if (b == C && c == C) { return str.prob.c0; }
          else if (b == D && c == C) { return str.prob.c1; }
          else if (b == C && c == D) { return str.prob.c1; }
          else { return str.prob.c2; }
        }
        else {
          if (b == C && c == C) { return str.prob.d0; }
          else if (b == D && c == C) { return str.prob.d1; }
          else if (b == C && c == D) { return str.prob.d1; }
          else { return str.prob.d2; }
        }
      };
      double c_A = cooperation_prob(last_a, last_b, last_c, *this);
      double c_B = cooperation_prob(last_b, last_c, last_a, Bstr);
      double c_C = cooperation_prob(last_c, last_a, last_b, Cstr);

      c_A = (1.0 - error) * c_A + error * (1.0 - c_A);
      c_B = (1.0 - error) * c_B + error * (1.0 - c_B);
      c_C = (1.0 - error) * c_C + error * (1.0 - c_C);
      A(0,j) = c_A * c_B * c_C;
      A(1,j) = (1.0-c_A) * c_B * c_C;
      A(2,j) = c_A * (1.0-c_B) * c_C;
      A(3,j) = (1.0-c_A) * (1.0-c_B) * c_C;
      A(4,j) = c_A * c_B * (1.0-c_C);
      A(5,j) = (1.0-c_A) * c_B * (1.0-c_C);
      A(6,j) = c_A * (1.0-c_B) * (1.0-c_C);
      A(7,j) = (1.0-c_A) * (1.0-c_B) * (1.0-c_C);
    }

    for(int i=0; i<8; i++) {
      A(i,i) -= 1.0;
    }
    for(int i=0; i<8; i++) {
      A(8-1,i) += 1.0;  // normalization condition
    }
    Eigen::VectorXd b(8);
    for(int i=0; i<8; i++) { b(i) = 0.0;}
    b(8-1) = 1.0;
    Eigen::VectorXd x = A.colPivHouseholderQr().solve(b);
    std::vector<double> ans(8, 0.0);
    for(int i=0; i<ans.size(); i++) {
      ans[i] = x(i);
    }

    return ans;
  }

  double CooperationProb(StateN3M5 s) const {
    size_t num_d = 0;
    if (s.hb[0]) num_d++;
    if (s.hc[0]) num_d++;
    if (s.ha[0]) {  // A's last action is d
      if (num_d == 2) { return prob.d2; }
      else if (num_d == 1) { return prob.d1; }
      else { return prob.d0; }  // num_d == 0
    }
    else {
      if (num_d == 2) { return prob.c2; }
      else if (num_d == 1) { return prob.c1; }
      else { return prob.c0; } // num_d == 0
    }
  }

  // returns false for mixed strategy
  bool IsDefensible() const {
    double tolerance = 1.0e-8;
    if (prob.c0 > tolerance && prob.c0 < 1.0 - tolerance) { return false; }
    if (prob.c1 > tolerance && prob.c1 < 1.0 - tolerance) { return false; }
    if (prob.c2 > tolerance && prob.c2 < 1.0 - tolerance) { return false; }
    if (prob.d0 > tolerance && prob.d0 < 1.0 - tolerance) { return false; }
    if (prob.d1 > tolerance && prob.d1 < 1.0 - tolerance) { return false; }
    if (prob.d2 > tolerance && prob.d2 < 1.0 - tolerance) { return false; }

    // N = 2^{3}
    const size_t N = 8;
    typedef std::array<std::array<int, N>, N> d_matrix_t;
    typedef std::bitset<3> b3_t;
    d_matrix_t d;

    // construct adjacency matrix
    const int INF = N; // N is large enough since the path length is between -N/4 to N/4.
    for (size_t i = 0; i < N; i++) {
      for (size_t j = 0; j < N; j++) {
        d[i][j] = INF;
      }
    }

    for (size_t i = 0; i < N; i++) {
      b3_t si(i);
      bool a0 = si[0];
      size_t nd0 = (si & std::bitset<3>(0b110)).count();
      bool act_a = false;
      if (a0) {
        if (nd0 == 0) { act_a = (prob.d0 > 0.5 ? false : true); }
        else if (nd0 == 1) { act_a = (prob.d1 > 0.5 ? false : true); }
        else if (nd0 == 2) { act_a = (prob.d2 > 0.5 ? false : true); }
        else { assert(false); }
      }
      else {
        if (nd0 == 0) { act_a = (prob.c0 > 0.5 ? false : true); }
        else if (nd0 == 1) { act_a = (prob.c1 > 0.5 ? false : true); }
        else if (nd0 == 2) { act_a = (prob.c2 > 0.5 ? false : true); }
        else { assert(false); }
      }

      // Get possible next states
      std::array<b3_t,4> sjs = { 0b000, 0b010, 0b100, 0b110 };
      for (auto sj: sjs) {
        if (act_a) {
          sj.set(0);
          if(sj[1]) {  // A:D, B:D
            d[i][sj.to_ulong()] = 0;
          }
          else {  // A: D, B: C
            d[i][sj.to_ulong()] = 1;
          }
        }
        else {
          sj.reset(0);
          if(sj[1]) {  // A:C, B:D
            d[i][sj.to_ulong()] = -1;
          }
          else {  // A: C, B: C
            d[i][sj.to_ulong()] = 0;
          }
        }
      }
      if (d[i][i] < 0) { return false; }
    }

    for (size_t k = 0; k < N; k++) {
      for (size_t i = 0; i < N; i++) {
        for (size_t j = 0; j < N; j++) {
          d[i][j] = std::min(d[i][j], d[i][k] + d[k][j]);
        }
        if (d[i][i] < 0) { return false; }
      }
    }
    return true;
  }

  bool IsEfficient() const {
    auto ss = StationaryState(*this, *this, 0.0001);
    return (ss[0] > 0.99);
  }
};

class Species { // either Mem1Species or StrategyN3M5
 public:
  Species(size_t ID, size_t DIS) : m1(Mem1Species(0, DIS)), m5(std::bitset<StrategyN3M5::N>()){
    const size_t N_M1 = Mem1Species::N_M1_Species(DIS);
    if (ID < N_M1) {
      is_m1 = true;
      m1 = Mem1Species(ID, DIS);
      m5 = StrategyN3M5(std::bitset<StrategyN3M5::N>());
      name = m1.ToString();
    }
    else if (ID == N_M1) {
      is_m1 = false;
      m1 = Mem1Species(0, DIS);
      m5 = StrategyN3M5::CAPRI3();
      name = "CAPRI3";
    }
    else if (ID == N_M1 + 1) {
      is_m1 = false;
      m1 = Mem1Species(0, DIS);
      m5 = StrategyN3M5::AON(3);
      name = "AON3";
    }
    else if (ID == N_M1 + 2) {
      is_m1 = false;
      m1 = Mem1Species(0, DIS);
      m5 = StrategyN3M5::FUSS_m3();
      name = "FUSS_m3";
    }
    else {
      throw std::runtime_error("must not happen");
    }
  };
  bool is_m1;
  Mem1Species m1;
  StrategyN3M5 m5;
  std::string name;
  std::string ToString() const { return name; }
  std::vector<double> StationaryState(const Species &sb, const Species &sc, double error) const {
    if (is_m1 && sb.is_m1 && sc.is_m1) {
      return m1.StationaryState(sb.m1, sc.m1, error);
    }
    std::cerr << "calculating stationary state: " << name << ' ' << sb.name << ' ' << sc.name << std::endl;

    typedef Eigen::Triplet<double> T;
    std::vector<T> tripletVec;

    for (size_t j = 0; j < StrategyN3M5::N; j++) {
      // calculate transition probability from j to i
      const StateN3M5 sj(j);
      double c_a = _CooperationProb(sj);
      double c_b = sb._CooperationProb(sj.StateFromB());
      double c_c = sc._CooperationProb(sj.StateFromC());
      // cooperation probability taking noise into account
      c_a = (1.0 - error) * c_a + error * (1.0 - c_a);
      c_b = (1.0 - error) * c_b + error * (1.0 - c_b);
      c_c = (1.0 - error) * c_c + error * (1.0 - c_c);

      for (size_t t = 0; t < 8; t++) {
        Action act_a = (t & 1ul) ? D : C;
        Action act_b = (t & 2ul) ? D : C;
        Action act_c = (t & 4ul) ? D : C;
        size_t i = sj.NextState(act_a, act_b, act_c).ID();
        double p_a = (act_a == C) ? c_a : (1.0-c_a);
        double p_b = (act_b == C) ? c_b : (1.0-c_b);
        double p_c = (act_c == C) ? c_c : (1.0-c_c);
        tripletVec.emplace_back(i, j, p_a * p_b * p_c);
      }
    }

    const size_t S = StrategyN3M5::N;
    Eigen::SparseMatrix<double> A(S, S);
    A.setFromTriplets(tripletVec.cbegin(), tripletVec.cend());

    // subtract unit matrix & normalization condition
    std::vector<T> iVec;
    for (int i = 0; i < S-1; i++) { iVec.emplace_back(i, i, -1.0); }
    for (int i = 0; i < S-1; i++) { iVec.emplace_back(S-1, i, 1.0); }
    Eigen::SparseMatrix<double> I(S, S);
    I.setFromTriplets(iVec.cbegin(), iVec.cend());
    A = A + I;
    // std::cerr << "  transition matrix has been created" << std::endl;

    Eigen::VectorXd b = Eigen::VectorXd::Zero(S);
    b(S-1) = 1.0;

    Eigen::BiCGSTAB<Eigen::SparseMatrix<double>, Eigen::IncompleteLUT<double> > solver;
    solver.compute(A);
    Eigen::VectorXd x = solver.solve(b);

    // std::cerr << "#iterations:     " << solver.iterations() << std::endl;
    // std::cerr << "estimated error: " << solver.error() << std::endl;

    std::vector<double> ans(S, 0.0);
    for (int i = 0; i < S; i++) { ans[i] = x[i]; }
    return ans;
  }
 private:
  double _CooperationProb(const StateN3M5 &s) const {
    if (is_m1) { return m1.CooperationProb(s); }
    else { return m5.ActionAt(s) == C ? 1.0 : 0.0; }
  }

 public:
  static std::vector<Species> Memory1Species(size_t discrete_level) {
    std::vector<Species> ans;
    size_t n = Mem1Species::N_M1_Species(discrete_level);
    for (size_t i = 0; i < n; i++) {
      ans.emplace_back(i, discrete_level);
    }
    return std::move(ans);
  }
  static std::vector<Species> ReactiveMem1Species(size_t discrete_level) {
    std::vector<Species> ans;
    size_t B = discrete_level + 1;
    size_t n = Mem1Species::N_M1_Species(discrete_level);
    size_t B3 = B * B * B;
    for (size_t i = 0; i < n/B3; i++) {
      size_t id = i * B3 + i;
      ans.emplace_back(id, discrete_level);
    }
    return std::move(ans);
  }
};

class Ecosystem {
 public:
  // Ecosystem(size_t discrete_level, bool with_capri) : DIS(discrete_level) {
  //   N_M1 = Mem1Species::N_M1_Species(DIS);
  //   if (with_capri) { N_SPECIES = N_M1 + 3; } // we add these three species CAPRI3, AON5, FUSS
  //   else { N_SPECIES = N_M1; }
  // };
  Ecosystem(const std::vector<Species> &species_pool, double error) :pool(species_pool), N_SPECIES(species_pool.size()), e(error) {
    CalculateSSCache();
  };
  size_t N_SPECIES;
  std::vector<Species> pool;
  const double e;
  typedef std::vector<double> ss_cache_t;
  std::vector<std::vector<ss_cache_t> > ss_cache;
  // ss_cache[i][i] stores the stationary state when PG game is played by (i,i,i)
  // ss_cache[i][j] stores the stationary state when PG game is played by (i,i,j)

  void CalculateSSCache() {
    ss_cache.resize(N_SPECIES);
    for (size_t i = 0; i < N_SPECIES; i++) {
      ss_cache[i].resize(N_SPECIES);
    }

#pragma omp parallel for schedule(dynamic,1)
    for (size_t I=0; I < N_SPECIES * N_SPECIES; I++) {
      size_t i = I / N_SPECIES;
      size_t j = I % N_SPECIES;
      ss_cache[i][j] = pool[i].StationaryState(pool[i], pool[j], e);
    }
  }

  // payoff of species i and j when the game is played by (i,i,j)
  std::array<double,2> PayoffVersus(size_t i, size_t j, double benefit, double cost) const {
    std::array<double, 3> ans = Payoffs(ss_cache[i][j], benefit, cost);
    assert( std::abs(ans[0] - ans[1]) < 0.00001 );
    return {ans[0], ans[2]};
  }

  std::array<double,3> Payoffs(const ss_cache_t &ss, double benefit, double cost) const {
    std::array<double, 3> ans = {0.0, 0.0, 0.0};
    if (ss.size() == 8) {
      for (size_t i = 0; i < 8; i++) {
        double pa = 0.0, pb = 0.0, pc = 0.0;
        if ((i & 1ul) == 0) {
          pa -= cost;
          pb += benefit / 2.0;
          pc += benefit / 2.0;
        }
        if ((i & 2ul) == 0) {
          pb -= cost;
          pc += benefit / 2.0;
          pa += benefit / 2.0;
        }
        if ((i & 4ul) == 0) {
          pc -= cost;
          pa += benefit / 2.0;
          pb += benefit / 2.0;
        }
        ans[0] += ss[i] * pa;
        ans[1] += ss[i] * pb;
        ans[2] += ss[i] * pc;
      }
    }
    else {
      assert(ss.size() == StrategyN3M5::N);
      for (size_t i = 0; i < StrategyN3M5::N; i++) {
        StateN3M5 s(i);
        double pa = 0.0, pb = 0.0, pc = 0.0;
        if (!s.ha[0]) {
          pa -= cost;
          pb += benefit / 2.0;
          pc += benefit / 2.0;
        }
        if (!s.hb[0]) {
          pb -= cost;
          pc += benefit / 2.0;
          pa += benefit / 2.0;
        }
        if (!s.hc[0]) {
          pc -= cost;
          pa += benefit / 2.0;
          pb += benefit / 2.0;
        }
        ans[0] += ss[i] * pa;
        ans[1] += ss[i] * pb;
        ans[2] += ss[i] * pc;
      }
    }
    return ans;
  }

  // calculate the equilibrium distribution exactly by linear algebra
  std::vector<double> CalculateEquilibrium(double benefit, double cost, uint64_t N, double sigma) const {
    Eigen::MatrixXd A(N_SPECIES, N_SPECIES);
    #pragma omp parallel for
    for (size_t ii = 0; ii < N_SPECIES * N_SPECIES; ii++) {
      size_t i = ii / N_SPECIES;
      size_t j = ii % N_SPECIES;
      if (i == j) { A(i, j) = 0.0; continue; }
      double p = FixationProb(benefit, cost, N, sigma, i, j);
      // std::cerr << "Fixation prob of mutant (mutant,resident): " << p << " (" << pool[i].ToString() << ", " << pool[j].ToString() << ")" << std::endl;
      A(i, j) = p * (1.0 / N_SPECIES);
    }

    for (size_t j = 0; j < N_SPECIES; j++) {
      double p_sum = 0.0;
      for (size_t i = 0; i < N_SPECIES; i++) {
        p_sum += A(i, j);
      }
      assert(p_sum <= 1.0);
      A(j, j) = 1.0 - p_sum; // probability that the state doesn't change
    }

    size_t n_row = A.rows();

    // subtract Ax = x => (A-I)x = 0
    for (size_t i = 0; i < A.rows(); i++) {
      A(i, i) -= 1.0;
    }
    // normalization condition
    for (size_t i = 0; i < A.rows(); i++) {
      A(A.rows()-1, i) += 1.0;
    }

    Eigen::VectorXd b(A.rows());
    for(int i=0; i<A.rows()-1; i++) { b(i) = 0.0;}
    b(A.rows()-1) = 1.0;
    Eigen::VectorXd x = A.householderQr().solve(b);
    std::vector<double> ans(A.rows());
    double prob_total = 0.0;
    for(int i=0; i<ans.size(); i++) {
      ans[i] = x(i);
      prob_total += x(i);
      assert(x(i) > -0.000001);
    }
    assert(std::abs(prob_total - 1.0) < 0.00001);
    return ans;
  }

  double FixationProb(double benefit, double cost, uint64_t N, double sigma, size_t mutant_idx, size_t resident_idx) const {
    // rho_inv = \sum_{i=0}^{N-1} exp(sigma[S]),
    // where S is defined as
    // S =  i/6(i^2−3iN+6i+3N^2−12N+11)s_{yyy}
    //     −i/6(i+1)(2i−3N+4)s_{yyx}
    //     +i/6(i−1)(i+1)s_{yxx}
    //     −i/6(i^2−3i(N−1)+3N^2−6N+2)s_{xyy}
    //     +i/6(i−1)(2i−3N+2))s_{xxy}
    //     −i/6(i^2−3i+2)s_{xxx}
    // std::cerr << "caclulating fixation prob for mutant " << pool[mutant_idx].ToString() << " against resident " << pool[resident_idx].ToString() << std::endl;

    double s_xxx = PayoffVersus(mutant_idx, mutant_idx, benefit, cost)[0];
    double s_yyy = PayoffVersus(resident_idx, resident_idx, benefit, cost)[0];
    auto xxy = PayoffVersus(mutant_idx, resident_idx, benefit, cost);
    double s_xxy = xxy[0];
    double s_yxx = xxy[1];
    auto yyx = PayoffVersus(resident_idx, mutant_idx, benefit, cost);
    double s_yyx = yyx[0];
    double s_xyy = yyx[1];

    // std::cerr << "s_xxx: " << s_xxx << ", s_xxy: " << s_xxy << ", s_xyy: " << s_xyy << std::endl;
    // std::cerr << "s_yyy: " << s_yyy << ", s_yxx: " << s_yxx << ", s_yyx: " << s_yyx << std::endl;

    double num_games = (N-1) * (N-2) / 2.0;
    double rho_inv = 0.0;
    for (int i=0; i < N; i++) {
      double x = sigma / num_games * (i / 6.0) * (
          (i*i - 3.0*i*N + 6.0*i + 3.0*N*N - 12.0*N + 11.0) * s_yyy
          - (i+1.0) * (2.0*i - 3.0*N + 4) * s_yyx
          + (i-1.0) * (i+1.0) * s_yxx
          - (i*i - 3.0*i*(N-1.0) + 3.0*N*N - 6.0*N + 2.0) * s_xyy
          + (i-1.0) * (2.0*i-3.0*N+2.0) * s_xxy
          - (i*i - 3.0*i + 2.0) * s_xxx
      );
      rho_inv += std::exp(x);
    }
    return 1.0 / rho_inv;
  }

  void PrintAbundance(const std::vector<double> &eq_rate, const std::string &fname) const {
    assert(eq_rate.size() == N_SPECIES);
    typedef std::pair<size_t, double> SS;
    auto compare_by_val = [](const SS &a, const SS &b) { return (a.second < b.second); };
    std::multiset<SS, decltype(compare_by_val)> sorted_histo(compare_by_val);
    for (size_t i = 0; i < eq_rate.size(); i++) {
      sorted_histo.insert( std::make_pair(i, eq_rate[i]) );
    }
    std::ofstream fout(fname);
    for (auto it = sorted_histo.rbegin(); it != sorted_histo.rend(); it++) {
      fout << pool[it->first].ToString() << ' ' << it->second << std::endl;
    }
    fout.close();
  }
  std::vector<std::string> SpeciesNames() const {
    std::vector<std::string> ans;
    for(auto s: pool) {
      ans.emplace_back( s.ToString() );
    }
    return std::move(ans);
  }
  double CooperationLevelSpecies(size_t i) const {
    const ss_cache_t &ss = ss_cache[i][i];
    if (ss.size() == 8) {
      double level = 0.0;
      for (size_t s = 0; s < 8; s++) {
        size_t num_c = 3 - std::bitset<3>(s).count();
        level += ss[s] * (num_c / 3.0);
      };
      return level;
    }
    else {
      double level = 0.0;
      for (size_t s = 0; s < StrategyN3M5::N; s++) {
        StateN3M5 state(s);
        size_t num_c = 3ul;
        if (state.ha[0]) num_c -= 1;
        if (state.hb[0]) num_c -= 1;
        if (state.hc[0]) num_c -= 1;
        level += ss[s] * (num_c / 3.0);
      }
      return level;
    }
  }
  double CooperationLevel(const std::vector<double> &eq_rate) const {
    assert(eq_rate.size() == N_SPECIES);
    double ans = 0.0;
    for (size_t i = 0; i < N_SPECIES; i++) {
      double c_lev = CooperationLevelSpecies(i);
      ans += eq_rate[i] * c_lev;
    }
    return ans;
  }
};

int main(int argc, char *argv[]) {
  Eigen::initParallel();
  if( argc != 9 ) {
    std::cerr << "Error : invalid argument" << std::endl;
    std::cerr << "  Usage : " << argv[0] << " <Nmax> <sigma> <error rate> <discrete_level> <0:reactive/1:full memory1> <1:capri3> <1:aon3> <1:fussm3>" << std::endl;
    return 1;
  }

  double cost = 1.0;
  uint64_t Nmax = std::strtoull(argv[1], nullptr,0);
  double sigma = std::strtod(argv[2], nullptr);
  double e = std::strtod(argv[3], nullptr);
  uint64_t discrete_level = std::strtoull(argv[4], nullptr,0);

  unsigned long full_or_reactive = std::strtoul(argv[5], nullptr, 0);
  unsigned long add_capri3 = std::strtoul(argv[6], nullptr, 0);
  unsigned long add_aon3 = std::strtoul(argv[7], nullptr, 0);
  unsigned long add_fussm3 = std::strtoul(argv[8], nullptr, 0);

  std::vector<Species> pool = (full_or_reactive == 1 ? Species::Memory1Species(discrete_level) : Species::ReactiveMem1Species(discrete_level));
  size_t N_M1 = Mem1Species::N_M1_Species(discrete_level);
  if (add_capri3 == 1) {
    pool.emplace_back(N_M1, discrete_level);
  }
  if (add_aon3) {
    pool.emplace_back(N_M1+1, discrete_level);
  }
  if (add_fussm3) {
    pool.emplace_back(N_M1+2, discrete_level);
  }
  Ecosystem eco(pool, e);

  {
    std::ofstream namout("species.txt");
    for(auto name: eco.SpeciesNames()) {
      namout << name << std::endl;
    }
    namout << std::endl;
  }

  auto SweepOverBeta = [&eco,cost,sigma](size_t N)->std::vector<std::pair<double,double>> {
    char fname1[100];
    sprintf(fname1, "abundance_%zu.dat", N);
    std::ofstream eqout(fname1);
    std::vector<std::pair<double,double>> c_levels;
    for (int i = 5; i <= 300; i+=5) {
      double benefit = 1.0 + i / 100.0;
      auto eq = eco.CalculateEquilibrium(benefit, cost, N, sigma);
      eqout << benefit << ' ';
      for (double x: eq) { eqout << x << ' '; }
      eqout << std::endl;
      double c_lev = eco.CooperationLevel(eq);
      c_levels.push_back(std::make_pair(benefit, c_lev));
    }
    return c_levels;
  };

  std::vector<std::vector<std::pair<double,double>>> ans;
  for (int N = 3; N <= Nmax; N++) {
    auto a = SweepOverBeta(N);
    ans.push_back(a);
  }

  std::ofstream fout("cooperation_level.dat");
  for (size_t i = 0; i < ans[0].size(); i++) {
    fout << ans[0][i].first;
    for (size_t j = 0; j < ans.size(); j++) {
      fout << ' ' << ans[j][i].second;
    }
    fout << "\n";
  }
  fout.close();

  return 0;
}


/**
 * @file LinearAlgebra.hpp
 * @author bwu
 * @brief Model of vector and matrix concept in static dimension size, implement based on boost ublas
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_MATH_LINEARALGEBRA_HPP
#define GENERIC_MATH_LINEARALGEBRA_HPP
#include "generic/common/Traits.hpp"
#include "generic/common/Version.hpp"
#include "MathUtility.hpp"
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/hermitian.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <initializer_list>
#include <cmath>
namespace generic{
namespace math{
///@brief linear algebra related classes and functions
namespace la {
using namespace generic::common;
using namespace generic::math;
using namespace boost::numeric;

///@brief constructs an unbounded array from initializer list
template <typename num_type>
inline ublas::unbounded_array<num_type> makeUbArray(std::initializer_list<num_type> l) {
    ublas::unbounded_array<num_type> res(l.size());
    for(size_t i = 0; i < l.size(); ++i)
        res[i] = *(l.begin() + i);
    return res;
}

///@brief constructs an unbounded array from collection begin/end iterator
template <typename num_type, typename iterator>
inline ublas::unbounded_array<num_type> makeUbArray(iterator begin, iterator end) {
    size_t size = std::distance(begin, end);
    ublas::unbounded_array<num_type> res(size);
    for(size_t i = 0; i < size; ++i)
        res[i] = *(begin + i);
    return res;
}

template <typename num_type, size_t M, size_t N> class Matrix;
template <typename num_type, size_t N>
class Vector
{
public:
    using data_type = ublas::vector<num_type>;
    Vector() : m_data(N) { std::fill(m_data.begin(), m_data.end(), 0); }
    Vector(num_type s) : m_data(N) { std::fill(m_data.begin(), m_data.end(), s); }
    Vector(data_type data) : m_data(data) { m_data.resize(N, true); }
    Vector(std::initializer_list<num_type> l) : m_data(makeUbArray(l)) { m_data.resize(N, true); }

    template <typename... Args>
    Vector(num_type first, num_type second, Args... args) : m_data(N) { Set(first, second, args...); }

    bool operator== (const Vector<num_type, N> & v) { return  Same(*this, v); }
    bool operator!= (const Vector<num_type, N> & v) { return !Same(*this, v); }

    num_type & operator() (size_t dim) { return m_data(dim); }
    const num_type & operator() (size_t dim) const { return m_data(dim); }

    num_type & operator[] (size_t dim) { return m_data(dim); }
    const num_type & operator[] (size_t dim) const { return m_data(dim); }

    Vector operator+ (const Vector<num_type, N> & v) const;
    Vector operator- () const;
    Vector operator- (const Vector<num_type, N> & v) const;
    Vector operator* (float_type<num_type> scale) const;
    Vector operator/ (float_type<num_type> scale) const;
    Vector operator* (const Matrix<num_type, N, N> & m) const;

    Vector & operator += (const Vector<num_type, N> & v);
    Vector & operator -= (const Vector<num_type, N> & v);
    Vector & operator *= (float_type<num_type> scale);
    Vector & operator /= (float_type<num_type> scale);

    void Swap(Vector<num_type, N> & v) { m_data.swap(v.Data()); }
    Matrix<num_type, N, 1> T() const;
    num_type Norm1() const;
    float_type<num_type> Norm2() const;
    num_type NormSquare() const;

    data_type & Data() { return m_data; }
    const data_type & Data() const { return m_data; }

    template <typename... Args>
    void Set(Args... args);

    static bool Same(const Vector<num_type, N> & v1, const Vector<num_type, N> & v2);
private:
    data_type m_data;
};

struct matrix_general_tag {};
struct matrix_identity_tag {};
struct matrix_diagonal_tag {};
struct matrix_upper_triangle_tag {};
struct matrix_lower_triangle_tag {};
struct matrix_symmetric_tag {};
struct matrix_hermitian_tag {};
struct matrix_transform2d_tag {};
struct matrix_transform3d_tag {};
struct matrix_unknown_tag {};

template <typename num_type, size_t M = 1, size_t N = 1>
class Matrix
{
public:
    using data_type = ublas::matrix<num_type>;
    Matrix();

    Matrix(num_type s);

    template<typename matrix_tag,
             typename std::enable_if<
                      std::is_same<matrix_tag, matrix_diagonal_tag>::value  && M == N, bool>::type = true>
    Matrix(num_type s, matrix_tag);

    Matrix(data_type data) { data.resize(M, N, true); m_data = data; }

    Matrix(const std::initializer_list<std::initializer_list<num_type> > & ll)
    {
        m_data.resize(M, N);
        auto iter1 = ll.begin();
        for(size_t i = 0; i < M && iter1 != ll.end(); ++i, ++iter1){
            const std::initializer_list<num_type> & l = *iter1;
            auto iter2 = l.begin();
            for(size_t j = 0; j < N && iter2 != l.end(); ++j, ++iter2)
                m_data(i, j) = *iter2;
        }
    }

    template <typename... Args>
    Matrix(Vector<num_type, N> first, Vector<num_type, N> second, Args... args);

    bool operator== (const Matrix<num_type, M, N> & m) { return  Same(*this, m); }
    bool operator!= (const Matrix<num_type, M, N> & m) { return !Same(*this, m); }

    num_type & operator() (size_t row, size_t col);
    const num_type & operator() (size_t row, size_t col) const;

    Matrix operator+ (const Matrix<num_type, M, N> & m) const;
    Matrix operator- () const;
    Matrix operator- (const Matrix<num_type, M, N> & m) const;
    Matrix operator* (float_type<num_type> scale) const;
    Matrix operator/ (float_type<num_type> scale) const;

    Vector<num_type, N> operator* (const Vector<num_type, N> & v) const;

    template <size_t O>
    Matrix<num_type, M, O> operator* (const Matrix<num_type, N, O> & m) const;

    Matrix & operator+= (const Matrix<num_type, M, N> & m);
    Matrix & operator-= (const Matrix<num_type, M, N> & m);
    Matrix & operator*= (float_type<num_type> scale);
    Matrix & operator/= (float_type<num_type> scale);
    Matrix & operator*= (const Matrix<num_type, N, N> & m);

    Vector<num_type, N> Row(size_t row) const;
    Vector<num_type, M> Col(size_t col) const;

    void Swap(Matrix<num_type, M, N> & m) { m_data.swap(m.Data()); }
    Matrix<num_type, N, M> T() const;
    float_type<num_type> Norm2() const { return std::sqrt(NormSquare()); }
    num_type NormSquare() const;

    data_type & Data() { return m_data; }
    const data_type & Data() const { return m_data; }

    template <typename... Args>
    void Set(Args... args);

    static bool Same(const Matrix<num_type, M, N> & m1, const Matrix<num_type, M, N> & m2);
private:
    data_type m_data;
};

template <typename num_type, size_t I, size_t N>
struct VectorSetter
{
    template <typename... Args>
    static void Set(Vector<num_type, N> & v, num_type s, Args... args)
    {
        v[I] = s;
        VectorSetter<num_type, I + 1, N>::Set(v, args...);
    }
};

template <typename num_type, size_t N>
struct VectorSetter<num_type, N, N>
{
    static void Set(Vector<num_type, N> &) {}
};

#if GENERIC_CURRENT_BOOST_LIBRARY_VER >= 172
template <typename num_type, size_t N>
inline num_type Vector<num_type, N>::Norm1() const
{
    return norm_1(m_data);
}

template <typename num_type, size_t N>
inline float_type<num_type> Vector<num_type, N>::Norm2() const
{
    return norm_2(m_data);
}

template <typename num_type, size_t N>
inline num_type Vector<num_type, N>::NormSquare() const
{
    return norm_2_square(m_data);
}
#else
template <typename num_type, size_t N>
inline num_type Vector<num_type, N>::Norm1() const
{
    num_type norm1(0);
    for(size_t i = 0; i < N; ++i)
        norm1 += std::abs(m_data[i]);
    return norm1;
}

template <typename num_type, size_t N>
inline float_type<num_type> Vector<num_type, N>::Norm2() const
{
    return std::sqrt(float_type<num_type>(NormSquare()));
}

template <typename num_type, size_t N>
inline num_type Vector<num_type, N>::NormSquare() const
{
    num_type normSquare(0);
    for(size_t i = 0; i < N; ++i)
        normSquare += m_data[i] * m_data[i];
    return normSquare;
}
#endif

template <typename num_type, size_t N>
template <typename... Args>
inline void Vector<num_type, N>::Set(Args... args)
{
    VectorSetter<num_type, 0, N>::Set(*this, num_type(args)...);
}

template <typename num_type, size_t N>
inline Vector<num_type, N> Vector<num_type, N>::operator+ (const Vector<num_type, N> & v) const
{
    return Vector<num_type, N>(m_data + v.Data());
}

template <typename num_type, size_t N>
inline Vector<num_type, N> Vector<num_type, N>::operator- () const
{
    return Vector<num_type, N>(0) - *this;
}

template <typename num_type, size_t N>
inline Vector<num_type, N> Vector<num_type, N>::operator- (const Vector<num_type, N> & v) const
{
    return Vector<num_type, N>(m_data - v.Data());
}

template <typename num_type, size_t N>
inline Vector<num_type, N> Vector<num_type, N>::operator* (float_type<num_type> scale) const
{
    return Vector<num_type, N>(m_data * scale);
}

template <typename num_type, size_t N>
inline Vector<num_type, N> Vector<num_type, N>::operator/ (float_type<num_type> scale) const
{
    return Vector<num_type, N>(m_data / scale);
}

template <typename num_type, size_t N>
inline Vector<num_type, N> Vector<num_type, N>::operator* (const Matrix<num_type, N, N> & m) const
{
    return Vector<num_type, N>(prod(m_data, m.Data()));
}

template <typename num_type, size_t N>
inline Vector<num_type, N> & Vector<num_type, N>::operator += (const Vector<num_type, N> & v)
{
    m_data += v.Data();
    return *this;
}

template <typename num_type, size_t N>
inline Vector<num_type, N> & Vector<num_type, N>::operator -= (const Vector<num_type, N> & v)
{
    m_data -= v.Data();
    return *this;
}

template <typename num_type, size_t N>
inline Vector<num_type, N> & Vector<num_type, N>::operator *= (float_type<num_type> scale)
{
    m_data *= scale;
    return *this;
}

template <typename num_type, size_t N>
inline Vector<num_type, N> & Vector<num_type, N>::operator /= (float_type<num_type> scale)
{
    m_data /= scale;
    return *this;
}

template <typename num_type, size_t N>
inline Matrix<num_type, N, 1> Vector<num_type, N>::T() const
{
    Matrix<num_type, N, 1> m;
    for(size_t i = 0; i < N; ++i)
        m(i, 0) = m_data(i);
    return m;
}

template <typename num_type, size_t N>
inline bool Vector<num_type, N>::Same(const Vector<num_type, N> & v1, const Vector<num_type, N> & v2)
{
    return math::EQ((v1 - v2).NormSquare(), num_type(0));
}

template <typename num_type, size_t I, size_t M, size_t N>
struct MatrixSetter
{
    template <typename... Args>
    static void Set(Matrix<num_type, M, N> & m, Vector<num_type, N> v, Args... args)
    {
        for(size_t j = 0; j < N; ++j)
            m(I, j) = v[j];
        MatrixSetter<num_type, I + 1, M, N>::Set(m, args...);
    }
};

template <typename num_type, size_t M, size_t N>
struct MatrixSetter<num_type, M, M, N>
{
    static void Set(Matrix<num_type, M, N> &) {}
};

template <typename num_type, size_t M, size_t N>
inline Matrix<num_type, M, N>::Matrix() : m_data(M, N)
{
    for(size_t i = 0; i < M; ++i)
        for(size_t j = 0; j < N; ++j)
            m_data(i, j) = 0;
}

template <typename num_type, size_t M, size_t N>
inline Matrix<num_type, M, N>::Matrix(num_type s) : Matrix()
{
    for(size_t i = 0; i < M; ++i)
        for(size_t j = 0; j < N; ++j)
            m_data(i, j) = s;
}

template <typename num_type, size_t M, size_t N>
template <typename matrix_tag,
          typename std::enable_if<
                   std::is_same<matrix_tag, matrix_diagonal_tag>::value && M == N, bool>::type>
inline Matrix<num_type, M, N>::Matrix(num_type s, matrix_tag) : Matrix()
{
    for(size_t i = 0; i < M; ++i)
        m_data(i, i) = s;
}

template <typename num_type, size_t M, size_t N>
template <typename... Args>
inline Matrix<num_type, M, N>::Matrix(Vector<num_type, N> first, Vector<num_type, N> second, Args... args)
 : m_data(M, N)
{
    Set(first, second, args...);
}

template <typename num_type, size_t M, size_t N>
inline num_type & Matrix<num_type, M, N>::operator() (size_t row, size_t col) { return m_data(row, col); }

template <typename num_type, size_t M, size_t N>
inline const num_type & Matrix<num_type, M, N>::operator() (size_t row, size_t col) const { return m_data(row, col); }

template <typename num_type, size_t M, size_t N>
inline Matrix<num_type, M, N> Matrix<num_type, M, N>::operator+ (const Matrix<num_type, M, N> & m) const
{
    return Matrix<num_type, M, N>(m_data + m.Data());
}

template <typename num_type, size_t M, size_t N>
inline Matrix<num_type, M, N> Matrix<num_type, M, N>::operator- () const
{
    return Matrix<num_type, M, N>(0) - *this;
}

template <typename num_type, size_t M, size_t N>
inline Matrix<num_type, M, N> Matrix<num_type, M, N>::operator- (const Matrix<num_type, M, N> & m) const
{
    return Matrix<num_type, M, N>(m_data - m.Data());
}

template <typename num_type, size_t M, size_t N>
inline Matrix<num_type, M, N> Matrix<num_type, M, N>::operator* (float_type<num_type> scale) const
{
    return Matrix<num_type, M, N>(m_data * scale);
}

template <typename num_type, size_t M, size_t N>
inline Matrix<num_type, M, N> Matrix<num_type, M, N>::operator/ (float_type<num_type> scale) const
{
    return Matrix<num_type, M, N>(m_data / scale);
}
template <typename num_type, size_t M, size_t N>
inline Vector<num_type, N> Matrix<num_type, M, N>:: operator* (const Vector<num_type, N> & v) const
{
    return Vector<num_type, N>(prod(m_data, v.Data()));
}

template <typename num_type, size_t M, size_t N>
template <size_t O>
inline Matrix<num_type, M, O> Matrix<num_type, M, N>::operator* (const Matrix<num_type, N, O> & m) const
{
    return Matrix<num_type, M, O>(prod(m_data, m.Data()));
}

template <typename num_type, size_t M, size_t N>
inline Matrix<num_type, M, N> & Matrix<num_type, M, N>::operator += (const Matrix<num_type, M, N> & m)
{
    m_data += m.Data();
    return *this;
}

template <typename num_type, size_t M, size_t N>
inline Matrix<num_type, M, N> & Matrix<num_type, M, N>::operator -= (const Matrix<num_type, M, N> & m)
{
    m_data -= m.Data();
    return *this;
}

template <typename num_type, size_t M, size_t N>
inline Matrix<num_type, M, N> & Matrix<num_type, M, N>::operator *= (float_type<num_type> scale)
{
    m_data *= scale;
    return *this;
}

template <typename num_type, size_t M, size_t N>
inline Matrix<num_type, M, N> & Matrix<num_type, M, N>::operator /= (float_type<num_type> scale)
{
    m_data /= scale;
    return *this;
}

template <typename num_type, size_t M, size_t N>
inline Matrix<num_type, M, N> & Matrix<num_type, M, N>::operator *= (const Matrix<num_type, N, N> & m)
{
    m_data = prod(m_data, m.Data());
    return *this;
}

template <typename num_type, size_t M, size_t N>
inline Vector<num_type, N> Matrix<num_type, M, N>::Row(size_t row) const
{
    Vector<num_type, N> v;
    for(size_t i = 0; i < N; ++i)
        v[i] = m_data(row, i);
    return v;
}

template <typename num_type, size_t M, size_t N>
inline Vector<num_type, M> Matrix<num_type, M, N>::Col(size_t col) const
{
    Vector<num_type, M> v;
    for(size_t j = 0; j < M; ++j)
        v[j] = m_data(j, col);
    return v;
}

template <typename num_type, size_t M, size_t N>
Matrix<num_type, N, M> Matrix<num_type, M, N>::T() const
{
    Matrix<num_type, N, M> mT;
    for(size_t i = 0; i < M; ++i)
        for(size_t j = 0; j < N; ++j)
            mT(j, i) = m_data(i, j);
    return mT;
}

template <typename num_type, size_t M, size_t N>
template <typename... Args>
inline void Matrix<num_type, M, N>::Set(Args... args)
{
    MatrixSetter<num_type, 0, M, N>::Set(*this, Vector<num_type, N>(args)...);
}

template <typename num_type, size_t M, size_t N>
inline num_type Matrix<num_type, M, N>::NormSquare() const
{
    num_type res(0);
    for(size_t i = 0; i < M; ++i)
        for(size_t j = 0; j < N; ++j)
            res += m_data(i, j) * m_data(i, j);
    return res;
}

template <typename num_type, size_t M, size_t N>
inline bool Matrix<num_type, M, N>::Same(const Matrix<num_type, M, N> & m1, const Matrix<num_type, M, N> & m2)
{
    return math::EQ((m1 - m2).NormSquare(), num_type(0));
}
}//namespace la
}//namespace math
}//namespace generic
#endif//GENERIC_MATH_LINEARALGEBRA_HPP