/*  _______________________________________________________________________

    DAKOTA: Design Analysis Kit for Optimization and Terascale Applications
    Copyright 2014 Sandia Corporation.
    This software is distributed under the GNU Lesser General Public License.
    For more information, see the README file in the top Dakota directory.
    _______________________________________________________________________ */

//- Description:  This file contains code related to data utilities that should
//-               be compiled, rather than inlined in data_util.hpp.
//-
//- Owner:        Mike Eldred
//- Version: $Id: dakota_data_util.cpp 7024 2010-10-16 01:24:42Z mseldre $

#include "dakota_data_util.hpp"
#include <boost/math/special_functions/round.hpp>

namespace Dakota {

// ------------
// == operators
// ------------

bool nearby(const RealVector& rv1, const RealVector& rv2, Real rel_tol)
{
  // Check for equality in array lengths
  size_t len = rv1.length();
  if ( rv2.length() != len )
    return false;

  // Check each value (labels are ignored!)
  Real abs_tol = DBL_MIN; // ~ 2.2e-308
  for (size_t i=0; i<len; i++)
    // prevent division by 0
    if (std::abs(rv1[i]) < abs_tol) { //(rv1[i] == 0.)
      if (std::abs(rv2[i]) > abs_tol) //(rv2[i] != 0.)
	return false;
    }
    else if ( std::abs(1. - rv2[i]/rv1[i]) > rel_tol ) // DBL_EPSILON
      return false;

  return true;
}

bool operator==(const ShortArray& dsa1, const ShortArray& dsa2)
{
  // Check for equality in array lengths
  size_t len = dsa1.size();
  if ( dsa2.size() != len )
    return false;

  // Check each value
  for (size_t i=0; i<len; ++i)
    if ( dsa2[i] != dsa1[i] )
      return false;

  return true;
}

bool operator==(const StringArray& dsa1, const StringArray& dsa2)
{
  // Check for equality in array lengths
  size_t len = dsa1.size();
  if ( dsa2.size() != len )
    return false;

  // Check each string
  for (size_t i=0; i<len; ++i)
    if ( dsa2[i] != dsa1[i] )
      return false;

  return true;
}

// ---------------------------------
// miscellaneous numerical utilities
// ---------------------------------

Real rel_change_L2(const RealVector& curr_rv, const RealVector& prev_rv)
{
  size_t i, rv_len = prev_rv.length();
  Real norm = 0.;

  // check previous vector for zeros
  bool zero_prev = false, zero_curr = false;
  for (i=0; i<rv_len; ++i)
    if (std::abs(prev_rv[i]) < Pecos::SMALL_NUMBER)
      { zero_prev = true; break; }
  // check current vector for zeros
  if (zero_prev)
    for (i=0; i<rv_len; ++i)
      if (std::abs(curr_rv[i]) < Pecos::SMALL_NUMBER)
	{ zero_curr = true; break; }

  // Compute norm of relative change one of three ways
  if (!zero_prev) { // change relative to previous
    for (i=0; i<rv_len; ++i)
      norm += std::pow(curr_rv[i] / prev_rv[i] - 1., 2.);
    return std::sqrt(norm);
  }
  else if (!zero_curr) { // change relative to current
    for (i=0; i<rv_len; ++i)
      norm += std::pow(prev_rv[i] / curr_rv[i] - 1., 2.);
    return std::sqrt(norm);
  }
  else { // absolute change scaled by norm of previous
    Real scaling = 0.;
    for (i=0; i<rv_len; ++i) {
      norm    += std::pow(curr_rv[i] - prev_rv[i], 2.);
      scaling += std::pow(prev_rv[i], 2.);
    }
    return (scaling > Pecos::SMALL_NUMBER) ?
      std::sqrt(norm / scaling) : std::sqrt(norm);
  }
}

Real rel_change_L2(const RealVector& curr_rv1, const RealVector& prev_rv1,
		   const IntVector&  curr_iv,  const IntVector&  prev_iv,
		   const RealVector& curr_rv2, const RealVector& prev_rv2)
{
  size_t i, rv1_len = prev_rv1.length(), iv_len = prev_iv.length(),
    rv2_len = prev_rv2.length();
  Real norm = 0.;

  // check previous vectors for zeros
  bool zero_prev = false, zero_curr = false;
  for (i=0; i<rv1_len; ++i)
    if (std::abs(prev_rv1[i]) < Pecos::SMALL_NUMBER)
      { zero_prev = true; break; }
  if (!zero_prev)
    for (i=0; i<iv_len; ++i)
      if (std::abs(prev_iv[i]))
	{ zero_prev = true; break; }
  if (!zero_prev)
    for (i=0; i<rv2_len; ++i)
      if (std::abs(prev_rv2[i]) < Pecos::SMALL_NUMBER)
	{ zero_prev = true; break; }
  // check current vectors for zeros
  if (zero_prev) {
    for (i=0; i<rv1_len; ++i)
      if (std::abs(curr_rv1[i]) < Pecos::SMALL_NUMBER)
	{ zero_curr = true; break; }
    if (!zero_prev)
      for (i=0; i<iv_len; ++i)
	if (std::abs(curr_iv[i]))
	  { zero_curr = true; break; }
    if (!zero_prev)
      for (i=0; i<rv2_len; ++i)
	if (std::abs(curr_rv2[i]) < Pecos::SMALL_NUMBER)
	  { zero_curr = true; break; }
  }

  // Compute norm of relative change one of three ways
  if (!zero_prev) { // change relative to previous
    for (i=0; i<rv1_len; ++i)
      norm += std::pow(curr_rv1[i] / prev_rv1[i] - 1., 2.);
    for (i=0; i<iv_len; ++i)
      norm += std::pow(curr_iv[i]  / prev_iv[i]  - 1., 2.);
    for (i=0; i<rv2_len; ++i)
      norm += std::pow(curr_rv2[i] / prev_rv2[i] - 1., 2.);
    return std::sqrt(norm);
  }
  else if (!zero_curr) { // change relative to current
    for (i=0; i<rv1_len; ++i)
      norm += std::pow(prev_rv1[i] / curr_rv1[i] - 1., 2.);
    for (i=0; i<iv_len; ++i)
      norm += std::pow(prev_iv[i]  / curr_iv[i]  - 1., 2.);
    for (i=0; i<rv2_len; ++i)
      norm += std::pow(prev_rv2[i] / curr_rv2[i] - 1., 2.);
    return std::sqrt(norm);
  }
  else { // absolute change scaled by norm of previous
    Real scaling = 0.;
    for (i=0; i<rv1_len; ++i) {
      norm    += std::pow(curr_rv1[i] - prev_rv1[i], 2.);
      scaling += prev_rv1[i] * prev_rv1[i];
    }
    for (i=0; i<iv_len; ++i) {
      norm    += std::pow(curr_iv[i] - prev_iv[i], 2.);
      scaling += prev_iv[i] * prev_iv[i];
    }
    for (i=0; i<rv2_len; ++i) {
      norm    += std::pow(curr_rv2[i] - prev_rv2[i], 2.);
      scaling += prev_rv2[i] * prev_rv2[i];
    }
    return (scaling > Pecos::SMALL_NUMBER) ?
      std::sqrt(norm / scaling) : std::sqrt(norm);
  }
}

void remove_column(RealMatrix& matrix, int index)
{
  int num_cols = matrix.numCols();
  RealMatrix matrix_new(matrix.numRows(), num_cols-1);
  for (int i = 0; i<num_cols; ++i){
      const RealVector& col_vec = Teuchos::getCol(Teuchos::View, matrix, i);
    if (i < index){
      Teuchos::setCol(col_vec, i, matrix_new);
    }
    if (i > index){
      Teuchos::setCol(col_vec, i-1, matrix_new);
    }
  }
  matrix.reshape(matrix.numRows(), num_cols-1);
  matrix = matrix_new;
}

void iround(const RealVector& input_vec, IntVector& rounded_vec)
{
  int len = input_vec.length();
  if (rounded_vec.length() != len)
    rounded_vec.resize(len);
  for (int i=0; i<len; ++i)
    rounded_vec[i] = boost::math::iround(input_vec[i]);
}

} // namespace Dakota


#include <fstream>
#include <iostream>
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/bind.hpp>

extern "C"
{
	#include "mtrand.h"
	#include "cvodesim.h"
	#include "ssa.h"
	#include "testing.h"
}

#include "ParameterMC.h"

static int NUM_THREADS = 4;
static boost::mutex mutex;

static double ** SUM_XiXj = 0;
static double * SUM_Xi = 0;

void setNumThreads(int n)
{
	if (n > 0)
		NUM_THREADS = n;
}

static void sampleParameterSpaceSingleRun(int numSamples, int numParameters, RandomParameterFunc getRandParams, TargetFunc isGood, int * count)
{
	int i=0,j=0,k=0,n=0,accept=0;
	double * params = (double*)malloc(numParameters * sizeof(double));

	//run n times
	for (k=0; k < numSamples; ++k)
	{
		//get a random set of parameter
		getRandParams(params);

		//check if it is acceptable (the time consuming step, usually)
		accept = isGood(params);

		if (accept)
		{
			//if acceptable, store it
			mutex.lock();
			++(*count);
			for (i=0; i < numParameters; ++i)
			{
				//std::cout << params[i] << "\t";
				SUM_Xi[i] += params[i];
				for (j=i; j < numParameters; ++j)
					SUM_XiXj[i][j] += params[i] * params[j];
			}
			//std::cout << std::endl;
			mutex.unlock();
		}
	}

	delete params;
}

void sampleParameterSpace(int numSamples, int numParameters, RandomParameterFunc getRandParams, TargetFunc isGood, double * Mu, double ** Sigma)
{
	int i = 0, j = 0, n = 0, n_last = 0, num_threads = 0, count = 0;
	boost::thread ** threads = 0;
	boost::thread * thread = 0;

	/**** global arrays for storing covariance and means ****/
	//SUM_XiXj = new double*[numParameters];
	//SUM_Xi = new double[numParameters];

	SUM_XiXj = Sigma;
	SUM_Xi = Mu;

	for (i=0; i < numParameters; ++i)
	{
		//SUM_XiXj[i] = new double[numParameters];
		SUM_Xi[i] = 0.0;

		for (j=0; j < numParameters; ++j)
			SUM_XiXj[i][j] = 0.0;
	}

	/**** divide the task amongst n threads ****/

	if (numSamples < NUM_THREADS)
		numSamples = NUM_THREADS;

	n = (int)(numSamples/NUM_THREADS); //each thread is allocated n runs
	num_threads = NUM_THREADS;

	n_last = numSamples - (n * NUM_THREADS);  //last thread is given n_last runs
	if (n_last > 0)
		++num_threads;

	threads = new boost::thread*[num_threads];

	/**** make Boost threads ****/

	for (i=0; i < num_threads; ++i)
	{
		if ((n_last > 0) && (i == (num_threads-1)))
			n = n_last;

		thread = new boost::thread(
			boost::bind(
			&sampleParameterSpaceSingleRun,
			n,
			numParameters,
			getRandParams,
			isGood,
			&count));

		threads[i] = thread;
	}

	for (i=0; i < num_threads; ++i)
		threads[i]->join();

	for (i=0; i < num_threads; ++i)
		delete threads[i];

	delete threads;

	--count; //unbiased mean and cov

	for (i = 0; i < numParameters; ++i)
		SUM_Xi[i] /= (double)count;

	for (i = 0; i < numParameters; ++i)
	{
		for (j = i; j < numParameters; ++j)
		{
			SUM_XiXj[i][j] = SUM_XiXj[i][j]/(double)count - SUM_Xi[i]*SUM_Xi[j];
			SUM_XiXj[j][i] = SUM_XiXj[i][j];
		}
	}
}

//testing

int isgood(double * p)
{
	int numPeaks = 0;
	int i,k,sz;
	double dt,dx,mu;
	double * y = 0;

	assignParameters(p);
	TCinitialize();

	//y = ODEsim2(TCvars, TCreactions, TCstoic, &TCpropensity, TCinit, 0, 100, 0.1, 0,0,0,0);
	//sz = 1000;
	y = SSA(TCvars, TCreactions, TCstoic, &TCpropensity, TCinit, 0, 100, 100000, &sz, 0,0,0,0) ;
	if (y)
	{
		mu = 0.0;
		for (i = sz/2; i < sz; ++i)
			mu += getValue(y,TCvars+1,i,8);

		mu = 2.0*mu/sz;
		dt = sz/100;

		for (i=1,k=0; i < sz; ++i)
		{
			dx = mu - getValue(y,TCvars+1,i,8);
			if ((dx*dx) < 1)
			{
				if (k==0 || (i > k+dt))
					++numPeaks;
				k = i;
			}
		}
		free(y);
	}

	return (int)( sz > 100 && numPeaks > 10 );
}

void randomParams(double * p)
{
	int i;
	for (i=0; i < TCparams; ++i)
		p[i] = mtrand() * pow(2.0,5 * mtrand());
	p[6] = 4.0*mtrand();
	p[8] = 4.0*mtrand();
	p[16] = 4.0*mtrand();
	p[18] = 4.0*mtrand();
}

int main(int argc, char* argv[])
{
	double * Mu = new double[TCparams];
	double ** Sigma = new double*[TCparams];

	for (int i=0; i < TCparams; ++i)
		Sigma[i] = new double[TCparams];

	FILE * fout = fopen( "params.tab", "w" );

	sampleParameterSpace(1000000, TCparams ,&randomParams, &isgood, Mu, Sigma);

	fprintf(fout, "\n\nMu\n\n");

	for (int i=0; i < TCparams; ++i)
	{
		fprintf(fout, "%lf\t",Mu[i]);
	}

	fprintf(fout, "\n\nSigma\n\n");

	for (int i=0; i < TCparams; ++i)
	{
		for (int j=0; j < TCparams; ++j)
			fprintf(fout, "%lf\t",Sigma[i][j]);
		fprintf(fout, "\n");
	}

	fclose(fout);

	for (int i=0; i < TCparams; ++i)
		delete Sigma[i];

	delete Sigma;
	delete Mu;

	return 0;
}


/* Author: Toby D. Young, Polish Academy of Sciences,             */
/*         Wolfgang Bangerth, Texas A&M University                */
/*    $Id: step-36.cc 28237 2013-02-05 16:53:54Z heister $*/
/*                                                                */
/*    Copyright (C) 2009, 2011-2012 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// @sect3{Include files}

// As mentioned in the introduction, this program is essentially only a
// slightly revised version of step-4. As a consequence, most of the following
// include files are as used there, or at least as used already in previous
// tutorial programs:
#include <deal.II/base/logstream.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/function_parser.h>
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/utilities.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/lac/full_matrix.h>

// PETSc appears here because SLEPc depends on this library:
#include <deal.II/lac/petsc_sparse_matrix.h>
#include <deal.II/lac/petsc_vector.h>

// And then we need to actually import the interfaces for solvers that SLEPc
// provides:
#include <deal.II/lac/slepc_solver.h>

// We also need some standard C++:
#include <fstream>
#include <iostream>

// Finally, as in previous programs, we import all the deal.II class and
// function names into the namespace into which everything in this program
// will go:
namespace Step36
{
  using namespace dealii;

  // @sect3{The EigenvalueProblem class template}

  // Following is the class declaration for the main class template. It looks
  // pretty much exactly like what has already been shown in step-4:
  template <int dim>
  class EigenvalueProblem
  {
  public:
    EigenvalueProblem (const std::string &prm_file);
    void run ();

  private:
    void make_grid_and_dofs ();
    void assemble_system ();
    unsigned int solve ();
    void output_results () const;

    Triangulation<dim> triangulation;
    FE_Q<dim>          fe;
    DoFHandler<dim>    dof_handler;

    // With these exceptions: For our eigenvalue problem, we need both a
    // stiffness matrix for the left hand side as well as a mass matrix for
    // the right hand side. We also need not just one solution function, but a
    // whole set of these for the eigenfunctions we want to compute, along
    // with the corresponding eigenvalues:
    PETScWrappers::SparseMatrix        stiffness_matrix, mass_matrix;
    std::vector<PETScWrappers::Vector> eigenfunctions;
    std::vector<double>                eigenvalues;

    // And then we need an object that will store several run-time parameters
    // that we will specify in an input file:
    ParameterHandler parameters;

    // Finally, we will have an object that contains "constraints" on our
    // degrees of freedom. This could include hanging node constraints if we
    // had adaptively refined meshes (which we don't have in the current
    // program). Here, we will store the constraints for boundary nodes
    // $U_i=0$.
    ConstraintMatrix constraints;
  };

  // @sect3{Implementation of the EigenvalueProblem class}

  // @sect4{EigenvalueProblem::EigenvalueProblem}

  // First up, the constructor. The main new part is handling the run-time
  // input parameters. We need to declare their existence first, and then read
  // their values from the input file whose name is specified as an argument
  // to this function:
  template <int dim>
  EigenvalueProblem<dim>::EigenvalueProblem (const std::string &prm_file)
    :
    fe (1),
    dof_handler (triangulation)
  {
//TODO investigate why the minimum number of refinement steps required to obtain the correct eigenvalue degeneracies is 6
    parameters.declare_entry ("Global mesh refinement steps", "5",
                              Patterns::Integer (0, 20),
                              "The number of times the 1-cell coarse mesh should "
                              "be refined globally for our computations.");
    parameters.declare_entry ("Number of eigenvalues/eigenfunctions", "5",
                              Patterns::Integer (0, 100),
                              "The number of eigenvalues/eigenfunctions "
                              "to be computed.");
    parameters.declare_entry ("Potential", "0",
                              Patterns::Anything(),
                              "A functional description of the potential.");

    parameters.read_input (prm_file);
  }

  // @sect4{EigenvalueProblem::make_grid_and_dofs}

  // The next function creates a mesh on the domain $[-1,1]^d$, refines it as
  // many times as the input file calls for, and then attaches a DoFHandler to
  // it and initializes the matrices and vectors to their correct sizes. We
  // also build the constraints that correspond to the boundary values
  // $u|_{\partial\Omega}=0$.
  //
  // For the matrices, we use the PETSc wrappers. These have the ability to
  // allocate memory as necessary as non-zero entries are added. This seems
  // inefficient: we could as well first compute the sparsity pattern,
  // initialize the matrices with it, and as we then insert entries we can be
  // sure that we do not need to re-allocate memory and free the one used
  // previously. One way to do that would be to use code like this:
  // @code
  //   CompressedSimpleSparsityPattern
  //      csp (dof_handler.n_dofs(),
  //           dof_handler.n_dofs());
  //   DoFTools::make_sparsity_pattern (dof_handler, csp);
  //   csp.compress ();
  //   stiffness_matrix.reinit (csp);
  //   mass_matrix.reinit (csp);
  // @endcode
  // instead of the two reinit() calls for the
  // stiffness and mass matrices below.
  //
  // This doesn't quite work, unfortunately. The code above may lead to a few
  // entries in the non-zero pattern to which we only ever write zero entries;
  // most notably, this holds true for off-diagonal entries for those rows and
  // columns that belong to boundary nodes. This shouldn't be a problem, but
  // for whatever reason, PETSc's ILU preconditioner, which we use to solve
  // linear systems in the eigenvalue solver, doesn't like these extra entries
  // and aborts with an error message.
  //
  // In the absence of any obvious way to avoid this, we simply settle for the
  // second best option, which is have PETSc allocate memory as
  // necessary. That said, since this is not a time critical part, this whole
  // affair is of no further importance.
  template <int dim>
  void EigenvalueProblem<dim>::make_grid_and_dofs ()
  {
    GridGenerator::hyper_cube (triangulation, -1, 1);
    triangulation.refine_global (parameters.get_integer ("Global mesh refinement steps"));
    dof_handler.distribute_dofs (fe);

    DoFTools::make_zero_boundary_constraints (dof_handler, constraints);
    constraints.close ();

    stiffness_matrix.reinit (dof_handler.n_dofs(),
                             dof_handler.n_dofs(),
                             dof_handler.max_couplings_between_dofs());
    mass_matrix.reinit (dof_handler.n_dofs(),
                        dof_handler.n_dofs(),
                        dof_handler.max_couplings_between_dofs());

    // The next step is to take care of the eigenspectrum. In this case, the
    // outputs are eigenvalues and eigenfunctions, so we set the size of the
    // list of eigenfunctions and eigenvalues to be as large as we asked for
    // in the input file:
    eigenfunctions
    .resize (parameters.get_integer ("Number of eigenvalues/eigenfunctions"));
    for (unsigned int i=0; i<eigenfunctions.size (); ++i)
      eigenfunctions[i].reinit (dof_handler.n_dofs ());

    eigenvalues.resize (eigenfunctions.size ());
  }

  // @sect4{EigenvalueProblem::assemble_system}

  // Here, we assemble the global stiffness and mass matrices from local
  // contributions $A^K_{ij} = \int_K \nabla\varphi_i(\mathbf x) \cdot
  // \nabla\varphi_j(\mathbf x) + V(\mathbf x)\varphi_i(\mathbf
  // x)\varphi_j(\mathbf x)$ and $M^K_{ij} = \int_K \varphi_i(\mathbf
  // x)\varphi_j(\mathbf x)$ respectively. This function should be immediately
  // familiar if you've seen previous tutorial programs. The only thing new
  // would be setting up an object that described the potential $V(\mathbf x)$
  // using the expression that we got from the input file. We then need to
  // evaluate this object at the quadrature points on each cell. If you've
  // seen how to evaluate function objects (see, for example the coefficient
  // in step-5), the code here will also look rather familiar.
  template <int dim>
  void EigenvalueProblem<dim>::assemble_system ()
  {
    QGauss<dim>   quadrature_formula(2);

    FEValues<dim> fe_values (fe, quadrature_formula,
                             update_values | update_gradients |
                             update_quadrature_points | update_JxW_values);

    const unsigned int dofs_per_cell = fe.dofs_per_cell;
    const unsigned int n_q_points    = quadrature_formula.size();

    FullMatrix<double> cell_stiffness_matrix (dofs_per_cell, dofs_per_cell);
    FullMatrix<double> cell_mass_matrix (dofs_per_cell, dofs_per_cell);

    std::vector<unsigned int> local_dof_indices (dofs_per_cell);

    FunctionParser<dim> potential;
    potential.initialize (FunctionParser<dim>::default_variable_names (),
                          parameters.get ("Potential"),
                          typename FunctionParser<dim>::ConstMap());

    std::vector<double> potential_values (n_q_points);

    typename DoFHandler<dim>::active_cell_iterator
    cell = dof_handler.begin_active (),
    endc = dof_handler.end ();
    for (; cell!=endc; ++cell)
      {
        fe_values.reinit (cell);
        cell_stiffness_matrix = 0;
        cell_mass_matrix      = 0;

        potential.value_list (fe_values.get_quadrature_points(),
                              potential_values);

        for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
          for (unsigned int i=0; i<dofs_per_cell; ++i)
            for (unsigned int j=0; j<dofs_per_cell; ++j)
              {
                cell_stiffness_matrix (i, j)
                += (fe_values.shape_grad (i, q_point) *
                    fe_values.shape_grad (j, q_point)
                    +
                    potential_values[q_point] *
                    fe_values.shape_value (i, q_point) *
                    fe_values.shape_value (j, q_point)
                   ) * fe_values.JxW (q_point);

                cell_mass_matrix (i, j)
                += (fe_values.shape_value (i, q_point) *
                    fe_values.shape_value (j, q_point)
                   ) * fe_values.JxW (q_point);
              }

        // Now that we have the local matrix contributions, we transfer them
        // into the global objects and take care of zero boundary constraints:
        cell->get_dof_indices (local_dof_indices);

        constraints
        .distribute_local_to_global (cell_stiffness_matrix,
                                     local_dof_indices,
                                     stiffness_matrix);
        constraints
        .distribute_local_to_global (cell_mass_matrix,
                                     local_dof_indices,
                                     mass_matrix);
      }

    // At the end of the function, we tell PETSc that the matrices have now
    // been fully assembled and that the sparse matrix representation can now
    // be compressed as no more entries will be added:
    stiffness_matrix.compress ();
    mass_matrix.compress ();
  }

  // @sect4{EigenvalueProblem::solve}

  // This is the key new functionality of the program. Now that the system is
  // set up, here is a good time to actually solve the problem: As with other
  // examples this is done using a "solve" routine. Essentially, it works as
  // in other programs: you set up a SolverControl object that describes the
  // accuracy to which we want to solve the linear systems, and then we select
  // the kind of solver we want. Here we choose the Krylov-Schur solver of
  // SLEPc, a pretty fast and robust choice for this kind of problem:
  template <int dim>
  unsigned int EigenvalueProblem<dim>::solve ()
  {

    // We start here, as we normally do, by assigning convergence control we
    // want:
    SolverControl solver_control (dof_handler.n_dofs(), 1e-9);
    SLEPcWrappers::SolverKrylovSchur eigensolver (solver_control);

    // Before we actually solve for the eigenfunctions and -values, we have to
    // also select which set of eigenvalues to solve for. Lets select those
    // eigenvalues and corresponding eigenfunctions with the smallest real
    // part (in fact, the problem we solve here is symmetric and so the
    // eigenvalues are purely real). After that, we can actually let SLEPc do
    // its work:
    eigensolver.set_which_eigenpairs (EPS_SMALLEST_REAL);

    eigensolver.solve (stiffness_matrix, mass_matrix,
                       eigenvalues, eigenfunctions,
                       eigenfunctions.size());

    // The output of the call above is a set of vectors and values. In
    // eigenvalue problems, the eigenfunctions are only determined up to a
    // constant that can be fixed pretty arbitrarily. Knowing nothing about
    // the origin of the eigenvalue problem, SLEPc has no other choice than to
    // normalize the eigenvectors to one in the $l_2$ (vector)
    // norm. Unfortunately this norm has little to do with any norm we may be
    // interested from a eigenfunction perspective: the $L_2(\Omega)$ norm, or
    // maybe the $L_\infty(\Omega)$ norm.
    //
    // Let us choose the latter and rescale eigenfunctions so that they have
    // $\|\phi_i(\mathbf x)\|_{L^\infty(\Omega)}=1$ instead of
    // $\|\Phi\|_{l_2}=1$ (where $\phi_i$ is the $i$th eigenfunction
    // and $\Phi_i$ the corresponding vector of nodal values). For the $Q_1$
    // elements chosen here, we know that the maximum of the function
    // $\phi_i(\mathbf x)$ is attained at one of the nodes, so $\max_{\mathbf
    // x}\phi_i(\mathbf x)=\max_j (\Phi_i)_j$, making the normalization in the
    // $L_\infty$ norm trivial. Note that this doesn't work as easily if we
    // had chosen $Q_k$ elements with $k>1$: there, the maximum of a function
    // does not necessarily have to be attained at a node, and so
    // $\max_{\mathbf x}\phi_i(\mathbf x)\ge\max_j (\Phi_i)_j$ (although the
    // equality is usually nearly true).
    for (unsigned int i=0; i<eigenfunctions.size(); ++i)
      eigenfunctions[i] /= eigenfunctions[i].linfty_norm ();

    // Finally return the number of iterations it took to converge:
    return solver_control.last_step ();
  }

  // @sect4{EigenvalueProblem::output_results}

  // This is the last significant function of this program. It uses the
  // DataOut class to generate graphical output from the eigenfunctions for
  // later visualization. It works as in many of the other tutorial programs.
  //
  // The whole collection of functions is then output as a single VTK file.
  template <int dim>
  void EigenvalueProblem<dim>::output_results () const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler (dof_handler);

    for (unsigned int i=0; i<eigenfunctions.size(); ++i)
      data_out.add_data_vector (eigenfunctions[i],
                                std::string("eigenfunction_") +
                                Utilities::int_to_string(i));

    // The only thing worth discussing may be that because the potential is
    // specified as a function expression in the input file, it would be nice
    // to also have it as a graphical representation along with the
    // eigenfunctions. The process to achieve this is relatively
    // straightforward: we build an object that represents $V(\mathbf x)$ and
    // then we interpolate this continuous function onto the finite element
    // space. The result we also attach to the DataOut object for
    // visualization.
    Vector<double> projected_potential (dof_handler.n_dofs());
    {
      FunctionParser<dim> potential;
      potential.initialize (FunctionParser<dim>::default_variable_names (),
                            parameters.get ("Potential"),
                            typename FunctionParser<dim>::ConstMap());
      VectorTools::interpolate (dof_handler, potential, projected_potential);
    }
    data_out.add_data_vector (projected_potential, "interpolated_potential");

    data_out.build_patches ();

    std::ofstream output ("eigenvectors.vtk");
    data_out.write_vtk (output);
  }

  // @sect4{EigenvalueProblem::run}

  // This is the function which has the top-level control over everything. It
  // is almost exactly the same as in step-4:
  template <int dim>
  void EigenvalueProblem<dim>::run ()
  {
    make_grid_and_dofs ();

    std::cout << "   Number of active cells:       "
              << triangulation.n_active_cells ()
              << std::endl
              << "   Number of degrees of freedom: "
              << dof_handler.n_dofs ()
              << std::endl;

    assemble_system ();

    const unsigned int n_iterations = solve ();
    std::cout << "   Solver converged in " << n_iterations
              << " iterations." << std::endl;

    output_results ();

    std::cout << std::endl;
    for (unsigned int i=0; i<eigenvalues.size(); ++i)
      std::cout << "      Eigenvalue " << i
                << " : " << eigenvalues[i]
                << std::endl;
  }
}

// @sect3{The main function}
int main (int argc, char **argv)
{
  try
    {

      // Here is another difference from other steps: We initialize the SLEPc
      // work space which inherently initializes the PETSc work space, then go
      // ahead run the whole program. After that is done, we finalize the
      // SLEPc-PETSc work.
      SlepcInitialize (&argc, &argv, 0, 0);

      {
        using namespace dealii;
        using namespace Step36;

        deallog.depth_console (0);

        EigenvalueProblem<2> problem ("step-36.prm");
        problem.run ();
      }

      SlepcFinalize ();
    }

  // All the while, we are watching out if any exceptions should have been
  // generated. If that is so, we panic...
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  // If no exceptions are thrown, then we tell the program to stop monkeying
  // around and exit nicely:
  std::cout << std::endl
            << "   Job done."
            << std::endl;

  return 0;
}


#include <cmath>
#include <aslam/backend/util/utils.hpp>
#include <aslam/backend/LineSearch.hpp>
#include <Eigen/Dense>
#include <sm/eigen/assert_macros.hpp>
#include <sm/PropertyTree.hpp>
#include <sm/logging.hpp>

/*
Most of the following is a c++ translations of code from https://github.com/scipy/scipy/blob/master/scipy/optimize/linesearch.py,
the function dcstep is based on https://github.com/scipy/scipy/blob/master/scipy/optimize/minpack2/dcstep.f,
the class Dcsrch is based on https://github.com/scipy/scipy/blob/master/scipy/optimize/minpack2/dcsrch.f .

For those parts the following license applies:

SciPy project (http://www.scipy.org/):

Copyright (c) 2001, 2002 Enthought, Inc.
All rights reserved.

Copyright (c) 2003-2016 SciPy Developers.
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

  a. Redistributions of source code must retain the above copyright notice,
     this list of conditions and the following disclaimer.
  b. Redistributions in binary form must reproduce the above copyright
     notice, this list of conditions and the following disclaimer in the
     documentation and/or other materials provided with the distribution.
  c. Neither the name of Enthought nor the names of the SciPy Developers
     may be used to endorse or promote products derived from this software
     without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS
BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
THE POSSIBILITY OF SUCH DAMAGE.

*/

using namespace std;

namespace aslam {
namespace backend {

using std::isnan;

void dcstep(double& stx, double& fx, double& dx, double& sty, double& fy, double& dy, double& stp,
            const double fp, const double dp, bool& brackt, const double stpmin, const double stpmax) {

  double sgnd = dp*(utils::sign(dx));
  double stpf;

  // First case: A higher function value. The minimum is bracketed.
  // If the cubic step is closer to stx than the quadratic step, the
  // cubic step is taken, otherwise the average of the cubic and
  // quadratic steps is taken.
  if (fp > fx) {

    const double theta = 3.0*(fx-fp)/(stp-stx) + dx + dp;
    const double s = max(abs(theta), max(abs(dx), abs(dp)));
    const double srec = 1./s;
    double gamma = s*sqrt(utils::sqr(theta*srec)-(dx*srec)*(dp*srec));
    if (stp < stx)
        gamma = -gamma;
    const double p = (gamma-dx) + theta;
    const double q = ((gamma-dx)+gamma) + dp;
    const double r = p/q;
    const double stpc = stx + r*(stp-stx);
    const double stpq = stx + ((dx/((fx-fp)/(stp-stx)+dx))/2.0)*(stp-stx);
    if (abs(stpc-stx) < abs(stpq-stx))
      stpf = stpc;
    else
      stpf = stpc + (stpq-stpc)/2.0;

    brackt = true;

  // Second case: A lower function value and derivatives of opposite
  // sign. The minimum is bracketed. If the cubic step is farther from
  // stp than the secant step, the cubic step is taken, otherwise the
  // secant step is taken.
  } else if (sgnd < 0.0) {

    const double theta = 3.0*(fx-fp)/(stp-stx) + dx + dp;
    const double s = max(abs(theta), max(abs(dx), abs(dp)));
    const double srec = 1./s;
    double gamma = s*sqrt(utils::sqr(theta*srec)-(dx*srec)*(dp*srec));
    if (stp > stx)
      gamma = -gamma;
    const double p = (gamma-dp) + theta;
    const double q = ((gamma-dp)+gamma) + dx;
    const double r = p/q;
    const double stpc = stp + r*(stx-stp);
    const double stpq = stp + (dp/(dp-dx))*(stx-stp);
    if (abs(stpc-stp) > abs(stpq-stp))
      stpf = stpc;
    else
      stpf = stpq;
    brackt = true;

  // Third case: A lower function value, derivatives of the same sign,
  // and the magnitude of the derivative decreases.
  } else if (abs(dp) < abs(dx)) {

      // The cubic step is computed only if the cubic tends to infinity
      // in the direction of the step or if the minimum of the cubic
      // is beyond stp. Otherwise the cubic step is defined to be the
      // secant step.
      const double theta = 3.0*(fx-fp)/(stp-stx) + dx + dp;
      const double s = max(abs(theta), max(abs(dx), abs(dp)));

      // The case gamma = 0 only arises if the cubic does not tend
      // to infinity in the direction of the step.
      const double srec = 1./s;
      double gamma = s*sqrt(max(0.0, utils::sqr(theta*srec)-(dx*srec)*(dp*srec)));
      if (stp > stx)
        gamma = -gamma;
      const double p = (gamma-dp) + theta;
      const double q = (gamma+(dx-dp)) + gamma;
      const double r = p/q;
      double stpc;
      if (r < 0.0 and gamma != 0.0)
        stpc = stp + r*(stx-stp);
      else if (stp > stx)
        stpc = stpmax;
      else
        stpc = stpmin;

      const double stpq = stp + (dp/(dp-dx))*(stx-stp);

      if (brackt) {

        // A minimizer has been bracketed. If the cubic step is
        // closer to stp than the secant step, the cubic step is
        // taken, otherwise the secant step is taken.

        if (abs(stpc-stp) < abs(stpq-stp))
          stpf = stpc;
        else
          stpf = stpq;

        if (stp > stx)
          stpf = min(stp+0.66*(sty-stp),stpf);
        else
          stpf = max(stp+0.66*(sty-stp),stpf);

      } else {

        // A minimizer has not been bracketed. If the cubic step is
        // farther from stp than the secant step, the cubic step is
        // taken, otherwise the secant step is taken.

        if (abs(stpc-stp) > abs(stpq-stp))
          stpf = stpc;
        else
          stpf = stpq;

        stpf = max( min(stpmax,stpf), stpmin);
      }

  // Fourth case: A lower function value, derivatives of the same sign,
  // and the magnitude of the derivative does not decrease. If the
  // minimum is not bracketed, the step is either stpmin or stpmax,
  // otherwise the cubic step is taken.
  } else {

    if (brackt) {
      const double theta = 3.0*(fp-fy)/(sty-stp) + dy + dp;
      const double s = max(abs(theta), max(abs(dy), abs(dp)));
      const double srec = 1./s;
      double gamma = s*sqrt(utils::sqr(theta*srec)-(dy*srec)*(dp*srec));
      if (stp > sty)
          gamma = -gamma;
      const double p = (gamma-dp) + theta;
      const double q = ((gamma-dp)+gamma) + dy;
      const double r = p/q;
      const double stpc = stp + r*(sty-stp);
      stpf = stpc;

    } else if (stp > stx) {
      stpf = stpmax;
    } else {
      stpf = stpmin;
    }
  }

  // Update the interval which contains a minimizer.
  if (fp > fx) {
    sty = stp;
    fy = fp;
    dy = dp;
  } else {
    if (sgnd < 0) {
      sty = stx;
      fy = fx;
      dy = dx;
    }

    stx = stp;
    fx = fp;
    dx = dp;
  }

  // Compute the new step.
  stp = stpf;
}

double cubicMin(double a, double fa, double fpa, double b, double fb, double c, double fc) {
  const double db = b - a;
  const double dc = c - a;
  const double denom = utils::sqr(db * dc) * (db - dc);
  Eigen::Matrix2d d1;
  d1(0,0) = utils::sqr(dc);
  d1(0,1) = -utils::sqr(db);
  d1(1,0) = -d1(0,0)*dc;
  d1(1,1) = -d1(0,1)*db;
  Eigen::Vector2d AB = d1*(Eigen::Vector2d(fb - fa - fpa * db, fc - fa - fpa * dc))/denom;
  const double radical = utils::sqr((double)AB[1]) - 3.0 * AB[0] * fpa;
  return a + (-AB[1] + sqrt(radical)) / (3.0 * AB[0]);
}

double quadMin(double a, double fa, double fpa, double b, double fb) {
  const double db = b - a;
  const double B = (fb - fa - fpa * db) / utils::sqr(db);
  return a - 0.5 * fpa / B;
}

Dcsrch::Dcsrch(double stepLengthInit, double error, double errorDerivative, double minStepLength, double maxStepLength, double ftol, double xtol, double gtol) :
    _xtol(xtol),
    _ftol (ftol),
    _gtol(gtol),
    _minStepLength(minStepLength),
    _maxStepLength(maxStepLength),
    _stepLength(stepLengthInit),
    _finit(error),
    _fx(error),
    _fy(error),
    _ginit(errorDerivative),
    _gtest(_ftol*_ginit),
    _ginitTimesNegGtol(-_gtol*_ginit),
    _gx(errorDerivative),
    _gy(errorDerivative),
    _stmax(stepLengthInit + _xtrapu*stepLengthInit),
    _width(maxStepLength - minStepLength),
    _width1(_width*2.0)
{

  SM_ASSERT_GT_DBG(Exception, _xtol, 0.0, "");
  SM_ASSERT_GT_DBG(Exception, _ftol, 0.0, "");
  SM_ASSERT_GT_DBG(Exception, _gtol, 0.0, "");
  SM_ASSERT_GT_DBG(Exception, _minStepLength, 0.0, "");
  SM_ASSERT_GT_DBG(Exception, _maxStepLength, 0.0, "");

  SM_ASSERT_LT(Exception, errorDerivative, 0.0, "");

  SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "Dcsrch: initial interval: [" << _stx << ", " << _sty << "], step length: " << _stepLength);

}

double Dcsrch::updateStepLength(double error, double errorDerivative) {

  if (_stage == 0) {
    _stage++;
    return _stepLength;
  }

  // If psi(stepLength) <= 0 and f'(stepLength) >= 0 for some step, then the
  // algorithm enters the second stage.
  const double ftest = _finit + _stepLength*_gtest;
  if (_stage == 1 && error <= ftest && errorDerivative >= 0.0)
    _stage = 2;

  SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: dcsrch -- stage: " << _stage << ", ftest: " << ftest <<
                      ", minimum step length: " << _stmin << ", maximum step length: " << _stmax);

  // Test for warnings.
  if (_brackt && (_stepLength <= _stmin || _stepLength >= _stmax)) {
    SM_WARN_STREAM(setprecision(20) << "LineSearch: dcsrch -- Rounding errors prevent progress: step length " << _stepLength <<
            " outside interval (" << _stmin << ", " << _stmax << ")");
    _status = WARNING;
  }
  if (_brackt && _stmax-_stmin <= _xtol*_stmax) {
    SM_WARN_STREAM(setprecision(20) << "LineSearch: dcsrch -- xtol test satisfied: " << (_stmax-_stmin) << " <= " << _xtol*_stmax);
    _status = WARNING;
  }
  if (_stepLength == _maxStepLength && error <= ftest && errorDerivative <= _gtest) {
    SM_WARN("LineSearch: dcsrch -- step length reached maximum step length");
    _status = WARNING;
  }
  if (_stepLength == _minStepLength && (error > ftest || errorDerivative >= _gtest)) {
    SM_WARN("LineSearch: dcsrch -- step length reached minimum step length");
    _status = WARNING;
  }

  // Test for convergence.
  if (error <= ftest) {

    SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: dcsrch -- sufficient decrease condition satisfied: " << error << " <= " << ftest);

    if (abs(errorDerivative) <= _ginitTimesNegGtol)
      _status = CONVERGED;
    else
      SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: dcsrch -- curvature condition not satisfied: " << abs(errorDerivative) << " <= " << _ginitTimesNegGtol);

  } else {
    SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: dcsrch -- sufficient decrease condition not satisfied: " << error << " <= " << ftest);
  }

  // Test for termination.
  if (_status == WARNING || _status == CONVERGED)
    return _stepLength;

  // A modified function is used to predict the step during the
  // first stage if a lower function value has been obtained but
  // the decrease is not sufficient.
  if (_stage == 1 && error <= _fx && error > ftest) {

    // Define the modified function and derivative values.
    const double fm = error - _stepLength*_gtest;
    double fxm = _fx - _stx*_gtest;
    double fym = _fy - _sty*_gtest;
    double gm = errorDerivative - _gtest;
    double gxm = _gx - _gtest;
    double gym = _gy - _gtest;

    // Call dcstep to update stx, sty, and to compute the new step.
    dcstep(_stx, fxm, gxm, _sty, fym, gym, _stepLength, fm, gm, _brackt, _stmin, _stmax);

    // Reset the function and derivative values for error.
    _fx = fxm + _stx*_gtest;
    _fy = fym + _sty*_gtest;
    _gx = gxm + _gtest;
    _gy = gym + _gtest;

  } else {
    // Call dcstep to update stx, sty, and to compute the new step.
    dcstep(_stx, _fx, _gx, _sty, _fy, _gy, _stepLength, error, errorDerivative, _brackt, _stmin, _stmax);
  }

  // Decide if a bisection step is needed.
  if (_brackt) {
    const double dstxy = _sty - _stx;
    const double absdstxy = abs(dstxy);
    if (absdstxy >= 0.66 * _width1)
      _stepLength = _stx + 0.5*dstxy;
    _width1 = _width;
    _width = absdstxy;
  }

  // Set the minimum and maximum steps allowed for stepLength.
  if (_brackt) {
    _stmin = min(_stx,_sty);
    _stmax = max(_stx,_sty);
  } else {
    const double dstpx = _stepLength - _stx;
    _stmin = _stepLength + _xtrapl*dstpx;
    _stmax = _stepLength + _xtrapu*dstpx;
  }

  // Force the step to be within the bounds.
  _stepLength = min( max(_stepLength, _minStepLength), _maxStepLength);

  // If further progress is not possible, let stepLength be the best
  // point obtained during the search.
  if ((_brackt && (_stepLength <= _minStepLength || _stepLength >= _maxStepLength)) ||
      (_brackt && _maxStepLength-_minStepLength <= _xtol*_maxStepLength))
    _stepLength = _stx;

  SM_ALL_STREAM_NAMED("optimization.linesearch", "Dcsrch: new interval: [" << _stx << ", " << _sty << "], step length = " << _stepLength);

  return _stepLength;
}

LineSearchOptions::LineSearchOptions() {
  check();
}

LineSearchOptions::LineSearchOptions(const sm::PropertyTree& config)
{
  c1WolfeCondition = config.getDouble("c1WolfeCondition", c1WolfeCondition);
  c2WolfeCondition = config.getDouble("c2WolfeCondition", c2WolfeCondition);
  maxStepLength = config.getDouble("maxStepLength", maxStepLength);
  minStepLength = config.getDouble("minStepLength", minStepLength);
  xtol = config.getDouble("xtol", xtol);
  initialStepLength = config.getDouble("initialStepLength", initialStepLength);
  nMaxIterWolfe1 = config.getInt("nMaxIterWolfe1", nMaxIterWolfe1);
  nMaxIterWolfe2 = config.getInt("nMaxIterWolfe2", nMaxIterWolfe2);
  nMaxIterZoom = config.getInt("nMaxIterZoom", nMaxIterZoom);
  check();
}

void LineSearchOptions::check() const {
  SM_ASSERT_GE(Exception, c1WolfeCondition, 0.0, "");
  SM_ASSERT_GE(Exception, c2WolfeCondition, c1WolfeCondition, "");
  SM_ASSERT_GE(Exception, maxStepLength, 0.0, "");
  SM_ASSERT_GE(Exception, minStepLength, 0.0, "");
  SM_ASSERT_GE(Exception, xtol, 0.0, "");
  SM_ASSERT_GT(Exception, initialStepLength, 0.0, "");
  SM_ASSERT_GT(Exception, nMaxIterWolfe1, 0, "");
  SM_ASSERT_GT(Exception, nMaxIterWolfe2, 0, "");
  SM_ASSERT_GT(Exception, nMaxIterZoom, 0, "");
}

ostream& operator<<(ostream& out, const aslam::backend::LineSearchOptions& options)
{
  out << "LineSearchOptions:\n";
  out << "\tc1WolfeCondition: " << options.c1WolfeCondition << endl;
  out << "\tc1WolfeCondition: " << options.c1WolfeCondition << endl;
  out << "\tc2WolfeCondition: " << options.c2WolfeCondition << endl;
  out << "\tmaxStepLength: " << options.maxStepLength << endl;
  out << "\tminStepLength: " << options.minStepLength << endl;
  out << "\txtol: " << options.xtol << endl;
  out << "\tinitialStepLength: " << options.initialStepLength << endl;
  out << "\tnMaxIterWolfe1: " << options.nMaxIterWolfe1 << endl;
  out << "\tnMaxIterWolfe2: " << options.nMaxIterWolfe2 << endl;
  out << "\tnMaxIterZoom: " << options.nMaxIterZoom;
  return out;
}

LineSearch::LineSearch(const boost::shared_ptr<CostFunctionInterface>& cf, const LineSearchOptions& options) :
    _costFunction(cf),
    _options(options)
{
  SM_ASSERT_TRUE(Exception, cf != nullptr, "");
  _options.check();
}

LineSearch::LineSearch(const boost::shared_ptr<CostFunctionInterface>& cf) :
    LineSearch::LineSearch(cf, LineSearchOptions())
{
}

LineSearch::LineSearch(const boost::shared_ptr<CostFunctionInterface>& cf, const sm::PropertyTree& config) :
    LineSearch::LineSearch(cf, LineSearchOptions(config))
{
}

LineSearch::~LineSearch()
{

}

void LineSearch::initialize(boost::optional<const RowVectorType&> searchDirection /*= boost::optional<const RowVectorType&>()*/,
                            boost::optional<double> error /*= boost::optional<double>()*/,
                            boost::optional<const RowVectorType&> gradient /*= boost::optional<const RowVectorType&>()*/)
{
  _stepLength = 0.0;
  _errorOutdated = _derrorOutdated = true;
  _errorOld = std::numeric_limits<double>::signaling_NaN();

  if (error)
    _error = error.get();
  else
    this->updateError();
  _errorOutdated = false;

  if (gradient)
    _gradient = gradient.get();
  else
    this->updateGradient();

  if (searchDirection)
    this->setSearchDirection(searchDirection.get());
  else
    _searchDirection = RowVectorType::Zero(0);

}

void LineSearch::setSearchDirection(const RowVectorType& searchDirection) {
  using namespace Eigen;
  _stepLength = 0.0; // if the search direction changed, we must avoid skipping updates with same step lengths
  _searchDirection = searchDirection;
  _derror = computeErrorDerivative();
  _derrorOutdated = false;
  SM_VERBOSE_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: set search direction to " << _searchDirection.format(IOFormat(15, DontAlignCols, ", ", ", ", "", "", "[", "]")));
  SM_VERBOSE_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: computed error derivative " << _derror);
  SM_ASSERT_LE(Exception, _derror, 0.0, "Wrong search direction supplied! In case approximate Hessian information is used, "
      "this could mean your Hessian estimate became negative");
}

void LineSearch::applyStateUpdate(const double s) {

  using namespace Eigen;
  static IOFormat fmt(15, 0, ", ", ", ", "", "", "[", "]");

  double ds = s - _stepLength;
  _stepLength = s;

  if (ds != 0.0) { // save computation time
    Eigen::RowVectorXd p = sm::logging::getLevel() <= sm::logging::Level::Verbose ?
        utils::getFlattenedDesignVariableParameters(_costFunction->getDesignVariables()).transpose() :  Eigen::RowVectorXd();
    utils::applyStateUpdate(_costFunction->getDesignVariables(), ds*_searchDirection);
    _errorOutdated = _derrorOutdated = true;
    SM_VERBOSE_STREAM_NAMED("optimization.linesearch", "LineSearch: update step length " << s - ds << " -> " << _stepLength << " (ds: " << ds<< ")");
    SM_VERBOSE_STREAM_NAMED("optimization.linesearch", "LineSearch: update state" << std::endl <<
                            "Old  : " << p.format(fmt) << std::endl <<
                            "New  : " << utils::getFlattenedDesignVariableParameters(_costFunction->getDesignVariables()).transpose().format(fmt) << std::endl <<
                            "Delta: " << (utils::getFlattenedDesignVariableParameters(_costFunction->getDesignVariables()).transpose() - p).format(fmt));
  } else {
    SM_ALL_NAMED("optimization.linesearch", "LineSearch: skipping unnecessary update of information");
  }
}

void LineSearch::updateError() {
  if (_errorOutdated) {
    const double errorOld = _error;
    _error = _costFunction->evaluateError();
    if (_evalErrorCallback) _evalErrorCallback();
    SM_VERBOSE_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: update error " << errorOld << " -> " << _error << " (" << _error - errorOld << ")");
  }
  _errorOutdated = false;
}

void LineSearch::updateGradient() {
  _costFunction->computeGradient(_gradient);
  if (_evalGradCallback) _evalGradCallback();
}

void LineSearch::updateErrorDerivative() {
  if (_derrorOutdated) {
    const double dErrorOld = _derror;
    this->updateGradient();
    _derror = computeErrorDerivative();
    SM_VERBOSE_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: update error derivative "<< dErrorOld << " -> " << _derror << " (" << _derror - dErrorOld << ")");
  }
  _derrorOutdated = false;
}

bool LineSearch::zoom(double minStepSize, double maxStepSize, double error_lo, double error_hi, double derror_lo, double error0, double derror0) {

  size_t i = 0;
  const double delta1 = 0.2;  // cubic interpolant check
  const double delta2 = 0.1;  // quadratic interpolant check
  double error_rec = error0;
  double stepSize_rec = 0.0;

  while (true) {
    // Interpolate to find a trial step length between a_lo and a_hi.
    // Use cubic interpolation in the first step.
    // If the result is within delta * dalpha or outside of the bounded interval defined by a_lo or a_hi use quadratic interpolation.
    // If the result is still too close, then use bisection

    const double dStepLength = maxStepSize - minStepSize;
    double a, b;
    if (dStepLength < 0.0) {
      a = maxStepSize;
      b = minStepSize;
    } else {
      a = minStepSize;
      b = maxStepSize;
    }

    // Try cubic interpolation
    double cubicchk;
    double stepSize_j = numeric_limits<double>::signaling_NaN();
    if (i > 0) {
      cubicchk = delta1 * dStepLength;
      stepSize_j = cubicMin(minStepSize, error_lo, derror_lo, maxStepSize, error_hi, stepSize_rec, error_rec);
      SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: zoom -- cubic interpolation through points [" << minStepSize <<
                          ", " << maxStepSize << ", " << stepSize_rec << "] with errors " << "[" << error_lo << ", " << error_hi << ", " << error_rec <<
                          "] and derivative at lower interval point " << derror_lo << " returned step length " << stepSize_j);
    }

    // Try quadratic interpolation
    if (i == 0 || isnan(stepSize_j) || stepSize_j > b - cubicchk || stepSize_j < a + cubicchk) {
      const double quadchk = delta2 * dStepLength;
      stepSize_j = quadMin(minStepSize, error_lo, derror_lo, maxStepSize, error_hi);
      SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: zoom -- quadratic interpolation through points [" << minStepSize <<
                          ", " << maxStepSize << "] with errors " << "[" << error_lo << ", " << error_hi << "] and derivative at lower interval point " <<
                          derror_lo << " returned step length " << stepSize_j);
      if (isnan(stepSize_j) || stepSize_j > b - quadchk || stepSize_j < a + quadchk)
        stepSize_j = minStepSize + 0.5*dStepLength;
    }

    // Move state to stepSize_j, do not compute gradient information at new point yet since
    // we have to check first whether the error decreased. If not, there's no need to compute the gradient.
    this->applyStateUpdate(stepSize_j);
    this->updateError();

    // Check new value of stepSize_j
    const double error_j = getError();

    // Check Wolfe condition 1 (Armijo rule)
    if ((error_j > error0 + _options.c1WolfeCondition*stepSize_j*derror_lo) or (error_j >= error_lo)) {
      // If condition is not satisfied, set endpoint of interval to new point stepSize_j
      error_rec = error_hi;
      stepSize_rec = maxStepSize;
      maxStepSize = stepSize_j;
      error_hi = error_j;
      SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: zoom -- sufficient decrease condition not satisfied: " << error_j << " <= " << error0 + _options.c1WolfeCondition*stepSize_j*derror_lo);
    } else {
      SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: zoom -- sufficient decrease condition satisfied: " << error_j << " <= " << error0 + _options.c1WolfeCondition*stepSize_j*derror_lo);
      // If Armijo rule is satisfied, also check curvature condition.
      // Therefore we have to update the gradient based information now.
      this->updateErrorDerivative();
      const double derror_j = getErrorDerivative();
      if (abs(derror_j) <= -_options.c2WolfeCondition*derror0)  { // If curvature condition is satisfied, we found a suitable point
        SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: zoom -- curvature condition satisfied: " << abs(derror_j) << " <= " << -_options.c2WolfeCondition*derror0);
        break;
      }

      SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: zoom -- curvature condition not satisfied: " << abs(derror_j) << " <= " << -_options.c2WolfeCondition*derror0);

      if (derror_j*(maxStepSize - minStepSize) >= 0) {
        error_rec = error_hi;
        stepSize_rec = maxStepSize;
        maxStepSize = minStepSize;
        error_hi = error_lo;
      } else {
        error_rec = error_lo;
        stepSize_rec = minStepSize;
      }
      minStepSize = stepSize_j;
      error_lo = error_j;
      derror_lo = derror_j;
    }

    i++;
    if (i == _options.nMaxIterZoom) {
      SM_ERROR("LineSearch: zoom -- Failed to find a conforming step size");
      return false;
    }

    SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: zoom -- update interval: [" << minStepSize << ", " << maxStepSize << "]");
  }

  return true;
}

double LineSearch::computeErrorDerivative() const {
  return _gradient*_searchDirection.transpose();
}

bool LineSearch::lineSearchWolfe1() {

  // Check that the error and gradient information is up to date and not NaN
  SM_ASSERT_FALSE(Exception, isnan(getError()), "");
  SM_ASSERT_FALSE(Exception, isnan(getErrorDerivative()), "");

  double stepLength = _options.initialStepLength;
  if (!isnan(_errorOld) && _derror != 0.0) {
    stepLength = min(_options.maxStepLength, 1.01*2.0*(_error - _errorOld)/_derror);
    if (stepLength < 0.0) stepLength = _options.initialStepLength;
  }

  _errorOld = _error;

  SM_ASSERT_GE(Exception, stepLength, _options.minStepLength, "");
  SM_ASSERT_LE(Exception, stepLength, _options.maxStepLength, "");

  SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: wolfe1 -- starting line search at error value " <<
                      _error << " and derivative " << _derror);

  if (_derror == 0.0) {
    SM_FINE_STREAM_NAMED("optimization.linesearch", "LineSearch: Error derivative is zero, seems like the system is at its optimum.");
    return true;
  }

  bool success = false;
  bool terminate = false;
  Dcsrch dcsrch(stepLength, getError(), getErrorDerivative(), _options.minStepLength,
                _options.maxStepLength, _options.c1WolfeCondition, _options.xtol, _options.c2WolfeCondition);

  size_t cnt = 0;
  while(!terminate && cnt < _options.nMaxIterWolfe1) {

    SM_ALL_STREAM_NAMED("optimization.linesearch", "LineSearch: wolfe1 -- iteration " << cnt);

    const double stp = dcsrch.updateStepLength(getError(), getErrorDerivative());

    switch(dcsrch.status()) {
      case Dcsrch::RUNNING:
        stepLength = stp;
        this->applyStateUpdate(stp);
        this->updateError();
        this->updateErrorDerivative();
        break;
      case Dcsrch::CONVERGED:
        SM_FINE_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: wolfe1 -- converged, final step length " << stp <<
                              ", final error " << getError() << ", final error derivative " << getErrorDerivative());
        success = terminate = true;
        break;
      case Dcsrch::WARNING:
        terminate = true;
        break;
    }

    cnt++;
  }

  if (cnt == _options.nMaxIterWolfe1) { // maxiter reached, the line search did not converge
    SM_ERROR_STREAM("LineSearch: wolfe1 -- no solution found in " << _options.nMaxIterWolfe1 << " iterations");
    return false;
  }

  if (!success) {
    SM_ERROR("LineSearch: wolfe1 -- dcsrch exited with a warning");
    return false;
  }

  return true;

}

bool LineSearch::lineSearchWolfe2() {

  // Check that the error and gradient information is up to date and not NaN
  SM_ASSERT_FALSE(Exception, isnan(getError()), "");
  SM_ASSERT_FALSE(Exception, isnan(getErrorDerivative()), "");

  double minStepLength = 0.0;
  double maxStepLength = _options.initialStepLength;
  if (!isnan(_errorOld) && _derror != 0) {
    maxStepLength = min(_options.initialStepLength, 1.01*2.0*(_error - _errorOld)/_derror);
    if (maxStepLength < 0.0) maxStepLength = _options.initialStepLength;
  }

  _errorOld = _error;

  SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: wolfe2 -- starting line search at error value " <<
                      _error << " and derivative " << _derror);

  if (_derror == 0.0) {
    SM_FINE_STREAM_NAMED("optimization.linesearch", "LineSearch: Error derivative is zero, seems like the system is at its optimum.");
    return true;
  }

  if (maxStepLength == 0.0) {
    SM_WARN("LineSearch: wolfe2 -- Maximum step length is zero. This shouldn't happen. "
        "Perhaps the increment has slipped below machine precision?");
    return false;
  }

  const double error0 = _error;
  const double derror0 = _derror;
  double errorStepMin = error0;
  double derrorStepMin = derror0;

  this->applyStateUpdate(maxStepLength); // Move to position x + maxStepLength*searchDirection
  this->updateError();
  double errorStepMax = getError();
//  double derrorStepMax; // evaluated below

  bool success = false;
  for (size_t i=0; i<_options.nMaxIterWolfe2; ++i) {

    SM_ALL_STREAM_NAMED("optimization.linesearch", "LineSearch: wolfe2 -- iteration " << i);

    if (maxStepLength == 0.0)
      break;

    // Check Wolfe condition 1 (Armijo rule)
    if ((errorStepMax > error0 + _options.c1WolfeCondition * maxStepLength * derror0) || ((errorStepMax >= errorStepMin) && (i > 0))) {
      SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: wolfe2 -- sufficient decrease condition not satisfied: " <<
                          errorStepMax << " <= " <<  error0 + _options.c1WolfeCondition * maxStepLength * derror0);
      // zoom will move the state to a good position in the interval [minStepLength, maxStepLength]
      SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: wolfe2 -- calling zoom with interval [ " << minStepLength << ", " << maxStepLength << "] with errors " <<
                          "[ " << errorStepMin << ", " << errorStepMax << "] and derivative at lower interval point " << derrorStepMin);
      success = this->zoom(minStepLength, maxStepLength, errorStepMin, errorStepMax, derrorStepMin, error0, derror0);
      break;
    }

    this->updateErrorDerivative();
    const double derrorStepMax = getErrorDerivative();

    // Check curvature condition
    if ((abs(derrorStepMax) <= -_options.c2WolfeCondition*derror0)) {
      success = true;
      SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: wolfe2 -- curvature condition satisfied: " << abs(derrorStepMax) << " <= " << -_options.c2WolfeCondition*derror0);
      break;
    } else {
      SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: wolfe2 -- curvature condition not satisfied: " << abs(derrorStepMax) << " <= " << -_options.c2WolfeCondition*derror0);
    }

    if ((derrorStepMax >= 0.0)) {
      SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: wolfe2 -- calling zoom with interval [ " << maxStepLength << ", " << minStepLength << "] with errors " <<
                          "[ " << errorStepMax << ", " << errorStepMin << "] and derivative at lower interval point " << derrorStepMax);
      success = zoom(maxStepLength, minStepLength, errorStepMax, errorStepMin, derrorStepMax, error0, derror0);
      break;
    }

    double maxStepLengthNew = 2.0 * maxStepLength; // increase by factor of two on each iteration
    minStepLength = maxStepLength;
    maxStepLength = maxStepLengthNew;
    errorStepMin = errorStepMax;

    this->applyStateUpdate(maxStepLength);
    this->updateError();
    errorStepMax = getError();
    derrorStepMin = derrorStepMax;

    SM_ALL_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: wolfe2 -- update interval: [" << minStepLength << ", " << maxStepLength << "]");

  }

  if (success)
    SM_FINE_STREAM_NAMED("optimization.linesearch", setprecision(20) << "LineSearch: wolfe2 -- converged, final step length " << getCurrentStepLength() <<
                         ", final error " << getError());
  else
    SM_ERROR_STREAM("LineSearch: wolfe2 -- no solution found in " << _options.nMaxIterWolfe2 << " iterations");

  return success;

}

bool LineSearch::lineSearchWolfe12() {

  const double errorOld0 = _errorOld; // _errorOld gets modified by lineSearchWolfe1
  const double error0 = _error;
  const double derror0 = _derror;

  utils::DesignVariableState dvstate(_costFunction->getDesignVariables());

  if (!lineSearchWolfe1()) {
    SM_FINE_STREAM_NAMED("optimization.linesearch", "LineSearch: method wolfe1 failed, trying method wolfe2");

    // restore error values to the ones before calling lineSearchWolfe1().
    // These are the values that correspond to step length zero.
    _errorOld = errorOld0;
    _error = error0;
    _derror = derror0;
    _stepLength = 0.0;
    dvstate.restore();

    return lineSearchWolfe2();
  }

  return true;
}

} // namespace backend
} // namespace aslam