// TRENTO: Reduced Thickness Event-by-event Nuclear Topology
// Copyright 2015 Jonah E. Bernhard, J. Scott Moreland
// TRENTO3D: Three-dimensional extension of TRENTO by Weiyao Ke
// MIT License

#include "nucleon.h"

#include <cmath>
#include <limits>
#include <random>
#include <stdexcept>

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/expint.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/program_options/variables_map.hpp>

#include "fwd_decl.h"

namespace trento {

namespace {

// Create ctor parameters for unit mean std::gamma_distribution.
//   mean = alpha*beta == 1  ->  beta = 1/alpha
// Used below in NucleonProfile ctor initializer list.

template <typename RealType> using param_type =
  typename std::gamma_distribution<RealType>::param_type;

template <typename RealType>
param_type<RealType> gamma_param_unit_mean(RealType alpha = 1.) {
  return param_type<RealType>{alpha, 1./alpha};
}

// These constants define distances in terms of the width of the nucleon profile
// Gaussian thickness function.

// Truncation radius of the thickness function.
constexpr double trunc_radius_widths = 5.;

// Maximum impact parameter for participation.
constexpr double max_impact_widths = 6.;

// Trivial helper function.
template <typename T>
constexpr T sqr(T value) {
  return value * value;
}

// Inelastic nucleon-nucleon cross section as function of beam energy sqrt(s)
// Fit coefficients explained in the docs.
double cross_sec_from_energy(double sqrts) {
  auto a = 3.1253;
  auto b = 0.1280;
  auto c = 2.0412;
  auto d = 1.8231;
  return a + b * pow(std::log(sqrts) - c, d);
}

// Determine the cross section parameter for sampling participants.
// See section "Fitting the cross section" in the online docs.
double compute_cross_sec_param(const VarMap& var_map) {
  // Read parameters from the configuration.

  // Use manual inelastic nucleon-nucleon cross section if specified.
  // Otherwise default to extrapolated cross section.
  auto sigma_nn = var_map["cross-section"].as<double>();
  if (sigma_nn < 0) {
    sigma_nn = cross_sec_from_energy(var_map["beam-energy"].as<double>());
  }
  auto width = var_map["nucleon-width"].as<double>();

  // Initialize arguments for boost root finding function.

  // Bracket min and max.
  auto a = -10.;
  auto b = 20.;

  // Tolerance function.
  // Require 3/4 of double precision.
  math::tools::eps_tolerance<double> tol{
    (std::numeric_limits<double>::digits * 3) / 4};

  // Maximum iterations.
  // This is overkill -- in testing only 10-20 iterations were required
  // (but no harm in overestimating).
  boost::uintmax_t max_iter = 1000;

  // The right-hand side of the equation.
  auto rhs = sigma_nn / (4 * math::double_constants::pi * sqr(width));

  // This quantity appears a couple times in the equation.
  auto c = sqr(max_impact_widths) / 4;

  try {
    auto result = math::tools::toms748_solve(
      [&rhs, &c](double x) {
        using std::exp;
        using math::expint;
        return c - expint(-exp(x)) + expint(-exp(x-c)) - rhs;
      },
      a, b, tol, max_iter);

    return .5*(result.first + result.second);
  }
  catch (const std::domain_error&) {
    // Root finding fails for very small nucleon widths, w^2/sigma_nn < ~0.01.
    throw std::domain_error{
      "unable to fit cross section -- nucleon width too small?"};
  }
}

}  // unnamed namespace

NucleonProfile::NucleonProfile(const VarMap& var_map)
    : width_sqr_(sqr(var_map["nucleon-width"].as<double>())),
      trunc_radius_sqr_(sqr(trunc_radius_widths)*width_sqr_),
      max_impact_sqr_(sqr(max_impact_widths)*width_sqr_),
      neg_one_div_two_width_sqr_(-.5/width_sqr_),
	  neg_one_div_four_width_sqr_(-.25/width_sqr_),
	  one_div_four_pi_(0.5*math::double_constants::one_div_two_pi),
      cross_sec_param_(compute_cross_sec_param(var_map)),
      fast_exp_(-.5*sqr(trunc_radius_widths), 0., 1000),
      fluct_dist_(gamma_param_unit_mean(var_map["fluctuation"].as<double>())),
      prefactor_(math::double_constants::one_div_two_pi/width_sqr_),
      with_ncoll_(var_map["ncoll"].as<bool>())
{}

}  // namespace trento


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EVecPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <gtest/gtest.h>

#include <Eigen/Core>

#include "smooth/feedback/ocp.hpp"
#include "smooth/feedback/ocp_to_qp.hpp"

template<typename T>
using X = Eigen::Vector<T, 2>;

template<typename T>
using U = Eigen::Vector<T, 1>;

template<typename T, std::size_t N>
using Vec = Eigen::Vector<T, N>;

TEST(OcpToQp, Basic)
{
  const auto theta = []<typename T>(T, X<T>, X<T> xf, Vec<T, 1> q) -> T {
    return xf.squaredNorm() + 2 * q.sum();
  };

  const auto f = []<typename T>(T, X<T> x, U<T> u) -> smooth::Tangent<X<T>> {
    return {x.y(), u.x()};
  };

  const auto g = []<typename T>(T, X<T>, U<T> u) -> Vec<T, 1> {
    return Vec<T, 1>{{u.x() * u.x()}};
  };

  const auto cr = []<typename T>(T, X<T>, U<T> u) -> Vec<T, 1> { return Vec<T, 1>{{u.x()}}; };

  const auto ce = []<typename T>(T, X<T>, X<T> xf, Vec<T, 1>) -> Vec<T, 2> { return xf; };

  smooth::feedback::
    OCP<X<double>, U<double>, decltype(theta), decltype(f), decltype(g), decltype(cr), decltype(ce)>
      ocp{
        .theta = theta,
        .f     = f,
        .g     = g,
        .cr    = cr,
        .crl   = Eigen::VectorXd{{-1}},
        .cru   = Eigen::VectorXd{{1}},
        .ce    = ce,
        .cel   = Eigen::Vector2d{-5, -5},
        .ceu   = Eigen::Vector2d{5, 5},
      };

  smooth::feedback::Mesh<5, 5> mesh;
  mesh.refine_ph(0, 10);

  constexpr auto tf = 2.;

  const auto xl_fun = []<typename T>(T t) -> X<T> { return X<T>{{0.05 * t * t, 0.1 * t}}; };

  const auto ul_fun = []<typename T>(T) -> U<T> { return U<T>{{0.1}}; };

  const auto qp = smooth::feedback::ocp_to_qp(ocp, mesh, tf, xl_fun, ul_fun);

  ASSERT_EQ(qp.P.cols(), qp.q.size());
  ASSERT_EQ(qp.P.rows(), qp.q.size());
  ASSERT_EQ(qp.P.cols(), qp.A.cols());

  ASSERT_EQ(qp.A.rows(), qp.l.size());
  ASSERT_EQ(qp.A.rows(), qp.u.size());

  // check that simple trajectory satisfies constraints

  static constexpr double x0 = 3;
  static constexpr double v0 = -0.3;
  static constexpr double u0 = 0.1;

  const auto xtraj = [](double t) {
    return X<double>{{x0 + v0 * t + u0 * t * t / 2, v0 + u0 * t}};
  };

  Eigen::Matrix<double, 2, -1> Xvar(ocp.Nx, mesh.N_colloc() + 1);
  Eigen::Matrix<double, 1, -1> Uvar(ocp.Nu, mesh.N_colloc());

  for (const auto [i, t] : smooth::utils::zip(std::views::iota(0u), mesh.all_nodes())) {
    Xvar.col(i) = xtraj(tf * t);
    if (i < mesh.N_colloc()) { Uvar.col(i).setConstant(u0); }
  }

  Eigen::VectorXd var(Xvar.size() + Uvar.size());
  var.head(Xvar.size()) = Xvar.reshaped();
  var.tail(Uvar.size()) = Uvar.reshaped();

  // check constraint satisfaction
  ASSERT_GE((qp.A * var - qp.l).minCoeff(), -1e-8);
  ASSERT_GE((qp.u - qp.A * var).minCoeff(), -1e-8);
}


#include <boost/math/common_factor_rt.hpp>


#pragma once

#include <Eigen/Dense>

namespace icarus {
    template <typename T>
    struct EllipsoidalCalibration
    {
        explicit EllipsoidalCalibration() :
            mTransformation(Eigen::Matrix<float, 3, 4>::Identity())
        {}

        explicit EllipsoidalCalibration(Eigen::Matrix<T, 3, 4> const & transformation) :
            mTransformation(transformation)
        {}

        void transformAxes(Eigen::Matrix<T, 3, 3> const & transform)
        {
            mTransformation.template block<3, 3>(0, 0) = transform * mTransformation.template block<3, 3>(0, 0);
        }

        void addOffset(Eigen::Matrix<T, 3, 1> const & offset)
        {
            mTransformation.col(3) += offset;
        }

        Eigen::Matrix<T, 3, 1> adjust(Eigen::Matrix<T, 3, 1> const & measurement) const
        {
            return mTransformation.template block<3, 3>(0, 0)
                * (measurement + mTransformation.col(3));
        }

        Eigen::Matrix<T, 3, 4> const & transformation() const
        {
            return mTransformation;
        }
    private:
        Eigen::Matrix<T, 3, 4> mTransformation;
    };
}


/** \file ITL_Solver.h */

#pragma once

// MTL4 includes
#include <boost/numeric/itl/krylov/bicg.hpp>
#include <boost/numeric/itl/krylov/bicgstab_2.hpp>
#include <boost/numeric/itl/krylov/bicgstab_ell.hpp>
#include <boost/numeric/itl/krylov/bicgstab.hpp>
#include <boost/numeric/itl/krylov/cg.hpp>
#include <boost/numeric/itl/krylov/cgs.hpp>
#include <boost/numeric/itl/krylov/gmres.hpp>
#include <boost/numeric/itl/krylov/idr_s.hpp>
#include <boost/numeric/itl/krylov/qmr.hpp>
#include <boost/numeric/itl/krylov/tfqmr.hpp>

// AMDiS includes
#include "MTL4Types.hpp"
#include "solver/LinearSolver.hpp"
#include "solver/ITL_Runner.hpp"

// more solvers defined in AMDiS
#include "solver/itl/minres.hpp"
#include "solver/itl/gcr.hpp"
#include "solver/itl/fgmres.hpp"
#include "solver/itl/fgmres_householder.hpp"
#include "solver/itl/gmres2.hpp"
#include "solver/itl/gmres_householder.hpp"
#include "solver/itl/preonly.hpp"

namespace AMDiS
{
  /**
   * \ingroup Solver
   *
   * \brief
   * Wrapper for MTL4 itl-solvers.
   *
   * One of the following solvers can be chosen:
   * - @ref CGSolver "cg" (conjugate gradient method)
   * - @ref CGSSolver "cgs" (squared conjugate gradient method)
   * - @ref BiCGSolver "bicg" (biconjugate gradient method)
   * - @ref BiCGStabSolver "bicgstab" (stabilized BiCG method)
   * - @ref BiCGStab2Solver "bicgstab2" (stabilized BiCG(l) method with l=2)
   * - @ref QMRSolver "qmr" (Quasi-Minimal Residual method)
   * - @ref TFQMRSolver "tfqmr" (Transposed-Free Quasi-Minimal Residual method)
   * - @ref BiCGStabEllSolver "bicgstab_ell" (stabilized BiCG(l) method)
   * - @ref GMResSolver "gmres" (generalized minimal residual method)
   * - @ref IDRsSolver "idr_s" (Induced Dimension Reduction method)
   * - @ref MinResSolver "minres" (minimal residual method)
   * - @ref GcrSolver "gcr" (generalized conjugate residual method)
   * - @ref FGMResSolver "fgmres" (flexible GMRes method)
   * - @ref PreOnly "preonly" (solver that implements pure preconditioning applied to the rhs)
   */
  template <class SolverType>
  using ITL_Solver = LinearSolver<
    MTLTypes::MTLMatrix,
    MTLTypes::MTLVector,
    ITL_Runner<SolverType, MTLTypes::MTLMatrix, MTLTypes::MTLVector>>;

  // ===========================================================================

  /**
   * \ingroup Solver
   * \class AMDiS::CGSolver
   * \brief ITL_Solver <\ref cg_solver_type> implementation of conjugate gradient
   * method \implements ITL_Solver
   *
   * Solves a linear system \f$ Ax=b \f$ by the conjugate gradient method (CG)
   * and can be used for symmetric positive definite system matrices.
   * Right preconditioner is ignored.
   */

  class cg_solver_type
  {
  public:
    cg_solver_type(std::string /*name*/) {}
    template <class LinOp, class X, class B, class L, class R, class I>
    int operator()(LinOp const& A, X& x, B const& b, L const& l, R const& r, I& iter)
    {
      return itl::cg(A, x, b, l, r, iter);
    }
  };
  using CGSolver = ITL_Solver<cg_solver_type>;

  // ===========================================================================

  /**
   * \ingroup Solver
   * \class AMDiS::CGSSolver
   * \brief ITL_Solver <\ref cgs_solver_type> implementation of squared conjugate
   * gradient method \implements ITL_Solver
   *
   * Solves a linear system \f$ Ax=b \f$ by the squared conjugate gradient method
   * (CGS). Right preconditioner is ignored.
   */

  class cgs_solver_type
  {
  public:
    cgs_solver_type(std::string /*name*/) {}
    template <class LinOp, class X, class B, class L, class R, class I>
    int operator()(LinOp const& A, X& x, B const& b, L const& l, R const&, I& iter)
    {
      return itl::cgs(A, x, b, l, iter);
    }
  };
  using CGSSolver = ITL_Solver<cgs_solver_type>;

  // ===========================================================================

  /**
   * \ingroup Solver
   * \class AMDiS::BiCGSolver
   * \brief ITL_Solver <\ref bicg_solver_type> implementation of bi-conjugate
   * gradient method \implements ITL_Solver
   *
   * Solves a linear system \f$ Ax=b \f$ by a BiCG method and can be used for
   * system matrices.
   */

  class bicg_solver_type
  {
  public:
    bicg_solver_type(std::string /*name*/) {}
    template <class LinOp, class X, class B, class L, class R, class I>
    int operator()(LinOp const& A, X& x, B const& b, L const& l, R const&, I& iter)
    {
      return itl::bicg(A, x, b, l, iter);
    }
  };
  using BiCGSolver = ITL_Solver<bicg_solver_type>;

  // ===========================================================================

  /**
   * \ingroup Solver
   * \class AMDiS::BiCGStabSolver
   * \brief ITL_Solver <\ref bicgstab_type> implementation of stabilized
   * bi-conjugate gradient method \implements ITL_Solver
   *
   * Solves a linear system \f$ Ax=b \f$ by a stabilized BiCG method and can be
   * used for system matrices.
   */

  class bicgstab_type
  {
  public:
    bicgstab_type(std::string /*name*/) {}
    template <class LinOp, class X, class B, class L, class R, class I>
    int operator()(LinOp const& A, X& x, B const& b, L const& l, R const&, I& iter)
    {
      return itl::bicgstab(A, x, b, l, iter);
    }
  };
  using BiCGStabSolver = ITL_Solver<bicgstab_type>;

  // ===========================================================================

  /**
   * \ingroup Solver
   * \class AMDiS::BiCGStab2Solver
   * \brief ITL_Solver <\ref bicgstab2_type> implementation of BiCGStab(l) method
   * with l=2 \implements ITL_Solver
   *
   * Solves a linear system \f$ Ax=b \f$ by a stabilized BiCG(2) method and can
   * be used for system matrices.
   */

  class bicgstab2_type
  {
  public:
    bicgstab2_type(std::string /*name*/) {}
    template <class LinOp, class X, class B, class L, class R, class I>
    int operator()(LinOp const& A, X& x, B const& b, L const& l, R const&, I& iter)
    {
      return itl::bicgstab_2(A, x, b, l, iter);
    }
  };
  using BiCGStab2Solver = ITL_Solver<bicgstab2_type>;

  // ===========================================================================

  /**
   * \ingroup Solver
   * \class AMDiS::QMRSolver
   * \brief ITL_Solver <\ref qmr_solver_type> implementation of Quasi-Minimal
   * Residual method \implements ITL_Solver
   *
   * Solves a linear system \f$ Ax=b \f$ by the Quasi-Minimal Residual method (QMR).
   */

  class qmr_solver_type
  {
  public:
    qmr_solver_type(std::string /*name*/) {}
    template <class LinOp, class X, class B, class L, class R, class I>
    int operator()(LinOp const& A, X& x, B const& b, L const& l, R const& r, I& iter)
    {
      return itl::qmr(A, x, b, l, r, iter);
    }
  };
  using QMRSolver = ITL_Solver<qmr_solver_type>;

  // ===========================================================================

  /**
   * \ingroup Solver
   * \class AMDiS::TFQMRSolver
   * \brief ITL_Solver <\ref tfqmr_solver_type> implementation of Transposed-Free
   * Quasi-Minimal Residual method \implements ITL_Solver
   *
   * Solves a linear system by the Transposed-Free Quasi-Minimal Residual method
   * (TFQMR). Does not use preconditioning currently.
   */

  class tfqmr_solver_type
  {
  public:
    tfqmr_solver_type(std::string /*name*/) {}
    template <class LinOp, class X, class B, class L, class R, class I>
    int operator()(LinOp const& A, X& x, B const& b, L const& l, R const& r, I& iter)
    {
      return itl::tfqmr(A, x, b, l, r, iter);
    }
  };
  using TFQMRSolver = ITL_Solver<tfqmr_solver_type>;

  // ===========================================================================

  /**
   * \ingroup Solver
   * \class AMDiS::BiCGStabEllSolver
   * \brief ITL_Solver <\ref bicgstab_ell_type> implementation of stabilized
   * BiCG(ell) method \implements ITL_Solver
   *
   * Solves a linear system by a stabilized BiCG(ell) method and can be used for
   * system matrices. The parameter ell [3] can be specified.
   */

  class bicgstab_ell_type
  {
    int ell;
  public:
    bicgstab_ell_type(std::string name) : ell(3)
    {
      Parameters::get(name + "->ell", ell);
    }
    template <class LinOp, class X, class B, class L, class R, class I>
    int operator()(LinOp const& A, X& x, B const& b, L const& l, R const& r, I& iter)
    {
      return itl::bicgstab_ell(A, x, b, l, r, iter, ell);
    }
  };
  using BiCGStabEllSolver = ITL_Solver<bicgstab_ell_type>;

  // ===========================================================================

  /**
   * \ingroup Solver
   * \class AMDiS::GMResSolver
   * \brief ITL_Solver <\ref gmres_type> implementation of generalized minimal
   * residual method \implements ITL_Solver
   *
   * Solves a linear system by the GMRES method.
   * The parameter restart [30] is the maximal number of orthogonalized vectors.
   * The method is not preconditioned
   */

  enum ORTHOGONALIZATION
  {
    GRAM_SCHMIDT = 1,
    HOUSEHOLDER = 2
  };

  class gmres_type
  {
    int restart;
    int ortho;

  public:
    gmres_type(std::string name) : restart(30), ortho(GRAM_SCHMIDT)
    {
      Parameters::get(name + "->restart", restart);
      Parameters::get(name + "->orthogonalization", ortho);
    }
    template <class LinOp, class X, class B, class L, class R, class I>
    int operator()(LinOp const& A, X& x, B const& b, L const& l, R const& r, I& iter)
    {
      switch ((ORTHOGONALIZATION)ortho)
      {
      default:
      case GRAM_SCHMIDT:
        return itl::gmres2(A, x, b, l, r, iter, restart);
        break;
#ifndef HAVE_PARALLEL_MTL4
      case HOUSEHOLDER:
        return itl::gmres_householder(A, x, b, l, iter, restart);
        break;
#endif
      }
    }
  };
  using GMResSolver = ITL_Solver<gmres_type>;

  // ===========================================================================

  /**
   * \ingroup Solver
   * \class AMDiS::IDRsSolver
   * \brief ITL_Solver <\ref idr_s_type> implementation of Induced Dimension
   * Reduction method \implements ITL_Solver
   *
   * Solves a linear system by an Induced Dimension Reduction method and can be
   * used for system matrices.  The parameter s [30] can be specified.
   *
   * Peter Sonneveld and Martin B. van Gijzen, IDR(s): a family of simple and fast
   * algorithms for solving large nonsymmetric linear systems.
   * SIAM J. Sci. Comput. Vol. 31, No. 2, pp. 1035-1062 (2008). (copyright SIAM)
   */

  class idr_s_type
  {
    int s;
  public:
    idr_s_type(std::string name) : s(30)
    {
      Parameters::get(name + "->s", s);
    }
    template <class LinOp, class X, class B, class L, class R, class I>
    int operator()(LinOp const& A, X& x, B const& b, L const& l, R const& r, I& iter)
    {
      return itl::idr_s(A, x, b, l, r, iter, s);
    }
  };
  using IDRsSolver = ITL_Solver<idr_s_type>;

  // ===========================================================================

  /**
   * \ingroup Solver
   * \class AMDiS::MinResSolver
   * \brief ITL_Solver <\ref minres_solver_type> implementation of minimal
   * residual method \implements ITL_Solver
   *
   * Solves a linear system by the Minres method. Can be used for symmetric
   * indefinite systems.
   */

  class minres_solver_type
  {
  public:
    minres_solver_type(std::string /*name*/) {}
    template <class LinOp, class X, class B, class L, class R, class I>
    int operator()(LinOp const& A, X& x, B const& b, L const& l, R const& r, I& iter)
    {
      return itl::minres(A, x, b, l, r, iter);
    }
  };
  using MinResSolver = ITL_Solver<minres_solver_type>;

  // ===========================================================================

  /**
   * \ingroup Solver
   * \class AMDiS::GcrSolver
   * \brief ITL_Solver <\ref gcr_type> implementation of generalized conjugate
   * residual method \implements ITL_Solver
   *
   * Solves a linear system by the GCR method - generalized conjugate residual
   * method. The parameter restart [30] is the maximal number of orthogonalized
   * vectors.
   */

  class gcr_type
  {
    int restart;

  public:
    gcr_type(std::string name) : restart(30)
    {
      Parameters::get(name + "->restart", restart);
    }
    template <class LinOp, class X, class B, class L, class R, class I>
    int operator()(LinOp const& A, X& x, B const& b, L const& l, R const& r, I& iter)
    {
      return itl::gcr(A, x, b, l, r, iter, restart);
    }
  };
  using GcrSolver = ITL_Solver<gcr_type>;

  // ===========================================================================

  /**
   * \ingroup Solver
   * \class AMDiS::FGMResSolver
   * \brief ITL_Solver <\ref fgmres_type> implementation of flexible GMRes method
   * \implements ITL_Solver
   *
   * Solves a linear system by the FGMRES method.
   * The parameter restart [30] is the maximal number of orthogonalized vectors.
   * See reference "A Flexible Inner-Outer Preconditiones GMRES Algorithm",
   * Youcef Saad, (1993)
   */

  class fgmres_type
  {
    int restart;
    int orthogonalization;

  public:
    fgmres_type(std::string name) : restart(30), orthogonalization(GRAM_SCHMIDT)
    {
      Parameters::get(name + "->restart", restart);
      Parameters::get(name + "->orthogonalization", orthogonalization);
    }
    template <class LinOp, class X, class B, class L, class R, class I>
    int operator()(LinOp const& A, X& x, B const& b, L const& l, R const& r, I& iter)
    {
      switch ((ORTHOGONALIZATION)orthogonalization)
      {
      default:
      case GRAM_SCHMIDT:
        return itl::fgmres(A, x, b, l, r, iter, restart);
        break;
#ifndef HAVE_PARALLEL_MTL4
      case HOUSEHOLDER:
        return itl::fgmres_householder(A, x, b, r, iter, restart);
        break;
#endif
      }
    }
  };
  using FGMResSolver = ITL_Solver<fgmres_type>;

  // ===========================================================================

  /**
   * \ingroup Solver
   * \class AMDiS::PreOnly
   * \brief ITL_Solver <\ref preonly_type> implementation of preconditioner as
   * \implements ITL_Solver
   *
   * Solves a linear system by applying a preconditioner only.
   */
  class preonly_type
  {
  public:
    preonly_type(std::string /*name*/) {}
    template <class LinOp, class X, class B, class L, class R, class I>
    int operator()(LinOp const& A, X& x, B const& b, L const& l, R const& /*r*/, I& iter)
    {
      return itl::preonly(A, x, b, l, iter);
    }
  };
  using PreOnly = ITL_Solver<preonly_type>;

  // ===========================================================================

} // end namespace AMDiS


#include <lost.hpp>
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "LOST tests"
#include <boost/test/unit_test.hpp>

boost::adjacency_list<boost::hash_setS, boost::vecS, boost::undirectedS> typedef graph;

BOOST_AUTO_TEST_CASE( info_branch_test )
{
  // 0-1-2-3-4-5-6
  //     |   |
  //     7   9
  //     |   |
  //     8   10
  graph T;
  add_edge(0, 1, T);
  add_edge(1, 2, T);
  add_edge(2, 3, T);
  add_edge(3, 4, T);
  add_edge(4, 5, T);
  add_edge(5, 6, T);
  add_edge(2, 7, T);
  add_edge(7, 8, T);
  add_edge(4, 9, T);
  add_edge(9, 10, T);
  leaf_info<graph> info(T);
  BOOST_CHECK( !info.is_path() );
  for(int i = 0; i < 3; ++i)
    BOOST_CHECK( info.on_branch(0, i) );
  for(int i = 3; i < 11; ++i)
    BOOST_CHECK( !info.on_branch(0, i) );
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_DETAIL_PRIMES_HPP
#define CRYPTO3_DETAIL_PRIMES_HPP

#include <boost/integer.hpp>

namespace nil {
    namespace crypto3 {
        namespace detail {

            template<int Bits>
            struct all_ones {
                typedef typename boost::uint_t<Bits>::least type;
                static type const value = (all_ones<Bits - 1>::value << 1) | 1;
            };
            template<>
            struct all_ones<0> {
                typedef boost::uint_t<0>::least type;
                static type const value = 0;
            };

            template<int Bits>
            struct largest_prime;

#define CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(B, D)                              \
    template<>                                                                         \
    struct largest_prime<B> {                                                          \
        constexpr static boost::uint_t<B>::least const value = all_ones<B>::value - D; \
    };                                                                                \
    constexpr boost::uint_t<B>::least const largest_prime<B>::value;

            // http://primes.utm.edu/lists/2small/0bit.html or
            // http://www.research.att.com/~njas/sequences/A013603
            // Though those offets are from 2**b; This code is offsets from 2**b-1
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(2, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(3, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(4, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(5, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(6, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(7, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(8, 4);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(9, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(10, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(11, 8);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(12, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(13, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(14, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(15, 18);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(16, 14);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(17, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(18, 4);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(19, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(20, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(21, 8);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(22, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(23, 14);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(24, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(25, 38);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(26, 4);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(27, 38);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(28, 56);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(29, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(30, 34);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(31, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(32, 4);

        }    // namespace detail
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_HASH_DETAIL_PRIMES_HPP


#include <gtest/gtest.h>
#include "pitts_multivector_tsqr.hpp"
#include "pitts_multivector_random.hpp"
#include "pitts_multivector.hpp"
#include "pitts_multivector_eigen_adaptor.hpp"
#include "pitts_tensor2.hpp"
#include "pitts_tensor2_eigen_adaptor.hpp"
#include <Eigen/Dense>
#include "eigen_test_helper.hpp"

TEST(PITTS_MultiVector_tsqr, internal_HouseholderQR_applyReflection_inplace)
{
  constexpr auto eps = 1.e-8;
  using Chunk = PITTS::Chunk<double>;
  using MultiVector = PITTS::MultiVector<double>;

  constexpr int n = 100;
  constexpr int m = 5;

  MultiVector X(n,m), X_ref(n,m);
  randomize(X);
  // copy X to X_ref
  for(int i = 0; i < n; i++)
    for(int j = 0; j < m; j++)
      X_ref(i,j) = X(i,j);

  MultiVector v(n,1);
  randomize(v);
  int col = 3;
  int firstRow = 1;
  constexpr int nChunks = 2;

  // mak upper part of v zero
  for(int i = (nChunks+firstRow+1)*Chunk::size; i < n; i++)
    v(i,0) = 0;

  PITTS::internal::HouseholderQR::applyReflection(nChunks, firstRow, col, &v.chunk(0,0), &X.chunk(0,0), X.colStrideChunks(), &X.chunk(0,0), X.colStrideChunks()); // memory layout ok because X is small enough

  // calculate reference result with Eigen
  auto mapX_ref = EigenMap(X_ref);
  auto mapV = ConstEigenMap(v);

  int offset = firstRow*Chunk::size;
  int n_ = n - offset;
  mapX_ref.block(offset, col, n_, 1) = (Eigen::MatrixXd::Identity(n_,n_) - mapV.bottomRows(n_) * mapV.bottomRows(n_).transpose()) * mapX_ref.block(offset, col, n_, 1);

  ASSERT_NEAR(ConstEigenMap(X_ref), ConstEigenMap(X), eps);
}

TEST(PITTS_MultiVector_tsqr, internal_HouseholderQR_applyReflection_inplace_firstRow_bigger_nChunks)
{
  constexpr auto eps = 1.e-8;
  using Chunk = PITTS::Chunk<double>;
  using MultiVector = PITTS::MultiVector<double>;

  constexpr int n = 100;
  constexpr int m = 5;

  MultiVector X(n,m), X_ref(n,m);
  randomize(X);
  // copy X to X_ref
  for(int i = 0; i < n; i++)
    for(int j = 0; j < m; j++)
      X_ref(i,j) = X(i,j);

  MultiVector v(n,1);
  randomize(v);
  int col = 3;
  int firstRow = 2;
  constexpr int nChunks = 1;

  // mak upper part of v zero
  for(int i = (nChunks+firstRow+1)*Chunk::size; i < n; i++)
    v(i,0) = 0;

  PITTS::internal::HouseholderQR::applyReflection(nChunks, firstRow, col, &v.chunk(0,0), &X.chunk(0,0), X.colStrideChunks(), &X.chunk(0,0), X.colStrideChunks()); // memory layout ok because X is small enough

  // calculate reference result with Eigen
  auto mapX_ref = EigenMap(X_ref);
  auto mapV = ConstEigenMap(v);

  int offset = firstRow*Chunk::size;
  int n_ = n - offset;
  mapX_ref.block(offset, col, n_, 1) = (Eigen::MatrixXd::Identity(n_,n_) - mapV.bottomRows(n_) * mapV.bottomRows(n_).transpose()) * mapX_ref.block(offset, col, n_, 1);

  ASSERT_NEAR(ConstEigenMap(X_ref), ConstEigenMap(X), eps);
}

TEST(PITTS_MultiVector_tsqr, internal_HouseholderQR_applyReflection_out_of_place)
{
  constexpr auto eps = 1.e-8;
  using Chunk = PITTS::Chunk<double>;
  using MultiVector = PITTS::MultiVector<double>;

  constexpr int n = 100;
  constexpr int m = 5;

  MultiVector X(n,m), X_ref(n,m), X_in(2*n,m);
  randomize(X);
  // copy X to X_ref
  for(int i = 0; i < n; i++)
    for(int j = 0; j < m; j++)
      X_ref(i,j) = X(i,j);

  MultiVector v(n,1);
  randomize(v);
  int col = 3;
  int firstRow = 1;
  constexpr int nChunks = 2;

  // mak upper part of v zero
  for(int i = (nChunks+firstRow+1)*Chunk::size; i < n; i++)
    v(i,0) = 0;

  // set parts of X that will be overwritten to just some number
  for(int i = firstRow; i < nChunks; i++)
    for(int j = 0; j < Chunk::size; j++)
    {
      X_in.chunk(i,col)[j] = X.chunk(i,col)[j];
      X.chunk(i,col)[j] = 77.;
    }

  PITTS::internal::HouseholderQR::applyReflection(nChunks, firstRow, col, &v.chunk(0,0), &X_in.chunk(0,0), X_in.colStrideChunks(), &X.chunk(0,0), X.colStrideChunks()); // memory layout ok because X is small enough

  // calculate reference result with Eigen
  auto mapX_ref = EigenMap(X_ref);
  auto mapV = ConstEigenMap(v);

  int offset = firstRow*Chunk::size;
  int n_ = n - offset;
  mapX_ref.block(offset, col, n_, 1) = (Eigen::MatrixXd::Identity(n_,n_) - mapV.bottomRows(n_) * mapV.bottomRows(n_).transpose()) * mapX_ref.block(offset, col, n_, 1);

  ASSERT_NEAR(ConstEigenMap(X_ref), ConstEigenMap(X), eps);
}

TEST(PITTS_MultiVector_tsqr, internal_HouseholderQR_applyReflection_out_of_place_firstRow_bigger_nChunks)
{
  constexpr auto eps = 1.e-8;
  using Chunk = PITTS::Chunk<double>;
  using MultiVector = PITTS::MultiVector<double>;

  constexpr int n = 100;
  constexpr int m = 5;

  MultiVector X(n,m), X_ref(n,m), X_in(2*n,m);
  randomize(X);
  // copy X to X_ref
  for(int i = 0; i < n; i++)
    for(int j = 0; j < m; j++)
      X_ref(i,j) = X(i,j);

  MultiVector v(n,1);
  randomize(v);
  int col = 3;
  int firstRow = 2;
  constexpr int nChunks = 1;

  // mak upper part of v zero
  for(int i = (nChunks+firstRow+1)*Chunk::size; i < n; i++)
    v(i,0) = 0;

  // X_in is completely ignored as firstRow >= nChunks

  PITTS::internal::HouseholderQR::applyReflection(nChunks, firstRow, col, &v.chunk(0,0), &X_in.chunk(0,0), X_in.colStrideChunks(), &X.chunk(0,0), X.colStrideChunks()); // memory layout ok because X is small enough

  // calculate reference result with Eigen
  auto mapX_ref = EigenMap(X_ref);
  auto mapV = ConstEigenMap(v);

  int offset = firstRow*Chunk::size;
  int n_ = n - offset;
  mapX_ref.block(offset, col, n_, 1) = (Eigen::MatrixXd::Identity(n_,n_) - mapV.bottomRows(n_) * mapV.bottomRows(n_).transpose()) * mapX_ref.block(offset, col, n_, 1);

  ASSERT_NEAR(ConstEigenMap(X_ref), ConstEigenMap(X), eps);
}

TEST(PITTS_MultiVector_tsqr, internal_HouseholderQR_applyReflection2_inplace)
{
  constexpr auto eps = 1.e-8;
  using Chunk = PITTS::Chunk<double>;
  using MultiVector = PITTS::MultiVector<double>;

  constexpr int n = 100;
  constexpr int m = 5;

  MultiVector X(n,m), X_ref(n,m);
  randomize(X);
  // copy X to X_ref
  for(int i = 0; i < n; i++)
    for(int j = 0; j < m; j++)
      X_ref(i,j) = X(i,j);

  MultiVector v(n,1);
  randomize(v);
  MultiVector w(n,1);
  randomize(w);

  int col = 3;
  int firstRow = 1;
  int nChunks = 2;

  // calculate vTw
  double vTw = 0;
  for(int i = firstRow; i <= nChunks+firstRow; i++)
    for(int j = 0; j < Chunk::size; j++)
      vTw += v.chunk(i,0)[j] * w.chunk(i,0)[j];
  Chunk vTw_chunk;
  for(int i = 0; i < Chunk::size; i++)
    vTw_chunk[i] = vTw;

  PITTS::internal::HouseholderQR::applyReflection2(nChunks, firstRow, col, &w.chunk(0,0), &v.chunk(0,0), vTw_chunk, &X.chunk(0,0), X.colStrideChunks(), &X.chunk(0,0), X.colStrideChunks()); // memory layout ok because X is small enough

  PITTS::internal::HouseholderQR::applyReflection(nChunks, firstRow, col, &w.chunk(0,0), &X_ref.chunk(0,0), X_ref.colStrideChunks(), &X_ref.chunk(0,0), X_ref.colStrideChunks());
  PITTS::internal::HouseholderQR::applyReflection(nChunks, firstRow, col, &v.chunk(0,0), &X_ref.chunk(0,0), X_ref.colStrideChunks(), &X_ref.chunk(0,0), X_ref.colStrideChunks());

  ASSERT_NEAR(ConstEigenMap(X_ref), ConstEigenMap(X), eps);
}

TEST(PITTS_MultiVector_tsqr, internal_HouseholderQR_applyReflection2_out_of_place)
{
  constexpr auto eps = 1.e-8;
  using Chunk = PITTS::Chunk<double>;
  using MultiVector = PITTS::MultiVector<double>;

  constexpr int n = 100;
  constexpr int m = 5;

  MultiVector X(n,m), X_ref(n,m), X_in(2*n,m);
  randomize(X);
  // copy X to X_ref
  for(int i = 0; i < n; i++)
    for(int j = 0; j < m; j++)
      X_ref(i,j) = X(i,j);

  MultiVector v(n,1);
  randomize(v);
  MultiVector w(n,1);
  randomize(w);

  int col = 3;
  int firstRow = 1;
  int nChunks = 2;

  // calculate vTw
  double vTw = 0;
  for(int i = firstRow; i <= nChunks+firstRow; i++)
    for(int j = 0; j < Chunk::size; j++)
      vTw += v.chunk(i,0)[j] * w.chunk(i,0)[j];
  Chunk vTw_chunk;
  for(int i = 0; i < Chunk::size; i++)
    vTw_chunk[i] = vTw;

  // set parts of X that will be overwritten to just some number
  for(int i = firstRow; i < nChunks; i++)
    for(int j = 0; j < Chunk::size; j++)
    {
      X_in.chunk(i,col)[j] = X.chunk(i,col)[j];
      X.chunk(i,col)[j] = 77.;
    }

  PITTS::internal::HouseholderQR::applyReflection2(nChunks, firstRow, col, &w.chunk(0,0), &v.chunk(0,0), vTw_chunk, &X_in.chunk(0,0), X_in.colStrideChunks(), &X.chunk(0,0), X.colStrideChunks()); // memory layout ok because X is small enough

  PITTS::internal::HouseholderQR::applyReflection(nChunks, firstRow, col, &w.chunk(0,0), &X_ref.chunk(0,0), X_ref.colStrideChunks(), &X_ref.chunk(0,0), X_ref.colStrideChunks());
  PITTS::internal::HouseholderQR::applyReflection(nChunks, firstRow, col, &v.chunk(0,0), &X_ref.chunk(0,0), X_ref.colStrideChunks(), &X_ref.chunk(0,0), X_ref.colStrideChunks());

  ASSERT_NEAR(ConstEigenMap(X_ref), ConstEigenMap(X), eps);
}

TEST(PITTS_MultiVector_tsqr, internal_HouseholderQR_transformBlock_inplace)
{
  constexpr auto eps = 1.e-8;
  using Chunk = PITTS::Chunk<double>;
  using MultiVector = PITTS::MultiVector<double>;

  constexpr int n = 16*Chunk::size - 7;   // force padding, we need some extra space in transformBlock
  constexpr int m = 19;
  constexpr int mChunks = (m-1) / Chunk::size + 1;
  constexpr int nTotalChunks = (n-1) / Chunk::size + 1;
  constexpr int nChunks = nTotalChunks - mChunks;

  MultiVector X(n,m), X_ref(n,m);
  randomize(X);
  // make lower triangular part zero...
  for(int j = 0; j < m; j++)
    for(int i = nChunks+j/Chunk::size+1; i < nTotalChunks; i++)
      X.chunk(i,j) = Chunk{};
  // copy X to X_ref
  for(int i = 0; i < n; i++)
    for(int j = 0; j < m; j++)
      X_ref(i,j) = X(i,j);

  PITTS::internal::HouseholderQR::transformBlock(nChunks, m, &X.chunk(0,0), X.colStrideChunks(), &X.chunk(0,0), X.colStrideChunks(), nChunks);

  // check that the result is upper triangular
  for(int i = 0; i < mChunks*Chunk::size; i++)
  {
    for(int j = 0; j < m; j++)
    {
      if( i > j )
      {
        ASSERT_NEAR(0., X(i,j), eps);
      }
    }
  }

  // use Eigen to check that the singular values and the right singular vectors are identical
  auto mapX = ConstEigenMap(X);
  auto mapX_ref = ConstEigenMap(X_ref);
  //std::cout << "X:\n" << mapX << std::endl;
  Eigen::BDCSVD<Eigen::MatrixXd> svd(mapX.topRows(m), Eigen::ComputeThinV);
  Eigen::BDCSVD<Eigen::MatrixXd> svd_ref(mapX_ref, Eigen::ComputeThinV);

  ASSERT_NEAR(svd_ref.singularValues(), svd.singularValues(), eps);
  // V can differ by sign, only consider absolute part
  ASSERT_NEAR(svd_ref.matrixV().array().abs(), svd.matrixV().array().abs(), eps);
}

TEST(PITTS_MultiVector_tsqr, internal_HouseholderQR_transformBlock_out_of_place)
{
  constexpr auto eps = 1.e-8;
  using Chunk = PITTS::Chunk<double>;
  using MultiVector = PITTS::MultiVector<double>;

  constexpr int n = 16*Chunk::size - 7;   // force padding, we need some extra space in transformBlock
  constexpr int m = 19;
  constexpr int mChunks = (m-1) / Chunk::size + 1;
  constexpr int nTotalChunks = (n-1) / Chunk::size + 1;
  constexpr int nChunks = nTotalChunks - mChunks;

  MultiVector X(n,m), X_ref(n,m), Xresult(n+2*Chunk::size,m);
  randomize(X);
  randomize(Xresult);
  // make lower triangular part zero...
  for(int j = 0; j < m; j++)
    for(int i = nChunks+j/Chunk::size+1; i < nTotalChunks; i++)
      X.chunk(i,j) = Chunk{};
  // copy X to X_ref
  for(int i = 0; i < n; i++)
    for(int j = 0; j < m; j++)
      X_ref(i,j) = X(i,j);
  // prepare lower part of result
  for(int col = 0; col < m; col++)
    for(int i = nChunks; i < nTotalChunks; i++)
      for(int j = 0; j < Chunk::size; j++)
      {
        Xresult.chunk(2+i,col)[j] = X.chunk(i,col)[j];
        X.chunk(i,col)[j] = 77;
      }

  PITTS::internal::HouseholderQR::transformBlock(nChunks, m, &X.chunk(0,0), X.colStrideChunks(), &Xresult.chunk(0,0), Xresult.colStrideChunks(), 2+nChunks);

  // check that the result is upper triangular, copied to the bottom
  for(int i = 0; i < n; i++)
  {
    for(int j = 0; j < m; j++)
    {
      if( i > nChunks*Chunk::size + j )
      {
        EXPECT_NEAR(0., Xresult(2*Chunk::size+i,j), eps);
      }
      // X shouldn't change
      if( i < nChunks*Chunk::size )
      {
        ASSERT_EQ(X_ref(i,j), X(i,j));
      }
      else
      {
        ASSERT_EQ(77., X(i,j));
      }
    }
  }

  // use Eigen to check that the singular values and the right singular vectors are identical
  auto mapXresult = ConstEigenMap(Xresult);
  //std::cout << "X:\n" << mapXresult << std::endl;
  auto mapX_ref = ConstEigenMap(X_ref);
  Eigen::BDCSVD<Eigen::MatrixXd> svd(mapXresult.bottomRows(n-nChunks*Chunk::size), Eigen::ComputeThinV);
  Eigen::BDCSVD<Eigen::MatrixXd> svd_ref(mapX_ref, Eigen::ComputeThinV);

  ASSERT_NEAR(svd_ref.singularValues(), svd.singularValues(), eps);
  // V can differ by sign, only consider absolute part
  ASSERT_NEAR(svd_ref.matrixV().array().abs(), svd.matrixV().array().abs(), eps);
}

TEST(PITTS_MultiVector_tsqr, internal_combineTwoBlocks)
{
  constexpr auto eps = 1.e-8;
  using Chunk = PITTS::Chunk<double>;
  using MultiVector = PITTS::MultiVector<double>;

  MultiVector R1;
  MultiVector R2;
  for(int m = 1; m < 77; m+=7)
  {
    // implementation also works with non-triangular factors, so for simplicity just use random square blocks
    R1.resize(m,m);
    R2.resize(m,m);
    randomize(R1);
    randomize(R2);
    // should both be upper triangular
    for(int i = 0; i < m; i++)
      for(int j = i+1; j < m; j++)
        R1(j,i) = R2(j,i) = 0;

    // we need buffers of correctly padded size...
    const auto mChunks = (m-1) / Chunk::size + 1;
    const int totalSize = int(mChunks*m*Chunk::size);
    std::vector<Chunk> buff1(mChunks*m);
    std::vector<Chunk> buff2(mChunks*m);

    for(int j = 0; j < m; j++)
    {
      for(int i = 0; i < mChunks; i++)
      {
        buff1[i+j*mChunks] = R1.chunk(i,j);
        buff2[i+j*mChunks] = R2.chunk(i,j);
      }
    }

    MPI_Datatype mpi_double = MPI_DOUBLE;
    PITTS::internal::HouseholderQR::combineTwoBlocks((const double*)(&(buff1[0][0])), &(buff2[0][0]), &totalSize, &mpi_double);

    // compara singular values with Eigen
    Eigen::MatrixXd R12(2*m,m);
    R12.block(0,0,m,m) = ConstEigenMap(R1);
    R12.block(m,0,m,m) = ConstEigenMap(R2);
    Eigen::JacobiSVD<Eigen::MatrixXd> svd_ref(R12);

    Eigen::Map<Eigen::MatrixXd> result(&(buff2[0][0]), mChunks*Chunk::size, m);
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(result);

    EXPECT_NEAR(svd_ref.singularValues(), svd.singularValues(), eps);
  }
}

namespace
{
  // helper function for testing block_TSQR with different data dimensions, etc
  void test_block_TSQR(int n, int m)
  {
    constexpr auto eps = 1.e-8;
    using Chunk = PITTS::Chunk<double>;
    using MultiVector = PITTS::MultiVector<double>;
    using Tensor2 = PITTS::Tensor2<double>;

    MultiVector M(n,m);
    randomize(M);

    // store the original matrix for later
    Eigen::MatrixXd M_ref = ConstEigenMap(M);

    Tensor2 R;
    block_TSQR(M, R, 4, false);
    ASSERT_EQ(m, R.r1());
    ASSERT_EQ(m, R.r2());
    for(int j = 0; j < m; j++)
    {
      for(int i = j+1; i < m; i++)
      {
        ASSERT_NEAR(0., R(i,j), eps);
      }
    }

    // check that the singular values and right singular vectors match...
    Eigen::BDCSVD<Eigen::MatrixXd> svd(PITTS::ConstEigenMap(R), Eigen::ComputeThinV);
    Eigen::BDCSVD<Eigen::MatrixXd> svd_ref(M_ref, Eigen::ComputeThinV);

    ASSERT_NEAR(svd_ref.singularValues(), svd.singularValues(), eps);
    // V can differ by sign, only consider absolute part
    ASSERT_NEAR(svd_ref.matrixV().array().abs(), svd.matrixV().array().abs(), eps);
  }

  // helper function to determine the currently default number of threads in a parallel region
  int get_default_num_threads()
  {
    int numThreads = 1;
#pragma omp parallel
    {
#pragma omp critical (PITTS_TEST_MULTIVECTOR_TSQR)
      numThreads = omp_get_num_threads();
    }
    return numThreads;
  }
}

TEST(PITTS_MultiVector_tsqr, block_TSQR_small_serial)
{
  int nThreads = get_default_num_threads();

  omp_set_num_threads(1);
  test_block_TSQR(50, 10);
  omp_set_num_threads(nThreads);
}

TEST(PITTS_MultiVector_tsqr, block_TSQR_small_4threads)
{
  int nThreads = get_default_num_threads();

  ASSERT_LE(4, omp_get_max_threads());
  omp_set_num_threads(4);

  test_block_TSQR(50, 10);

  omp_set_num_threads(nThreads);
}

TEST(PITTS_MultiVector_tsqr, block_TSQR_large_serial)
{
  int nThreads = get_default_num_threads();

  omp_set_num_threads(1);
  test_block_TSQR(200, 30);
  omp_set_num_threads(nThreads);
}

TEST(PITTS_MultiVector_tsqr, block_TSQR_large_varying_sizes_serial)
{
  int nThreads = get_default_num_threads();

  omp_set_num_threads(1);
  for(int m = 1; m < 30; m++)
  {
    std::cout << "Testing #cols = " << m << "\n";
    test_block_TSQR(250, m);
  }
  omp_set_num_threads(nThreads);
}

TEST(PITTS_MultiVector_tsqr, block_TSQR_large_parallel)
{
  test_block_TSQR(200, 30);
}

TEST(PITTS_MultiVector_tsqr, block_TSQR_manyRows_differentNumbersOfThreads)
{
  int nThreads = get_default_num_threads();

  ASSERT_LE(4, omp_get_max_threads());
  for(int iThreads = 1; iThreads < 5; iThreads++)
  {
    omp_set_num_threads(iThreads);
    test_block_TSQR(1000, 1);
  }
  omp_set_num_threads(nThreads);
}

TEST(PITTS_MultiVector_tsqr, block_TSQR_mpiGlobal)
{
  constexpr auto eps = 1.e-8;
  using Chunk = PITTS::Chunk<double>;
  using MultiVector = PITTS::MultiVector<double>;
  using Tensor2 = PITTS::Tensor2<double>;

  const long long nTotal = 500;
  const long long m = 4;

  const auto& [iProc,nProcs] = PITTS::internal::parallel::mpiProcInfo();
  const auto& [nFirst,nLast] = PITTS::internal::parallel::distribute(nTotal, {iProc,nProcs});
  const long long n = nLast - nFirst + 1;

  Eigen::MatrixXd Mglobal = Eigen::MatrixXd::Random(nTotal, 4);
  MPI_Bcast(Mglobal.data(), nTotal*m, MPI_DOUBLE, 0, MPI_COMM_WORLD);

  MultiVector M(n,m);
  {
    auto mapM = EigenMap(M);
    mapM = Mglobal.block(nFirst,0,n,m);
  }

  Tensor2 R;
  block_TSQR(M, R);
  ASSERT_EQ(m, R.r1());
  ASSERT_EQ(m, R.r2());
  for(int j = 0; j < m; j++)
  {
    for(int i = j+1; i < m; i++)
    {
      ASSERT_NEAR(0., R(i,j), eps);
    }
  }

  // check that the singular values and right singular vectors match...
  Eigen::BDCSVD<Eigen::MatrixXd> svd(ConstEigenMap(R), Eigen::ComputeThinV);
  Eigen::BDCSVD<Eigen::MatrixXd> svd_ref(Mglobal, Eigen::ComputeThinV);

  ASSERT_NEAR(svd_ref.singularValues(), svd.singularValues(), eps);
  // V can differ by sign, only consider absolute part
  ASSERT_NEAR(svd_ref.matrixV().array().abs(), svd.matrixV().array().abs(), eps);
}


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

#ifndef LP_HPP
#define LP_HPP

#include <stdexcept>
#include <list>
#include <limits>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T> inline T lp_minimize(ub::vector<T>& objfunc, std::list< ub::vector<T> >& constraints)
{
	int i, j;
	int m = constraints.size();
	int lp_n = objfunc.size() - 1;
	int n = lp_n + m;
	int pivot_i, pivot_j;
	T tmp, minmax;
	typename std::list< ub::vector<T> >::iterator p;

	ub::matrix<T> a(m+1, n+1);

	a(0, 0) = objfunc(0);

	for (i=1; i<n+1; i++) {
		if (i <= lp_n) a(0, i) = -objfunc(i);
		else a(0, i) = 0.;
	}

	p = constraints.begin();
	for (i=0; i<m; i++) {
		if ((*p)[0] > 0) {
			throw std::domain_error("lp_minimize: constraints sign error");
		}
		a(i+1, 0) = -(*p)[0];
		for (j=1; j<=lp_n; j++) {
			if (j < (*p).size()) a(i+1, j) = (*p)[j];
			else a(i+1, j) = 0.;
		}
		for (j=0; j<m; j++) {
			a(i+1, lp_n+j+1) = (i == j) ? 1 : 0;
		}
		p++;
	}

	ub::vector<bool> isbasic(n+1);
	for (i=0; i<lp_n+1; i++) isbasic(i) = false;
	for (i=0; i<m; i++) isbasic(lp_n+i+1) = true;

	ub::vector<int> basic(m+1);
	for (i=0; i<m; i++) basic(i+1) = lp_n+i+1;

	while (true) {
		pivot_j = -1;
		minmax = 0.;
		for (i=1; i<n+1; i++) {
			if (isbasic(i)) continue;
			if (a(0, i) > minmax) {
				pivot_j = i;
				minmax = a(0, i);
			}
		}
		if (pivot_j == -1) break;

		pivot_i = -1;
		minmax = std::numeric_limits<T>::max();
		for (i=1; i<m+1; i++) {
			if (a(i, pivot_j) <= 0.) continue;
			tmp = a(i, 0) / a(i, pivot_j);
			if (tmp < minmax) {
				minmax = tmp;
				pivot_i = i;
			}
		}
		if (pivot_i == -1) {
			throw std::domain_error("lp_minimize: no optimal solution");
		}

		isbasic(basic(pivot_i)) = false;
		isbasic(pivot_j) = true;
		basic(pivot_i) = pivot_j;

		tmp = a(pivot_i, pivot_j);
		for (i=0; i<n+1; i++) {
			if (isbasic(i)) {
				if (i == pivot_j) a(pivot_i, i) = 1.;
				continue;
			}
			a(pivot_i, i) /= tmp;
		}

		for (i=0; i<m+1; i++) {
			if (i == pivot_i) continue;
			tmp = a(i, pivot_j);
			for (j=0; j<n+1; j++) {
				if (isbasic(j)) {
					if (j == pivot_j) a(i, j) = 0.;
					continue;
				}
				a(i, j) -= a(pivot_i, j) * tmp;
			}
		}
	}

	return a(0, 0);
}

template <class T> inline T lp_minimize_verified(ub::vector<T>& objfunc, std::list< ub::vector<T> >& constraints, int round)
{
	int i, j;
	int m = constraints.size();
	int lp_n = objfunc.size() - 1;
	int n = lp_n + m;
	int pivot_i, pivot_j;
	T tmp, minmax;
	typename std::list< ub::vector<T> >::iterator p;
	interval<T> Itmp, Imin;

	ub::matrix<T> a(m+1, n+1);
	ub::matrix< interval<T> > Ia(m+1, n+1);

	a(0, 0) = objfunc(0);

	for (i=1; i<n+1; i++) {
		if (i <= lp_n) a(0, i) = -objfunc(i);
		else a(0, i) = 0.;
	}

	p = constraints.begin();
	for (i=0; i<m; i++) {
		if ((*p)[0] > 0) {
			throw std::domain_error("lp_minimize: constraints sign error");
		}
		a(i+1, 0) = -(*p)[0];
		for (j=1; j<=lp_n; j++) {
			if (j < (*p).size()) a(i+1, j) = (*p)[j];
			else a(i+1, j) = 0.;
		}
		for (j=0; j<m; j++) {
			a(i+1, lp_n+j+1) = (i == j) ? 1 : 0;
		}
		p++;
	}

	ub::vector<bool> isbasic(n+1);
	for (i=0; i<lp_n+1; i++) isbasic(i) = false;
	for (i=0; i<m; i++) isbasic(lp_n+i+1) = true;

	ub::vector<int> basic(m+1);
	for (i=0; i<m; i++) basic(i+1) = lp_n+i+1;

	while (true) {
		pivot_j = -1;
		minmax = 0.;
		for (i=1; i<n+1; i++) {
			if (isbasic(i)) continue;
			if (a(0, i) > minmax) {
				pivot_j = i;
				minmax = a(0, i);
			}
		}
		if (pivot_j == -1) break;

		pivot_i = -1;
		minmax = std::numeric_limits<T>::max();
		for (i=1; i<m+1; i++) {
			if (a(i, pivot_j) <= 0.) continue;
			tmp = a(i, 0) / a(i, pivot_j);
			if (tmp < minmax) {
				minmax = tmp;
				pivot_i = i;
			}
		}
		if (pivot_i == -1) {
			throw std::domain_error("lp_minimize: no optimal solution");
		}

		Imin = (interval<T>)a(pivot_i, 0) / a(pivot_i, pivot_j);

		for (i=1; i<m+1; i++) {
			if (a(i, pivot_j) <= 0.) continue;
			if (i == pivot_i) {continue;}
			if (round == -1) {
				while (true) {
					Itmp = (interval<T>)a(i, 0) / a(i, pivot_j);
					if (Imin.upper() < Itmp.lower()) {
						break;
					}
					// succ
					a(i, 0) = ((interval<T>)a(i, 0) + std::numeric_limits<T>::denorm_min()).upper();
				}
			} else {
				Itmp = (interval<T>)a(i, 0) / a(i, pivot_j);
				while (true) {
					if (Imin.upper() < Itmp.lower()) {
						break;
					}
					// pred
					a(pivot_i, 0) = ((interval<T>)a(pivot_i, 0) - std::numeric_limits<T>::denorm_min()).lower();

					Imin = (interval<T>)a(pivot_i, 0) / a(pivot_i, pivot_j);
				}
			}
		}

		isbasic(basic(pivot_i)) = false;
		isbasic(pivot_j) = true;
		basic(pivot_i) = pivot_j;

		Itmp = a(pivot_i, pivot_j);
		for (i=0; i<n+1; i++) {
			if (isbasic(i)) {
				if (i == pivot_j) a(pivot_i, i) = 1.;
				continue;
			}
			Ia(pivot_i, i) = a(pivot_i, i) / Itmp;
		}

		for (i=0; i<m+1; i++) {
			if (i == pivot_i) continue;
			Itmp = a(i, pivot_j);
			for (j=0; j<n+1; j++) {
				if (isbasic(j)) {
					if (j == pivot_j) a(i, j) = 0.;
					continue;
				}
				Ia(i, j) = a(i, j) - Ia(pivot_i, j) * Itmp;
			}
		}

		for (i=0; i<n+1; i++) {
			if (isbasic(i)) continue;
			if ((i == 0) ^ (round == 1))  {
				a(pivot_i, i) = Ia(pivot_i, i).upper();
			} else {
				a(pivot_i, i) = Ia(pivot_i, i).lower();
			}
		}
		for (i=0; i<m+1; i++) {
			if (i == pivot_i) continue;
			for (j=0; j<n+1; j++) {
				if (isbasic(j)) continue;
				if ((j == 0) ^ (round == 1) ^ (i == 0))  {
					a(i, j) = Ia(i, j).upper();
				} else {
					a(i, j) = Ia(i, j).lower();
				}
			}
		}
	}

	return a(0, 0);
}

} // namespace kv

#endif // LP_HPP


/*
 * This file belongs to the Galois project, a C++ library for exploiting
 * parallelism. The code is being released under the terms of the 3-Clause BSD
 * License (a copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */

#include <boost/rational.hpp>

#include <random>
#include <iostream>

#include <cstdio>

using Rational = boost::rational<size_t>;

void multiplyTest(const double mpcand, const double mplier, const double ans) {

  double lim = mplier / 100.0;
  assert(lim >= 1.0);

  std::mt19937 eng;
  eng.seed(0);

  std::uniform_real_distribution<double> dist(0.0, lim);

  double remainMplier = mplier;

  double computed = 0.0;

  while (remainMplier > 0.0) {

    double partial = dist(eng);

    if (partial > remainMplier) {
      partial = remainMplier;
    }

    remainMplier -= partial;

    computed += mpcand * partial;
  }

  std::printf("Error in multiplication with doubles = %g\n", (ans - computed));
}

void multiplyTestRational(const Rational& mpcand, const Rational& mplier,
                          const Rational& ans) {

  size_t lim = boost::rational_cast<size_t>(mplier / Rational(100));

  std::mt19937 eng;
  eng.seed(0);

  std::uniform_int_distribution<size_t> dist(1, lim);

  Rational remainMplier = mplier;

  Rational computed(0);

  while (remainMplier > Rational(0)) {

    Rational partial(dist(eng), lim);

    if (partial > remainMplier) {
      partial = remainMplier;
    }

    // std::cout << "Rational partial mpcand: " << partial << std::endl;

    remainMplier -= partial;

    computed += mpcand * partial;
  }

  std::cout << "Error in multiplication with Rational: " << (ans - computed)
            << std::endl;
}

void rationalConversionError(double fpVal) {

  static const unsigned SIGNIFICANT_BITS = 40;

  size_t q = (size_t(1) << SIGNIFICANT_BITS);
  size_t p = size_t(fpVal * q);

  Rational r(p, q);

  std::printf("Conversion error = %g\n",
              (fpVal - boost::rational_cast<double>(r)));
}

int main() {
  multiplyTest(0.125, 1000.0, 125.0);

  multiplyTestRational(Rational(125, 1000), Rational(1000), Rational(125));

  rationalConversionError(boost::rational_cast<double>(Rational(1, 3)));

  rationalConversionError(sqrt(2.0));
  rationalConversionError(sqrt(3.0));
  rationalConversionError(sqrt(1000.0));
  rationalConversionError(sqrt(100000.0));
  rationalConversionError(sqrt(15485867)); // prime number

  return 0;
}


/*! \file MeshPointsMPI.hpp
  \brief Generates grids for parallel simulations
  \author Elad Steinberg
 */

#ifndef MESHPOINTSMPI_HPP
#define MESHPOINTSMPI_HPP 1
#ifdef RICH_MPI
#define _USE_MATH_DEFINES
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include "../tessellation/ConvexHull.hpp"
#include "../tessellation/VoronoiMesh.hpp"
#include "../misc/utils.hpp"

/*!
\brief Creates a cartesian mesh for MPI
\param nx The total number of point in the x direction
\param ny The total number of point in the y direction
\param tess The tessellation of the processors
\param lower_left Lower left point
\param upper_right Upper right point
\return The set of points corresponding to the local process
*/
vector<Vector2D> SquareMeshM(int nx,int ny,Tessellation const& tess,
			     Vector2D const& lower_left,
			     Vector2D const& upper_right);

/*!
  \brief Generates a round grid with constant point density
  \param PointNum The number of points.
  \param Rmin The min radius
  \param Rmax The max radius
  \param bottomleft The lower left corner of a boundaing box to cut off the circle
  \param topright The top right corner of a boundaing box to cut off the circle
  \param tess The tessellation of the processors
  \param xc X of circle center
  \param yc Y of circle center
  \return List of two dimensional points
*/
vector<Vector2D> CirclePointsRmaxM(int PointNum,double Rmin,double Rmax,
	Vector2D const& bottomleft,Vector2D const& topright,
	Tessellation const& tess,double xc=0,double yc=0);
/*!
  \brief Generates a round grid with r^alpha point density
  \param PointNum The number of points.
  \param Rmin The min radius
  \param Rmax The max radius
  \param xc X of circle center
  \param yc Y of circle center
  \param alpha The point density, should not be -1 or -2
  \param tess The tessellation of the processors
  \return List of two dimensional points
*/
vector<Vector2D> CirclePointsRmax_aM(int PointNum,double Rmin,double Rmax,
	double xc,double yc,double alpha,Tessellation const& tess);

/*!
  \brief Creates a circle of evenly spaced points
  \param point_number Number of points along the circumference
  \param radius Radius of the circle
  \param center Position of the center of the circle
  \param tproc The tessellation of the processors
  \return List of two dimensional points
*/
vector<Vector2D> circle_circumferenceM(int point_number,double radius,
	Vector2D const& center,Tessellation const& tproc);

/*!
\brief Creates a uniform random mesh for MPI
\param npoints The total number of point
\param tess The tessellation of the processors
\param lowerleft The lower left point of the domain
\param upperright The upper right point of the domain
\return The set of points corresponding to the local process
*/
vector<Vector2D> RandSquare(int npoints,Tessellation const& tess,
	Vector2D const& lowerleft,Vector2D const& upperright);

#endif
#endif //MESHPOINTSMPI_HPP


//          Copyright Gavin Band 2008 - 2012.
// 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)

#include <boost/math/special_functions/gamma.hpp>
#include "metro/rBF.hpp"

namespace metro {
	namespace {
		// compute the log of the density of the Dirichlet-multinomial distribution.
		// See http://en.wikipedia.org/wiki/Dirichlet-multinomial_distribution
		template< typename Row1, typename Row2 >
		double log_dirichlet_multinomial( Row1 const& counts, Row2 const& lambdas ) {
			using boost::math::lgamma ;
			double result = 0.0 ;
			if( lambdas.maxCoeff() > 0 ) {
				result
					+= lgamma( lambdas.sum() )
					- lgamma( ( lambdas + counts ).sum() )
				;
			}

			for( int i = 0; i < counts.size(); ++i ) {
				if( lambdas(i) > 0 ) {
					result += lgamma( lambdas(i) + counts(i) ) - lgamma( lambdas(i) ) ;
				}
			}
			return result ;
		}
	}

	double compute_rBF( Eigen::MatrixXd const& counts, double const lambda ) {
		double const n = counts.sum() ;
		Eigen::RowVectorXd const colSums = counts.colwise().sum() ;
		Eigen::RowVectorXd const lambdas = lambda * colSums / n ;
		return std::exp(
			log_dirichlet_multinomial( counts.row(0), lambdas )
			+ log_dirichlet_multinomial( counts.row(1), lambdas )
			- log_dirichlet_multinomial( colSums, lambdas )
		) ;
	}
}


//Code for project 5 in FYS4150
//Made by Ingvild, Hanne and Ola
//No copyrights or copywrongs, but still inspired by the work of Morten H. Jensen from project 4.
//Thanks to the one and only Jostein for discussions, help and inspiration.
//Also thanks to Håkon for late night studies.
//Most importantly the and the introuduction of [stepnum-2] and [1] for boundary calculations.
//No elegant coding implemented here, a lot of brute force. Sorry for that, but it still gives us OK results in reasonable time
//IMPORTANT: For reasonable results in 1D periodic, you need to have armadillo version later than 4.600!!

#include <iostream>
#include <fstream>
#include <iomanip>
#include <time.h>
//#include <armadillo> //Comment out when working from forskningsparken where no armadillo is to be found
#include <cmath>
#include "project5header.h"

using namespace std;
//using namespace arma; //Comment out when working from forskningsparken where no armadillo is to be found

//Initializing file objects for 1D and 2D;
ofstream ofile;

int main(int argc, char *argv[])
{

    //------------------------------------ Start up stuff -----------------------------------------------//

    //Declearing some global variables
    double dx, delta_time, initialtime, finaltime;
    int dimension, option;
    char* outfilename;

    // Read in output file, abort if there are too few command-line arguments
    if ( argc <= 1 ){cout << "Bad Usage: " << argv[0] << " read also output file on same line " << endl;}
    else {outfilename=argv[1];}

    //Opening file
    ofile.open(outfilename);

    //Read inputs, at set up inital psi and vorticity
    read_input(dx, delta_time, finaltime, initialtime, dimension, option, argv);

    //Set time equal to initialtime
    double time = initialtime;

    //calculating number of stepshttps://www.tvedestrandsposten.no/
    int stepnum = (int) ((1/dx) + 1);

    //allocates memory for psi and vorticity and some vectors needed in tridiagonal function.
    double *psi = new double[stepnum]; //Stream function array
    double *vorticity = new double[stepnum]; //Vorticity array
    double *e1 = new double[stepnum]; //upper diagonal
    double *d = new double[stepnum]; //mid diagonal
    double *e2 = new double[stepnum]; //lower diagonal
    double *force = new double[stepnum]; //forcing term in tridiagonal algo
    double *first_vorticity = new double [stepnum]; //vorticity vector for centered difference 1D
    double **temp_vorticity2D = new double* [stepnum]; //vorticity vector for 2D forward difference.
    double **psi2D = new double* [stepnum]; //2D psi
    double **vorticity2D = new double* [stepnum]; //2D vorticity
    double **first_vorticity2D = new double* [stepnum]; //2D temporarly vorticity used in centered diff
    double **psi2D_temp = new double* [stepnum]; //2D temporarly psi used in iterative solver

    //initializes the psi and vorticity arrays.
    if(dimension == 1){initialize(psi, vorticity, stepnum, option, dx);}
    if(dimension == 2){initialize2D(psi2D,vorticity2D,stepnum,dx, option);}

    //Initialize the temporary arrays
    first_vorticity = vorticity;
    psi2D_temp = psi2D;

    //initializing the force 1D array
    for(int i = 0; i < (stepnum); i++){force[i] = vorticity[i]*dx*dx;}

    //precalculating constants needed in the different schemes.
    //Named in honour of ourselves, and jostein.
    //Sorry about this, but alpha, beta etc. doesn't make it any better, just less fun.
    double ola = delta_time/(2*dx);
    double ingvild = delta_time/(dx);
    double hanne = 4*(dx*dx); //since dx = dy
    double jostein = dx*dx;
    double jostein2 = jostein*jostein;

    //-------------------------------------- Calculations and write to file  ---------------------------//

    while(time < finaltime){

        //One dimension
        if(dimension == 1){

        //write to file
        output(psi,vorticity,stepnum);

        //Calculate next step with forward or centered method. Comment in/out the one you want
        //forward_diff_solidb(psi, vorticity, stepnum, ola);
        center_diff_solidb(psi,vorticity, first_vorticity, stepnum, ingvild, ola, time);
        //forward_diff_periodicb(psi,vorticity,stepnum,ola);
        //center_diff_periodicb(psi,vorticity, first_vorticity, stepnum, ingvild, ola, time);

        //update the force 1D array
        for(int i = 0; i < (stepnum); i++){force[i] = vorticity[i]*dx*dx;}

        //Reuse project 1 code or armadillo (periodic case) to calculate psi, comment in/out the one you want.
        tridiagonal_solidb(psi, force, e1, e2, d, dx, stepnum);
        //tridiagonal_periodicb(psi, force, e1, e2, d, dx, stepnum);
	//Armadillo_1D_periodic_solver(psi,force,stepnum);

        }//end 1D

        //Two dimensions
        if(dimension == 2){

        //Write to file
        output2d(psi2D, vorticity2D, stepnum);

        //Calculate next time step vorticity
        centered_diff_solidb_2D(psi2D,vorticity2D,first_vorticity2D,stepnum,ingvild,ola,time);

        //Iterative Jacobi solver to find psi next time step
        Jacobi_Iterative_solidb_2D(psi2D,psi2D_temp,vorticity2D,stepnum,jostein,hanne,jostein2);

        }

        //adds a time step
        time += delta_time;

    }

    cout << "great success"<< endl;

  return 0;

}//end main

//----------------------------------- 1D Functions -----------------------------------------------------//

void read_input(double& dx, double& delta_time, double& finaltime, double& initialtime,int& dimension, int& option, char* argv[]){

        //reading in variables
        dx = atof(argv[2]);
        delta_time = atof(argv[3]);
        initialtime = atof(argv[4]);
        finaltime = atof(argv[5]);
        dimension = atof(argv[6]);
        option = atoi(argv[7]);

}

void initialize(double*& psi, double*& vorticity, int stepnum, int option, double dx){

    //Setting up initial values for psi and vorticity (option = 1 ---> sine)
    if(option == 1){
        for(int i = 0; i < stepnum; i++ ){

            psi[i] = sin(4*M_PI*i*dx);
            vorticity[i] = -16*M_PI*M_PI*cos(4*M_PI*i*dx);

        }//end for
    }//end if

    //Setting up initial values for psi and vorticity (option = 2 --> exp)
    if(option == 2){
        for(int i = 0; i < stepnum; i++){

            psi[i] = exp(-(pow(((dx*i - 0.5)/0.1),2)));
            vorticity[i] = exp(-100*(-0.5 + dx*i)*(-0.5 + dx*i))*(9800. - 40000*dx*i + 40000.*(dx*i*dx*i)); //wolframalpha

        }
    }
}

void output(double*& psi, double*& vorticity, int stepnum){

    ofile << setiosflags(ios::showpoint | ios::uppercase);

    for(int i = 0; i < stepnum; i++){

        ofile << setw(15) << setprecision(8) << psi[i];
        ofile << setw(15) << setprecision(8) << vorticity[i] << endl;
    }

}

void tridiagonal_solidb(double*& psi, double*& force, double*& e1, double*& e2, double*& d, double dx, int stepnum){

    //Initializing our matrix and the forcing term.
    for(int i = 0 ; i < stepnum; i++){

        e1[i] = 1.0;
        e2[i] = 1.0;
        d[i] = -2.0;

    }//end for

    //Declare x_n and the endpoints
    psi[stepnum-1] = 0.0;
    psi[0] = 0.0;

    //Forward substitution algorithm
    for(int i = 2; i < stepnum ; i++){

         //initializing dtilde
         d[i] -= (e1[i]*e2[i-1])/d[i-1];

         //initializing ytilde
         force[i] -= ((e1[i])*(force[i-1]))/d[i-1];

    }//end for

    //Backward substitution with ytilde and dtilde
    for(int i = (stepnum-2); i >= 1; i--){

        psi[i] = (force[i] - (e2[i]*(psi[i+1])))/d[i];

    }//end for

}

void forward_diff_solidb(double*& psi, double*& vorticity, int stepnum, double ola){

    //Set the boundary conditions for solid walls (just to be sure!)
    psi[0] = 0;
    psi[stepnum-1] = 0;

    //loop over all interior points
    for(int i = 1; i < (stepnum-1); i++){

    double temp_vorticity = vorticity[i];
    double temp_psi1    = psi[i+1];
    double temp_psi2    = psi[i-1];

    vorticity[i] = temp_vorticity + (temp_psi2 - temp_psi1)*ola;

    }//end for

}

void center_diff_solidb(double*& psi, double*& vorticity, double*& first_vorticity, int stepnum, double ingvild, double ola, double time){

    //Calculate forward on first step
    if(time == 0){

    //Update first_vorticity
    first_vorticity = vorticity;

    //Calculate first time step with loop over all points except boundary points
    for(int i = 1; i < (stepnum-1); i++){

    double temp_vorticity = vorticity[i];
    double temp_psi1    = psi[i+1];
    double temp_psi2    = psi[i-1];

    vorticity[i] = temp_vorticity + (temp_psi2 - temp_psi1)*ola;

    }
    }

    //Centered on next steps
    if(time != 0){

    //Calculate first time step with loop over all points except boundary points
    for(int i = 1; i < (stepnum-1); i++){

    //Decleare and initialize some temporare variables
    double temp_first_vorticity = first_vorticity[i];
    double temp_psi1    = psi[i+1];
    double temp_psi2    = psi[i-1];

    //Update first_vorticity
    first_vorticity = vorticity;

    //Calculate new vorticity
    vorticity[i] = temp_first_vorticity + (temp_psi2 - temp_psi1)*ingvild;

    }

    }//end for

}

void forward_diff_periodicb(double* psi, double* vorticity, int stepnum, double ola){

    //Saving some temporary values
    double temp_vorticity0 = vorticity[0];
    double temp_vorticity_end = vorticity[stepnum - 1];

    //loop over all interior points
    for(int i = 1; i < (stepnum-1); i++){

        //Save some temporary variables
        double temp_vorticity = vorticity[i];
        double temp_psi1    = psi[i+1];
        double temp_psi2    = psi[i-1];

        //Calculate vorticity at next timestep
        vorticity[i] = temp_vorticity + (temp_psi2 - temp_psi1)*ola;

    }//end for

    //Calculating boundary values
    vorticity[0] = temp_vorticity0 + (psi[stepnum - 2] - psi[1])*ola;
    vorticity[stepnum - 1] = vorticity[0];

}

void center_diff_periodicb(double*& psi, double*& vorticity, double*& first_vorticity, int stepnum, double ingvild, double ola, double time){

    //Calculate forward on first step
    if(time == 0){

    //Save some temporary variables
    double temp_vorticity0 = vorticity[0];
    double temp_vorticity_end = vorticity[stepnum - 1];

    //Calculate first time step with loop over all points except boundary points
    for(int i = 1; i < (stepnum-1); i++){

    double temp_vorticity = vorticity[i];
    double temp_psi1    = psi[i+1];
    double temp_psi2    = psi[i-1];

    vorticity[i] = temp_vorticity + (temp_psi2 - temp_psi1)*ola;

    }

    //Calculating boundary values
    vorticity[0] = temp_vorticity0 + (psi[stepnum-2] - psi[1])*ola;
    vorticity[stepnum - 1] = vorticity[0];

    }

    //Centered on next steps
    if(time != 0){

    //Calculate next time step with loop over all points except boundary points
    for(int i = 1; i < (stepnum-1); i++){

    //Decleare and initialize some temporare variables
    double temp_first_vorticity = first_vorticity[i];
    double temp_psi1    = psi[i+1];
    double temp_psi2    = psi[i-1];

    //Savce current vorticity
    first_vorticity = vorticity;

    //Calculate new vorticity
    vorticity[i] = temp_first_vorticity + (temp_psi2 - temp_psi1)*ingvild;

    }

    //Calculating boundary values
    vorticity[0] = first_vorticity[0] + (psi[stepnum-2] - psi[1])*ingvild;
    vorticity[stepnum - 1] = first_vorticity[stepnum-1] + (psi[stepnum - 2] - psi[1])*ingvild;

    }//end else

}

void tridiagonal_periodicb(double*& psi, double*& force, double*& e1, double*& e2, double*& d, double dx, int stepnum){

    //Initializing our matrix and the forcing term.
    for(int i = 0 ; i < stepnum; i++){

        e1[i] = 1.0;
        e2[i] = 1.0;
        d[i] = -2.0;

    }//end for

    //Calculating lower endpoint dtile and ytilde explicit with periodic boundary conditions (The Jostein condition: stepnum - 2 = much better )
    d[0] -= (e1[0]*e2[stepnum-2])/d[stepnum-2];
    force[0] -= ((e1[stepnum-2])*(force[stepnum-2]))/d[stepnum-2];

    //Forward substitution algorithm
    for(int i = 1; i < stepnum ; i++){

         //initializing dtilde
         d[i] -= (e1[i]*e2[i-1])/d[i-1];

         //initializing ytilde
         force[i] -= ((e1[i])*(force[i-1]))/d[i-1];

    }//end for

    //Calculating upper endpoint explicit with periodic jostein boundary conditions
    //error arises from psi[1] which is taken from timestep n and not n+1.
    psi[stepnum-1] = (force[stepnum-1] - (e2[stepnum-2]*(psi[1])))/d[stepnum-2];

    //Backward substitution with ytilde and dtilde
    for(int i = (stepnum-2); i >= 0; i--){

        psi[i] = (force[i] - (e2[i]*(psi[i+1])))/d[i];

    }//end for

    //prints the error using the modified jostein method
    cout << "error" << fabs(psi[0] - psi[stepnum-1]) << endl;
}

//Comment out due to no armadillo in forskningsparken
/*void Armadillo_1D_periodic_solver(double*& psi, double*& force, int stepnum){

    //initializing vectors and matrix
    vec psivec(stepnum);
    vec forcevec(stepnum-1);
    vec dvec(stepnum-1, fill::ones);
    vec evec(stepnum-2, fill::ones);
    mat A(stepnum-1, stepnum-1, fill::zeros);

    //for loop to convert force from array to armadillo vectors
    for(int i = 0; i < (stepnum-1); i++){

        forcevec(i) = -force[i];

    }//end for

    //Initializing our matrix and the forcing term.
    A.diag() = 2*dvec;
    A.diag(1)= -evec;
    A.diag(-1)= -evec;
    A(0,stepnum-2) = -1;
    A(stepnum-2,0) = -1;

    //Solve system with armadillo-function
    vec solution = solve(A, forcevec);

	for(int i=0; i<stepnum-1;i++) {
		psi[i] = solution(i);
    }

	psi[stepnum-1] = solution(0);

}//end function

*/
//----------------------------------- 2D functions -----------------------------------------------------//

void initialize2D(double **&psi2D, double **&vorticity2D, int stepnum, double dx, int option){

    psi2D = new double*[stepnum];
    vorticity2D = new double*[stepnum];

    if(option == 1){
    for(int j = 0; j < stepnum; j++){

        psi2D[j] = new double[stepnum];
        vorticity2D[j] = new double[stepnum];

        for(int i = 0; i < stepnum; i++){

                psi2D[j][i] = sin(M_PI*j*dx)*sin(4*M_PI*i*dx);  //Assumed a periodicity in y direction as well
                vorticity2D[j][i] = -(17)*M_PI*M_PI*sin(M_PI*j*dx)*sin(4*M_PI*i*dx); //Vorticity = second derivative of psi.

                if((i==0)|| (j==0)){

                    psi2D[j][i] = 0.0;
                    vorticity2D[j][i] = 0.0;
                }
        }//end i
    }//end j
    }//end option 1

    if(option == 2){
    for(int j = 0; j < stepnum; j++){

        psi2D[j] = new double[stepnum];
        vorticity2D[j] = new double[stepnum];

        for(int i = 0; i < stepnum; i++){

                psi2D[j][i] = exp(-(pow(((dx*i - 0.5)/0.1),2)))*(exp(-(pow(((dx*j - 0.5)/0.1),2)))); //Assume exp in y dir as well
                vorticity2D[j][i] = exp(-100*(-0.5 + i*dx)*(-0.5 + i*dx) - 100*(-0.5 + j*dx)*(-0.5 + j*dx))*(19600. - 40000.*i*dx + 40000.*(i*dx)*(i*dx) - 40000.*(j*dx) + 40000.*(j*dx)*(j*dx)); //Vorticity = second derivative of psi.

                if((i==0)||(j==0)){

                    psi2D[j][i] = 0.0;
                    vorticity2D[j][i] = 0.0;

                }

        }//end i
    }//end j
    }//end option 2

}

void output2d(double**& psi2D, double**& vorticity2D, int stepnum){

    for(int j = 0; j < stepnum; j++){

        //Writes the psi and vorticity to each file
        for(int i = 0; i < stepnum; i++){
        ofile << ' ' << psi2D[j][i];
        }
        ofile << endl;
    }

}

void forward_diff_solidb_2D(double** psi2D, double** vorticity2D, int stepnum, double ola){

    for(int j = 0; j<stepnum; j++){

        //Set the boundary conditions for solid walls (just to be sure!)
        psi2D[j][0] = 0;
        psi2D[j][stepnum-1] = 0;

        //loop over all interior points
        for(int i = 1; i < (stepnum-1); i++){

        double temp_vorticity = vorticity2D[j][i];
        double temp_psi1    = psi2D[j][i+1];
        double temp_psi2    = psi2D[j][i-1];

        vorticity2D[j][i] = temp_vorticity + (temp_psi2 - temp_psi1)*ola;

        }//end for col

    }//end for rows

}

void centered_diff_solidb_2D(double**& psi2D, double**& vorticity2D, double**& first_vorticity2D, int stepnum, double ingvild, double ola, double time){

//forward solution in the first step
if (time == 0){

    first_vorticity2D = vorticity2D;

    for(int j = 0; j<stepnum; j++){

    //Set the boundary conditions for solid walls (just to be sure!)
    psi2D[j][0] = 0;
    psi2D[j][stepnum-1] = 0;

    //loop over all interior points
    for(int i = 1; i < (stepnum-1); i++){

    double temp_vorticity = vorticity2D[j][i];
    double temp_psi1    = psi2D[j][i+1];
    double temp_psi2    = psi2D[j][i-1];

    vorticity2D[j][i] = temp_vorticity + (temp_psi2 - temp_psi1)*ola;
    }//end for i
    }//end for j
}//end if time == 0

else{

    for(int j = 0; j<stepnum; j++){

    //Calculate first time step with loop over all points except boundary points
    for(int i = 1; i < (stepnum-1); i++){

    //Decleare and initialize some temporare variables
    double temp_first_vorticity = first_vorticity2D[j][i];
    double temp_psi1    = psi2D[j][i+1];
    double temp_psi2    = psi2D[j][i-1];

    //Update first_vorticity
    first_vorticity2D = vorticity2D;

    //Calculate new vorticity
    vorticity2D[j][i] = temp_first_vorticity + (temp_psi2 - temp_psi1)*ingvild;

    }//end for i
    }//end for j
}//end else

}

void forward_diff_periodicb_2D(double**& psi2D, double**& vorticity2D, double**& temp_vorticity2D, int stepnum, double ola){

    //Update first_vorticity temporarly array
    temp_vorticity2D = vorticity2D;

    for(int j = 0; j<stepnum; j++){

	//Set the periodic boundaries at right and left endpoints for all rows
	vorticity2D[j][0] = temp_vorticity2D[j][0] + (psi2D[j][stepnum - 2] - psi2D[j][1])*ola;
    	vorticity2D[j][stepnum-1] = temp_vorticity2D[j][stepnum-1] + (psi2D[j][stepnum - 2] - psi2D[j][1])*ola;

        //loop over all interior points
        for(int i = 1; i < (stepnum-1); i++){

        vorticity2D[j][i] = temp_vorticity2D[j][i] + (psi2D[j][i-1]- psi2D[j][i+1])*ola;

	}//end for col

    }//end for rows

}

void Jacobi_Iterative_solidb_2D(double**& psi2D, double**& psi2D_temp, double**& vorticity2D, int stepnum, double jostein, double hanne, double jostein2){

//Declearing the iteration limit and counter
int maxiter = 100;
int iter = 0;

//Calculate next step by iterative method which uses the four neighbours
while(iter < maxiter){

    //Declaring temporary psi
    psi2D_temp = psi2D;

    //Calculate the interior psi's. Endpoint just stays zero.
    for(int j = 1; j < (stepnum -1); j++){
        for(int i = 1; i < (stepnum - 1); i++){

        psi2D[j][i] = (jostein*((psi2D_temp[j][i+1] + psi2D_temp[j][i-1]) + (psi2D_temp[j+1][i] + psi2D_temp[j-1][i])) - jostein2*vorticity2D[j][i])/hanne;

        }//end i
    }//end j

 //Update iteration counter
 iter += 1;

}//end iteration while

}

void Jacobi_Iterative_periodicb_2D(double**& psi2D, double**& psi2D_temp, double**& vorticity2D, int stepnum, double jostein, double hanne, double jostein2){

//Declearing the iteration limit and counter
int maxiter = 100;
int iter = 0;

//Calculate next step by iterative method which uses the four neighbours. Brute force and ugly code. sorry sorry sorry.
while(iter < maxiter){

    //Declaring temporary psi
    psi2D_temp = psi2D;

    for(int j = 0; j < (stepnum); j++){
        for(int i = 0; i < (stepnum); i++){

	//The points and corners on left border ----------------------------------------------------------------------//
        if((i == 0)){

		//upper left corner. OK
		if(j==(stepnum-1)){
	        		psi2D[j][i] = (jostein*((psi2D_temp[j][1] + psi2D_temp[j][stepnum-2]) + (psi2D_temp[1][i] + 			psi2D_temp[j-1][i])) - jostein2*vorticity2D[j][i])/hanne;
		}

		//lower left corner OK
		else if(j==0){
        	psi2D[j][i] = (jostein*((psi2D_temp[j][i+1] + psi2D_temp[j][stepnum-2]) + (psi2D_temp[j+1][i] + psi2D_temp[stepnum-2][i])) - 			jostein2*vorticity2D[j][i])/hanne;
		}

		//Internal points on left border OK
		else if(j != 0 && j!= (stepnum-1)){
		psi2D[j][i] = (jostein*((psi2D_temp[j][i+1] + psi2D_temp[j][stepnum - 2]) + (psi2D_temp[j+1][i] + psi2D_temp[j-1][i])) - 			jostein2*vorticity2D[j][i])/hanne;

        	}
	}//end left endpoints

	//Points and corners on right border----------------------------------------------------------------------//
        if(i == (stepnum-1)){

		//upper right corner OK
		if(j == (stepnum-1)){
		psi2D[j][i] = (jostein*((psi2D_temp[j][1] + psi2D_temp[j][i-1]) + (psi2D_temp[1][i] + psi2D_temp[j-1][i])) - 			jostein2*vorticity2D[j][i])/hanne;
		}

		//lower right corner OK
		else if(j == 0){
        	psi2D[j][i] = (jostein*((psi2D_temp[j][1] + psi2D_temp[j][i-1]) + (psi2D_temp[j+1][i] + psi2D_temp[stepnum-2][i])) -    	  	jostein2*vorticity2D[j][i])/hanne;
		}

		//Internal points on right border OK
		else if(j != 0 && j!= (stepnum-1)){
		psi2D[j][i] = (jostein*((psi2D_temp[j][1] + psi2D_temp[j][i-1]) + (psi2D_temp[j+1][i] + psi2D_temp[j-1][i])) - 			jostein2*vorticity2D[j][i])/hanne;
		}
        }//end right endpoints

	//Points on upper border------------------------------------------------------------------------//
        if(j == (stepnum-1)){

		//interior points on upper border (corners already been taken)	OK
		if(i != 0 && i != (stepnum-1)){
		psi2D[j][i] = (jostein*((psi2D_temp[j][i+1] + psi2D_temp[j][i-1]) + (psi2D_temp[1][i] + psi2D_temp[j-1][i])) - 	  			jostein2*vorticity2D[j][i])/hanne;}

        	}//end upper endpoints

	//Points on lower border ------------------------------------------------------------------------//
        if(j == 0){

		//interior points on lower border (corners already been taken) OK
		if(i != 0 && i != (stepnum-1)){
		psi2D[j][i] = (jostein*((psi2D_temp[j][i+1] + psi2D_temp[j][i-1]) + (psi2D_temp[j+1][i] + psi2D_temp[stepnum-2][i])) - 			jostein2*vorticity2D[j][i])/hanne;
		}
        	}//end lower endpoints

	//Interior interior points. hehe.---------------------------------------------------------------//
	if(i != 0 && j!= 0 && j != (stepnum-1) && i != (stepnum-1))  {
        psi2D[j][i] = (jostein*((psi2D_temp[j][i+1] + psi2D_temp[j][i-1]) + (psi2D_temp[j+1][i] + psi2D_temp[j-1][i])) -
        jostein2*vorticity2D[j][i])/hanne;
	}

      }//end i
    }//end j

 //Update iteration counter
 iter += 1;

}//end iteration while

}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm accumulator variance
#include <boost/test/unit_test.hpp>
#include "fern/algorithm/accumulator/variance.h"

namespace faa = fern::algorithm::accumulator;

// TODO Once we have a full blown square algorithm we must use that
//      instead of this one.
template<
    typename T>
inline constexpr T square(
    T const& value)
{
    return value * value;
}

BOOST_AUTO_TEST_CASE(default_construct)
{
    faa::Variance<int> variance;
}

BOOST_AUTO_TEST_CASE(accumulate)
{
    {
        faa::Variance<int> variance(5);
        // 5
        BOOST_CHECK_EQUAL(variance(), (
            square(5 - 5)) / 1);

        variance(2);
        // 5 2
        BOOST_CHECK_EQUAL(variance(), (
            square(3 - 5) +
            square(3 - 2)) / 2);

        variance(3);
        // 5 2 3
        BOOST_CHECK_EQUAL(variance(), (
            square(3 - 5) +
            square(3 - 2) +
            square(3 - 3)) / 3);

        variance = 8;
        // 8
        BOOST_CHECK_EQUAL(variance(), (
            square(8 - 8)) / 1);
    }

    {
        faa::Variance<int, double> variance(5);
        // 5
        BOOST_CHECK_EQUAL(variance(), (
            square(5.0 - 5.0)) / 1.0);

        variance(2);
        // 5 2
        BOOST_CHECK_EQUAL(variance(), (
            square(3.5 - 5.0) +
            square(3.5 - 2.0)) / 2.0);

        variance = 3;
        // 3
        BOOST_CHECK_EQUAL(variance(), (
            square(3.0 - 3.0)) / 1.0);
    }
}

BOOST_AUTO_TEST_CASE(merge)
{
    {
        auto variance(faa::Variance<int>(15) | faa::Variance<int>(5));
        // 15 5
        BOOST_CHECK_EQUAL(variance(), (
            square(10 - 15) +
            square(10 - 5)) / 2);
    }

    {
        auto variance(faa::Variance<int, double>(5) |
            faa::Variance<int, double>(20));
        // 5 20
        BOOST_CHECK_EQUAL(variance(), (
            square(12.5 - 5.0) +
            square(12.5 - 20.0)) / 2.0);
    }
}


#include <possumwood_sdk/node_implementation.h>
#include <tbb/blocked_range.h>
#include <tbb/parallel_reduce.h>

#include <boost/noncopyable.hpp>

#include "sequence.h"

namespace {

dependency_graph::InAttr<possumwood::opencv::Sequence> a_in;
dependency_graph::OutAttr<possumwood::opencv::Frame> a_out;

struct Reduce : public boost::noncopyable {
	explicit Reduce(const possumwood::opencv::Sequence& seq) {
		for(auto& f : seq)
			mats.push_back(*f);  // "shallow" copy
	}

	// splitting constructor - copies the input matrices but there is no need to initialise the accumulator
	// as it will get overwritten anyway
	Reduce(const Reduce& r, tbb::split) : mats(r.mats) {
	}

	Reduce(const Reduce&) = delete;
	Reduce& operator=(const Reduce&) = delete;

	/// converts a range of input images int double format, and sums them together
	void operator()(const tbb::blocked_range<std::size_t>& range) {
		assert(!range.empty());

		auto it = range.begin();

		if(accum.empty()) {
			mats[it].convertTo(accum, CV_MAKETYPE(CV_64F, mats[it].channels()));
			++it;
		}

		cv::Mat tmp;
		for(; it != range.end(); ++it) {
			mats[it].convertTo(tmp, CV_MAKETYPE(CV_64F, mats[it].channels()));
			accum += tmp;
		}
	}

	// combine two results together
	void join(const Reduce& r) {
		assert(accum.rows == r.accum.rows && accum.cols == r.accum.cols);
		accum += r.accum;
	}

	std::vector<cv::Mat> mats;  // input matrices (shallow copy)
	cv::Mat accum;              // resulting accumulator
};

dependency_graph::State compute(dependency_graph::Values& data) {
	const possumwood::opencv::Sequence& in = data.get(a_in);

	if(!in.isValid())
		throw std::runtime_error("Input sequence does not have consistent size or type.");

	cv::Mat out;

	if(in.size() > 1) {
		// parallelized reduction implementing the accumulation and data conversion on multiple threads
		Reduce r(in);
		tbb::parallel_reduce(tbb::blocked_range<std::size_t>(0, in.size()), r);

		// normalize the result of the accumulation, and convert back to the original type
		r.accum /= (double)in.size();
		r.accum.convertTo(out, in[0]->type());
	}

	else
		throw std::runtime_error("At least two frames in the input sequence required!");

	data.set(a_out, possumwood::opencv::Frame(out));

	return dependency_graph::State();
}

void init(possumwood::Metadata& meta) {
	meta.addAttribute(a_in, "in");
	meta.addAttribute(a_out, "out", possumwood::opencv::Frame(), possumwood::AttrFlags::kVertical);

	meta.addInfluence(a_in, a_out);

	meta.setCompute(compute);
}

possumwood::NodeImplementation s_impl("opencv/sequence/mean", init);

}  // namespace


/**
 *      @file slowMetropolis.cpp
 *      @date May 23, 2012
 *      @author Brian Peasley
 *      @author Frank Dellaert
 */

#include "OccupancyGrid/OccupancyGrid.h"
#include "OccupancyGrid/visualiser.h"
#include "OccupancyGrid/TwoAssumptionAlgorithm.h"
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

using namespace std;
using namespace gtsam;

/**
 * @brief Run a metropolis sampler.
 * @param iterations defines the number of iterations to run.
 * @return  vector of marginal probabilities.
 */
OccupancyGrid::Marginals runSlowMetropolis(const OccupancyGrid &occupancyGrid,
    size_t iterations, double max_clock) {

  // Create a data structure to hold the estimated marginal occupancy probabilities
  // and initialize to zero.
  size_t size = occupancyGrid.cellCount();
  OccupancyGrid::Marginals marginals(size);
  for (size_t it = 0; it < marginals.size(); it++)
    marginals[it] = 0;

  // Initialize randum number generator
  size_t nrows = occupancyGrid.height();
  //size_t ncols = occupancyGrid.width();
  boost::mt19937 rng;
  boost::uniform_int < Index > random_cell(0, size - 1);
  double sigma = 0.05 * nrows ; // next sample point will be within 2*5 cells (95% of the times)

  boost::normal_distribution< double > normal_dist(0, sigma);
  boost::variate_generator<boost::mt19937&,
    boost::normal_distribution< double > > var_nor(rng, normal_dist);

  // double dsize   = static_cast<double>(size);
  // double dheight = floor(sqrt(dsize));
  // size_t height  = static_cast<size_t>(dheight);
  // size_t width   = static_cast<size_t>(floor(dsize/dheight));
  clock_t st = clock();

  // Create empty occupancy as initial state and
  // compute initial neg log-probability of occupancy grid, - log P(x_t)
  LaserFactor::Occupancy occupancy = occupancyGrid.emptyOccupancy();
  std::vector<double> two_energy(occupancyGrid.cellCount());
  two_assumption_algorithm(occupancyGrid, occupancy, two_energy);

  double Ex = occupancyGrid(occupancy);
  global_vis_.init(occupancyGrid.height(), occupancyGrid.width());

  // for logging
  vector<double> energy;

  // Choose a random cell
  //Index x_;// = random_cell(rng);

  // run Metropolis for the requested number of operations
  for (size_t it = 0; it < iterations && clock() < (max_clock - st); it++) {

    // Log and print
    energy.push_back(Ex);
    if (it % 100 == 0) {
      clock_t et = clock();
      std::cout << "<Energy>\t" << ((float)(et - st)) / CLOCKS_PER_SEC << "\t" << Ex << std::endl;
      printf("%lf\n", (double) it / (double) iterations);

      if (it % 10000) {
        global_vis_.enable_show();
        global_vis_.reset();
        global_vis_.setMarginals(marginals);
        global_vis_.show();
      }
    }

    // Sample a point close to the previous point with gaussian probability
    // This is a heuristic strategy that high occupancy regions (high
    // probability) are going to be few but close together in space. This is
    // not same as the choosing the proposal distribution for metropolis
    // algorithm as the space we are working is a 100x100 dimensional space
    // rather than a 2D space. But some of the properties of this 2D space can
    // be made use of. This idea is similar to that of a heat map.
    // double col = x % ncols;
    // double row = x / ncols;
    // row += var_nor();
    // col += var_nor();
    // Index row_lu = (row < 0) ? 0
    //   : (row >= nrows) ? nrows - 1
    //   : static_cast<Index>(row);
    // Index col_lu = (col < 0) ? 0
    //   : (col >= ncols) ? ncols - 1
    //   : static_cast<Index>(col);
    // Index x_prime = row_lu * occupancyGrid.width() + col_lu;
    Index x_prime = random_cell(rng);

    // Compute neg log-probability of new occupancy grid, -log P(x')
    // by summing over all LaserFactor::operator()
    double oldValue = occupancy[x_prime];
    double deltaEx =
      occupancyGrid.computeDelta(occupancy, x_prime, 1 - occupancy[x_prime]);
    assert(occupancy[x_prime] == oldValue);

    // Calculate acceptance ratio, a
    // See e.g. MacKay 96 "Intro to Monte Carlo Methods"
    // a = P(x')/P(x) = exp {-E(x')} / exp {-E(x)} = exp {E(x)-E(x')}
    double a = exp(- deltaEx);

    // If a <= 1 otherwise accept with probability a
    double rn = static_cast<double>(std::rand()) / (RAND_MAX);
    bool accept = (a>=1) ? true // definitely accept
      : (a >= rn) ?  true       // accept with probability a
      : false;

    //printf("%lu : %lu; accepted: %d\n", x_prime, occupancy.at(x_prime), accept);
    if (accept) {
      Ex += deltaEx;
      //x_ = x_prime;
      // we accept: flip it !
      occupancy[x_prime] = 1 - occupancy[x_prime];
    } else {
      //x_ = random_cell(rng);
    }

    //increment the number of iterations each cell has been on
    for (size_t i = 0; i < size; i++) {
      if (occupancy[i] == 1)
        marginals[i]++;
    }
  }

  FILE *fptr = fopen("Data/Metropolis_Energy.txt", "w");
  for (size_t i = 0; i < iterations; i++)
    fprintf(fptr, "%lf ", energy[i]);

  //compute the marginals
  for (size_t it = 0; it < size; it++)
    marginals[it] /= iterations;

  return marginals;
}


#include <algorithm>
#include <fstream>
#include <functional>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <map>
#include <numeric>
#include <sstream>
#include <string>
#include <vector>
#include <deque>

#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/function_input_iterator.hpp>
#include <boost/iterator/filter_iterator.hpp>
//#include <boost/bind.hpp>
//
namespace ph=std::placeholders;

using namespace std;

void init_flags(deque<bool>& flags) {
  if (flags.size() > 0) flags[0] = false;
  if (flags.size() > 1) flags[1] = false;
}

void cross_off(deque<bool>& flags, int prime) {
  for (int i = prime * prime; i < flags.size(); i += prime) {
    flags[i] = false;
  }
}

int get_next_prime(deque<bool>& flags, int prime) {
  int next = prime + 1;
  while (next < flags.size() && !flags[next]) {
    next++;
  }
  return next;
}

void sieve_of_eratos_thenes(int max, std::deque<bool>& flags) {
  flags.assign(max, true);
  init_flags(flags);

  int prime = 2;
  int count = 0;

  while (prime <= max) {
    cross_off(flags, prime);
    prime = get_next_prime(flags, prime);
    if (prime >= flags.size()) {
      break;
    }
  }
}

typedef std::pair<int, bool> PrimePair;

int my_f(int a, int b, int c) { return a * b * c; }

PrimePair my_make_pair(int a, bool b) {
  return std::make_pair(a, b);
}

void write_primes(int max) {
  std::deque<bool> flags;
  sieve_of_eratos_thenes(max, flags);

  std::vector<PrimePair> pairs;
  std::transform(boost::counting_iterator<int>(0),
            boost::counting_iterator<int>(max),
            flags.begin(),
            back_inserter(pairs),
            make_pair<const int&, const bool&>);

  std::transform(
      boost::make_filter_iterator(std::bind(&PrimePair::second, ph::_1), pairs.begin(), pairs.end()),
      boost::make_filter_iterator(std::bind(&PrimePair::second, ph::_1), pairs.end(), pairs.end()),
      ostream_iterator<int>(cout, " "),
      std::bind(&PrimePair::first, ph::_1));
}

int main(int argc, char **argv) {
  write_primes(1000);
  cout << endl;

  return 0;
}