/**
 * @file upwindfinitevolume_main.cc
 * @brief NPDE homework UpwindFiniteVolume code
 * @author Philipp Egg
 * @date 08.09.2020
 * @copyright Developed at ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/geometry/geometry.h>
#include <lf/io/gmsh_reader.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/refinement/refinement.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <cmath>
#include <iostream>
#include <memory>
#include <vector>

#include "upwindfinitevolume.h"

int main() {
  /* SAM_LISTING_BEGIN_1 */
#if SOLUTION
  // Read in mesh
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader(std::move(mesh_factory),
                            CURRENT_SOURCE_DIR "/mesh.msh");

  std::shared_ptr<lf::mesh::Mesh> mesh_p = reader.mesh();

  double eps = 1e-6;

  auto v = [](Eigen::Vector2d x) -> Eigen::Vector2d {
    return Eigen::Vector2d(1, 0.5 - x[1]);
  };

  auto u = [](Eigen::Vector2d x) -> double {
    return std::sin(4 * M_PI * x[1]) * std::exp(-x[0]);
  };

  auto f = [&u, eps](Eigen::Vector2d x) -> double {
    return eps * (1.0 - 16.0 * M_PI * M_PI) * u(x) - 2 * u(x) +
           (0.5 - x[1]) * 4 * M_PI * std::cos(4.0 * M_PI * x[1]) *
               std::exp(-x[0]);
  };

  // Convergence study
  auto mesh_seq_p{
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(mesh_p, 6)};
  int num_meshes = mesh_seq_p->NumLevels();

  std::vector<double> vec_l2error;
  std::vector<double> vec_Ndofs;
  for (int level = 0; level < num_meshes; ++level) {
    // Get the current mesh
    auto cur_mesh = mesh_seq_p->getMesh(level);

    // Create a DOF Hander for the current mesh
    const lf::assemble::UniformFEDofHandler cur_dofh(
        cur_mesh, {{lf::base::RefEl::kPoint(), 1},
                   {lf::base::RefEl::kSegment(), 0},
                   {lf::base::RefEl::kTria(), 0},
                   {lf::base::RefEl::kQuad(), 0}});
    int N_dofs = cur_dofh.NumDofs();

    lf::assemble::COOMatrix<double> A(N_dofs, N_dofs);
    UpwindFiniteVolume::ElementMatrixProvider my_mat_provider(v, eps);
    lf::assemble::AssembleMatrixLocally(0, cur_dofh, cur_dofh, my_mat_provider,
                                        A);

    Eigen::VectorXd phi(N_dofs);
    phi.setZero();
    UpwindFiniteVolume::ElementVectorProvider my_vec_provider(f);
    lf::assemble::AssembleVectorLocally(0, cur_dofh, my_vec_provider, phi);

    // Dirichlet boundary condition
    auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(cur_mesh, 2)};
    auto my_selector = [&cur_dofh, &u, &bd_flags](unsigned int dof_idx) {
      if (bd_flags(cur_dofh.Entity(dof_idx))) {
        const lf::mesh::Entity &entity{cur_dofh.Entity(dof_idx)};
        const lf::geometry::Geometry *geo_p = entity.Geometry();
        const Eigen::MatrixXd corners = lf::geometry::Corners(*geo_p);
        Eigen::Vector2d corner = corners.col(0);
        return std::make_pair(true, u(corner));
      } else {
        return std::make_pair(false, 42.0);
      }
    };
    lf::assemble::FixFlaggedSolutionComponents<double>(my_selector, A, phi);

    const Eigen::SparseMatrix<double> A_crs = A.makeSparse();

    // Numerical Solution
    Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
    solver.compute(A_crs);
    if (solver.info() != Eigen::Success) {
      printf("Decomposition Failed.\n");
    }
    Eigen::VectorXd sol_vec = solver.solve(phi);

    // Exact Solution
    Eigen::VectorXd exact_sol(N_dofs);
    for (int i = 0; i < N_dofs; ++i) {
      const lf::mesh::Entity &entity{cur_dofh.Entity(i)};
      const lf::geometry::Geometry *geo_p = entity.Geometry();
      const Eigen::MatrixXd corners = lf::geometry::Corners(*geo_p);
      exact_sol[i] = u(corners.col(0));
    }

    // L2 Error
    double l2_error = (sol_vec - exact_sol).norm() / N_dofs;
    vec_l2error.push_back(l2_error);
    vec_Ndofs.push_back(N_dofs);
  }

  for (int i = 0; i < vec_l2error.size(); ++i) {
    if (i > 0) {
      double conv_rate =
          (std::log(vec_l2error.at(i - 1)) - std::log(vec_l2error.at(i))) /
          (std::log(vec_Ndofs.at(i)) - std::log(vec_Ndofs.at(i - 1)));
      std::cout << "N_dofs: " << vec_Ndofs.at(i)
                << "; L2 Error: " << vec_l2error.at(i)
                << "; Conv. rate: " << conv_rate << std::endl;
    } else {
      std::cout << "N_dofs: " << vec_Ndofs.at(i)
                << "; L2 Error: " << vec_l2error.at(i) << std::endl;
    }
  }
  /* SAM_LISTING_END_1 */

#else
  //====================
  // Your code goes here
  //====================
#endif
  return 0;
}


/* Copyright (C) 2012-2020 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/* Test_extractDigits.cpp - extracting digits.
 *   For a plaintext space modulo a prime-power $p^e$, extracting
 *   the base-$p$ representation of an encrypted values.
 */
#include <NTL/ZZ.h>
NTL_CLIENT
#include <helib/EncryptedArray.h>
#include <helib/polyEval.h>
#include <helib/debugging.h>
#include <helib/ArgMap.h>

using namespace helib;

static bool noPrint = false;
static bool debug = 0;

int main(int argc, char *argv[])
{
  ArgMap amap;
  long p = 5;
  long r=0;
  long m = 0;
  bool dry = false;

  amap.arg("p", p, "plaintext base");
  amap.arg("r", r, "lifting");
  amap.arg("m", m, "the cyclotomic ring", "heuristic");
  amap.arg("noPrint", noPrint, "suppress printouts");
  amap.arg("debug", debug, "extra debugging");
  amap.arg("dry", dry, "dry=1 for a dry-run");

  // get parameters from the command line
  amap.parse(argc, argv);

  if (p<2) exit(0);
  double lBound = 30.0;
  long bound = floor(lBound/log2((double)p));
  if (r<2 || r>bound) r = bound;
  long p2r = power_long(p,r); // p^r

  long ll = NextPowerOfTwo(p);
  long L = (r*ll*3 +2)*30; // how many levels do we need
  if (m==0)
    m = p+1; // FindM(/*secparam=*/80, L, /*c=*/4, p, /*d=*/1, 0, m);
  setDryRun(dry);

  if (!noPrint) {
    if (dry) cout << "dry run: ";
    cout << "m="<<m<<", p="<<p<<", r="<<r<<", L="<<L<<endl;
  }
  Context context(m, p, r);
  buildModChain(context, L, /*c=*/4);

  SecKey secretKey(context);
  const PubKey& publicKey = secretKey;
  secretKey.GenSecKey(); // A +-1/0 secret key
  addSome1DMatrices(secretKey); // compute key-switching matrices that we need

  if (debug) {
      dbgKey = &secretKey; // debugging key and ea
      dbgEa = context.ea;
  }
#ifdef HELIB_DEBUG
  dbgKey = &secretKey; // debugging key and ea
  dbgEa = context.ea;
#endif //HELIB_DEBUG

  EncryptedArray ea(context);
  vector<long> v;
  vector<long> pDigits;
  ea.random(v); // random values in the slots

  Ctxt c(publicKey);
  ea.encrypt(c, publicKey, v);
  ea.decrypt(c, secretKey, pDigits);
  if (ea.size()<=20 && !noPrint)
    cout << "plaintext="<<pDigits<<endl;

  if (!noPrint)
    cout << "extracting "<<r<<" digits..." << std::flush;
  vector<Ctxt> digits;
  extractDigits(digits, c);
  if (!noPrint)
    cout << " done\n" << std::flush;

  vector<long> tmp = v;
  long pp = p2r;
  for (long i=0; i<(long)digits.size(); i++) {
#if 0
// removed isCorrect() for now as it is incorrectly defined
    if (!digits[i].isCorrect()) {
      cout << " BAD, potential decryption error for "<<i<<"th digit ";
      CheckCtxt(digits[i], "");
      exit(0);
    }
#endif
    ea.decrypt(digits[i], secretKey, pDigits);
    if (ea.size()<=20 && !noPrint)
      cout << i << "th digit="<<pDigits<<endl;

    // extract the next digit from the plaintext, compare to pDigits
    for (long j=0; j<(long)v.size(); j++) {
      long digit = tmp[j] % p;
      if (digit > p/2) digit -= p;
      else if (digit < -p/2) digit += p;

      // assert ((pDigits[j]-digit) % p == 0);
      if ((pDigits[j]-digit) % pp != 0) {
        cout << "BAD\n";
        if (!noPrint)
          cout << " error: v["<<j<<"]="<<v[j]
               << " but "<<i<<"th digit comes "<< pDigits[j]
               << " rather than "<<digit<<endl<<endl;
	exit(0);
      }
      tmp[j] -= digit;
      tmp[j] /= p;
    }
    pp /= p;
  }
  cout << "GOOD\n";
}


#pragma once
#include <Eigen/Dense>
#include <unsupported/Eigen/FFT>

namespace ppl {
namespace math {

inline size_t padded_length(size_t N)
{
    return std::pow(2, std::ceil(std::log(N)/std::log(2.)));
}

/**
 * Computes autocorrelation of x where each column of x
 * is a component of a process and hence each row is a time point.
 * More mathematically, x(i,...) is the process value at time i.
 *
 * For more detail, see:
 * https://lingpipe-blog.com/2012/06/08/autocorrelation-fft-kiss-eigen/
 * https://github.com/stan-dev/math/blob/41e548e19da5675121c245b535d4019c8bbd754b/stan/math/prim/mat/fun/autocorrelation.hpp
 *
 * @tparam  T   underlying value type (usually double)
 * @param   x   process matrix
 */
template <class T>
inline auto autocorrelation(const Eigen::MatrixBase<T>& x)
{
    using scalar_t = typename T::Scalar;
    using complex_t = std::complex<scalar_t>;

    size_t n_rows = x.rows();
    size_t padded_len = 2*padded_length(n_rows);

    Eigen::Matrix<scalar_t, Eigen::Dynamic, Eigen::Dynamic> out(x.rows(), x.cols());

    Eigen::FFT<scalar_t> fft;

    for (int i = 0; i < x.cols(); ++i) {

        // create centered copy of x
        Eigen::Matrix<scalar_t, Eigen::Dynamic, 1> x_cent(padded_len);
        x_cent.setZero();
        x_cent.head(n_rows) = x.col(i).array() - x.col(i).mean();

        // FFT
        Eigen::Matrix<complex_t, Eigen::Dynamic, 1> freq(padded_len);
        fft.fwd(freq, x_cent);

        // compute complex-norm element-wise
        freq = freq.array().abs2();

        // inverse FFT and trim to shape of x
        Eigen::Matrix<complex_t, Eigen::Dynamic, 1> ifreq(padded_len);
        fft.inv(ifreq, freq);

        // get autocorrelation by normalizing by variance
        out.col(i) = ifreq.head(n_rows).real() / (n_rows * n_rows * 2.);
        out.col(i) /= out(0,i);

    }

    return out;
}

} // namespace math
} // namespace ppl


#include "adapter_creator.hpp"
#include "experiment_helpers.hpp"
#include <Eigen/SVD>
#include <iostream>

void create_random_pose(opengv::rotation_t & rotation, opengv::translation_t & position, const double & noise, const double & outlierFraction, const size_t & numberPoints, const int & numberCameras,opengv::bearingVectors_t & bearingVectors, std::vector<int> & camCorrespondences, opengv::points_t & points, opengv::translations_t & camOffsets, opengv::rotations_t & camRotations){

  //create a random camera-system
  opengv::generateRandomCameraSystem( numberCameras, camOffsets, camRotations );

  //derive correspondences based on random point-cloud
  Eigen::MatrixXd gt(3,numberPoints);
  opengv::generateRandom2D3DCorrespondences(position, rotation, camOffsets, camRotations, numberPoints, noise, outlierFraction, bearingVectors, points, camCorrespondences, gt );

  //print the experiment characteristics
  opengv::printExperimentCharacteristics(
				 position, rotation, noise, outlierFraction );
}

void choose_best_transformation(const opengv::transformations_t &solutions, const opengv::transformation_t & ref, opengv::transformation_t & transformation_method){
  double error = 1e7;
  int index = 0;
  for(int i = 0; i < solutions.size(); ++i){
    if(error > (ref - solutions[i] ).norm()){
      error = (ref - solutions[i] ).norm();
      index = i;
    }
  }
  transformation_method = solutions[index];
 }

void choose_best_essential_matrix(const opengv::essentials_t & solutions, const opengv::essential_t & ref, opengv::essential_t & essential_matrix){
  if(solutions.size() == 1){
    essential_matrix = solutions[0];
  }
  double error = 1e7;
  int index = 0;
  for(int i = 0; i < solutions.size(); ++i){
    if(error > (ref - solutions[i]).norm()){
      error = (ref - solutions[i]).norm();
      index = i;
    }
  }
  essential_matrix = solutions[index];
}

opengv::essential_t calculate_essential_matrix(const opengv::translation_t & position, const opengv::rotation_t & rotation)
{
  //E transforms vectors from vp 2 to 1: x_1^T * E * x_2 = 0
  //and E = (t)_skew*R
  Eigen::Matrix3d t_skew = Eigen::Matrix3d::Zero();
  t_skew(0,1) = -position[2];
  t_skew(0,2) = position[1];
  t_skew(1,0) = position[2];
  t_skew(1,2) = -position[0];
  t_skew(2,0) = -position[1];
  t_skew(2,1) = position[0];
  opengv::essential_t E = t_skew * rotation;
  std::cout << "the random essential matrix is:" << std::endl;
  std::cout << E << std::endl;
  return E;
}

opengv::transformation_t calculate_transformation_from_essential_matrix(const opengv::essential_t & essential_matrix, const opengv::rotation_t &ref_rotation, const opengv::translation_t & ref_translation){
  //Obtain matrices U and V
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(essential_matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::Matrix3d U = svd.matrixU();
  Eigen::Matrix3d V = svd.matrixV();
  Eigen::Matrix3d sing_values = Eigen::Matrix3d::Zero(3,3);
  sing_values(0,0) = svd.singularValues()(0,0);
  sing_values(1,1) = svd.singularValues()(1,0);

  double detU = U.determinant();
  if (detU < 0){
    U = -U;
  }

  double detV = V.determinant();
  if (detV < 0){
    V = -V;
  }

  Eigen::Matrix3d Rz = Eigen::Matrix3d::Zero(3,3);
  Rz(0,1) = -1;Rz(1,0) = 1; Rz(2,2) = 1;

  opengv::rotation_t r = U * Rz.transpose() * V.transpose();
  Eigen::MatrixXd skew_t = U * Rz * sing_values * U.transpose();
  Eigen::MatrixXd t = Eigen::MatrixXd::Zero(3,1);
  t(0,0) = skew_t(2,1);t(1,0) = skew_t(0,2); t(2,0) = skew_t(1,0);

  if ( (r - ref_rotation).norm() > 1e-6){
    r = U * Rz * V.transpose();
  }
  if( (t - ref_translation).norm() > 1e-6){
    skew_t = U * Rz.transpose() * sing_values * U.transpose();
    t(0,0) = skew_t(2,1);t(1,0) = skew_t(0,2); t(2,0) = skew_t(1,0);
  }

  opengv::transformation_t result;
  result.block<3,3>(0,0) = r;
  result.block<3,1>(0,3) = t;
  return result;
}


#include <Eigen/StdVector>

#include <pcl/kdtree/kdtree_flann.h>
#include <week_scanmatching/icp/icp.h>
#include <Eigen/Core>

namespace scanmatcher
{
Result ICP::scanmatch(const pcl::PointCloud<pcl::PointXYZ>& input, const pcl::PointCloud<pcl::PointXYZ>& target,
                      const Eigen::Isometry3d& prediction)
{
  // Transformation is sR(p_i) + t
  Eigen::Isometry3d total_transform = Eigen::Isometry3d::Identity();
  double error = std::numeric_limits<double>::infinity();

  // TODO(Oswin) Parametrize this
  int max_iterations = 500;
  double threshold = 1e-5;
  double correspondance_outlier_threshold = 0.5;

  int num_input = input.points.size();

  Eigen::Matrix3Xd aligned_input(3, num_input);
  for (int i = 0; i < num_input; i++)
  {
    aligned_input.col(i) = input[i].getVector3fMap().cast<double>();
  }

  Eigen::Matrix3Xd target_corresp(3, num_input);

  // Construct KD tree for target for finding closest points
  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
  pcl::PointCloud<pcl::PointXYZ>::Ptr target_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  *target_ptr = target;
  kdtree.setInputCloud(target_ptr);

  for (int i = 0; i < max_iterations; i++)
  {
    // TODO(Oswin) Make this a KD tree
    std::vector<int> inlier_indices{};
    for (int j = 0; j < num_input; j++)
    {
      pcl::PointXYZ pcl_point;
      pcl_point.getVector3fMap() = aligned_input.col(j).cast<float>();
      std::vector<int> closest_idx;
      std::vector<float> distance;
      kdtree.nearestKSearch(pcl_point, 1, closest_idx, distance);
      auto closest_point = target[closest_idx.front()];
      target_corresp.col(j) = closest_point.getVector3fMap().cast<double>();

      if (std::sqrt(distance.front()) < correspondance_outlier_threshold)
      {
        inlier_indices.emplace_back(j);
      }
    }

    int num_inliers = inlier_indices.size();
    if (num_inliers < 4)
    {
      return { Eigen::Affine3d::Identity(), std::numeric_limits<double>::infinity() };
    }
    Eigen::Matrix3Xd inlier_input(3, num_inliers);
    Eigen::Matrix3Xd inlier_corresp(3, num_inliers);

    for (int idx = 0; idx < num_inliers; idx++)
    {
      int inlier_index = inlier_indices[idx];
      inlier_input.col(idx) = aligned_input.col(inlier_index);
      inlier_corresp.col(idx) = target_corresp.col(inlier_index);
    }

    Eigen::VectorXd weights = Eigen::VectorXd::Ones(num_inliers);
    Eigen::Isometry3d transform = getRigidTransform(weights, inlier_input, inlier_corresp);
    total_transform = transform * total_transform;

    error = 0.0;
    for (int j = 0; j < num_input; j++)
    {
      aligned_input.col(j) = transform * aligned_input.col(j);
      error += (target_corresp.col(j) - aligned_input.col(j)).squaredNorm();
    }
    error /= num_input;

    if (error < threshold)
    {
      break;
    }
  }

  Eigen::Affine3d affine_transform{ total_transform };
  return { affine_transform, error };
}

Eigen::Isometry3d ICP::getRigidTransform(const Eigen::VectorXd& weights, const Eigen::Matrix3Xd& input,
                                         const Eigen::Matrix3Xd& target) const
{
  const int dims = input.cols();

  // 1: Compute weighted centroids of input and target
  const double weights_sum_inverse = 1.0 / weights.sum();
  const Eigen::Vector3d input_centroid =
      (input.array().rowwise() * weights.array().transpose()).rowwise().sum() * weights_sum_inverse;
  const Eigen::Vector3d target_centroid =
      (target.array().rowwise() * weights.array().transpose()).rowwise().sum() * weights_sum_inverse;

  // 2: Subtract centroids from input and target
  const Eigen::Matrix3Xd input_prime = input.colwise() - input_centroid;
  const Eigen::Matrix3Xd target_prime = target.colwise() - target_centroid;

  // 3: Compute S = X W Y^T
  const Eigen::MatrixXd s = target_prime * weights.asDiagonal() * input_prime.transpose();
  const Eigen::JacobiSVD<Eigen::MatrixXd> svd(s, Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::Matrix3d rotation_matrix = svd.matrixV() * svd.matrixU().transpose();

  if (rotation_matrix.determinant() < 0)
  {
    Eigen::MatrixXd altered_v = svd.matrixV();
    altered_v.col(dims) *= -1;
    rotation_matrix = altered_v * svd.matrixU().transpose();
  }

  Eigen::Isometry3d transform{ rotation_matrix.transpose() };
  transform.translation() = target_centroid - rotation_matrix * input_centroid;

  return transform;
}
}  // namespace scanmatcher


#include "RasRandomNumber.h"

#include <chrono> // for seconds, milliseconds, nanoseconds, picoseconds
#include <Eigen/Cholesky>
using namespace Eigen;

unsigned RasRandomNumber::ras_now_nanoseconds(void)
{
    std::chrono::nanoseconds ns = std::chrono::duration_cast< std::chrono::nanoseconds >(std::chrono::system_clock::now().time_since_epoch());
    return unsigned(ns.count() % 100000000)+std::rand();
}

std::vector<std::vector<double> > RasRandomNumber::ras_mvnorm(unsigned long int n, std::vector<double> &mu, std::vector<std::vector<double> > &corr, unsigned seed)
{
    if (seed==0) seed=ras_now_nanoseconds();
    std::default_random_engine generator(seed);
    std::normal_distribution<double> distribution(0.0,1.0);

    // chol is cholesky decomposition, should be square
    int ncol=(int)corr.size();

    MatrixXd A(ncol,ncol);
    for (int i=0; i<ncol; i++)
        for (int j=0; j<ncol; j++)
            A(i,j)=corr[i][j];

    MatrixXd U = A.llt().matrixU();
    std::vector<std::vector<double> > chol(ncol,std::vector<double> (ncol,0) );
    for (int i=0; i<ncol; i++)
        for (int j=0; j<ncol; j++)
            chol[i][j]=U(i,j);

    // initialization
    std::vector<std::vector<double> > ret(n,std::vector<double> (ncol,0));
    std::vector<std::vector<double> > z  (n,std::vector<double> (ncol,0));

    for (unsigned long int i=0; i<n; i++){
        for (int j=0; j<ncol; j++){
            z[i][j] = distribution(generator); // standard normal rv
        }
    }
    std::vector<std::vector<double> > C= RasMatrix::ras_prod_mat(z,chol);
    for (unsigned long int i=0; i<n; i++){
        for (int j=0; j<ncol; j++){
            ret[i][j]=mu[j]+C[i][j];
        }
    }
    return ret;
}

std::vector<int> RasRandomNumber::ras_rpois(unsigned long int n, double lam, unsigned seed)
{
    if (seed==0) seed=ras_now_nanoseconds();
    std::default_random_engine generator(seed);
    std::poisson_distribution<int> distribution(lam);

    std::vector<int> number(n);
    for (unsigned long int i=0; i<n; i++)
        number[i] = distribution(generator);
    return number;
}

int RasRandomNumber::ras_rpois(double lam, unsigned seed)
{
    //    std::default_random_engine generator(time(0));
    if (seed==0) seed=ras_now_nanoseconds();
    std::default_random_engine generator(seed);
    std::poisson_distribution<int> distribution(lam);

    int number = distribution(generator);
    return number;
}

double RasRandomNumber::ras_uniform(unsigned seed)
{
    if (seed==0) seed=ras_now_nanoseconds();
    static std::default_random_engine re(seed);
    static std::uniform_real_distribution<double> Dist(0,1);
    return Dist(re);
}

std::vector<unsigned long int> RasRandomNumber::ras_SampleWithoutReplacement(unsigned long int populationSize, unsigned long int sampleSize, unsigned seed)
{
    if (seed==0) seed=ras_now_nanoseconds();
    static std::default_random_engine re(seed);
    static std::uniform_real_distribution<double> Dist(0,1);

    // Use Knuth's variable names
    std::vector<unsigned long int> samples(sampleSize);
    int n = sampleSize;
    int N = populationSize;

    int t = 0; // total input records dealt with
    int m = 0; // number of items selected so far
    double u;

    while (m < n)
    {
        u = Dist(re); // call a uniform(0,1) random number generator

        if ( (N - t)*u >= n - m )
        {
            t++;
        }
        else
        {
            samples[m] = t;
            t++; m++;
        }
    }
    return samples;
}


#pragma once
// gauss_jordan.hpp: Gauss-Jordan matrix inversion
//
// Copyright (C) 2017-2019 Stillwater Supercomputing, Inc.
//
// This file is part of the HPR-BLAS project, which is released under an MIT Open Source license.

#include <boost/numeric/mtl/mtl.hpp>

namespace sw {
namespace hprblas {

template<typename Matrix>
Matrix GaussJordanInversion(const Matrix& A) {
	using Scalar = typename mtl::Collection<Matrix>::value_type;

	size_t m = A.num_rows();
	size_t n = A.num_cols();
	Matrix a(n, m), inv(m, n);
	a = A; // you need a deep copy
	inv = Scalar(1);

	// Performing elementary operations
	for (unsigned i = 0; i < m; ++i) {
		if (a[i][i] == 0) {
			unsigned c = 1;
			while (a[i + c][i] == 0 && (i + c) < n)	++c;
			if ((i + c) == n) break;

			for (unsigned j = i, k = 0; k < n; ++k) {
				std::swap(a[j][k], a[j + c][k]);
				std::cerr << "TBD" << std::endl; // need to create a permutation matrix
			}
		}
		// transform to diagonal matrix
		for (unsigned j = 0; j < m; j++) {
			if (i != j) {
				Scalar scale = a[j][i] / a[i][i];
				for (unsigned k = 0; k < n; ++k) {
					a[j][k] = a[j][k] - a[i][k] * scale;
					inv[j][k] = inv[j][k] - inv[i][k] * scale;
				}
			}
			//std::cout << i << "," << j << std::endl;
			//sw::hprblas::printMatrix(std::cout, "a", a);
			//sw::hprblas::printMatrix(std::cout, "inv", inv);
		}
	}
	// transform to identity matrix
	for (unsigned i = 0; i < m; ++i) {
		Scalar normalize = a[i][i];
		a[i][i] = Scalar(1);
		for (unsigned j = 0; j < n; ++j) {
			inv[i][j] /= normalize;
		}
	}
	//printMatrix(std::cout, "conversion", a);
	return inv;
}

} // namespace hprblas
} // namespace sw


#include <stan/math/rev/scal.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/scal/fun/nan_util.hpp>
#include <test/unit/math/rev/scal/util.hpp>
#include <boost/math/special_functions/expm1.hpp>

TEST(AgradRev,expm1) {
  AVAR a = 1.3;
  AVAR f = expm1(a);
  EXPECT_FLOAT_EQ(boost::math::expm1(1.3), f.val());

  AVEC x = createAVEC(a);
  VEC grad_f;
  f.grad(x,grad_f);
  EXPECT_FLOAT_EQ(std::exp(1.3), grad_f[0]);
}

struct expm1_fun {
  template <typename T0>
  inline T0
  operator()(const T0& arg1) const {
    return expm1(arg1);
  }
};

TEST(AgradRev,expm1_NaN) {
  expm1_fun expm1_;
  test_nan(expm1_,false,true);
}

TEST(AgradRev, check_varis_on_stack) {
  AVAR a = 1.3;
  test::check_varis_on_stack(expm1(a));
}


///////////////////////////////////////////////////////////////////////////////
//      Copyright Christopher Kormanyos 2012 - 2015, 2020.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)
//

// This example uses Boost.Multiprecision to implement
// a high-precision Gauss-Laguerre quadrature integration.
// The quadrature is used to calculate the airy_ai(x) function
// for real-valued x on the positive axis with x.ge.1.

// In this way, the integral representation could be seen
// as part of a scheme to calculate real-valued Airy functions
// on the positive axis for medium to large argument.
// A Taylor series or hypergeometric function (not part
// of this example) could be used for smaller arguments.

// This example has been tested with decimal digits counts
// ranging from 21...301, by adjusting the parameter
// local::my_digits10 at compile time.

// The quadrature integral representaion of airy_ai(x) used
// in this example can be found in:

// A. Gil, J. Segura, N.M. Temme, "Numerical Methods for Special
// Functions" (SIAM Press 2007), Sect. 5.3.3, in particular Eq. 5.110,
// page 145. Subsequently, Gil et al's book cites the another work:
// W. Gautschi, "Computation of Bessel and Airy functions and of
// related Gaussian quadrature formulae", BIT, 42 (2002), pp. 110-118.

#include <cmath>
#include <cstdint>
#include <functional>
#include <iomanip>
#include <iostream>
#include <numeric>
#include <sstream>
#include <tuple>
#include <vector>

#include <boost/cstdfloat.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/cbrt.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/noncopyable.hpp>

namespace gauss { namespace laguerre {

namespace util {

void progress_bar(std::ostream& os, const float percent);

void progress_bar(std::ostream& os, const float percent)
{
  std::stringstream strstrm;

  strstrm.precision(1);

  strstrm << std::fixed << percent << "%";

  os << strstrm.str() << "\r";

  os.flush();
}

}

namespace detail
{
  template<typename T>
  class laguerre_l_object BOOST_FINAL
  {
  public:
    laguerre_l_object(const int n, const T a) noexcept
      : order(n),
        alpha(a),
        p1   (0),
        d2   (0) { }

    laguerre_l_object& operator=(const laguerre_l_object& other)
    {
      if(this != other)
      {
        order = other.order;
        alpha = other.alpha;
        p1    = other.p1;
        d2    = other.d2;
      }

      return *this;
    }

    T operator()(const T& x) const noexcept
    {
      // Calculate (via forward recursion):
      // * the value of the Laguerre function L(n, alpha, x), called (p2),
      // * the value of the derivative of the Laguerre function (d2),
      // * and the value of the corresponding Laguerre function of
      //   previous order (p1).

      // Return the value of the function (p2) in order to be used as a
      // function object with Boost.Math root-finding. Store the values
      // of the Laguerre function derivative (d2) and the Laguerre function
      // of previous order (p1) in class members for later use.

        p1 = T(0);
      T p2 = T(1);
        d2 = T(0);

      T j_plus_alpha = alpha;
      T two_j_plus_one_plus_alpha_minus_x = (1 + alpha) - x;

      const T my_two = 2;

      for(int j = 0; j < order; ++j)
      {
        const T p0(p1);

        // Set the value of the previous Laguerre function.
        p1 = p2;

        // Use a recurrence relation to compute the value of the Laguerre function.
        p2 = ((two_j_plus_one_plus_alpha_minus_x * p1) - (j_plus_alpha * p0)) / (j + 1);

        ++j_plus_alpha;
        two_j_plus_one_plus_alpha_minus_x += my_two;
      }

      // Set the value of the derivative of the Laguerre function.
      d2 = ((p2 * order) - (j_plus_alpha * p1)) / x;

      // Return the value of the Laguerre function.
      return p2;
    }

    const T previous  () const noexcept { return p1; }
    const T derivative() const noexcept { return d2; }

    static bool root_tolerance(const T& a, const T& b) noexcept
    {
      using std::fabs;

      // The relative tolerance here is: ((a - b) * 2) / (a + b).
      return ((fabs(a - b) * 2) < (fabs(a + b) * std::numeric_limits<T>::epsilon()));
    }

  private:
    const   int order;
    const   T   alpha;
    mutable T   p1;
    mutable T   d2;
  };

  template<typename T>
  class abscissas_and_weights : private boost::noncopyable
  {
  public:
    abscissas_and_weights(const int n, const T a) : order(n),
                                                    alpha(a),
                                                    xi   (),
                                                    wi   ()
    {
      if(alpha < -20.0F)
      {
        // Users can decide to perform different range checking.
        std::cout << "Range error: the order of the Laguerre function must exceed -20.0."
                  << std::endl;
      }
      else
      {
        calculate();
      }
    }

    const std::vector<T>& abscissa_n() const noexcept { return xi; }
    const std::vector<T>& weight_n  () const noexcept { return wi; }

  private:
    const int order;
    const T   alpha;

    std::vector<T> xi;
    std::vector<T> wi;

    abscissas_and_weights() : order(),
                              alpha(),
                              xi   (),
                              wi   () { }

    void calculate()
    {
      using std::abs;

      std::cout << "Finding the approximate roots..." << std::endl;

      std::vector<std::tuple<T, T>> root_estimates;

      root_estimates.reserve(static_cast<typename std::vector<std::tuple<T, T>>::size_type>(order));

      const laguerre_l_object<T> laguerre_root_object(order, alpha);

      // Set the initial values of the step size and the running step
      // to be used for finding the estimate of the first root.
      T step_size  = 0.01F;
      T step       = step_size;

      T first_laguerre_root = 0.0F;

      if(alpha < -1.0F)
      {
        // Iteratively step through the Laguerre function using a
        // small step-size in order to find a rough estimate of
        // the first zero.

        const bool this_laguerre_value_is_negative = (laguerre_root_object(T(0)) < 0);

        constexpr int j_max = 10000;

        int j = 0;

        while((j < j_max) && (this_laguerre_value_is_negative != (laguerre_root_object(step) < 0)))
        {
          // Increment the step size until the sign of the Laguerre function
          // switches. This indicates a zero-crossing, signalling the next root.
          step += step_size;

          ++j;
        }
      }
      else
      {
        // Calculate an estimate of the 1st root of a generalized Laguerre
        // function using either a Taylor series or an expansion in Bessel
        // function zeros. The Bessel function zeros expansion is from Tricomi.

        // Here, we obtain an estimate of the first zero of cyl_bessel_j(alpha, x).

        T j_alpha_m1;

        if(alpha < 1.4F)
        {
          // For small alpha, use a short series obtained from Mathematica(R).
          // Series[BesselJZero[v, 1], {v, 0, 3}]
          // N[%, 12]
          j_alpha_m1 = (((          T(0.09748661784476F)
                          * alpha - T(0.17549359276115F))
                          * alpha + T(1.54288974259931F))
                          * alpha + T(2.40482555769577F));
        }
        else
        {
          // For larger alpha, use the first line of Eqs. 10.21.40 in the NIST Handbook.
          const T alpha_pow_third(boost::math::cbrt(alpha));
          const T alpha_pow_minus_two_thirds(T(1) / (alpha_pow_third * alpha_pow_third));

          j_alpha_m1 = alpha * (((((                             + T(0.043F)
                                    * alpha_pow_minus_two_thirds - T(0.0908F))
                                    * alpha_pow_minus_two_thirds - T(0.00397F))
                                    * alpha_pow_minus_two_thirds + T(1.033150F))
                                    * alpha_pow_minus_two_thirds + T(1.8557571F))
                                    * alpha_pow_minus_two_thirds + T(1.0F));
        }

        const T vf             = (  T(order * 4U)
                                  + T(alpha * 2U)
                                  + T(2U));
        const T vf2            = vf * vf;
        const T j_alpha_m1_sqr = j_alpha_m1 * j_alpha_m1;

        first_laguerre_root = (j_alpha_m1_sqr * (   -T(0.6666666666667F)
                                                 + ((T(0.6666666666667F) * alpha) * alpha)
                                                 +  (T(0.3333333333333F) * j_alpha_m1_sqr) + vf2)) / (vf2 * vf);
      }

      bool this_laguerre_value_is_negative = (laguerre_root_object(T(0)) < 0);

      // Re-set the initial value of the step-size based on the
      // estimate of the first root.
      step_size = first_laguerre_root / 2;
      step      = step_size;

      // Step through the Laguerre function using a step-size
      // of dynamic width in order to find the zero crossings
      // of the Laguerre function, providing rough estimates
      // of the roots. Refine the brackets with a few bisection
      // steps, and store the results as bracketed root estimates.

      while(root_estimates.size() < static_cast<std::size_t>(order))
      {
        // Increment the step size until the sign of the Laguerre function
        // switches. This indicates a zero-crossing, signalling the next root.
        step += step_size;

        if(this_laguerre_value_is_negative != (laguerre_root_object(step) < 0))
        {
          // We have found the next zero-crossing.

          // Change the running sign of the Laguerre function.
          this_laguerre_value_is_negative = (!this_laguerre_value_is_negative);

          // We have found the first zero-crossing. Put a loose bracket around
          // the root using a window. Here, we know that the first root lies
          // between (x - step_size) < root < x.

          // Before storing the approximate root, perform a couple of
          // bisection steps in order to tighten up the root bracket.
          std::uintmax_t a_couple_of_iterations = 4U;

          const std::pair<T, T>
            root_estimate_bracket = boost::math::tools::bisect(laguerre_root_object,
                                                               step - step_size,
                                                               step,
                                                               laguerre_l_object<T>::root_tolerance,
                                                               a_couple_of_iterations);

          static_cast<void>(a_couple_of_iterations);

          // Store the refined root estimate as a bracketed range in a tuple.
          root_estimates.push_back(std::tuple<T, T>(std::get<0>(root_estimate_bracket),
                                                    std::get<1>(root_estimate_bracket)));

          if(    (root_estimates.size() == 1U)
             || ((root_estimates.size() % 8U) == 0U)
             ||  (root_estimates.size() == static_cast<std::size_t>(order)))
          {
            const float progress = (100.0F * static_cast<float>(root_estimates.size())) / static_cast<float>(order);

            std::cout << "root_estimates.size(): "
                      << root_estimates.size()
                      << ", "
                      ;

            util::progress_bar(std::cout, progress);
          }

          if(root_estimates.size() >= static_cast<std::size_t>(2U))
          {
            // Determine the next step size. This is based on the distance between
            // the previous two roots, whereby the estimates of the previous roots
            // are computed by taking the average of the lower and upper range of
            // the root-estimate bracket.

            const T r0 = (  std::get<0>(*(root_estimates.crbegin() + 1U))
                          + std::get<1>(*(root_estimates.crbegin() + 1U))) / 2;

            const T r1 = (  std::get<0>(*root_estimates.crbegin())
                          + std::get<1>(*root_estimates.crbegin())) / 2;

            const T distance_between_previous_roots = r1 - r0;

            step_size = distance_between_previous_roots / 3;
          }
        }
      }

      const T norm_g =
        ((alpha == 0) ? T(-1)
                      : -boost::math::tgamma(alpha + order) / boost::math::factorial<T>(order - 1));

      xi.reserve(root_estimates.size());
      wi.reserve(root_estimates.size());

      std::cout << std::endl;

      // Calculate the abscissas and weights to full precision.
      for(std::size_t i = static_cast<std::size_t>(0U); i < root_estimates.size(); ++i)
      {
        if(   ((i % 8U) == 0U)
           || ( i == root_estimates.size() - 1U))
        {
          const float progress = (100.0F * static_cast<float>(i + 1U)) / static_cast<float>(root_estimates.size());

          std::cout << "Calculating abscissas and weights. Processed "
                    << (i + 1U)
                    << ", "
                    ;

          util::progress_bar(std::cout, progress);
        }

        // Calculate the abscissas using iterative root-finding.

        // Select the maximum allowed iterations, being at least 20.
        // The determination of the maximum allowed iterations is
        // based on the number of decimal digits in the numerical
        // type T.
        constexpr int local_math_tools_digits10 =
          static_cast<int>(static_cast<boost::float_least32_t>(boost::math::tools::digits<T>()) * BOOST_FLOAT32_C(0.301));

        const std::uintmax_t number_of_iterations_allowed = (std::max)(20, local_math_tools_digits10 / 2);

        std::uintmax_t number_of_iterations_used = number_of_iterations_allowed;

        // Perform the root-finding using ACM TOMS 748 from Boost.Math.
        const std::pair<T, T>
          laguerre_root_bracket = boost::math::tools::toms748_solve(laguerre_root_object,
                                                                    std::get<0>(root_estimates.at(i)),
                                                                    std::get<1>(root_estimates.at(i)),
                                                                    laguerre_l_object<T>::root_tolerance,
                                                                    number_of_iterations_used);

        static_cast<void>(number_of_iterations_used);

        // Compute the Laguerre root as the average of the values from
        // the solved root bracket.
        const T laguerre_root = (  std::get<0>(laguerre_root_bracket)
                                 + std::get<1>(laguerre_root_bracket)) / 2;

        // Calculate the weight for this Laguerre root. Here, we calculate
        // the derivative of the Laguerre function and the value of the
        // previous Laguerre function on the x-axis at the value of this
        // Laguerre root.
        static_cast<void>(laguerre_root_object(laguerre_root));

        // Store the abscissa and weight for this index.
        xi.push_back(laguerre_root);
        wi.push_back(norm_g / ((laguerre_root_object.derivative() * order) * laguerre_root_object.previous()));
      }

      std::cout << std::endl;
    }
  };

  template<typename T>
  struct airy_ai_object BOOST_FINAL
  {
  public:
    airy_ai_object(const T& x)
      : my_x     (x),
        my_zeta  (((sqrt(x) * x) * 2) / 3),
        my_factor(make_factor(my_zeta)) { }

    T operator()(const T& t) const
    {
      using std::sqrt;

      return my_factor / sqrt(boost::math::cbrt(2 + (t / my_zeta)));
    }

  private:
    const T my_x;
    const T my_zeta;
    const T my_factor;

    airy_ai_object() : my_x     (),
                       my_zeta  (),
                       my_factor() { }

    static T make_factor(const T& z)
    {
      using std::exp;
      using std::sqrt;

      return 1 / ((sqrt(boost::math::constants::pi<T>()) * sqrt(boost::math::cbrt(z * 48))) * (exp(z) * boost::math::tgamma(T(5) / 6)));
    }
  };
} // namespace detail

} } // namespace gauss::laguerre

// A float_type is created to handle the desired number of decimal digits from `cpp_dec_float` without using __expression_templates.
struct local
{
  static constexpr unsigned int my_digits10 = 101U;

  typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<my_digits10>,
                                        boost::multiprecision::et_off>
  float_type;
};

static_assert(local::my_digits10 > 20U,
                        "Error: This example is intended to have more than 20 decimal digits");

int main()
{
  // Use Gauss-Laguerre quadrature integration to compute airy_ai(x / 7)
  // with 7 <= x <= 120 and where x is incremented in steps of 1.

  // During development of this example, we have empirically found
  // the numbers of Gauss-Laguerre coefficients needed for convergence
  // when using various counts of base-10 digits.

  // Let's calibrate, for instance, the number of coefficients needed
  // at the point x = 1.

  // Empirical data lead to:
  // Fit[{{21.0, 3.5}, {51.0, 11.1}, {101.0, 22.5}, {201.0, 46.8}}, {1, d, d^2}, d]
  // FullSimplify[%]
  // -1.28301 + (0.235487 + 0.0000178915 d) d

  // We need significantly more coefficients at smaller real values than are needed
  // at larger real values because the slope derivative of airy_ai(x) gets more
  // steep as x approaches zero.

  // This Gauss-Laguerre quadrature is designed for airy_ai(x) with real-valued x >= 1.

  constexpr boost::float_least32_t d = static_cast<boost::float_least32_t>(std::numeric_limits<local::float_type>::digits10);

  constexpr boost::float_least32_t laguerre_order_factor = -1.28301F + ((0.235487F + (0.0000178915F * d)) * d);

  constexpr int laguerre_order = static_cast<int>(laguerre_order_factor * d);

  std::cout << "std::numeric_limits<local::float_type>::digits10: " << std::numeric_limits<local::float_type>::digits10 << std::endl;
  std::cout << "laguerre_order: " << laguerre_order << std::endl;

  typedef gauss::laguerre::detail::abscissas_and_weights<local::float_type> abscissas_and_weights_type;

  const abscissas_and_weights_type the_abscissas_and_weights(laguerre_order, local::float_type(-1) / 6);

  bool result_is_ok = true;

  for(std::uint32_t u = 7U; u < 121U; ++u)
  {
    const local::float_type x = local::float_type(u) / 7;

    typedef gauss::laguerre::detail::airy_ai_object<local::float_type> airy_ai_object_type;

    const airy_ai_object_type the_airy_ai_object(x);

    const local::float_type airy_ai_value =
      std::inner_product(the_abscissas_and_weights.abscissa_n().cbegin(),
                         the_abscissas_and_weights.abscissa_n().cend(),
                         the_abscissas_and_weights.weight_n().cbegin(),
                         local::float_type(0U),
                         std::plus<local::float_type>(),
                         [&the_airy_ai_object](const local::float_type& this_abscissa,
                                               const local::float_type& this_weight) -> local::float_type
                         {
                           return the_airy_ai_object(this_abscissa) * this_weight;
                         });

    static const local::float_type one_third = 1.0F / local::float_type(3U);

    static const local::float_type one_over_pi_times_one_over_sqrt_three =
      sqrt(one_third) * boost::math::constants::one_div_pi<local::float_type>();

    const local::float_type sqrt_x = sqrt(x);

    const local::float_type airy_ai_control =
       (sqrt_x * one_over_pi_times_one_over_sqrt_three)
      * boost::math::cyl_bessel_k(one_third, ((2.0F * x) * sqrt_x) * one_third);

    std::cout << std::setprecision(std::numeric_limits<local::float_type>::digits10)
              << "airy_ai_value  : "
              << airy_ai_value
              << std::endl;

    std::cout << std::setprecision(std::numeric_limits<local::float_type>::digits10)
              << "airy_ai_control: "
              << airy_ai_control
              << std::endl;

    const local::float_type delta = fabs(1.0F - (airy_ai_control / airy_ai_value));

    static const local::float_type tol("1E-" + boost::lexical_cast<std::string>(std::numeric_limits<local::float_type>::digits10 - 7U));

    result_is_ok &= (delta < tol);
  }

  std::cout << std::endl
            << "Total... result_is_ok: "
            << std::boolalpha
            << result_is_ok
            << std::endl;
} // int main()

/*

Partial output:

//[gauss_laguerre_quadrature_output_1

std::numeric_limits<local::float_type>::digits10: 101
laguerre_order: 2291

Finding the approximate roots...
root_estimates.size(): 1, 0.0%
root_estimates.size(): 8, 0.3%
root_estimates.size(): 16, 0.7%
...
root_estimates.size(): 2288, 99.9%
root_estimates.size(): 2291, 100.0%

Calculating abscissas and weights. Processed 1, 0.0%
Calculating abscissas and weights. Processed 9, 0.4%
...
Calculating abscissas and weights. Processed 2289, 99.9%
Calculating abscissas and weights. Processed 2291, 100.0%
//] [/gauss_laguerre_quadrature_output_1]

//[gauss_laguerre_quadrature_output_2

airy_ai_value  : 0.13529241631288141552414742351546630617494414298833070600910205475763353480226572366348710990874867334
airy_ai_control: 0.13529241631288141552414742351546630617494414298833070600910205475763353480226572366348710990874868323
airy_ai_value  : 0.11392302126009621102904231059693500086750049240884734708541630001378825889924647699516200868335286103
airy_ai_control: 0.1139230212600962110290423105969350008675004924088473470854163000137882588992464769951620086833528582
...
airy_ai_value  : 3.8990420982303275013276114626640705170145070824317976771461533035231088620152288641360519429331427451e-22
airy_ai_control: 3.8990420982303275013276114626640705170145070824317976771461533035231088620152288641360519429331426481e-22

Total... result_is_ok: true

//] [/gauss_laguerre_quadrature_output_2]

*/


//
// Created by Amir Masoud Abdol on 2020-04-11
//

#include "TestStrategy.h"

#include <boost/math/distributions/fisher_f.hpp>

using namespace sam;
using boost::math::fisher_f;

void FTest::run(Experiment *experiment) {

  static ResultType res;

  // The first group is always the control group
  for (int i{experiment->setup.nd()}, d{0}; i < experiment->setup.ng();
       ++i, ++d %= experiment->setup.nd()) {

    res =
        f_test((*experiment)[d].stddev_, (*experiment)[d].nobs_,
               (*experiment)[i].stddev_, (*experiment)[i].nobs_, params.alpha);

    (*experiment)[i].stats_ = res.fstat;
    (*experiment)[i].pvalue_ = res.pvalue;
    (*experiment)[i].sig_ = res.sig;
  }
}

FTest::ResultType FTest::f_test(float Sd1,   // Sample 1 std deviation
                                unsigned Sn1, // Sample 1 size
                                float Sd2,   // Sample 2 std deviation
                                unsigned Sn2, // Sample 2 size
                                float alpha) // Significance level
{
  // F-statistic:
  double f_stats = (Sd1 / Sd2);

  //
  // Finally define our distribution, and get the probability:
  //
  fisher_f dist(Sn1 - 1, Sn2 - 1);
  double p = cdf(dist, f_stats);

  double ucv = quantile(complement(dist, alpha));
  double ucv2 = quantile(complement(dist, alpha / 2));
  double lcv = quantile(dist, alpha);
  double lcv2 = quantile(dist, alpha / 2);

  //
  // Finally print out results of null and alternative hypothesis:
  //
  if ((ucv2 < f_stats) || (lcv2 > f_stats)) { // Alternative "NOT REJECTED"
    return {.fstat = static_cast<float>(f_stats), .df1 = Sn1 - 1, .df2 = Sn2 - 1, .pvalue = static_cast<float>(p), .sig = true};
  }

  if (lcv > f_stats) {// Alternative "NOT REJECTED"
    return {.fstat = static_cast<float>(f_stats), .df1 = Sn1 - 1, .df2 = Sn2 - 1, .pvalue = static_cast<float>(p), .sig = true};
  }

  if (ucv < f_stats) { // Alternative "NOT REJECTED"
    return {.fstat = static_cast<float>(f_stats), .df1 = Sn1 - 1, .df2 = Sn2 - 1, .pvalue = static_cast<float>(p), .sig = true};
  }

  // Alternative "Rejected"
  return {.fstat = static_cast<float>(f_stats), .df1 = Sn1 - 1, .df2 = Sn2 - 1, .pvalue = static_cast<float>(p), .sig = false};

}


#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 "Kernel.h"
#include <armadillo>
#include <assert.h>
#include <iostream>
#include <math.h>

using namespace arma;

Kernel::Kernel(KernelType type) : kernelType(type) {
  if (this->kernelType == LINEAR) {
    KernelFunction = &Kernel::KernelLinear;
  } else if (this->kernelType == RBF) {
    KernelFunction = &Kernel::KernelRBF;
  } /* else if (this->kernelType == POLY) {
     KernelFunction = &Kernel::KernelPoly;
   } else if (this->kernelType == SIGMOID) {
     KernelFunction = &Kernel::kernel_sigmoid;
   } else if (this->kernelType == KernelSigmoid) {
     KernelFunction = &Kernel::kernel_precomputed;
   }*/
}

/*
double Kernel::KernelLinear(int i1, int i2) {
  return this->KernelLinear(this->x.rows(i1).t(),this->x.rows(i2).t()); }
double Kernel::KernelRBF(vec x, vec y) {
  return this->KernelRBF(this->x.rows(i1).t(),this->x.rows(i2).t());
}
*/
double Kernel::KernelLinear(vec &x1, vec &x2) const {
  return arma::dot(x1, x2);
}
double Kernel::KernelRBF(vec &x1, vec &x2) const {
  double temp =norm(x1 - x2);
  return exp(-this->gamma * pow(temp,2.0));
}


#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();
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd X = MatrixXd::Random(5,5);
MatrixXd A = X + X.transpose();
cout << "Here is a random symmetric 5x5 matrix:" << endl << A << endl << endl;
Tridiagonalization<MatrixXd> triOfA(A);
MatrixXd Q = triOfA.matrixQ();
cout << "The orthogonal matrix Q is:" << endl << Q << endl;
MatrixXd T = triOfA.matrixT();
cout << "The tridiagonal matrix T is:" << endl << T << endl << endl;
cout << "Q * T * Q^T = " << endl << Q * T * Q.transpose() << endl;

  return 0;
}


// 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;
}