/** \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


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


/*
 * 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

}


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


#pragma once

#include <cmath>
#include <vector>
#include <stdexcept>
#include <string.h>
#include <Eigen/Core>

#include "traits.hh"
#include "../util/assert.hh"

namespace bold
{
  template<typename T>
  class MovingAverage
  {
  public:
    MovingAverage(unsigned short windowSize)
    : d_items(new T[windowSize]),
      d_length(0),
      d_nextPointer(0),
      d_sum(),
      d_windowSize(windowSize)
    {
      if (windowSize == 0)
        throw new std::runtime_error("Cannot have zero window size.");
      AveragingTraits::zero(d_sum);
    }

    MovingAverage(MovingAverage const& other)
    {
      copy(other);
    }

    ~MovingAverage()
    {
      destroy();
    }

    MovingAverage& operator=(MovingAverage const& other)
    {
      destroy();
      copy(other);
      return *this;
    }

    int count() const { return d_length; }
    int getWindowSize() const { return d_windowSize; }
    bool isMature() const { return d_length == d_windowSize; }

    T next(T value)
    {
      if (d_length == d_windowSize)
      {
        d_sum -= d_items[d_nextPointer];
      }
      else
      {
        d_length++;
      }

      d_items[d_nextPointer] = value;
      d_sum += value;
      d_nextPointer = (d_nextPointer + 1) % d_windowSize;

      d_avg = d_sum / int{d_length};

      return d_avg;
    }

    void reset()
    {
      d_length = 0;
      d_nextPointer = 0;
      AveragingTraits::zero(d_sum);
    }

    T getAverage() const { return d_avg; }

    T calculateStdDev()
    {
      // TODO unit test this
      T sum;
      AveragingTraits::zero(sum);
      for (int i = 0; i < d_length; i++)
      {
        int index = (d_nextPointer - i - 1) % d_windowSize;
        if (index < 0)
          index += d_windowSize;
        ASSERT(index >= 0 && index < d_length);
        T diff = d_items[index] - d_avg;
        sum += diff * diff;
      }
      return sqrt(sum / d_length);
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  private:
    T* d_items;
    unsigned short d_length;
    int d_nextPointer;
    T d_sum;
    T d_avg;
    unsigned short d_windowSize;

    void destroy()
    {
      delete[] d_items;
    }

    void copy(MovingAverage const& other)
    {
      d_windowSize = other.d_windowSize;
      d_items = new T[other.d_windowSize];
      for (unsigned i = 0; i < d_windowSize; ++i)
        d_items[i] = other.d_items[i];

      d_length = other.d_length;
      d_nextPointer = other.d_nextPointer;
      d_sum = other.d_sum;
      d_avg = other.d_avg;
    }
  };

  template <>
  inline Eigen::Vector3d MovingAverage<Eigen::Vector3d>::calculateStdDev()
  {
    Eigen::Matrix3d sum = Eigen::Matrix3d::Zero();
    for (int i = 0; i < d_length; i++)
    {
      int index = (d_nextPointer - i - 1) % d_windowSize;
      if (index < 0)
        index += d_windowSize;
      ASSERT(index >= 0 && index < d_length);
      Eigen::Vector3d diff = d_items[index] - d_avg;
      sum += diff * diff.transpose();
    }
    Eigen::Vector3d r = sum.diagonal();
    for (int i = 0; i < r.size(); i++)
      r[i] = sqrt(r[i]);
    return r / d_length;
  }

  template <>
  inline Eigen::Vector2d MovingAverage<Eigen::Vector2d>::calculateStdDev()
  {
//     std::cout << "------------------------" << std::endl;
//     std::cout << "d_avg = " << d_avg.transpose() << std::endl;
    Eigen::Matrix2d sum = Eigen::Matrix2d::Zero();
    for (int i = 0; i < d_length; i++)
    {
      int index = (d_nextPointer - i - 1) % d_windowSize;
      if (index < 0)
        index += d_windowSize;
      ASSERT(index >= 0 && index < d_length);
      Eigen::Vector2d diff = d_items[index] - d_avg;
      sum += diff * diff.transpose();
//       std::cout << "---- index " << index << std::endl
//           << "diff " << diff.transpose() << std::endl
//           << "sum" << std::endl << sum << std::endl;
    }
    Eigen::Vector2d r = sum.diagonal();
//     std::cout << "r = " << r.transpose() << std::endl;
    for (int i = 0; i < r.size(); i++)
      r[i] = sqrt(r[i]);
//     std::cout << "r_squared = " << r.transpose() << std::endl;
//     std::cout << "stddev = " << (r/d_length).transpose() << std::endl;
    return r / d_length;
  }
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2015 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------
 *
 * based on deal.II step-2
 */

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_renumbering.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/manifold_lib.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/sparse_matrix.h>

#include <fstream>

using namespace dealii;

void
show_statistics(const SparsityPattern &sparsity_pattern)
{
  Vector<int> v;
  double      sum = 0.0;
  for (unsigned int i = 0; i < sparsity_pattern.n_cols(); ++i)
    {
      sum += sparsity_pattern.row_length(i);
    }

  std::cout << "Statistics for the current sparsity pattern:"
            << "\n"
            << "bandwidth: " << sparsity_pattern.bandwidth() << "\t"
            << " number of unknowns : " << sparsity_pattern.n_cols() << "\t"
            << "average number of of entries per row"
            << sum / sparsity_pattern.n_cols() << "\n";
}

void
make_grid(Triangulation<2> &triangulation)
{
  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 5);

  static const SphericalManifold<2> manifold_description(center);
  triangulation.set_all_manifold_ids(0);
  triangulation.set_manifold(0, manifold_description);

  for (unsigned int step = 0; step < 3; ++step)
    {
      Triangulation<2>::active_cell_iterator cell =
                                               triangulation.begin_active(),
                                             endc = triangulation.end();

      for (; cell != endc; ++cell)
        for (unsigned int v = 0; v < GeometryInfo<2>::vertices_per_cell; ++v)
          {
            const double distance_from_center =
              center.distance(cell->vertex(v));

            if (std::fabs(distance_from_center - inner_radius) < 1e-10)
              {
                cell->set_refine_flag();
                break;
              }
          }

      triangulation.execute_coarsening_and_refinement();
    }
}

void
distribute_dofs(DoFHandler<2> &dof_handler)
{
  static const FE_Q<2> finite_element(
    1); // degree of poly 1: bilinear element, 2:bi-quadratic element, ...
  // std::cout << "Value at (1,1) "
  //           << finite_element.shape_value(2, Point<2>(1, 1)) << "\n";

  dof_handler.distribute_dofs(
    finite_element); // we have associated a degree of freedom with a global
                     // number to each vertex

  DynamicSparsityPattern dynamic_sparsity_pattern(dof_handler.n_dofs(),
                                                  dof_handler.n_dofs());

  DoFTools::make_sparsity_pattern(dof_handler, dynamic_sparsity_pattern);

  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from(dynamic_sparsity_pattern);
  std::cout << "Number of entries per row in the sparsity pattern"
            << "\n";
  // SparseMatrix<double> system_matrix;
  // system_matrix.reinit(sparsity_pattern);

  for (unsigned int i = 0; i < dof_handler.n_dofs(); ++i)
    {
      std::cout << "Length of " << i << "-th row"
                << "\t" << sparsity_pattern.row_length(i) << "\n";
    }

  show_statistics(sparsity_pattern);
  std::ofstream out("sparsity_pattern1.svg");
  sparsity_pattern.print_svg(out);

  std::cout
    << "Printing all the non-zero entries for row 42 before renumbering: "
    << "\n";

  auto start_row_42 = sparsity_pattern.begin(42);
  auto end_row_42   = sparsity_pattern.end(42);
  for (auto it = start_row_42; it != end_row_42; ++it)
    {
      std::cout << (*it).column() << "\n";
    }
}

void
renumber_dofs(DoFHandler<2> &dof_handler)
{
  DoFRenumbering::Cuthill_McKee(dof_handler);

  DynamicSparsityPattern dynamic_sparsity_pattern(dof_handler.n_dofs(),
                                                  dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler, dynamic_sparsity_pattern);

  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from(dynamic_sparsity_pattern);

  std::cout << "Row length for Line 42: " << sparsity_pattern.row_length(42)
            << "\n";

  show_statistics(sparsity_pattern); // Bonus
  std::ofstream out("sparsity_pattern2.svg");
  sparsity_pattern.print_svg(out);

  std::cout
    << "Printing all the non-zero entries for row 42 after renumbering: "
    << "\n";
  for (auto it = sparsity_pattern.begin(42); it != sparsity_pattern.end(42);
       ++it)
    {
      std::cout << (*it).column() << "\n";
    }
}

void
make_grid_square(Triangulation<2> &square)
{
  GridGenerator::hyper_cube(square, -1, 1);
  square.refine_global(3);
}

int
main()
{
  Triangulation<2> triangulation;
  make_grid(triangulation);

  DoFHandler<2> dof_handler(triangulation);

  distribute_dofs(dof_handler);
  renumber_dofs(dof_handler);

  // //Test with a square
  // Triangulation<2> square;
  // make_grid_square(square);

  // DoFHandler<2> dof_handler_square(square);

  // distribute_dofs(dof_handler_square);
  // renumber_dofs(dof_handler_square);
}


#include <tuple>
#include <vector>
#include <string>
#include <cmath>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <boost/optional/optional_io.hpp>

#include "CommonSetting.h"
#include "SamplingGrid.h"

using cgal = CGAL::Exact_predicates_inexact_constructions_kernel;

pl::SamplingGrid::SamplingGrid() : width(0), height(0) {}

pl::SamplingGrid::SamplingGrid(int w, int h) : width(w), height(h) {}

pl::SamplingGrid::SamplingGrid(CommonSetting common_setting) : SamplingGrid() {
  std::string sampling_grid = common_setting.val;

  auto t = pl::split(sampling_grid, 'x');
  width = std::stoi(t[0]);
  height = std::stoi(t[1]);
}

pl::SamplingGrid &pl::SamplingGrid::operator=(const SamplingGrid &cs) {
  width = cs.width;
  height = cs.height;
  return *this;
}

bool pl::SamplingGrid::isOutOfArea(const cgal::Point_2 p) const {
  return p.x() < 0 || width < p.x() || p.y() < 0 || height < p.y();
}

bool pl::SamplingGrid::isInsideOfArea(const cgal::Point_2 p) const {
  return (0 - 1.0e-10) <= p.x() && p.x() <= (width + 1.0e-10) && (0 - 1.0e-10) <= p.y() && p.y() <= (height + 1.0e-10);
}

std::tuple<cgal::Point_2, cgal::Point_2> pl::SamplingGrid::intersection(const cgal::Line_2 &line) const {
  std::vector<cgal::Segment_2> border = {
    {{width, height}, {0, height}},
    {{width, 0}, {width, height}},
    {{0, 0}, {width, 0}},
    {{0, height}, {0, 0}}
  };

  std::vector<cgal::Point_2> intersections;

  for (auto segment : border)
    if (const auto point = CGAL::intersection(cgal::Line_2{segment}, line); point && isInsideOfArea(boost::get<cgal::Point_2>(*point)))
      intersections.push_back(boost::get<cgal::Point_2>(*point));

  if (intersections.size() < 2 || 2 < intersections.size()) {
    std::cout << "intersection with border does have more then 2 or less then 2 intersections" << "\n";
    std::exit(-1);
  }

  return {intersections[0], intersections[1]};
}

cgal::Point_2 pl::SamplingGrid::center() const {
  cgal::Point_2 p(0, 0), q(width, 0), r(0, height);
  const auto circle = cgal::Circle_2(p, q, r);
  return circle.center();
}

std::vector<cgal::Point_2> pl::SamplingGrid::points() const {
  return {{0,0},{0,height},{width,height},{width,0}};
}


/*
 * NativeFilterMatrixOps.cpp
 *
 *  Created on: Sep 5, 2018
 *      Author: Georg Wiedebach
 */

#include <jni.h>
#include <Eigen/Dense>
#include "us_ihmc_ekf_filter_NativeFilterMatrixOpsWrapper.h"

using Eigen::MatrixXd;

typedef Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> JMatrixMap;

JNIEXPORT void JNICALL Java_us_ihmc_ekf_filter_NativeFilterMatrixOpsWrapper_computeABAt(JNIEnv *env, jobject thisObj, jdoubleArray result,
      jdoubleArray aData, jdoubleArray bData, jint n, jint m)
{
   jdouble *aDataArray = (jdouble*) env->GetPrimitiveArrayCritical(aData, NULL);
   jdouble *bDataArray = (jdouble*) env->GetPrimitiveArrayCritical(bData, NULL);
   jdouble *resultDataArray = (jdouble*) env->GetPrimitiveArrayCritical(result, NULL);

   JMatrixMap A(aDataArray, n, m);
   JMatrixMap B(bDataArray, m, m);
   JMatrixMap ABAt(resultDataArray, n, n);

   ABAt.noalias() = A * B * A.transpose();

   env->ReleasePrimitiveArrayCritical(aData, aDataArray, 0);
   env->ReleasePrimitiveArrayCritical(bData, bDataArray, 0);
   env->ReleasePrimitiveArrayCritical(result, resultDataArray, 0);
}

JNIEXPORT void JNICALL Java_us_ihmc_ekf_filter_NativeFilterMatrixOpsWrapper_predictErrorCovariance
  (JNIEnv *env, jobject thisObj, jdoubleArray result, jdoubleArray fData, jdoubleArray pData, jdoubleArray qData, jint n)
{
   jdouble *fDataArray = (jdouble*) env->GetPrimitiveArrayCritical(fData, NULL);
   jdouble *pDataArray = (jdouble*) env->GetPrimitiveArrayCritical(pData, NULL);
   jdouble *qDataArray = (jdouble*) env->GetPrimitiveArrayCritical(qData, NULL);
   jdouble *resultDataArray = (jdouble*) env->GetPrimitiveArrayCritical(result, NULL);

   JMatrixMap F(fDataArray, n, n);
   JMatrixMap P(pDataArray, n, n);
   JMatrixMap Q(qDataArray, n, n);
   JMatrixMap errorCovariance(resultDataArray, n, n);

   MatrixXd Qdiag = Q.diagonal().asDiagonal();
   errorCovariance.noalias() = F * P.selfadjointView<Eigen::Upper>() * F.transpose() + Qdiag;

   env->ReleasePrimitiveArrayCritical(fData, fDataArray, 0);
   env->ReleasePrimitiveArrayCritical(pData, pDataArray, 0);
   env->ReleasePrimitiveArrayCritical(qData, qDataArray, 0);
   env->ReleasePrimitiveArrayCritical(result, resultDataArray, 0);
}

JNIEXPORT void JNICALL Java_us_ihmc_ekf_filter_NativeFilterMatrixOpsWrapper_updateErrorCovariance
  (JNIEnv *env, jobject thisObj, jdoubleArray result, jdoubleArray kData, jdoubleArray hData, jdoubleArray pData, jint n, jint m)
{
   jdouble *kDataArray = (jdouble*) env->GetPrimitiveArrayCritical(kData, NULL);
   jdouble *hDataArray = (jdouble*) env->GetPrimitiveArrayCritical(hData, NULL);
   jdouble *pDataArray = (jdouble*) env->GetPrimitiveArrayCritical(pData, NULL);
   jdouble *resultDataArray = (jdouble*) env->GetPrimitiveArrayCritical(result, NULL);

   JMatrixMap K(kDataArray, m, n);
   JMatrixMap H(hDataArray, n, m);
   JMatrixMap P(pDataArray, m, m);
   JMatrixMap errorCovariance(resultDataArray, m, m);

   errorCovariance.noalias() = (MatrixXd::Identity(m, m) - K * H) * P.selfadjointView<Eigen::Upper>();

   env->ReleasePrimitiveArrayCritical(kData, kDataArray, 0);
   env->ReleasePrimitiveArrayCritical(hData, hDataArray, 0);
   env->ReleasePrimitiveArrayCritical(pData, pDataArray, 0);
   env->ReleasePrimitiveArrayCritical(result, resultDataArray, 0);
}

JNIEXPORT void JNICALL Java_us_ihmc_ekf_filter_NativeFilterMatrixOpsWrapper_computeKalmanGain
  (JNIEnv *env, jobject thisObj, jdoubleArray result, jdoubleArray pData, jdoubleArray hData, jdoubleArray rData, jint n, jint m)
{
   jdouble *pDataArray = (jdouble*) env->GetPrimitiveArrayCritical(pData, NULL);
   jdouble *hDataArray = (jdouble*) env->GetPrimitiveArrayCritical(hData, NULL);
   jdouble *rDataArray = (jdouble*) env->GetPrimitiveArrayCritical(rData, NULL);
   jdouble *resultDataArray = (jdouble*) env->GetPrimitiveArrayCritical(result, NULL);

   JMatrixMap P(pDataArray, m, m);
   JMatrixMap H(hDataArray, n, m);
   JMatrixMap R(rDataArray, n, n);
   JMatrixMap gain(resultDataArray, m, n);

   MatrixXd PHt = P.selfadjointView<Eigen::Upper>() * H.transpose();
   MatrixXd Rdiag = R.diagonal().asDiagonal();
   MatrixXd toInvert = H * PHt + Rdiag;
   gain.noalias() = PHt * toInvert.inverse();

   env->ReleasePrimitiveArrayCritical(pData, pDataArray, 0);
   env->ReleasePrimitiveArrayCritical(hData, hDataArray, 0);
   env->ReleasePrimitiveArrayCritical(rData, rDataArray, 0);
   env->ReleasePrimitiveArrayCritical(result, resultDataArray, 0);
}

JNIEXPORT void JNICALL Java_us_ihmc_ekf_filter_NativeFilterMatrixOpsWrapper_updateState
  (JNIEnv *env, jobject thisObj, jdoubleArray result, jdoubleArray xData, jdoubleArray kData, jdoubleArray rData, jint n, jint m)
{
   jdouble *xDataArray = (jdouble*) env->GetPrimitiveArrayCritical(xData, NULL);
   jdouble *kDataArray = (jdouble*) env->GetPrimitiveArrayCritical(kData, NULL);
   jdouble *rDataArray = (jdouble*) env->GetPrimitiveArrayCritical(rData, NULL);
   jdouble *resultDataArray = (jdouble*) env->GetPrimitiveArrayCritical(result, NULL);

   JMatrixMap x(xDataArray, n, 1);
   JMatrixMap K(kDataArray, n, m);
   JMatrixMap r(rDataArray, m, 1);
   JMatrixMap state(resultDataArray, n, 1);

   state.noalias() = x + K * r;

   env->ReleasePrimitiveArrayCritical(xData, xDataArray, 0);
   env->ReleasePrimitiveArrayCritical(kData, kDataArray, 0);
   env->ReleasePrimitiveArrayCritical(rData, rDataArray, 0);
   env->ReleasePrimitiveArrayCritical(result, resultDataArray, 0);
}


//  (C) Copyright Nick Thompson 2018.
//  Use, modification and distribution are subject to 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)

#ifndef BOOST_MATH_DIFFERENTIATION_FINITE_DIFFERENCE_HPP
#define BOOST_MATH_DIFFERENTIATION_FINITE_DIFFERENCE_HPP

/*
 * Performs numerical differentiation by finite-differences.
 *
 * All numerical differentiation using finite-differences are ill-conditioned, and these routines are no exception.
 * A simple argument demonstrates that the error is unbounded as h->0.
 * Take the one sides finite difference formula f'(x) = (f(x+h)-f(x))/h.
 * The evaluation of f induces an error as well as the error from the finite-difference approximation, giving
 * |f'(x) - (f(x+h) -f(x))/h| < h|f''(x)|/2 + (|f(x)|+|f(x+h)|)eps/h =: g(h), where eps is the unit roundoff for the type.
 * It is reasonable to choose h in a way that minimizes the maximum error bound g(h).
 * The value of h that minimizes g is h = sqrt(2eps(|f(x)| + |f(x+h)|)/|f''(x)|), and for this value of h the error bound is
 * sqrt(2eps(|f(x+h) +f(x)||f''(x)|)).
 * In fact it is not necessary to compute the ratio (|f(x+h)| + |f(x)|)/|f''(x)|; the error bound of ~\sqrt{\epsilon} still holds if we set it to one.
 *
 *
 * For more details on this method of analysis, see
 *
 * http://www.uio.no/studier/emner/matnat/math/MAT-INF1100/h08/kompendiet/diffint.pdf
 * http://web.archive.org/web/20150420195907/http://www.uio.no/studier/emner/matnat/math/MAT-INF1100/h08/kompendiet/diffint.pdf
 *
 *
 * It can be shown on general grounds that when choosing the optimal h, the maximum error in f'(x) is ~(|f(x)|eps)^k/k+1|f^(k-1)(x)|^1/k+1.
 * From this we can see that full precision can be recovered in the limit k->infinity.
 *
 * References:
 *
 * 1) Fornberg, Bengt. "Generation of finite difference formulas on arbitrarily spaced grids." Mathematics of computation 51.184 (1988): 699-706.
 *
 *
 * The second algorithm, the complex step derivative, is not ill-conditioned.
 * However, it requires that your function can be evaluated at complex arguments.
 * The idea is that f(x+ih) = f(x) +ihf'(x) - h^2f''(x) + ... so f'(x) \approx Im[f(x+ih)]/h.
 * No subtractive cancellation occurs. The error is ~ eps|f'(x)| + eps^2|f'''(x)|/6; hard to beat that.
 *
 * References:
 *
 * 1) Squire, William, and George Trapp. "Using complex variables to estimate derivatives of real functions." Siam Review 40.1 (1998): 110-112.
 */

#include <complex>
#include <boost/math/special_functions/next.hpp>

namespace boost{ namespace math{ namespace differentiation {

namespace detail {
    template<class Real>
    Real make_xph_representable(Real x, Real h)
    {
        using std::numeric_limits;
        // Redefine h so that x + h is representable. Not using this trick leads to large error.
        // The compiler flag -ffast-math evaporates these operations . . .
        Real temp = x + h;
        h = temp - x;
        // Handle the case x + h == x:
        if (h == 0)
        {
            h = boost::math::nextafter(x, (numeric_limits<Real>::max)()) - x;
        }
        return h;
    }
}

template<class F, class Real>
Real complex_step_derivative(const F f, Real x)
{
    // Is it really this easy? Yes.
    // Note that some authors recommend taking the stepsize h to be smaller than epsilon(), some recommending use of the min().
    // This idea was tested over a few billion test cases and found the make the error *much* worse.
    // Even 2eps and eps/2 made the error worse, which was surprising.
    using std::complex;
    using std::numeric_limits;
    constexpr const Real step = (numeric_limits<Real>::epsilon)();
    constexpr const Real inv_step = 1/(numeric_limits<Real>::epsilon)();
    return f(complex<Real>(x, step)).imag()*inv_step;
}

namespace detail {

   template <unsigned>
   struct fd_tag {};

   template<class F, class Real>
   Real finite_difference_derivative(const F f, Real x, Real* error, const fd_tag<1>&)
   {
      using std::sqrt;
      using std::pow;
      using std::abs;
      using std::numeric_limits;

      const Real eps = (numeric_limits<Real>::epsilon)();
      // Error bound ~eps^1/2
      // Note that this estimate of h differs from the best estimate by a factor of sqrt((|f(x)| + |f(x+h)|)/|f''(x)|).
      // Since this factor is invariant under the scaling f -> kf, then we are somewhat justified in approximating it by 1.
      // This approximation will get better as we move to higher orders of accuracy.
      Real h = 2 * sqrt(eps);
      h = detail::make_xph_representable(x, h);

      Real yh = f(x + h);
      Real y0 = f(x);
      Real diff = yh - y0;
      if (error)
      {
         Real ym = f(x - h);
         Real ypph = abs(yh - 2 * y0 + ym) / h;
         // h*|f''(x)|*0.5 + (|f(x+h)+|f(x)|)*eps/h
         *error = ypph / 2 + (abs(yh) + abs(y0))*eps / h;
      }
      return diff / h;
   }

   template<class F, class Real>
   Real finite_difference_derivative(const F f, Real x, Real* error, const fd_tag<2>&)
   {
      using std::sqrt;
      using std::pow;
      using std::abs;
      using std::numeric_limits;

      const Real eps = (numeric_limits<Real>::epsilon)();
      // Error bound ~eps^2/3
      // See the previous discussion to understand determination of h and the error bound.
      // Series[(f[x+h] - f[x-h])/(2*h), {h, 0, 4}]
      Real h = pow(3 * eps, static_cast<Real>(1) / static_cast<Real>(3));
      h = detail::make_xph_representable(x, h);

      Real yh = f(x + h);
      Real ymh = f(x - h);
      Real diff = yh - ymh;
      if (error)
      {
         Real yth = f(x + 2 * h);
         Real ymth = f(x - 2 * h);
         *error = eps * (abs(yh) + abs(ymh)) / (2 * h) + abs((yth - ymth) / 2 - diff) / (6 * h);
      }

      return diff / (2 * h);
   }

   template<class F, class Real>
   Real finite_difference_derivative(const F f, Real x, Real* error, const fd_tag<4>&)
   {
      using std::sqrt;
      using std::pow;
      using std::abs;
      using std::numeric_limits;

      const Real eps = (numeric_limits<Real>::epsilon)();
      // Error bound ~eps^4/5
      Real h = pow(11.25*eps, (Real)1 / (Real)5);
      h = detail::make_xph_representable(x, h);
      Real ymth = f(x - 2 * h);
      Real yth = f(x + 2 * h);
      Real yh = f(x + h);
      Real ymh = f(x - h);
      Real y2 = ymth - yth;
      Real y1 = yh - ymh;
      if (error)
      {
         // Mathematica code to extract the remainder:
         // Series[(f[x-2*h]+ 8*f[x+h] - 8*f[x-h] - f[x+2*h])/(12*h), {h, 0, 7}]
         Real y_three_h = f(x + 3 * h);
         Real y_m_three_h = f(x - 3 * h);
         // Error from fifth derivative:
         *error = abs((y_three_h - y_m_three_h) / 2 + 2 * (ymth - yth) + 5 * (yh - ymh) / 2) / (30 * h);
         // Error from function evaluation:
         *error += eps * (abs(yth) + abs(ymth) + 8 * (abs(ymh) + abs(yh))) / (12 * h);
      }
      return (y2 + 8 * y1) / (12 * h);
   }

   template<class F, class Real>
   Real finite_difference_derivative(const F f, Real x, Real* error, const fd_tag<6>&)
   {
      using std::sqrt;
      using std::pow;
      using std::abs;
      using std::numeric_limits;

      const Real eps = (numeric_limits<Real>::epsilon)();
      // Error bound ~eps^6/7
      // Error: h^6f^(7)(x)/140 + 5|f(x)|eps/h
      Real h = pow(eps / 168, (Real)1 / (Real)7);
      h = detail::make_xph_representable(x, h);

      Real yh = f(x + h);
      Real ymh = f(x - h);
      Real y1 = yh - ymh;
      Real y2 = f(x - 2 * h) - f(x + 2 * h);
      Real y3 = f(x + 3 * h) - f(x - 3 * h);

      if (error)
      {
         // Mathematica code to generate fd scheme for 7th derivative:
         // Sum[(-1)^i*Binomial[7, i]*(f[x+(3-i)*h] + f[x+(4-i)*h])/2, {i, 0, 7}]
         // Mathematica to demonstrate that this is a finite difference formula for 7th derivative:
         // Series[(f[x+4*h]-f[x-4*h] + 6*(f[x-3*h] - f[x+3*h]) + 14*(f[x-h] - f[x+h] + f[x+2*h] - f[x-2*h]))/2, {h, 0, 15}]
         Real y7 = (f(x + 4 * h) - f(x - 4 * h) - 6 * y3 - 14 * y1 - 14 * y2) / 2;
         *error = abs(y7) / (140 * h) + 5 * (abs(yh) + abs(ymh))*eps / h;
      }
      return (y3 + 9 * y2 + 45 * y1) / (60 * h);
   }

   template<class F, class Real>
   Real finite_difference_derivative(const F f, Real x, Real* error, const fd_tag<8>&)
   {
      using std::sqrt;
      using std::pow;
      using std::abs;
      using std::numeric_limits;

      const Real eps = (numeric_limits<Real>::epsilon)();
      // Error bound ~eps^8/9.
      // In double precision, we only expect to lose two digits of precision while using this formula, at the cost of 8 function evaluations.
      // Error: h^8|f^(9)(x)|/630 + 7|f(x)|eps/h assuming 7 unstabilized additions.
      // Mathematica code to get the error:
      // Series[(f[x+h]-f[x-h])*(4/5) + (1/5)*(f[x-2*h] - f[x+2*h]) + (4/105)*(f[x+3*h] - f[x-3*h]) + (1/280)*(f[x-4*h] - f[x+4*h]), {h, 0, 9}]
      // If we used Kahan summation, we could get the max error down to h^8|f^(9)(x)|/630 + |f(x)|eps/h.
      Real h = pow(551.25*eps, (Real)1 / (Real)9);
      h = detail::make_xph_representable(x, h);

      Real yh = f(x + h);
      Real ymh = f(x - h);
      Real y1 = yh - ymh;
      Real y2 = f(x - 2 * h) - f(x + 2 * h);
      Real y3 = f(x + 3 * h) - f(x - 3 * h);
      Real y4 = f(x - 4 * h) - f(x + 4 * h);

      Real tmp1 = 3 * y4 / 8 + 4 * y3;
      Real tmp2 = 21 * y2 + 84 * y1;

      if (error)
      {
         // Mathematica code to generate fd scheme for 7th derivative:
         // Sum[(-1)^i*Binomial[9, i]*(f[x+(4-i)*h] + f[x+(5-i)*h])/2, {i, 0, 9}]
         // Mathematica to demonstrate that this is a finite difference formula for 7th derivative:
         // Series[(f[x+5*h]-f[x- 5*h])/2 + 4*(f[x-4*h] - f[x+4*h]) + 27*(f[x+3*h] - f[x-3*h])/2 + 24*(f[x-2*h]  - f[x+2*h]) + 21*(f[x+h] - f[x-h]), {h, 0, 15}]
         Real f9 = (f(x + 5 * h) - f(x - 5 * h)) / 2 + 4 * y4 + 27 * y3 / 2 + 24 * y2 + 21 * y1;
         *error = abs(f9) / (630 * h) + 7 * (abs(yh) + abs(ymh))*eps / h;
      }
      return (tmp1 + tmp2) / (105 * h);
   }

   template<class F, class Real, class tag>
   Real finite_difference_derivative(const F, Real, Real*, const tag&)
   {
      // Always fails, but condition is template-arg-dependent so only evaluated if we get instantiated.
      static_assert(sizeof(Real) == 0, "Finite difference not implemented for this order: try 1, 2, 4, 6 or 8");
   }

}

template<class F, class Real, size_t order=6>
inline Real finite_difference_derivative(const F f, Real x, Real* error = nullptr)
{
   return detail::finite_difference_derivative(f, x, error, detail::fd_tag<order>());
}

}}}  // namespaces
#endif


//  Copyright (c) 2015 John Maddock
//  Use, modification and distribution are subject to 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)
//

#ifndef BOOST_MATH_ELLINT_JZ_HPP
#define BOOST_MATH_ELLINT_JZ_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/ellint_1.hpp>
#include <boost/math/special_functions/ellint_rj.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/tools/workaround.hpp>

// Elliptic integral the Jacobi Zeta function.

namespace boost { namespace math {

namespace detail{

// Elliptic integral - Jacobi Zeta
template <typename T, typename Policy>
T jacobi_zeta_imp(T phi, T k, const Policy& pol)
{
    BOOST_MATH_STD_USING
    using namespace boost::math::tools;
    using namespace boost::math::constants;

    bool invert = false;
    if(phi < 0)
    {
       phi = fabs(phi);
       invert = true;
    }

    T result;
    T sinp = sin(phi);
    T cosp = cos(phi);
    T s2 = sinp * sinp;
    T k2 = k * k;
    T kp = 1 - k2;
    if(k == 1)
       result = sinp * (boost::math::sign)(cosp);  // We get here by simplifying JacobiZeta[w, 1] in Mathematica, and the fact that 0 <= phi.
    else
       result = k2 * sinp * cosp * sqrt(1 - k2 * s2) * ellint_rj_imp(T(0), kp, T(1), T(1 - k2 * s2), pol) / (3 * ellint_k_imp(k, pol));
    return invert ? T(-result) : result;
}

} // detail

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type jacobi_zeta(T1 k, T2 phi, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::jacobi_zeta_imp(static_cast<value_type>(phi), static_cast<value_type>(k), pol), "boost::math::jacobi_zeta<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type jacobi_zeta(T1 k, T2 phi)
{
   return boost::math::jacobi_zeta(k, phi, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_D_HPP