#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <tuple>
#include <random>
#include <iterator>
#include <numeric>

#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "VNS.hpp"
#include "MODI.hpp"
#include "Solution.hpp"
#include "Helper.hpp"

using namespace std;
using namespace t_simplex;
namespace ublas = boost::numeric::ublas;

/*********************************************************************************/
/*                            UPDATE XIJ PROCEDURES                              */
/*********************************************************************************/

// CFLP with Modi procedure (gets optimal solution in deterministic time)
bool VNS::updateXij(
	ublas::matrix<double> &cij,
	const vector<double> &dj,
	const vector<double> &bi,
	const vector<bool> &yi,
	const unsigned int &update_xij_mode,
	double &fx)
{
	if (!canUpdateXij(bi, yi, m_sum_dj))
		return false;

	unsigned int open_facilities = 0, i_out = 0, flow_vars = 0;
	double transportation_cost = 0.0, gap = 0.0, sum_bi = 0.0;
	bool check = false;

	open_facilities = accumulate(yi.begin(), yi.end(), 0);

	ublas::matrix<double> custom_cij(open_facilities, m_customerNumber, 0.0);

	int *customers = new int[m_customerNumber];
	double *demand = new double[m_customerNumber];
	int *facilities = new int[open_facilities];
	double *capacity = new double[open_facilities];

	for (size_t j = 0; j != cij.size2(); ++j)
	{
		customers[j] = j;
		demand[j] = dj[j];
	}

	for (size_t i = 0; i != cij.size1(); ++i)
	{
		if (yi[i] == 1)
		{
			facilities[i_out] = i;
			capacity[i_out] = bi[i];
			sum_bi += bi[i];

			ublas::matrix_row<ublas::matrix<double>> row_custom_cij(custom_cij, i_out);
			ublas::matrix_row<ublas::matrix<double>> row_cij(cij, i);
			row_custom_cij = row_cij;

			i_out++;
		}
	}

	// Signature of Facilities and Customers
	TsSignature *facility = new TsSignature(open_facilities, facilities, capacity);
	TsSignature *customer = new TsSignature(m_customerNumber, customers, demand);
	// Save Stepping Stone Path
	TsFlow *flow = new TsFlow[open_facilities + m_customerNumber - 1];

	// Result value
	transportation_cost = t_simplex::transportSimplex(update_xij_mode, custom_cij, flow,
		&flow_vars, facility, customer ,m_sum_dj, sum_bi, dj);

	// f(x) and Return Value
	fx = f(yi, m_fi, transportation_cost);

	// Flow Xij for checkSolution
	m_flow_tpl.resize(flow_vars);
	for (size_t i = 0; i < flow_vars; ++i)
		m_flow_tpl[i] = make_tuple(facilities[flow[i].from], flow[i].to, flow[i].amount);

	check = checkSolution(yi, m_flow_tpl, flow_vars, cij, bi, dj, m_sum_dj);

	delete[] capacity;
	capacity = NULL;
	delete[] demand;
	demand = NULL;
	delete[] facilities;
	facilities = NULL;
	delete[] customers;
	customers = NULL;
	delete facility;
	facility = NULL;
	delete customer;
	customer = NULL;

	return check;
}


/*
 *
 * Copyright (c) Kresimir Fresl 2002
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Author assumes no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_CLAPACK_HPP
#define BOOST_NUMERIC_BINDINGS_CLAPACK_HPP

#include <cassert>
#include <new>

#include <boost/numeric/bindings/traits/traits.hpp>
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
#  include <boost/numeric/bindings/traits/detail/symm_herm_traits.hpp>
#endif
#include <boost/numeric/bindings/atlas/cblas_enum.hpp>

// see libs/numeric/bindings/atlas/doc/index.html, section 2.5.2
//#define BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG

#include <boost/numeric/bindings/atlas/clapack_overloads.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/same_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace atlas {

    /////////////////////////////////////////////////////////////////////
    //
    // general system of linear equations A * X = B
    //
    /////////////////////////////////////////////////////////////////////

    // gesv(): 'driver' function
    //
    // [comments from 'clapack_dgesv.c':]
    /* clapack_xgesv computes the solution to a system of linear equations
     *   A * X = B,
     * where A is an N-by-N matrix and X and B are N-by-NRHS matrices.
     */
    // [but ATLAS FAQ says:]
    /* What's the deal with the RHS in the row-major factorization/solves?
     * Most users are confused by the row major factorization and related
     * solves. The right-hand side vectors are probably the biggest source
     * of confusion. The RHS array does not represent a matrix in the
     * mathematical sense, it is instead a pasting together of the various
     * RHS into one array for calling convenience. As such, RHS vectors are
     * always stored contiguously, regardless of the row/col major that is
     * chosen. This means that ldb/ldx is always independent of NRHS, and
     * dependant on N, regardless of the row/col major setting.
     */
    // That is, it seems that, if B is row-major, it should be NRHS-by-N,
    // and RHS vectors should be its rows, not columns.
    //
    // [comments from 'clapack_dgesv.c':]
    /* The LU factorization used to factor A is dependent on the Order
     * parameter, as detailed in the leading comments of clapack_dgetrf.
     * The factored form of A is then used to solve the system of equations
     *   A * X = B.
     * A is overwritten with the appropriate LU factorization, and B [...]
     * is overwritten with the solution X on output.
     */
    // If B is row-major, solution vectors are its rows.
    template <typename MatrA, typename MatrB, typename IVec>
    inline
    int gesv (MatrA& a, IVec& ipiv, MatrB& b) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));

      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::ordering_type,
        typename traits::matrix_traits<MatrB>::ordering_type
      >::value));
#endif

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<MatrA>::ordering_type
#else
           typename MatrA::orientation_category
#endif
         >::value);

      int const n = traits::matrix_size1 (a);
      int const nrhs = stor_ord == CblasColMajor
        ? traits::matrix_size2 (b)
        : traits::matrix_size1 (b);
      assert (n == traits::matrix_size2 (a));
      assert (n == (stor_ord == CblasColMajor
                    ? traits::matrix_size1 (b)
                    : traits::matrix_size2 (b)));
      assert (n == traits::vector_size (ipiv));

      return detail::gesv (stor_ord, n, nrhs,
                           traits::matrix_storage (a),
                           traits::leading_dimension (a),
                           traits::vector_storage (ipiv),
                           traits::matrix_storage (b),
                           traits::leading_dimension (b));
    }

    template <typename MatrA, typename MatrB>
    inline
    int gesv (MatrA& a, MatrB& b) {
      // with 'internal' pivot vector
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));

      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::ordering_type,
        typename traits::matrix_traits<MatrB>::ordering_type
      >::value));
#endif

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<MatrA>::ordering_type
#else
           typename MatrA::orientation_category
#endif
         >::value);

      int const n = traits::matrix_size1 (a);
      int const nrhs = stor_ord == CblasColMajor
        ? traits::matrix_size2 (b)
        : traits::matrix_size1 (b);
      assert (n == traits::matrix_size2 (a));
      assert (n == (stor_ord == CblasColMajor
                    ? traits::matrix_size1 (b)
                    : traits::matrix_size2 (b)));

      int *ipiv = new (std::nothrow) int[n];
      int ierr = -101;
      // clapack_dgesv() errors:
      //   if (ierr == 0), successful
      //   if (ierr < 0), the -ierr argument had an illegal value
      //   -- we will use -101 if allocation fails
      //   if (ierr > 0), U(i-1,i-1) (or L(i-1,i-1)) is exactly zero

      if (ipiv) {
        ierr = detail::gesv (stor_ord, n, nrhs,
                             traits::matrix_storage (a),
                             traits::leading_dimension (a),
                             ipiv,
                             traits::matrix_storage (b),
                             traits::leading_dimension (b));
        delete[] ipiv;
      }
      return ierr;
    }

    template <typename MatrA, typename MatrB>
    inline
    int lu_solve (MatrA& a, MatrB& b) {
      return gesv (a, b);
    }

    // getrf(): LU factorization of A
    // [comments from 'clapack_dgetrf.c':]
    /* Computes one of two LU factorizations based on the setting of
     * the Order parameter, as follows:
     * ---------------------------------------------------------------
     *                     Order == CblasColMajor
     * Column-major factorization of form
     *   A = P * L * U
     * where P is a row-permutation matrix, L is lower triangular with
     * unit diagonal elements (lower trapezoidal if M > N), and U is
     * upper triangular (upper trapezoidal if M < N).
     * ---------------------------------------------------------------
     *                     Order == CblasRowMajor
     * Row-major factorization of form
     *   A = P * L * U
     * where P is a column-permutation matrix, L is lower triangular
     * (lower trapezoidal if M > N), and U is upper triangular with
     * unit diagonals (upper trapezoidal if M < N).
     */
    template <typename MatrA, typename IVec>
    inline
    int getrf (MatrA& a, IVec& ipiv) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<MatrA>::ordering_type
#else
           typename MatrA::orientation_category
#endif
         >::value);

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      assert (traits::vector_size (ipiv) == (m < n ? m : n));

      return detail::getrf (stor_ord, m, n,
                            traits::matrix_storage (a),
                            traits::leading_dimension (a),
                            traits::vector_storage (ipiv));
    }

    template <typename MatrA, typename IVec>
    inline
    int lu_factor (MatrA& a, IVec& ipiv) {
      return getrf (a, ipiv);
    }

    // getrs(): solves a system of linear equations
    //          A * X = B  or  A' * X = B
    //          using the LU factorization previously computed by getrf()
    template <typename MatrA, typename MatrB, typename IVec>
    inline
    int getrs (CBLAS_TRANSPOSE const Trans,
               MatrA const& a, IVec const& ipiv, MatrB& b)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));

      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::ordering_type,
        typename traits::matrix_traits<MatrB>::ordering_type
      >::value));
#endif

      assert (Trans == CblasNoTrans
              || Trans == CblasTrans
              || Trans == CblasConjTrans);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<MatrA>::ordering_type
#else
           typename MatrA::orientation_category
#endif
         >::value);

      int const n = traits::matrix_size1 (a);
      int const nrhs = stor_ord == CblasColMajor
        ? traits::matrix_size2 (b)
        : traits::matrix_size1 (b);
      assert (n == traits::matrix_size2 (a));
      assert (n == (stor_ord == CblasColMajor
                    ? traits::matrix_size1 (b)
                    : traits::matrix_size2 (b)));
      assert (n == traits::vector_size (ipiv));

      return detail::getrs (stor_ord, Trans, n, nrhs,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                            traits::matrix_storage (a),
#else
                            traits::matrix_storage_const (a),
#endif
                            traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                            traits::vector_storage (ipiv),
#else
                            traits::vector_storage_const (ipiv),
#endif
                            traits::matrix_storage (b),
                            traits::leading_dimension (b));
    }

    // getrs(): solves A * X = B (after getrf())
    template <typename MatrA, typename MatrB, typename IVec>
    inline
    int getrs (MatrA const& a, IVec const& ipiv, MatrB& b) {
      return getrs (CblasNoTrans, a, ipiv, b);
    }

    template <typename MatrA, typename MatrB, typename IVec>
    inline
    int lu_substitute (MatrA const& a, IVec const& ipiv, MatrB& b) {
      return getrs (CblasNoTrans, a, ipiv, b);
    }

    // getri(): computes the inverse of a matrix A
    //          using the LU factorization previously computed by getrf()
    template <typename MatrA, typename IVec>
    inline
    int getri (MatrA& a, IVec const& ipiv) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<MatrA>::ordering_type
#else
           typename MatrA::orientation_category
#endif
         >::value);

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (ipiv) == n);

      return detail::getri (stor_ord, n,
                            traits::matrix_storage (a),
                            traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                            traits::vector_storage (ipiv)
#else
                            traits::vector_storage_const (ipiv)
#endif
                            );
    }

    template <typename MatrA, typename IVec>
    inline
    int lu_invert (MatrA& a, IVec& ipiv) {
      return getri (a, ipiv);
    }

    /////////////////////////////////////////////////////////////////////
    //
    // system of linear equations A * X = B
    // with A symmetric or Hermitian positive definite matrix
    //
    /////////////////////////////////////////////////////////////////////

#ifndef BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG
    // posv(): 'driver' function
    //
    // [from 'dposv.f' (slightly edited):]
    /* XPOSV computes the solution to a system of linear equations
     *    A * X = B,
     * where A is an N-by-N symmetric/Hermitian positive definite matrix
     * and X and B are N-by-NRHS matrices. [See also comments of gesv().]
     *
     * A -- On entry, the symmetric/Hermitian matrix A.
     * If UPLO = 'U', the leading N-by-N upper triangular part of A
     * contains the upper triangular part of the matrix A, and the
     * strictly lower triangular part of A is not referenced.
     * If UPLO = 'L', the leading N-by-N lower triangular part of A
     * contains the lower triangular part of the matrix A, and the
     * strictly upper triangular part of A is not referenced.
     *
     * On exit, if INFO = 0, the factor U or L from the Cholesky
     * factorization A = U**T*U or A = L*L**T
     * [or A = U**H*U or A = L*L**H].
     *
     * B -- On entry, the right hand side matrix B.
     * On exit, if INFO = 0, the solution matrix X.
     */
    namespace detail {

      template <typename SymmA, typename MatrB>
      inline
      int posv (CBLAS_UPLO const uplo, SymmA& a, MatrB& b) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<SymmA>::ordering_type,
          typename traits::matrix_traits<MatrB>::ordering_type
        >::value));
#endif

        CBLAS_ORDER const stor_ord
          = enum_cast<CBLAS_ORDER const>
          (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
            typename traits::matrix_traits<SymmA>::ordering_type
#else
            typename SymmA::orientation_category
#endif
           >::value);

        int const n = traits::matrix_size1 (a);
        int const nrhs = stor_ord == CblasColMajor
          ? traits::matrix_size2 (b)
          : traits::matrix_size1 (b);
        assert (n == traits::matrix_size2 (a));
        assert (n == (stor_ord == CblasColMajor
                      ? traits::matrix_size1 (b)
                      : traits::matrix_size2 (b)));

        return posv (stor_ord, uplo, n, nrhs,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::matrix_storage (b),
                     traits::leading_dimension (b));
      }

    } // detail

    template <typename SymmA, typename MatrB>
    inline
    int posv (CBLAS_UPLO const uplo, SymmA& a, MatrB& b) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (uplo == CblasUpper || uplo == CblasLower);
      return detail::posv (uplo, a, b);
    }

    template <typename SymmA, typename MatrB>
    inline
    int posv (SymmA& a, MatrB& b) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        typename traits::detail::symm_herm_t<val_t>::type
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
          typename traits::matrix_traits<SymmA>::uplo_type
#else
          typename SymmA::packed_category
#endif
         >::value);

      return detail::posv (uplo, a, b);
    }

    template <typename SymmA, typename MatrB>
    inline
    int cholesky_solve (SymmA& a, MatrB& b) { return posv (a, b); }
#endif // BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG

    // potrf(): Cholesky factorization of A
    namespace detail {

      template <typename SymmA>
      inline
      int potrf (CBLAS_UPLO const uplo, SymmA& a) {

        CBLAS_ORDER const stor_ord
          = enum_cast<CBLAS_ORDER const>
          (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
            typename traits::matrix_traits<SymmA>::ordering_type
#else
            typename SymmA::orientation_category
#endif
           >::value);

        int const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));

        return potrf (stor_ord, uplo, n,
                      traits::matrix_storage (a),
                      traits::leading_dimension (a));
      }

    } // detail

    template <typename SymmA>
    inline
    int potrf (CBLAS_UPLO const uplo, SymmA& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (uplo == CblasUpper || uplo == CblasLower);
      return detail::potrf (uplo, a);
    }

    template <typename SymmA>
    inline
    int potrf (SymmA& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        typename traits::detail::symm_herm_t<val_t>::type
      >::value));
#endif

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
          typename traits::matrix_traits<SymmA>::uplo_type
#else
          typename SymmA::packed_category
#endif
         >::value);

      return detail::potrf (uplo, a);
    }

    template <typename SymmA>
    inline
    int cholesky_factor (SymmA& a) { return potrf (a); }

    // potrs(): solves a system of linear equations A * X = B
    //          using the Cholesky factorization computed by potrf()
    namespace detail {

      template <typename SymmA, typename MatrB>
      inline
      int potrs (CBLAS_UPLO const uplo, SymmA const& a, MatrB& b) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<SymmA>::ordering_type,
          typename traits::matrix_traits<MatrB>::ordering_type
        >::value));
#endif

        CBLAS_ORDER const stor_ord
          = enum_cast<CBLAS_ORDER const>
          (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
            typename traits::matrix_traits<SymmA>::ordering_type
#else
            typename SymmA::orientation_category
#endif
           >::value);

        int const n = traits::matrix_size1 (a);
        int const nrhs = stor_ord == CblasColMajor
          ? traits::matrix_size2 (b)
          : traits::matrix_size1 (b);
        assert (n == traits::matrix_size2 (a));
        assert (n == (stor_ord == CblasColMajor
                      ? traits::matrix_size1 (b)
                      : traits::matrix_size2 (b)));

#ifndef BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG
        return potrs (stor_ord, uplo, n, nrhs,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                      traits::matrix_storage (a),
#else
                      traits::matrix_storage_const (a),
#endif
                      traits::leading_dimension (a),
                      traits::matrix_storage (b),
                      traits::leading_dimension (b));
#else // BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG
        int ierr;
        if (stor_ord == CblasColMajor)
          ierr = potrs (stor_ord, uplo, n, nrhs,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                        traits::matrix_storage (a),
#else
                        traits::matrix_storage_const (a),
#endif
                        traits::leading_dimension (a),
                        traits::matrix_storage (b),
                        traits::leading_dimension (b));
        else // ATLAS bug with CblasRowMajor
          ierr = potrs_bug (stor_ord, uplo, n, nrhs,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                            traits::matrix_storage (a),
#else
                            traits::matrix_storage_const (a),
#endif
                            traits::leading_dimension (a),
                            traits::matrix_storage (b),
                            traits::leading_dimension (b));
        return ierr;
#endif // BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG
      }

    } // detail

    template <typename SymmA, typename MatrB>
    inline
    int potrs (CBLAS_UPLO const uplo, SymmA const& a, MatrB& b) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (uplo == CblasUpper || uplo == CblasLower);
      return detail::potrs (uplo, a, b);
    }

    template <typename SymmA, typename MatrB>
    inline
    int potrs (SymmA const& a, MatrB& b) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        typename traits::detail::symm_herm_t<val_t>::type
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
          typename traits::matrix_traits<SymmA>::uplo_type
#else
          typename SymmA::packed_category
#endif
         >::value);

      return detail::potrs (uplo, a, b);
    }

    template <typename SymmA, typename MatrB>
    inline
    int cholesky_substitute (SymmA const& a, MatrB& b) { return potrs (a, b); }

#ifdef BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG
    // posv(): 'driver' function
    template <typename SymmA, typename MatrB>
    inline
    int posv (CBLAS_UPLO const uplo, SymmA& a, MatrB& b) {
      int ierr = potrf (uplo, a);
      if (ierr == 0)
        ierr = potrs (uplo, a, b);
      return ierr;
    }

    template <typename SymmA, typename MatrB>
    inline
    int posv (SymmA& a, MatrB& b) {
      int ierr = potrf (a);
      if (ierr == 0)
        ierr = potrs (a, b);
      return ierr;
    }

    template <typename SymmA, typename MatrB>
    inline
    int cholesky_solve (SymmA& a, MatrB& b) {
      return posv (a, b);
    }
#endif // BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG

    // potri(): computes the inverse of a symmetric or Hermitian positive
    //          definite matrix A using the Cholesky factorization
    //          previously computed by potrf()
    namespace detail {

      template <typename SymmA>
      inline
      int potri (CBLAS_UPLO const uplo, SymmA& a) {

        CBLAS_ORDER const stor_ord
          = enum_cast<CBLAS_ORDER const>
          (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
            typename traits::matrix_traits<SymmA>::ordering_type
#else
            typename SymmA::orientation_category
#endif
           >::value);

        int const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));

        return potri (stor_ord, uplo, n,
                      traits::matrix_storage (a),
                      traits::leading_dimension (a));
      }

    } // detail

    template <typename SymmA>
    inline
    int potri (CBLAS_UPLO const uplo, SymmA& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (uplo == CblasUpper || uplo == CblasLower);
      return detail::potri (uplo, a);
    }

    template <typename SymmA>
    inline
    int potri (SymmA& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        typename traits::detail::symm_herm_t<val_t>::type
      >::value));
#endif

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
          typename traits::matrix_traits<SymmA>::uplo_type
#else
          typename SymmA::packed_category
#endif
         >::value);

      return detail::potri (uplo, a);
    }

    template <typename SymmA>
    inline
    int cholesky_invert (SymmA& a) { return potri (a); }

  } // namespace atlas

}}}

#endif // BOOST_NUMERIC_BINDINGS_CLAPACK_HPP


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// 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/gil/extension/io/jpeg_dynamic_io.hpp>

// Example to demonstrate a way to compute gradients along x-axis

using namespace boost::gil;

template <typename Out>
struct halfdiff_cast_channels {
    template <typename T> Out operator()(const T& in1, const T& in2) const {
        return Out((in2-in1)/2);
    }
};

template <typename SrcView, typename DstView>
void x_gradient(const SrcView& src, const DstView& dst) {
    typedef typename channel_type<DstView>::type dst_channel_t;

    for (int y=0; y<src.height(); ++y) {
        typename SrcView::x_iterator src_it = src.row_begin(y);
        typename DstView::x_iterator dst_it = dst.row_begin(y);

        for (int x=1; x<src.width()-1; ++x) {
            static_transform(src_it[x-1], src_it[x+1], dst_it[x],
                             halfdiff_cast_channels<dst_channel_t>());
        }
    }
}

template <typename SrcView, typename DstView>
void x_luminosity_gradient(const SrcView& src, const DstView& dst) {
    typedef pixel<typename channel_type<SrcView>::type, gray_layout_t> gray_pixel_t;
    x_gradient(color_converted_view<gray_pixel_t>(src), dst);
}

int main() {
    rgb8_image_t img;
    jpeg_read_image("test.jpg",img);

    gray8s_image_t img_out(img.dimensions());
    fill_pixels(view(img_out),int8_t(0));

    x_luminosity_gradient(const_view(img), view(img_out));
    jpeg_write_view("out-x_gradient.jpg",color_converted_view<gray8_pixel_t>(const_view(img_out)));

    return 0;
}


// Copyright (c) 2009-2019 The Bitcoin Core developers
// Copyright (c) 2014-2019 The DigiByte Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <chainparams.h>
#include <validation.h>
#include <net.h>

#include <test/test_digibyte.h>

#include <boost/signals2/signal.hpp>
#include <boost/test/unit_test.hpp>

#define END_OF_SUPPLY_CURVE 110579600

// Set OUTPUT_SUPPLY_SAMPLES_ENABLED to 1 to output
// sampled Supply-curve data points (SAMPLE_INTERVAL)
#ifndef OUTPUT_SUPPLY_SAMPLES_ENABLED
    #define OUTPUT_SUPPLY_SAMPLES_ENABLED 0
#endif

#ifndef SAMPLE_INTERVAL
    #define SAMPLE_INTERVAL 100
#endif

#ifndef WHOLE_COIN
    #define WHOLE_COIN false
#endif

#define CALC_COIN(amount) (WHOLE_COIN ? (amount / COIN) : (amount))

#if OUTPUT_SUPPLY_SAMPLES_ENABLED
    #define DEBUG(x,y) if (x % SAMPLE_INTERVAL == 0) { \
        std::cerr << x << ';' << CALC_COIN(y) << std::endl; \
    }
#else
    #define DEBUG(x,y) (void) (x)
#endif

#ifndef BLOCK_TIME_SECONDS
    #define BLOCK_TIME_SECONDS 15
#endif

#ifndef SECONDS_PER_MONTH
    #define SECONDS_PER_MONTH (60 * 60 * 24 * 365 / 12)
#endif

#ifndef ENABLE_TESTNET_SUBSIDY_TESTS
    #define ENABLE_TESTNET_SUBSIDY_TESTS 0
#endif

BOOST_FIXTURE_TEST_SUITE(main_tests, TestingSetup)

static void TestBlockSubsidy(const Consensus::Params& consensusParams, int nMaxBlocks, CAmount* nSumOut)
{
    CAmount nSum = 0;
    CAmount nInitialSubsidy = 72000 * COIN;

    CAmount nPreviousSubsidy = nInitialSubsidy * 2; // for height == 0
    BOOST_CHECK_EQUAL(nPreviousSubsidy, nInitialSubsidy * 2);

    /* Before first hard fork */

    // 72000 reward for the first 1440 blocks
    for (int nBlocks = 0; nBlocks < 1440 && nBlocks < consensusParams.nDiffChangeTarget; ++nBlocks)
    {
        int nHeight = nBlocks;
        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
        BOOST_CHECK_EQUAL(nSubsidy, nInitialSubsidy);

        nSum += nSubsidy;
        DEBUG(nBlocks, nSubsidy);
    }

    // 16000 rewards until block height 5760
    for (int nBlocks = 1440; nBlocks < 5760 && nBlocks < consensusParams.nDiffChangeTarget; ++nBlocks)
    {
        int nHeight = nBlocks;
        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
        BOOST_CHECK_EQUAL(nSubsidy, 16000 * COIN);

        nSum += nSubsidy;
        DEBUG(nBlocks, nSubsidy);
    }

    // 8000 mining rewards until block height 67,200
    for (int nBlocks = 5760; nBlocks < consensusParams.nDiffChangeTarget; ++nBlocks)
    {
        int nHeight = nBlocks;
        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
        BOOST_CHECK_EQUAL(nSubsidy, 8000 * COIN);

        nSum += nSubsidy;
        DEBUG(nBlocks, nSubsidy);
    }

    // Dynamic mining rewards from block height 67,200 to block height 400,000
    for (int nBlocks = consensusParams.nDiffChangeTarget; nBlocks < consensusParams.alwaysUpdateDiffChangeTarget; ++nBlocks)
    {
        int nHeight = nBlocks;
        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);

        CAmount nExpectedSubsidy = 8000 * COIN;
        int nHeightWithinFork = (nHeight - consensusParams.nDiffChangeTarget);

        for (int i = 0; i < (nHeightWithinFork / consensusParams.patchBlockRewardDuration) + 1; ++i) {
            nExpectedSubsidy -= nExpectedSubsidy / 200; // dec by 0.5%
        }

        BOOST_CHECK_EQUAL(nSubsidy, nExpectedSubsidy);

        nSum += nSubsidy;
        DEBUG(nBlocks, nSubsidy);
    }

    // Updated dynamic mining rewards from block height 400,000 to block height 1,430,000
    for (int nBlocks = consensusParams.alwaysUpdateDiffChangeTarget; nBlocks < consensusParams.workComputationChangeTarget; ++nBlocks)
    {
        int nHeight = nBlocks;
        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);

        CAmount nExpectedSubsidy = 2459 * COIN;
        int nHeightWithinFork = (nHeight - consensusParams.alwaysUpdateDiffChangeTarget);

        for (int i = 0; i < (nHeightWithinFork / consensusParams.patchBlockRewardDuration2) + 1; ++i) {
            nExpectedSubsidy -= nExpectedSubsidy / 100; // dec by 1% per month
        }

        BOOST_CHECK_EQUAL(nSubsidy, nExpectedSubsidy);

        nSum += nSubsidy;
        DEBUG(nBlocks, nSubsidy);
    }

    {
        // Updated dynamic mining rewards from block height 1,430,000 to max block height.
        // Intended blockheight: 41.6 million
        // Actual blockheight: 110.5 million
        CAmount nExpectedSubsidyStart = 2157 * COIN / 2;
        CAmount nExpectedSubsidy = nExpectedSubsidyStart;
        int nMonthsConsidered = 0;

        for (int nBlocks = consensusParams.workComputationChangeTarget; nBlocks < nMaxBlocks; ++nBlocks) {
            int nHeight = nBlocks;
            CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);

            int nHeightWithinFork = (nHeight - consensusParams.workComputationChangeTarget);
            int nMonths = nHeightWithinFork * BLOCK_TIME_SECONDS / SECONDS_PER_MONTH;

            if (nMonthsConsidered < nMonths) {
                // Calculate new subsidy for number of months `nMonths`.
                // This is a major optimization in order to reduce the
                // number of inner loops

                // Recalculate subsidy
                for (int i = nMonthsConsidered; i < nMonths; ++i) {
                    // Decay factor: 98884/100000
                    nExpectedSubsidy *= 98884;
                    nExpectedSubsidy /= 100000;
                    ++nMonthsConsidered;
                }
            }

            if (nExpectedSubsidy < COIN) { // ToDo: Alter consensus
                nExpectedSubsidy = COIN;
            }

            BOOST_CHECK_EQUAL(nSubsidy, nExpectedSubsidy);

            nSum += nSubsidy;
            DEBUG(nBlocks, nSubsidy);
        }
    }

    CAmount nSubsidy = GetBlockSubsidy(nMaxBlocks, consensusParams);
    CAmount nExpectedSubsidy = 1 * COIN;

    BOOST_CHECK_EQUAL(nSubsidy, nExpectedSubsidy);

    if (nSumOut != NULL) {
        *nSumOut = nSum;
    }
}

BOOST_AUTO_TEST_CASE(block_subsidy_test)
{
    CAmount sum;
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    const auto testChainParams = CreateChainParams(CBaseChainParams::TESTNET);
    TestBlockSubsidy(chainParams->GetConsensus(), END_OF_SUPPLY_CURVE, &sum); // Mainnet

    CAmount nExpectedTotalSupply = 2239167398214795680ULL;
    BOOST_CHECK_EQUAL(sum, nExpectedTotalSupply);

#if OUTPUT_SUPPLY_SAMPLES_ENABLED
    // Output the accumulated supply until END_OF_SUPPLY_CURVE
    std::cout << "(mainnet): MAXIMUM SUPPLY: " << sum << " dgbSATS (" << (sum / COIN) << " DGB)";
#elif ENABLE_TESTNET_SUBSIDY_TESTS != 0
    // Only perform test on TESTNET too if requested so.
    TestBlockSubsidy(testChainParams->GetConsensus(), END_OF_SUPPLY_CURVE, NULL); // Testnet
#endif
}

static bool ReturnFalse() { return false; }
static bool ReturnTrue() { return true; }

BOOST_AUTO_TEST_CASE(test_combiner_all)
{
    boost::signals2::signal<bool (), CombinerAll> Test;
    BOOST_CHECK(Test());
    Test.connect(&ReturnFalse);
    BOOST_CHECK(!Test());
    Test.connect(&ReturnTrue);
    BOOST_CHECK(!Test());
    Test.disconnect(&ReturnFalse);
    BOOST_CHECK(Test());
    Test.disconnect(&ReturnTrue);
    BOOST_CHECK(Test());
}
BOOST_AUTO_TEST_SUITE_END()


#pragma once
#include <Eigen/Core>

namespace mdk {
    using Vector = Eigen::Vector3d;
    using VRef = Vector const&;

    using VectorBase = Eigen::Matrix<double, 3, Eigen::Dynamic, Eigen::ColMajor>;

    /**
     * This class is a sort of a decorator over standard Eigen::Matrix3Xd. The
     * chief difference is that this class offers an interface more appropriate
     * for dealing with it as though it were a list of vectors (for example, one
     * needs not use m.col(i) to get t'th vector in the matrix. Also, now size()
     * corresponds to the number of vectors and not the elements of the array.
     * The array itself is not (easily) resizeable.
     */
    class Vectors: public VectorBase {
    public:
        Vectors() = default;

        /**
         * Create an unitialized list of n vectors.
         * @param n Number of vectors
         */
        explicit Vectors(int n):
            VectorBase(3, n) {};

        /**
         * Create an initialized list of n vectors.
         * @param n Number of vectors
         * @param init Initial value
         */
        Vectors(int n, Vector const& init) {
            resize(3, n);
            colwise() = init;
        }

        /**
         * Returns an iterator over constituent vectors.
         * @return An interator over constituent vectors.
         */
        auto vectorwise() {
            return colwise();
        }

        /**
         * Returns a const iterator over constituent vectors.
         * @return A const interator over constituent vectors.
         */
        auto vectorwise() const {
            return colwise();
        }

        /**
         *
         * @return A number of vectors.
         */
        int size() const {
            return cols();
        }

        /**
         * Access to vector.
         * @param i Index of a vector to access
         * @return A slice of a matrix corresponding to i'th vector.
         */
        inline auto operator[](int i) {
            return col(i);
        }

        /**
         * Const access to vector.
         * @param i Index of a vector to access
         * @return A const slice of a matrix corresponding to i'th vector.
         */
        inline auto operator[](int i) const {
            return col(i);
        }
    };
}


#pragma once

#include <boost/range/irange.hpp>

namespace km
{
template <typename T>
auto range(T end)
{
  return boost::irange(static_cast<T>(0), end);
}

template <typename T>
auto range(T start, T end)
{
  return boost::irange(start, end);
}

template <typename T, typename U>
auto range(T start, T end, U step)
{
  return boost::irange(start, end, step);
}
}


#include "my_pearce.cpp"
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/graphml.hpp>
#include <boost/property_map/dynamic_property_map.hpp>
#include <boost/graph/transitive_closure.hpp>

using namespace boost;

#ifndef TYPEDEF
#define TYPEDEF

typedef adjacency_list <vecS, vecS, directedS> Graph;
typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
typedef typename graph_traits<Graph>::vertex_iterator vertex_iter;
typedef graph_traits<adjacency_list<vecS, vecS, directedS> >::vertex_descriptor Vertex;
typedef typename property_map<Graph, vertex_index_t>::type IndexMap;

#endif

int main(int, char*[])
{

    //This function takes a graph formatted by graphml fashion from the stdin
    Graph g;
    dynamic_properties dp;
    read_graphml(std::cin, g, dp);

    //Printing the graph
    std::cout << "A directed graph:" << std::endl;
    print_graph(g, get(vertex_index,g));
    std::cout << std::endl;

    //PearceClass object
    PearceClass<typeInt> pearce(&g);
    std::vector<int>* rindex = pearce.pearce_scc();

    //Printing the result
    IndexMap index=get(vertex_index,g);
    for (int i = 0; i != num_vertices(g); ++i){
        std::cout << index[i] << " -> " << (*rindex)[i] << std::endl;
    }
    return 0;
}


#include <iostream>
#include <iomanip>
#include <algorithm>
#include <numeric>
#include <functional>
#include <limits>
#include <ctime>
#include <cmath>
#include <cassert>

#include <boost/timer.hpp>
#include <boost/random.hpp>
#include <boost/math/tr1.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/lambda/lambda.hpp>

#include "FunctionMinimization.h"

using namespace boost::lambda;

namespace Grante {

double FunctionMinimization::BarzilaiBorweinMinimize(
	FunctionMinimizationProblem& prob,
	std::vector<double>& x_opt, double conv_tol,
	unsigned int max_iter, bool verbose) {
	unsigned int dim = prob.Dimensions();

	// Gradient, last gradient and alpha value required for BB iteration
	std::vector<double> grad(dim, 0.0);
	std::vector<double> grad_last(dim, 0.0);
	double alpha_last = std::numeric_limits<double>::infinity();

	// Initialize x
	std::vector<double> x(dim);
	prob.ProvideStartingPoint(x);

	boost::timer total_timer;
	double obj = std::numeric_limits<double>::signaling_NaN();
	for (unsigned int iter = 0; max_iter == 0 || iter < max_iter; ++iter) {
		obj = prob.Eval(x, grad);

		// Convergence check
		double grad_norm = EuclideanNorm(grad);
		if (verbose && (iter % 20 == 0)) {
			std::cout << std::endl;
			std::cout << "  iter     time      objective      |grad|" << std::endl;
		}
		if (verbose) {
			std::ios_base::fmtflags original_format = std::cout.flags();
			std::streamsize original_prec = std::cout.precision();

			// Iteration
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << iter << "  ";
			// Total runtime
			std::cout << std::setiosflags(std::ios::left)
				<< std::resetiosflags(std::ios::scientific)
				<< std::setiosflags(std::ios::fixed)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setprecision(1)
				<< std::setw(6) << total_timer.elapsed() << "s  ";
			std::cout << std::resetiosflags(std::ios::fixed);

			// Objective function
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(5)
				<< std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::showpos)
				<< std::setw(7) << obj << "   ";
			// Gradient norm
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(2)
				<< std::resetiosflags(std::ios::showpos)
				<< std::setiosflags(std::ios::left) << grad_norm;
			std::cout << std::endl;

			std::cout.precision(original_prec);
			std::cout.flags(original_format);
		}

		if (grad_norm < conv_tol) {
			x_opt = x;
			return (obj);
		}

		// Choose alpha
		double alpha = grad_norm;	// Initialization heuristic
		if (iter == 0) {
			// First iteration: assert feasibility by line search
			WolfeLineSearch linesearch(&prob, x, grad, grad, obj, 1e-4, 0.9);
			linesearch.ComputeStepLength(alpha);
			alpha = 1.0 / alpha;
		}
		if (iter >= 1) {
			double anom = 0.0;
			double adenom = 0.0;
			for (unsigned int d = 0; d < dim; ++d) {
				anom += -grad_last[d]*(grad[d] - grad_last[d]);
				adenom += grad_last[d]*grad_last[d];
			}
			alpha = alpha_last * (anom / adenom);
#if 0
			std::cout << "   alpha = alpha_last(" << alpha_last
				<< ") * (anom(" << anom << ") / adenom(" << adenom << ")"
				<< " = " << alpha << std::endl;
#endif
		}
		assert(alpha > 0.0);

		// Update x
		for (unsigned int d = 0; d < dim; ++d)
			x[d] -= grad[d] / alpha;

		// Keep iterates of the gradient and stepsize
		alpha_last = alpha;
		grad_last = grad;
	}

	// Iteration limit reached
	x_opt = x;
	return (prob.Eval(x, grad));
}

double FunctionMinimization::LimitedMemoryBFGSMinimize(
	FunctionMinimizationProblem& prob,
	std::vector<double>& x_opt, double conv_tol, unsigned int max_iter,
	bool verbose, unsigned int lbfgs_m) {
	unsigned int dim = prob.Dimensions();

	// Gradient, last gradient and alpha value required for BB iteration
	std::vector<double> grad(dim, 0.0);
	std::vector<double> grad_last(dim, 0.0);

	// Initialize x
	std::vector<double> x(dim);
	prob.ProvideStartingPoint(x);
	std::vector<double> xprev(dim);
	std::vector<double> gradprev(dim);

	// List of previous s,y,rho_i
	lbfgs_mem_type lbfgs_mem;

	boost::timer total_timer;
	double obj = std::numeric_limits<double>::signaling_NaN();
	bool is_valid = false;
	bool is_restart = false;
	unsigned int ls_evals = 0;
	unsigned int iter = 0;
	for ( ; max_iter == 0 || iter < max_iter; ++iter) {
		// If no information about current query point has been computed, do
		// so now
		if (is_valid == false) {
			obj = prob.Eval(x, grad);
			is_valid = true;
		}
		double grad_norm = EuclideanNorm(grad);

		if (verbose && (iter % 20 == 0)) {
			std::cout << std::endl;
			std::cout << "  iter     time      objective      |grad|   "
				<< "mem     ls#" << std::endl;
		}
		if (verbose) {
			std::ios_base::fmtflags original_format = std::cout.flags();
			std::streamsize original_prec = std::cout.precision();

			// Iteration
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << iter << "  ";
			// Total runtime
			std::cout << std::setiosflags(std::ios::left)
				<< std::resetiosflags(std::ios::scientific)
				<< std::setiosflags(std::ios::fixed)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setprecision(1)
				<< std::setw(6) << total_timer.elapsed() << "s  ";
			std::cout << std::resetiosflags(std::ios::fixed);

			// Objective function
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(5)
				<< std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::showpos)
				<< std::setw(7) << obj << "   ";
			// Gradient norm
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(2)
				<< std::resetiosflags(std::ios::showpos)
				<< std::setiosflags(std::ios::left) << grad_norm;
			// LBFGS memory size
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << lbfgs_mem.size() << "  ";
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << ls_evals << "  ";
			std::cout << std::endl;

			std::cout.precision(original_prec);
			std::cout.flags(original_format);
		}

		// Convergence check based on gradient norm
		if (prob.HasConverged(x, grad, conv_tol))
			break;	// converged

		// Insert differential information into Hessian approximation
		if (iter > 0 && is_restart == false) {
			// xprev: s_k
			std::transform(x.begin(), x.end(), xprev.begin(),
				xprev.begin(), _1 - _2);
			// gradprev: y_k
			std::transform(grad.begin(), grad.end(), gradprev.begin(),
				gradprev.begin(), _1 - _2);

			// Heuristically ensure stability by ignoring unstable updates.
			// As to what entails 'unstable' there exist different opinions.
			// TODO: replace with true damped-Newton update (in inverse H
			// form used by L-BFGS)
			double ys_p = std::inner_product(xprev.begin(), xprev.end(),
				gradprev.begin(), 0.0);
			double yy_p = std::inner_product(gradprev.begin(), gradprev.end(),
				gradprev.begin(), 0.0);
//			if (ys_p >= 1.0e-12) {
			if (ys_p >= 1.0e-12*yy_p) {
#if 0
				std::cout << "    lbfgs update with ys_p " << ys_p
					<< std::endl;
#endif
				double rho = 1.0 / ys_p;
				lbfgs_mem.push_front(lbfgs_mem_type::value_type(
					xprev, gradprev, rho));

				// Remove old element from the lbfgs memory, if necessary
				if (lbfgs_mem.size() > lbfgs_m)
					lbfgs_mem.pop_back();
			} else {
				std::cout << "    LBFGS update too large (ys "
					<< ys_p << ", yy " << yy_p << ")" << std::endl;
			}
		}
		// Save current iterate for next update
		std::copy(x.begin(), x.end(), xprev.begin());
		std::copy(grad.begin(), grad.end(), gradprev.begin());

		// Compute new ascent direction H_k \nabla_x f(x_k)
		// Recent-to-oldest
		std::list<double> alpha_list;
		for (lbfgs_mem_type::const_iterator li = lbfgs_mem.begin();
			li != lbfgs_mem.end(); ++li) {
			// alpha_i = rho_i s_i' q
			double alpha_i = li->get<2>() * std::inner_product(
				grad.begin(), grad.end(), li->get<0>().begin(), 0.0);
			// q <= q - alpha_i y_i
			std::transform(grad.begin(), grad.end(), li->get<1>().begin(),
				grad.begin(), _1 - alpha_i * _2);
			alpha_list.push_back(alpha_i);
		}
		// Diagonal scaling: q = H^0 q
		double gamma = 1.0;
		if (iter > 0 && is_restart == false && lbfgs_mem.empty() == false) {
			lbfgs_mem_type::const_iterator li_last = lbfgs_mem.begin();

			// gamma = (s_{k-1}' y_{k-1}) / (y_{k-1}' y_{k-1})
			gamma = std::inner_product(li_last->get<0>().begin(),
				li_last->get<0>().end(), li_last->get<1>().begin(), 0.0);
			gamma /= std::inner_product(li_last->get<1>().begin(),
				li_last->get<1>().end(), li_last->get<1>().begin(), 0.0);
		}
		std::transform(grad.begin(), grad.end(), grad.begin(), gamma * _1);
		// Reverse: oldest-to-recent
		std::list<double>::const_reverse_iterator ai = alpha_list.rbegin();
		for (lbfgs_mem_type::const_reverse_iterator li = lbfgs_mem.rbegin();
			li != lbfgs_mem.rend(); ++li, ++ai) {
			// beta = rho_i y_i' q
			double beta = li->get<2>() * std::inner_product(
				grad.begin(), grad.end(), li->get<1>().begin(), 0.0);

			// q <- q + (alpha_i-beta)*s_i
			std::transform(grad.begin(), grad.end(), li->get<0>().begin(),
				grad.begin(), _1 + (*ai-beta)*_2);
		}
		// Now 'grad' contains an adjusted gradient direction
		is_restart = false;

		// Check cosine angle between gradient and transformed gradient
		double x0_phi_grad = std::inner_product(grad.begin(), grad.end(),
			gradprev.begin(), 0.0);
		double cos_a = x0_phi_grad /
			(EuclideanNorm(gradprev) * EuclideanNorm(grad));
		if (cos_a <= -1.0e-8) {
			std::cout << "### FATAL: LBFGS approximation lost psd, angle "
				<< cos_a << std::endl;
			assert(0);
		} else if (cos_a <= 1.0e-7) {
			// Numerical issues, degenerate true Hessian or converged.
			std::cout << "### WARNING: LBFGS gradient orthogonality issue, "
				<< "aborting." << std::endl;
			break;
		}

		if ((boost::math::isnan)(x0_phi_grad)) {
			std::cout << "### WARNING: LBFGS gradient or perturbed gradient NaN, "
				<< "aborting." << std::endl;
			break;
		}
		// Check it is a descent direction
		if (x0_phi_grad <= -1.0e-8) {
			std::cout << "### FATAL: LBFGS approximation lost psd, x0_phi_grad "
				<< x0_phi_grad << std::endl;
			assert(0);
		} else if (x0_phi_grad <= 1.0e-10) {
#if 0
			std::cout << "### WARNING: LBFGS gradient, numerical issue, "
				<< "phi'(0) = " << x0_phi_grad << std::endl;
#endif
			// Numerical issues, degenerate true Hessian or converged.
			break;
		}

		// Perform linesearch in descent direction
		WolfeLineSearch linesearch(&prob, x, gradprev, grad, obj, 1e-4, 0.9);
		//SimpleLineSearch linesearch(&prob, x, gradprev, grad, obj);
		double alpha = 1.0;
		// Be very careful on the first step
		if (iter == 0) {
//			alpha = 1.0e-5;
			if (EuclideanNorm(grad) >= 1.0)
				alpha = 1.0 / EuclideanNorm(grad);
			alpha = std::max(1.0e-12, alpha);
		}

		ls_evals = linesearch.ComputeStepLengthUpdate(x, grad, obj, alpha);
#if 0
		std::cout << "   step size " << alpha
			<< " in " << eval << " evaluations" << std::endl;
#endif

		if ((boost::math::isnan)(alpha)) {
			std::cout << "   * Line search failed (alpha="
				<< alpha << ")" << std::endl;
			std::copy(xprev.begin(), xprev.end(), x.begin());
			break;
		}
		if (alpha <= 1.0e-12) {
			std::cout << "   * Line search yielded step size alpha="
				<< alpha << std::endl;
			if (iter > 0) {
				std::cout << "   * Assuming convergence." << std::endl;
				break;
			}
		}
		// Successful line search with up-to-date step
		is_valid = true;
	}

	// Iteration limit reached
	x_opt = x;
	return (prob.Eval(x, grad));
}

double FunctionMinimization::SubgradientMethodMinimize(
	FunctionMinimizationProblem& prob,
	std::vector<double>& x_opt, double conv_tol, unsigned int max_iter,
	bool verbose) {
	unsigned int dim = prob.Dimensions();
	std::vector<double> grad(dim, 0.0);

	// Initialize x
	std::vector<double> x(dim);
	prob.ProvideStartingPoint(x);

	boost::timer total_timer;
	for (unsigned int iter = 0; (max_iter == 0) || iter < max_iter; ++iter) {
		double obj = prob.Eval(x, grad);

		// Convergence check
		double grad_norm = EuclideanNorm(grad);
		if (verbose && (iter % 20 == 0)) {
			std::cout << std::endl;
			std::cout << "  iter     time      objective      |grad|" << std::endl;
		}
		if (verbose) {
			std::ios_base::fmtflags original_format = std::cout.flags();
			std::streamsize original_prec = std::cout.precision();

			// Iteration
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << iter << "  ";
			// Total runtime
			std::cout << std::setiosflags(std::ios::left)
				<< std::resetiosflags(std::ios::scientific)
				<< std::setiosflags(std::ios::fixed)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setprecision(1)
				<< std::setw(6) << total_timer.elapsed() << "s  ";
			std::cout << std::resetiosflags(std::ios::fixed);

			// Objective function
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(5)
				<< std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::showpos)
				<< std::setw(7) << obj << "   ";
			// Gradient norm
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(2)
				<< std::resetiosflags(std::ios::showpos)
				<< std::setiosflags(std::ios::left) << grad_norm;
			std::cout << std::endl;

			std::cout.precision(original_prec);
			std::cout.flags(original_format);
		}

		if (grad_norm < conv_tol) {
			x_opt = x;
			return (obj);
		}

		// Choose step size
		double alpha_m = 200.0;
		double alpha = (1.0 + alpha_m) /
			(static_cast<double>(iter + 1) + alpha_m);
		alpha /= grad_norm * grad_norm;

		// Update
		for (unsigned int d = 0; d < dim; ++d)
			x[d] -= alpha * grad[d];
	}

	// Iteration limit reached
	x_opt = x;
	return (prob.Eval(x, grad));
}

double FunctionMinimization::GradientMethodMinimize(
	FunctionMinimizationProblem& prob,
	std::vector<double>& x_opt, double conv_tol, unsigned int max_iter,
	bool verbose) {
	unsigned int dim = prob.Dimensions();
	std::vector<double> grad(dim, 0.0);
	std::vector<double> grad_prev(dim, 0.0);

	// Initialize x
	std::vector<double> x(dim);
	prob.ProvideStartingPoint(x);

	boost::timer total_timer;
	double alpha = -1.0;
	double beta = 0.5;
	for (unsigned int iter = 0; (max_iter == 0) || iter < max_iter; ++iter) {
		double obj = prob.Eval(x, grad);

		// Convergence check
		double grad_norm = EuclideanNorm(grad);
		if (verbose && (iter % 20 == 0)) {
			std::cout << std::endl;
			std::cout << "  iter     time      objective      |grad|       alpha"
				<< std::endl;
		}
		if (verbose) {
			std::ios_base::fmtflags original_format = std::cout.flags();
			std::streamsize original_prec = std::cout.precision();

			// Iteration
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << iter << "  ";
			// Total runtime
			std::cout << std::setiosflags(std::ios::left)
				<< std::resetiosflags(std::ios::scientific)
				<< std::setiosflags(std::ios::fixed)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setprecision(1)
				<< std::setw(6) << total_timer.elapsed() << "s  ";
			std::cout << std::resetiosflags(std::ios::fixed);

			// Objective function
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(5)
				<< std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::showpos)
				<< std::setw(7) << obj << "   ";
			// Gradient norm
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(2)
				<< std::resetiosflags(std::ios::showpos)
				<< std::setiosflags(std::ios::left) << grad_norm
				<< "   ";
			// alpha
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(2)
				<< std::resetiosflags(std::ios::showpos)
				<< std::setw(5)
				<< std::setiosflags(std::ios::left) << alpha;
			std::cout << std::endl;

			std::cout.precision(original_prec);
			std::cout.flags(original_format);
		}

		if (grad_norm < conv_tol) {
			x_opt = x;
			return (obj);
		}

		// Choose step size
		if (alpha < 0.0) {
			alpha = 1.0 / (grad_norm * grad_norm);
		} else {
			if (std::inner_product(grad.begin(), grad.end(),
				grad_prev.begin(), 0.0) < 0.0) {
				alpha *= beta;
			}
		}
		std::copy(grad.begin(), grad.end(), grad_prev.begin());

		// Update
		for (unsigned int d = 0; d < dim; ++d)
			x[d] -= alpha * grad[d];
	}

	// Iteration limit reached
	x_opt = x;
	return (prob.Eval(x, grad));
}

bool FunctionMinimization::CheckDerivative(FunctionMinimizationProblem& prob,
	double x_range, unsigned int test_count, double dim_eps, double grad_tol) {
	assert(dim_eps > 0.0);
	assert(grad_tol > 0.0);

	// Random number generation, for random perturbations
	boost::mt19937 rgen(static_cast<const boost::uint32_t>(std::time(0))+1);
	boost::uniform_real<double> rdestu;	// range [0,1]
	boost::variate_generator<boost::mt19937,
		boost::uniform_real<double> > rand_perturb(rgen, rdestu);

	// Random number generation, for random dimensions
	unsigned int dim = prob.Dimensions();
	boost::mt19937 rgen2(static_cast<const boost::uint32_t>(std::time(0))+2);
	boost::uniform_int<unsigned int> rdestd(0, dim-1);
	boost::variate_generator<boost::mt19937,
		boost::uniform_int<unsigned int> > rand_dim(rgen2, rdestd);

	// Get base
	std::vector<double> x0(dim);
	prob.ProvideStartingPoint(x0);
	std::vector<double> xtest(dim);
	std::vector<double> grad(dim);
	std::vector<double> grad_d(dim);	// dummy

	for (unsigned int test_id = 0; test_id < test_count; ++test_id) {
		xtest = x0;
		for (unsigned int d = 0; d < dim; ++d)
			xtest[d] += 2.0*x_range*rand_perturb() - x_range;

		// Get exact derivative
		double xtest_fval = prob.Eval(xtest, grad);

		// Compute first-order finite difference approximation
		unsigned int test_dim = rand_dim();
		xtest[test_dim] += dim_eps;
		double xtest_d_fval = prob.Eval(xtest, grad_d);
		double deriv_fd = (xtest_d_fval - xtest_fval) / dim_eps;

		// Check accuracy
		if (fabs(deriv_fd - grad[test_dim]) > grad_tol) {
			std::ios_base::fmtflags original_format = std::cout.flags();
			std::streamsize original_prec = std::cout.precision();

			std::cout << std::endl;
			std::cout << "### DERIVATIVE CHECKER WARNING" << std::endl;
			std::cout << "### during test " << (test_id+1) << " a violation "
				<< "in gradient computation was found:" << std::endl;
			std::cout << std::setprecision(6)
				<< std::setiosflags(std::ios::scientific);
			std::cout << "### dim " << test_dim << ", exact " << grad[test_dim]
				<< ", finite-diff " << deriv_fd
				<< ", absdiff " << fabs(deriv_fd - grad[test_dim])
				<< std::endl;
			std::cout << std::endl;

			std::cout.precision(original_prec);
			std::cout.flags(original_format);

			return (false);
		}
	}
	return (true);
}

double FunctionMinimization::EuclideanNorm(const std::vector<double>& vec) {
	double rs = 0.0;
	for (std::vector<double>::const_iterator vi = vec.begin();
		vi != vec.end(); ++vi) {
		rs += (*vi) * (*vi);
	}
	return (sqrt(rs));
}

// Wolfe line-search method, see [Nocedal&Wright], page 60.
FunctionMinimization::WolfeLineSearch::WolfeLineSearch(
	FunctionMinimizationProblem* prob, const std::vector<double>& x0,
	const std::vector<double>& x0_grad,
	const std::vector<double>& H_grad, double x0_fval,
	double c1, double c2)
	: prob(prob), x0(x0), x0_grad(x0_grad), H_grad(H_grad),
		x0_fval(x0_fval), x0_phi_grad(0),
		evaluation_count(0),
		xalpha_val(std::numeric_limits<double>::signaling_NaN()),
		c1(c1), c2(c2)
{
	assert(x0.size() > 0);
	xalpha.resize(x0.size());
	xalphagrad.resize(x0.size());

	// phi'(0) = - p' \nabla_x f(x_k)
	x0_phi_grad = -std::inner_product(x0_grad.begin(), x0_grad.end(),
		H_grad.begin(), 0.0);
	assert(x0_phi_grad < 0.0);
}

unsigned int FunctionMinimization::WolfeLineSearch::ComputeStepLength(
	double& alpha) {
	// Previous alpha
	double alpha_prev = 0.0;
	double alpha_prev_fval = x0_fval;
	double alpha_prev_grad = x0_phi_grad;
	double alpha_max = 1e6;
//	alpha = 1.0;

#if 0
	for (double bf = 0.0; bf < 1.0; bf += 0.01) {
		double tfval, tfgrad;
		Evaluate(bf, tfval, tfgrad);
		std::cout << bf << " " << tfval << " " << tfgrad << " # TEST" << std::endl;
	}
#endif

	// Current alpha
	double phi_alpha_fval;
	double phi_alpha_grad;

	// Find a stepsize interval satisfying the strong Wolfe conditions for a
	// given iterate x and gradient gx and descent direction d:
	//   1. Armijo: f(x + alpha d) <= f(x) + c1 alpha gx' d,
	//   2. Curvature: |nabla_alpha f(x + alpha d)| <= c2 |gx' d|.
	for (unsigned int n = 0; true; ++n) {
		Evaluate(alpha, phi_alpha_fval, phi_alpha_grad);

		// If Armijo condition is violated: zoom, as a point satisfying the
		// Wolfe condition must exist in [alpha_{i-1}, alpha_i].
		if (phi_alpha_fval > (x0_fval + c1*alpha*x0_phi_grad)) {
			if (n == 0) {
				alpha = Zoom(0.0, alpha, x0_fval, phi_alpha_fval, x0_phi_grad);
			} else {
				alpha = Zoom(alpha_prev, alpha, alpha_prev_fval,
					phi_alpha_fval, alpha_prev_grad);
			}
			break;
		}
		if (n > 0 && phi_alpha_fval >= alpha_prev_fval) {
			alpha = Zoom(alpha_prev, alpha, alpha_prev_fval,
				phi_alpha_fval, alpha_prev_grad);
			break;
		}

		// The Armijo condition is satisfied.  If in addition the curvature
		// condition ("function must be flat around alpha") is satisfied, then
		// we found a point satisfying the Wolfe conditions.
#if 0
		// STRONG
		if (std::fabs(phi_alpha_grad) <= -c2*x0_phi_grad)
			break;
#endif
		if (phi_alpha_grad >= c2*x0_phi_grad)
			break;

		// If the gradient increases again, then a point satisfying the Wolfe
		// condition must exist in [alpha_{i-1}, alpha_i].
		if (phi_alpha_grad >= 0) {
			alpha = Zoom(alpha_prev, alpha, alpha_prev_fval,
				phi_alpha_fval, alpha_prev_grad);
			break;
		}

		// Further progress can be made, scale step size up by a factor
		alpha_prev = alpha;
		alpha_prev_fval = phi_alpha_fval;
		alpha_prev_grad = phi_alpha_grad;
		alpha *= std::sqrt(2.0);
		assert(alpha <= alpha_max);
	}

#if 0
	// If line search failed: try fall back to backtracking line search
	if ((boost::math::isnan)(alpha)) {
		alpha = 1.0;
		for (unsigned int bt_try = 0; bt_try < 20; ++bt_try) {
			alpha *= 0.25;
			Evaluate(alpha, phi_alpha_fval, phi_alpha_grad);
			std::cout << "    backtrack, alpha " << alpha
				<< ", phi(a) " << phi_alpha_fval
				<< ", phi'(a) " << phi_alpha_grad << std::endl;
			std::cout << "    phi(a) = " << phi_alpha_fval
				<< " <= " << x0_fval << " + " << c1*alpha*x0_phi_grad
				<< "?" << std::endl;
			if (phi_alpha_fval <= (x0_fval + c1*alpha*x0_phi_grad))
				return (evaluation_count);
		}
		std::cout << "  Backtracking line search failed." << std::endl;
		alpha = std::numeric_limits<double>::signaling_NaN();
	}
#endif

	return (evaluation_count);
}

unsigned int FunctionMinimization::WolfeLineSearch::ComputeStepLengthUpdate(
	std::vector<double>& x_out, std::vector<double>& grad_out,
	double& fval_out, double& alpha) {
//	alpha = 0.0;
	unsigned int ecount = ComputeStepLength(alpha);

	if ((boost::math::isnan)(alpha))
		return (ecount);

	// A valid step size has been computed
	if (xalpha_val != alpha) {
		// Is not up-to-date, update
		double d1;	// dummy
		double d2;
		Evaluate(alpha, d1, d2);
	}
	assert(xalpha_val == alpha);
	x_out = xalpha;
	grad_out = xalphagrad;
	fval_out = xalphaobj;

	return (ecount);
}

// phi(alpha), alpha >= 0
void FunctionMinimization::WolfeLineSearch::Evaluate(
	double alpha, double& phi_fval, double& phi_grad) {
	// x(alpha) = x - alpha*p
	std::transform(x0.begin(), x0.end(), H_grad.begin(),
		xalpha.begin(), _1 - alpha * _2);
	std::fill(xalphagrad.begin(), xalphagrad.end(), 0.0);

	// Evaluate phi(alpha) and derivative phi'(alpha)
	// phi(alpha) = f(x_k - alpha H_grad)
	// phi'(alpha) = -H_grad' \nabla_x f(x_k - alpha H_grad)
	phi_fval = prob->Eval(xalpha, xalphagrad);
	xalphaobj = phi_fval;	// save phi(alpha)
	xalpha_val = alpha;	// save alpha

	// Univariate derivative is projection onto ascent direction
	phi_grad = -std::inner_product(xalphagrad.begin(), xalphagrad.end(),
		H_grad.begin(), 0.0);
	evaluation_count += 1;
#if 0
	std::cout << "   phi(" << alpha << ") = " << phi_fval << ", grad "
		<< phi_grad << std::endl;
#endif
}

double FunctionMinimization::WolfeLineSearch::Zoom(double alpha_lo,
	double alpha_hi, double alpha_lo_fval, double alpha_hi_fval,
	double alpha_lo_grad) {
	double phi_trial_fval;
	double phi_trial_grad;

#if 0
	double tfval, tfgrad;
	Evaluate(0.0, tfval, tfgrad);
	std::cout << "  phi(0) = " << tfval << ", grad " << tfgrad << std::endl;
#endif
#if 0
	for (double bf = alpha_lo; bf < alpha_hi; bf += 0.01) {
		double tfval, tfgrad;
		Evaluate(bf, tfval, tfgrad);
		std::cout << bf << " " << tfval << " " << tfgrad << " # TEST" << std::endl;
	}
#endif

	unsigned int tries_max = 150;
	unsigned int tries = 0;
	for (; tries < tries_max; ++tries) {
		if ((alpha_hi - alpha_lo) < 1.0e-14) {
			std::cout << "   * Wolfe line search, too small bracket: ["
				<< alpha_lo << "; " << alpha_hi << "]" << std::endl;
			//return (std::numeric_limits<double>::signaling_NaN());
			return (alpha_hi);
		}
#if 0
		{	// check preconditions: alpha_lo satisfies Armijo cond
			double tfval_lo, tfgrad_lo;
			Evaluate(alpha_lo, tfval_lo, tfgrad_lo);
			std::cout << "      ### lo: alpha = " << alpha_lo
				<< ", phi(a) = " << tfval_lo
				<< ", phi'(a) = " << tfgrad_lo << std::endl;
			assert(tfval_lo <= (x0_fval + c1*alpha_lo*x0_phi_grad));

			double tfval_hi, tfgrad_hi;
			Evaluate(alpha_hi, tfval_hi, tfgrad_hi);
			std::cout << "      ### hi: alpha = " << alpha_hi
				<< ", phi(a) = " << tfval_hi
				<< ", phi'(a) = " << tfgrad_hi << std::endl;

			// Check derivative
			double cd_fval, cd_grad;
			Evaluate(alpha_lo + 1e-8, cd_fval, cd_grad);
			double cd_apx = (cd_fval - tfval_lo) / 1e-8;
			std::cout << "        # lo deriv: " << tfgrad_lo
				<< " (exa) vs " << cd_apx << " (apx)"
				<< ", phi(0) " << x0_phi_grad
				<< std::endl;
			Evaluate(alpha_hi + 1e-8, cd_fval, cd_grad);
			cd_apx = (cd_fval - tfval_hi) / 1e-8;
			std::cout << "        # hi deriv: " << tfgrad_hi
				<< " (exa) vs " << cd_apx << " (apx)"
				<< ", phi(hi) " << tfval_hi
				<< std::endl;

#if 1
			if (std::fabs(cd_apx - tfgrad_hi) >= 1.0) {
				for (double bf = alpha_lo; bf < alpha_hi; bf +=
0.01*(alpha_hi-alpha_lo)) {
					double tfval, tfgrad;
					Evaluate(bf, tfval, tfgrad);
					std::cout << bf << " " << tfval << " " << tfgrad << " # TEST" << std::endl;
				}
			}
#endif
		}
#endif
		double alpha_trial = std::numeric_limits<double>::signaling_NaN();
		if ((boost::math::isinf)(alpha_hi_fval)) {
			alpha_trial = 0.9*alpha_lo + 0.1*alpha_hi;
			std::cout << "   * Warning: phi(" << alpha_hi << ") is NaN, "
				<< "trying bisection." << std::endl;
		} else {
			// Interpolation by quadratic, fixing
			// phi(alpha_lo), phi'(alpha_lo), phi(alpha_hi)
			double q_a = (alpha_lo_fval - alpha_hi_fval +
				alpha_lo_grad*(alpha_hi-alpha_lo)) /
				(-alpha_lo*alpha_lo - alpha_hi*alpha_hi +
					2.0*alpha_lo*alpha_hi);
			double q_b = alpha_lo_grad - 2.0*q_a*alpha_lo;
			// double q_c = alpha_hi_fval - q_a*alpha_hi*alpha_hi
			//    - alpha_lo_grad*alpha_hi + 2.0*q_a*alpha_lo*alpha_hi;
			if (q_a <= 1.0e-15) {
				std::cout << "   * Wolfe line search failed due to small "
					<< "quadratic coefficient (q_a=" << q_a << ")." << std::endl;
				std::cout << "     alpha_lo/hi " << alpha_lo << ", "
					<< alpha_hi << std::endl;
				std::cout << "     alpha_lo/hi fval " << alpha_lo_fval << ", "
					<< alpha_hi_fval << std::endl;
				std::cout << "     alpha_lo grad " << alpha_lo_grad << std::endl;
				break;
			}
			alpha_trial = - q_b / (2.0*q_a);
		}
#if 0
		std::cout << "        # alpha_trial: " << alpha_trial
			<< std::endl;
#endif
		Evaluate(alpha_trial, phi_trial_fval, phi_trial_grad);

#if 0
		std::cout << "        # Armijo (c1): "
			<< ((phi_trial_fval <= x0_fval + c1*alpha_trial*x0_phi_grad) ?
				"SATISFIED" : "NOT SATISFIED") << std::endl;
		std::cout << "        # Curvature (c2): "
			<< ((phi_trial_grad >= c2*x0_phi_grad) ? "SATISFIED"
				: "NOT SATISFIED") << std::endl;
		std::cout << "        #   phi'(a) = " << phi_trial_grad
			<< " >= c2*" << x0_phi_grad
			<< " = " << (c2*x0_phi_grad) << std::endl;
#endif

		// Check Armijo condition:
		// phi(alpha) <= phi(0) + c1 alpha phi'(0)
		if (phi_trial_fval > (x0_fval + c1*alpha_trial*x0_phi_grad) ||
			phi_trial_fval >= alpha_lo_fval) {
			// Decrease upper bracket
			alpha_hi = alpha_trial;
			continue;
		}

		// Found a point satisfying the Wolfe conditions
#if 0
		// Strong Wolfe
		if (std::fabs(phi_trial_grad) <= -c2*x0_phi_grad)
			return (alpha_trial);
#endif
		if (phi_trial_grad >= c2*x0_phi_grad)
			return (alpha_trial);

		// Flip bracket
		if (phi_trial_grad*(alpha_hi - alpha_lo) >= 0.0)
			alpha_hi = alpha_lo;

		// Increase lower bracket
		alpha_lo = alpha_trial;
		alpha_lo_fval = phi_trial_fval;
	}

	if (tries >= tries_max) {
		std::cout << "   * Wolfe line search exhausted function evaluation "
			<< "budget (" << tries_max << ")." << std::endl;
	}
	return (std::numeric_limits<double>::signaling_NaN());
}

FunctionMinimization::SimpleLineSearch::SimpleLineSearch(
	FunctionMinimizationProblem* prob, const std::vector<double>& x0,
	const std::vector<double>& x0_grad,
	const std::vector<double>& H_grad, double x0_fval)
	: prob(prob), x0(x0), x0_grad(x0_grad), H_grad(H_grad),
		x0_fval(x0_fval), x0_phi_grad(0),
		evaluation_count(0),
		xalpha_val(std::numeric_limits<double>::signaling_NaN())
{
	assert(x0.size() > 0);
	xalpha.resize(x0.size());
	xalphagrad.resize(x0.size());

	// phi'(0) = - p' \nabla_x f(x_k)
	x0_phi_grad = -std::inner_product(x0_grad.begin(), x0_grad.end(),
		H_grad.begin(), 0.0);
	assert(x0_phi_grad < 0.0);
}

unsigned int FunctionMinimization::SimpleLineSearch::ComputeStepLength(
	double& alpha) {
	// Previous alpha
	double alpha_min = 1.0e-12;
	double alpha_max = 1e6;

	double fa_enlarge = 1.7;
	double fa_shrink = 0.5;

	// Current alpha
	double phi_alpha_fval;
	double phi_alpha_grad;

	// Find a stepsize interval satisfying the strong Wolfe conditions for a
	// given iterate x and gradient gx and descent direction d:
	//   1. Armijo: f(x + alpha d) <= f(x) + c1 alpha gx' d,
	//   2. Curvature: |nabla_alpha f(x + alpha d)| <= c2 |gx' d|.
	unsigned int max_test = 200;
	for (unsigned int n = 0; true; ++n) {
#if 0
		std::cout << "SL n " << n << ", alpha " << alpha << std::endl;
#endif
		if (alpha <= alpha_min) {
			break;
		} else if (alpha >= alpha_max || n >= max_test) {
			if (n >= max_test)
				std::cout << "### WARNING: line search count exceeded, alpha "
<< alpha << std::endl;
			alpha = std::numeric_limits<double>::signaling_NaN();
			break;
		}
		Evaluate(alpha, phi_alpha_fval, phi_alpha_grad);

		// If Armijo condition is violated, shrink.
		if (phi_alpha_fval > (x0_fval + 1.0e-4*alpha*x0_phi_grad)) {
			alpha *= fa_shrink;
#if 0
			std::cout << "    armijo fail, shrink to " << alpha << std::endl;
#endif
			continue;
		}

		// The Armijo condition is satisfied.  If in addition the curvature
		// condition ("function must be flat around alpha") is satisfied, then
		// we found a point satisfying the Wolfe conditions.
		// Otherwise, enlarge alpha.
		if (phi_alpha_grad < 0.9*x0_phi_grad) {
			alpha *= fa_enlarge;
#if 0
			std::cout << "    wolfe fail, increase to " << alpha << std::endl;
#endif
			continue;
		}

		break;
	}

	return (evaluation_count);
}

unsigned int FunctionMinimization::SimpleLineSearch::ComputeStepLengthUpdate(
	std::vector<double>& x_out, std::vector<double>& grad_out,
	double& fval_out, double& alpha) {
	unsigned int ecount = ComputeStepLength(alpha);

	if ((boost::math::isnan)(alpha))
		return (ecount);

	// A valid step size has been computed
	if (xalpha_val != alpha) {
		// Is not up-to-date, update
		double d1;	// dummy
		double d2;
		Evaluate(alpha, d1, d2);
	}
	assert(xalpha_val == alpha);
	x_out = xalpha;
	grad_out = xalphagrad;
	fval_out = xalphaobj;

	return (ecount);
}

void FunctionMinimization::SimpleLineSearch::Evaluate(
	double alpha, double& phi_fval, double& phi_grad) {
	// x(alpha) = x - alpha*p
	std::transform(x0.begin(), x0.end(), H_grad.begin(),
		xalpha.begin(), _1 - alpha * _2);
	std::fill(xalphagrad.begin(), xalphagrad.end(), 0.0);

	// Evaluate phi(alpha) and derivative phi'(alpha)
	// phi(alpha) = f(x_k - alpha H_grad)
	// phi'(alpha) = -H_grad' \nabla_x f(x_k - alpha H_grad)
	phi_fval = prob->Eval(xalpha, xalphagrad);
	xalphaobj = phi_fval;	// save phi(alpha)
	xalpha_val = alpha;	// save alpha

	// Univariate derivative is projection onto ascent direction
	phi_grad = -std::inner_product(xalphagrad.begin(), xalphagrad.end(),
		H_grad.begin(), 0.0);
	evaluation_count += 1;
#if 0
	std::cout << "   phi(" << alpha << ") = " << phi_fval << ", grad "
		<< phi_grad << std::endl;
#endif
}

}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the kcentroid object
    from the dlib C++ Library.

    The kcentroid object is an implementation of an algorithm that recursively
    computes the centroid (i.e. average) of a set of points.  The interesting
    thing about dlib::kcentroid is that it does so in a kernel induced feature
    space.  This means that you can use it as a non-linear one-class classifier.
    So you might use it to perform online novelty detection (although, it has
    other uses, see the svm_pegasos or kkmeans examples for example).

    This example will train an instance of it on points from the sinc function.

*/

#include <iostream>
#include <vector>

#include <dlib/svm.h>
#include <dlib/statistics.h>

using namespace std;
using namespace dlib;

// Here is the sinc function we will be trying to learn with the kcentroid
// object.
double sinc(double x)
{
    if (x == 0)
        return 1;
    return sin(x)/x;
}

int main()
{
    // Here we declare that our samples will be 2 dimensional column vectors.
    // (Note that if you don't know the dimensionality of your vectors at compile time
    // you can change the 2 to a 0 and then set the size at runtime)
    typedef matrix<double,2,1> sample_type;

    // Now we are making a typedef for the kind of kernel we want to use.  I picked the
    // radial basis kernel because it only has one parameter and generally gives good
    // results without much fiddling.
    typedef radial_basis_kernel<sample_type> kernel_type;

    // Here we declare an instance of the kcentroid object.  The kcentroid has 3 parameters
    // you need to set.  The first argument to the constructor is the kernel we wish to
    // use.  The second is a parameter that determines the numerical accuracy with which
    // the object will perform the centroid estimation.  Generally, smaller values
    // give better results but cause the algorithm to attempt to use more dictionary vectors
    // (and thus run slower and use more memory).  The third argument, however, is the
    // maximum number of dictionary vectors a kcentroid is allowed to use.  So you can use
    // it to control the runtime complexity.
    kcentroid<kernel_type> test(kernel_type(0.1),0.01, 15);

    // now we train our object on a few samples of the sinc function.
    sample_type m;
    for (double x = -15; x <= 8; x += 1)
    {
        m(0) = x;
        m(1) = sinc(x);
        test.train(m);
    }

    running_stats<double> rs;

    // Now lets output the distance from the centroid to some points that are from the sinc function.
    // These numbers should all be similar.  We will also calculate the statistics of these numbers
    // by accumulating them into the running_stats object called rs.  This will let us easily
    // find the mean and standard deviation of the distances for use below.
    cout << "Points that are on the sinc function:\n";
    m(0) = -1.5; m(1) = sinc(m(0)); cout << "   " << test(m) << endl;  rs.add(test(m));
    m(0) = -1.5; m(1) = sinc(m(0)); cout << "   " << test(m) << endl;  rs.add(test(m));
    m(0) = -0;   m(1) = sinc(m(0)); cout << "   " << test(m) << endl;  rs.add(test(m));
    m(0) = -0.5; m(1) = sinc(m(0)); cout << "   " << test(m) << endl;  rs.add(test(m));
    m(0) = -4.1; m(1) = sinc(m(0)); cout << "   " << test(m) << endl;  rs.add(test(m));
    m(0) = -1.5; m(1) = sinc(m(0)); cout << "   " << test(m) << endl;  rs.add(test(m));
    m(0) = -0.5; m(1) = sinc(m(0)); cout << "   " << test(m) << endl;  rs.add(test(m));

    cout << endl;
    // Lets output the distance from the centroid to some points that are NOT from the sinc function.
    // These numbers should all be significantly bigger than previous set of numbers.  We will also
    // use the rs.scale() function to find out how many standard deviations they are away from the
    // mean of the test points from the sinc function.  So in this case our criterion for "significantly bigger"
    // is > 3 or 4 standard deviations away from the above points that actually are on the sinc function.
    cout << "Points that are NOT on the sinc function:\n";
    m(0) = -1.5; m(1) = sinc(m(0))+4;   cout << "   " << test(m) << " is " << rs.scale(test(m)) << " standard deviations from sinc." << endl;
    m(0) = -1.5; m(1) = sinc(m(0))+3;   cout << "   " << test(m) << " is " << rs.scale(test(m)) << " standard deviations from sinc." << endl;
    m(0) = -0;   m(1) = -sinc(m(0));    cout << "   " << test(m) << " is " << rs.scale(test(m)) << " standard deviations from sinc." << endl;
    m(0) = -0.5; m(1) = -sinc(m(0));    cout << "   " << test(m) << " is " << rs.scale(test(m)) << " standard deviations from sinc." << endl;
    m(0) = -4.1; m(1) = sinc(m(0))+2;   cout << "   " << test(m) << " is " << rs.scale(test(m)) << " standard deviations from sinc." << endl;
    m(0) = -1.5; m(1) = sinc(m(0))+0.9; cout << "   " << test(m) << " is " << rs.scale(test(m)) << " standard deviations from sinc." << endl;
    m(0) = -0.5; m(1) = sinc(m(0))+1;   cout << "   " << test(m) << " is " << rs.scale(test(m)) << " standard deviations from sinc." << endl;

    // And finally print out the mean and standard deviation of points that are actually from sinc().
    cout << "\nmean: " << rs.mean() << endl;
    cout << "standard deviation: " << rs.stddev() << endl;

    // The output is as follows:
    /*
        Points that are on the sinc function:
            0.869913
            0.869913
            0.873408
            0.872807
            0.870432
            0.869913
            0.872807

        Points that are NOT on the sinc function:
            1.06366 is 119.65 standard deviations from sinc.
            1.02212 is 93.8106 standard deviations from sinc.
            0.921382 is 31.1458 standard deviations from sinc.
            0.918439 is 29.3147 standard deviations from sinc.
            0.931428 is 37.3949 standard deviations from sinc.
            0.898018 is 16.6121 standard deviations from sinc.
            0.914425 is 26.8183 standard deviations from sinc.

            mean: 0.871313
            standard deviation: 0.00160756
    */

    // So we can see that in this example the kcentroid object correctly indicates that
    // the non-sinc points are definitely not points from the sinc function.
}


#pragma once
#include <mtao/types.hpp>
#include <Eigen/Sparse>
#include <mtao/iterator/enumerate.hpp>
#include <variant>

class Curve {
    public:
        enum class InterpMode {
            Wachpress, MeanValue, GaussianRBF, SplineGaussianRBF,
            DesbrunSplineRBF
        };
        template <InterpMode>
        struct InterpParameters {};
        class CurveEvaluator;
        mtao::vector<mtao::Vec2d> points;
        void clear() ;
        mtao::ColVecs2d edge_normals() const;
        mtao::ColVecs2d area_edge_normals() const;

        mtao::ColVecs2d V() const;
        mtao::ColVecs2i E() const;

        bool inside(const mtao::Vec2d& V) const;
        double winding_number(const mtao::Vec2d& V) const;

        size_t vertex_size() const { return points.size(); }
        size_t edge_size() const { return points.size(); }

        CurveEvaluator evaluator(InterpMode = InterpMode::MeanValue) const;

        template <InterpMode Mode>
        CurveEvaluator evaluator(const InterpParameters<Mode>& p = {}) const;

};
template <>
struct Curve::InterpParameters<Curve::InterpMode::GaussianRBF> {double radius = 1.;};
template <>
struct Curve::InterpParameters<Curve::InterpMode::SplineGaussianRBF> {double radius = 1.;};
template <>
struct Curve::InterpParameters<Curve::InterpMode::DesbrunSplineRBF> {double radius = 1.;};

class Curve::CurveEvaluator {
    public:
        using PermutationMat = Eigen::PermutationMatrix<Eigen::Dynamic,Eigen::Dynamic, int>;

        template <InterpMode Mode>
        CurveEvaluator(const Curve& c, const InterpParameters<Mode>& params): curve(c), vertices(c.V()), interp_params(params) {}
        mtao::MatXd distances(const mtao::ColVecs2d& Vs) const;

        mtao::VecXd operator()(const mtao::VecXd& f, const mtao::ColVecs2d& V) const;
        mtao::VecXd from_coefficients(const mtao::VecXd& coeffs, const mtao::ColVecs2d& V) const;
        mtao::ColVecs2d grad(const mtao::VecXd& f, const mtao::ColVecs2d& V) const;
        mtao::ColVecs2d grad_from_coefficients(const mtao::VecXd& coeffs, const mtao::ColVecs2d& V) const;

        int size() const { return vertices.cols(); }
        int vertex_size() const { return curve.vertex_size(); }
        auto& V() const { return vertices; }
    private:
        const Curve& curve;

        mtao::ColVecs2d vertices;
        std::variant<
            InterpParameters<InterpMode::MeanValue>
            ,InterpParameters<InterpMode::Wachpress>
            ,InterpParameters<InterpMode::GaussianRBF>
            ,InterpParameters<InterpMode::SplineGaussianRBF>
            ,InterpParameters<InterpMode::DesbrunSplineRBF>
            > interp_params;
};

        template <Curve::InterpMode Mode>
        Curve::CurveEvaluator Curve::evaluator(const InterpParameters<Mode>& p ) const {
            return CurveEvaluator(*this, p);
        }