/* lsqcpp.h
 *
 * Author: Fabian Meyer
 * Created On: 22 Jul 2019
 * License: MIT
 */

#ifndef LSQCPP_LSQCPP_H_
#define LSQCPP_LSQCPP_H_

#include <Eigen/Geometry>
#include <vector>
#include <limits>
#include <iostream>
#include <iomanip>
#include <functional>

namespace lsq
{
    typedef Eigen::MatrixXd::Index Index;

    /** Functor to compute forward differences.
      * Computes the gradient of the objective f(x) as follows:
      *
      * grad(x) = (f(x + eps) - f(x)) / eps
      *
      * The computation requires len(x) evaluations of the objective.
      */
    template<typename Scalar>
    class ForwardDifferences
    {
    public:
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
        typedef std::function<void(const Vector &, Vector &)> ErrorFunction;
    private:
        Scalar eps_;
        int threads_;
        ErrorFunction objective_;
    public:
        ForwardDifferences()
            : ForwardDifferences(
                std::sqrt(std::numeric_limits<Scalar>::epsilon()))
        { }

        ForwardDifferences(const Scalar eps)
            : eps_(eps), threads_(1), objective_()
        { }

        void setNumericalEpsilon(const Scalar eps)
        {
            eps_ = eps;
        }

        void setThreads(const int threads)
        {
            threads_ = threads;
        }

        void setErrorFunction(const ErrorFunction &objective)
        {
            objective_ = objective;
        }

        void operator()(const Vector &xval,
            const Vector &fval,
            Matrix &jacobian)
        {
            assert(objective_);

            jacobian.resize(fval.size(), xval.size());
            #pragma omp parallel for num_threads(threads_)
            for(Index i = 0; i < xval.size(); ++i)
            {
                Vector fvalN;
                Vector xvalN = xval;
                xvalN(i) += eps_;
                objective_(xvalN, fvalN);

                jacobian.col(i) = (fvalN - fval) / eps_;
            }
        }
    };

    /** Functor to compute backward differences.
      * Computes the gradient of the objective f(x) as follows:
      *
      * grad(x) = (f(x) - f(x - eps)) / eps
      *
      * The computation requires len(x) evaluations of the objective.
      */
    template<typename Scalar>
    class BackwardDifferences
    {
    public:
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
        typedef std::function<void(const Vector &, Vector &)> ErrorFunction;
    private:
        Scalar eps_;
        int threads_;
        ErrorFunction objective_;
    public:
        BackwardDifferences()
            : BackwardDifferences(
                std::sqrt(std::numeric_limits<Scalar>::epsilon()))
        { }

        BackwardDifferences(const Scalar eps)
            : eps_(eps), threads_(1), objective_()
        { }

        void setNumericalEpsilon(const Scalar eps)
        {
            eps_ = eps;
        }

        void setThreads(const int threads)
        {
            threads_ = threads;
        }

        void setErrorFunction(const ErrorFunction &objective)
        {
            objective_ = objective;
        }

        void operator()(const Vector &xval,
            const Vector &fval,
            Matrix &jacobian)
        {
            assert(objective_);

            jacobian.resize(fval.size(), xval.size());
            #pragma omp parallel for num_threads(threads_)
            for(Index i = 0; i < xval.size(); ++i)
            {
                Vector fvalN;
                Vector xvalN = xval;
                xvalN(i) -= eps_;
                objective_(xvalN, fvalN);
                jacobian.col(i) = (fval - fvalN) / eps_;
            }
        }
    };

    /** Functor to compute central differences.
      * Computes the gradient of the objective f(x) as follows:
      *
      * grad(x) = (f(x + 0.5 eps) - f(x - 0.5 eps)) / eps
      *
      * The computation requires 2 * len(x) evaluations of the objective.
      */
    template<typename Scalar>
    struct CentralDifferences
    {
    public:
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
        typedef std::function<void(const Vector &, Vector &)> ErrorFunction;
    private:
        Scalar eps_;
        int threads_;
        ErrorFunction objective_;
    public:
        CentralDifferences()
            : CentralDifferences(
                std::sqrt(std::numeric_limits<Scalar>::epsilon()))
        { }

        CentralDifferences(const Scalar eps)
            : eps_(eps), threads_(1), objective_()
        { }

        void setNumericalEpsilon(const Scalar eps)
        {
            eps_ = eps;
        }

        void setThreads(const int threads)
        {
            threads_ = threads;
        }

        void setErrorFunction(const ErrorFunction &objective)
        {
            objective_ = objective;
        }

        void operator()(const Vector &xval,
            const Vector &fval,
            Matrix &jacobian)
        {
            assert(objective_);

            std::vector<Vector> fvalN(xval.size() * 2);
            #pragma omp parallel for num_threads(threads_)
            for(Index i = 0; i < static_cast<Index>(fvalN.size()); ++i)
            {
                Index idx = i / 2;
                Vector xvalN = xval;
                if(i % 2 == 0)
                    xvalN(idx) += eps_ / 2;
                else
                    xvalN(idx) -= eps_ / 2;

                objective_(xvalN, fvalN[i]);
            }

            jacobian.resize(fval.size(), xval.size());
            for(Index i = 0; i < xval.size(); ++i)
                jacobian.col(i) = (fvalN[i * 2] - fvalN[i * 2 + 1]) / eps_;
        }
    };

    /** Dummy callback functor, which does nothing. */
    template<typename Scalar>
    struct NoCallback
    {
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;

        bool operator()(const Index,
            const Vector &,
            const Vector &,
            const Matrix &,
            const Vector &,
            const Vector &) const
        {
            return true;
        }
    };

    /** Step size functor, which returns a constant step size. */
    template<typename Scalar>
    class ConstantStepSize
    {
    public:
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
        typedef std::function<void(const Vector &, Vector &, Matrix &)> ErrorFunction;
    private:
        Scalar stepSize_;
    public:

        ConstantStepSize()
            : ConstantStepSize(static_cast<Scalar>(1.0))
        {

        }

        ConstantStepSize(const Scalar stepSize)
            : stepSize_(stepSize)
        {

        }

        /** Set the step size returned by this functor.
          * @param stepSize step size returned by functor */
        void setStepSize(const Scalar stepSize)
        {
            stepSize_ = stepSize;
        }

        void setErrorFunction(const ErrorFunction &)
        { }

        Scalar operator()(const Vector &,
            const Vector &,
            const Matrix &,
            const Vector &,
            const Vector &)
        {
            return stepSize_;
        }
    };

    /** Step size functor to compute Barzilai-Borwein (BB) steps.
      * The functor can either compute the direct or inverse BB step.
      * The steps are computed as follows:
      *
      * s_k = x_k - x_k-1         k >= 1
      * y_k = step_k - step_k-1   k >= 1
      * Direct:  stepSize = (s_k^T * s_k) / (y_k^T * s_k)
      * Inverse: stepSize = (y_k^T * s_k) / (y_k^T * y_k)
      *
      * The very first step is computed as a constant. */
    template<typename Scalar>
    class BarzilaiBorwein
    {
    public:
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
        typedef std::function<void(const Vector &, Vector &, Matrix &)> ErrorFunction;

        enum class Method
        {
            Direct,
            Inverse
        };
    private:
        Vector lastXval_;
        Vector lastStep_;
        Method method_;
        Scalar constStep_;

        Scalar constantStep() const
        {
            return constStep_;
        }

        Scalar directStep(const Vector &xval,
            const Vector &step)
        {
            auto sk = xval - lastXval_;
            auto yk = step - lastStep_;
            Scalar num = sk.dot(sk);
            Scalar denom = sk.dot(yk);

            if(denom == 0)
                return 1;
            else
                return num / denom;
        }

        Scalar inverseStep(const Vector &xval,
            const Vector &step)
        {
            auto sk = xval - lastXval_;
            auto yk = step - lastStep_;
            Scalar num = sk.dot(yk);
            Scalar denom = yk.dot(yk);

            if(denom == 0)
                return 1;
            else
                return num / denom;
        }
    public:
        BarzilaiBorwein()
            : BarzilaiBorwein(Method::Direct, static_cast<Scalar>(1e-2))
        { }

        BarzilaiBorwein(const Method method, const Scalar constStep)
            : lastXval_(), lastStep_(), method_(method),
            constStep_(constStep)
        { }

        void setErrorFunction(const ErrorFunction &)
        { }

        void setMethod(const Method method)
        {
            method_ = method;
        }

        void setConstStepSize(const Scalar stepSize)
        {
            constStep_ = stepSize;
        }

        Scalar operator()(const Vector &xval,
            const Vector &,
            const Matrix &,
            const Vector &,
            const Vector &step)
        {
            Scalar stepSize = 0;
            if(lastXval_.size() == 0)
            {
                stepSize = (1 / step.norm()) * constStep_;
            }
            else
            {
                switch(method_)
                {
                case Method::Direct:
                    stepSize = directStep(xval, step);
                    break;
                case Method::Inverse:
                    stepSize = inverseStep(xval, step);
                    break;
                default:
                    assert(false);
                    break;
                }
            }

            lastStep_ = step;
            lastXval_ = xval;

            return stepSize;
        }
    };

    /** Step size functor to perform Armijo Linesearch with backtracking.
      * The functor iteratively decreases the step size until the following
      * conditions are met:
      *
      * Armijo: f(x - stepSize * grad(x)) <= f(x) - c1 * stepSize * grad(x)^T * grad(x)
      *
      * If the condition does not hold the step size is decreased:
      *
      * stepSize = decrease * stepSize
      */
    template<typename Scalar>
    class ArmijoBacktracking
    {
    public:
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
        typedef std::function<void(const Vector &, Vector &, Matrix &)> ErrorFunction;
    private:
        Scalar decrease_;
        Scalar c1_;
        Scalar minStep_;
        Scalar maxStep_;
        Index maxIt_;
        ErrorFunction objective_;

    public:
        ArmijoBacktracking()
            : ArmijoBacktracking(
                static_cast<Scalar>(0.8),
                static_cast<Scalar>(1e-4),
                static_cast<Scalar>(1e-10),
                static_cast<Scalar>(1.0),
                0)
        { }

        ArmijoBacktracking(const Scalar decrease,
            const Scalar c1,
            const Scalar minStep,
            const Scalar maxStep,
            const Index iterations)
            : decrease_(decrease), c1_(c1), minStep_(minStep),
            maxStep_(maxStep), maxIt_(iterations), objective_()
        { }

        /** Set the decreasing factor for backtracking.
          * Assure that decrease in (0, 1).
          * @param decrease decreasing factor */
        void setBacktrackingDecrease(const Scalar decrease)
        {
            assert(decrease > static_cast<Scalar>(0));
            assert(decrease < static_cast<Scalar>(1));
            decrease_ = decrease;
        }

        /** Set the relaxation constant for the Armijo condition (see class description).
          * Typically c1 is chosen to be quite small, e.g. 1e-4.
          * Assure that c1 in (0, 0.5).
          * @param c1 armijo constant */
        void setArmijoConstant(const Scalar c1)
        {
            assert(c1 > static_cast<Scalar>(0));
            assert(c1 < static_cast<Scalar>(0.5));
            c1_ = c1;
        }

        /** Set the bounds for the step size during linesearch.
          * The final step size is guaranteed to be in [minStep, maxStep].
          * The
          * @param minStep minimum step size
          * @param maxStep maximum step size */
        void setStepBounds(const Scalar minStep, const Scalar maxStep)
        {
            assert(minStep < maxStep);
            minStep_ = minStep;
            maxStep_ = maxStep;
        }

        /** Set the maximum number of iterations.
          * Set to 0 or negative for infinite iterations.
          * @param iterations maximum number of iterations */
        void setMaxIterations(const Index iterations)
        {
            maxIt_ = iterations;
        }

        void setErrorFunction(const ErrorFunction &objective)
        {
            objective_ = objective;
        }

        Scalar operator()(const Vector &xval,
            const Vector &fval,
            const Matrix &,
            const Vector &gradient,
            const Vector &step)
        {
            assert(objective_);

            Scalar stepSize = maxStep_ / decrease_;
            Matrix jacobianN;
            Vector gradientN;
            Vector xvalN;
            Vector fvalN;

            Scalar error = static_cast<Scalar>(0.5) * fval.squaredNorm();
            Scalar stepGrad = gradient.dot(step);
            bool armijoCondition = false;

            Index iterations = 0;
            while((maxIt_ <= 0 || iterations < maxIt_) &&
                stepSize * decrease_ >= minStep_ &&
                !armijoCondition)
            {
                stepSize = decrease_ * stepSize;
                xvalN = xval - stepSize * step;
                objective_(xvalN, fvalN, jacobianN);
                Scalar errorN = static_cast<Scalar>(0.5) * fvalN.squaredNorm();
                gradientN = jacobianN.transpose() * fvalN;

                armijoCondition = errorN <= error + c1_ * stepSize * stepGrad;

                ++iterations;
            }

            return stepSize;
        }
    };

    /** Step size functor to perform Wolfe Linesearch with backtracking.
      * The functor iteratively decreases the step size until the following
      * conditions are met:
      *
      * Armijo: f(x - stepSize * grad(x)) <= f(x) - c1 * stepSize * grad(x)^T * grad(x)
      * Wolfe: grad(x)^T grad(x - stepSize * grad(x)) <= c2 * grad(x)^T * grad(x)
      *
      * If either condition does not hold the step size is decreased:
      *
      * stepSize = decrease * stepSize
      */
    template<typename Scalar>
    class WolfeBacktracking
    {
    public:
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
        typedef std::function<void(const Vector &, Vector &, Matrix &)> ErrorFunction;
    private:
        Scalar decrease_;
        Scalar c1_;
        Scalar c2_;
        Scalar minStep_;
        Scalar maxStep_;
        Index maxIt_;
        ErrorFunction objective_;

    public:
        WolfeBacktracking()
            : WolfeBacktracking(
                static_cast<Scalar>(0.8),
                static_cast<Scalar>(1e-4),
                static_cast<Scalar>(0.9),
                static_cast<Scalar>(1e-10),
                static_cast<Scalar>(1.0),
                0)
        { }

        WolfeBacktracking(const Scalar decrease,
            const Scalar c1,
            const Scalar c2,
            const Scalar minStep,
            const Scalar maxStep,
            const Index iterations)
            : decrease_(decrease), c1_(c1), c2_(c2), minStep_(minStep),
            maxStep_(maxStep), maxIt_(iterations), objective_()
        { }

        /** Set the decreasing factor for backtracking.
          * Assure that decrease in (0, 1).
          * @param decrease decreasing factor */
        void setBacktrackingDecrease(const Scalar decrease)
        {
            decrease_ = decrease;
        }

        /** Set the wolfe constants for Armijo and Wolfe condition (see class
          * description).
          * Assure that c1 < c2 < 1 and c1 in (0, 0.5).
          * Typically c1 is chosen to be quite small, e.g. 1e-4.
          * @param c1 armijo constant
          * @param c2 wolfe constant */
        void setWolfeConstants(const Scalar c1, const Scalar c2)
        {
            assert(c1 > static_cast<Scalar>(0));
            assert(c1 < static_cast<Scalar>(0.5));
            assert(c1 < c2);
            assert(c2 < static_cast<Scalar>(1));
            c1_ = c1;
            c2_ = c2;
        }

        /** Set the bounds for the step size during linesearch.
          * The final step size is guaranteed to be in [minStep, maxStep].
          * @param minStep minimum step size
          * @param maxStep maximum step size */
        void setStepBounds(const Scalar minStep, const Scalar maxStep)
        {
            assert(minStep < maxStep);
            minStep_ = minStep;
            maxStep_ = maxStep;
        }

        /** Set the maximum number of iterations.
          * Set to 0 or negative for infinite iterations.
          * @param iterations maximum number of iterations */
        void setMaxIterations(const Index iterations)
        {
            maxIt_ = iterations;
        }

        void setErrorFunction(const ErrorFunction &objective)
        {
            objective_ = objective;
        }

        Scalar operator()(const Vector &xval,
            const Vector &fval,
            const Matrix &,
            const Vector &gradient,
            const Vector &step)
        {
            assert(objective_);

            Scalar stepSize = maxStep_ / decrease_;
            Matrix jacobianN;
            Vector gradientN;
            Vector xvalN;
            Vector fvalN;

            Scalar error = fval.squaredNorm() / 2;
            Scalar stepGrad = gradient.dot(step);
            bool armijoCondition = false;
            bool wolfeCondition = false;

            Index iterations = 0;
            while((maxIt_ <= 0 || iterations < maxIt_) &&
                stepSize * decrease_ >= minStep_ &&
                !(armijoCondition && wolfeCondition))
            {
                stepSize = decrease_ * stepSize;
                xvalN = xval - stepSize * step;
                objective_(xvalN, fvalN, jacobianN);
                Scalar errorN = fvalN.squaredNorm() / 2;
                gradientN = jacobianN.transpose() * fvalN;

                armijoCondition = errorN <= error + c1_ * stepSize * stepGrad;
                wolfeCondition = gradientN.dot(step) >= c2_ * stepGrad;

                ++iterations;
            }

            return stepSize;
        }
    };

    template<typename Scalar>
    struct DenseSVDSolver
    {
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
        typedef Eigen::JacobiSVD<Matrix, Eigen::FullPivHouseholderQRPreconditioner>
            Solver;

        void operator()(const Matrix &A, const Vector &b, Vector &result) const
        {
            Solver solver(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
            result = solver.solve(b);
        }
    };

    template<typename Scalar>
    struct DenseCholeskySolver
    {
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
        typedef Eigen::LDLT<Matrix> Solver;

        void operator()(const Matrix &A, const Vector &b, Vector &result) const
        {
            Solver decomp;
            decomp.compute(A);

            if(!decomp.isPositive())
                throw std::runtime_error("DenseCholeskySolver: matrix is not positive semi-definite");

            result = decomp.solve(b);
        }
    };

    /** Base class for least squares algorithms.
      * It implements the whole optimization strategy except the step
      * calculation. Cannot be instantiated. */
    template<typename Scalar,
        typename ErrorFunction,
        typename StepSize,
        typename Callback,
        typename FiniteDifferences>
    class LeastSquaresAlgorithm
    {
    public:
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
    protected:
        ErrorFunction errorFunction_;
        StepSize stepSize_;
        Callback callback_;
        FiniteDifferences finiteDifferences_;

        Index maxIt_;
        Scalar minStepLen_;
        Scalar minGradLen_;
        Scalar minError_;
        Index verbosity_;

        void evaluateErrorFunction(const Vector &xval, Vector &fval, Matrix &jacobian)
        {
            jacobian.resize(0, 0);
            errorFunction_(xval, fval, jacobian);
            if(jacobian.size() == 0)
                finiteDifferences_(xval, fval, jacobian);
        }

        std::string vector2str(const Vector &vec) const
        {
            std::stringstream ss1;
            ss1 << std::fixed << std::showpoint << std::setprecision(6);
            std::stringstream ss2;
            ss2 << '[';
            for(Index i = 0; i < vec.size(); ++i)
            {
                ss1 << vec(i);
                ss2 << std::setfill(' ') << std::setw(10) << ss1.str();
                if(i != vec.size() - 1)
                    ss2 << ' ';
                ss1.str("");
            }
            ss2 << ']';

            return ss2.str();
        }

    public:
        struct Result
        {
            Vector xval;
            Vector fval;
            Scalar error;
            Index iterations;
            bool converged;
        };

        LeastSquaresAlgorithm()
            : errorFunction_(),
            stepSize_(),
            callback_(),
            finiteDifferences_(),
            maxIt_(0),
            minStepLen_(static_cast<Scalar>(1e-9)),
            minGradLen_(static_cast<Scalar>(1e-9)),
            minError_(static_cast<Scalar>(0)),
            verbosity_(0)
        { }

        virtual ~LeastSquaresAlgorithm()
        { }

        /** Set the number of threads used to compute gradients.
          * This only works if OpenMP is enabled.
          * Set to 0 to allow automatic detection of thread number.
          * @param threads number of threads to be used */
        void setThreads(const Index threads)
        {
            finiteDifferences_.setThreads(threads);
        }

        /** Set the difference for gradient estimation with finite differences.
          * @param eps numerical epsilon */
        void setNumericalEpsilon(const Scalar eps)
        {
            finiteDifferences_.setNumericalEpsilon(eps);
        }

        /** Sets the instance values of the custom error function.
          * Should be used if the error function requires custom data parameters.
          * @param errorFunction instance that should be copied */
        void setErrorFunction(const ErrorFunction &errorFunction)
        {
            errorFunction_ = errorFunction;
        }

        void setCallback(const Callback &callback)
        {
            callback_ = callback;
        }

        /** Sets the instance values of the step size functor.
          * @param stepSize instance that should be copied */
        void setStepSize(const StepSize &stepSize)
        {
            stepSize_ = stepSize;
        }

        /** Set the maximum number of iterations.
          * Set to 0 or negative for infinite iterations.
          * @param iterations maximum number of iterations */
        void setMaxIterations(const Index iterations)
        {
            maxIt_ = iterations;
        }

        /** Set the minimum step length between two iterations.
          * If the step length falls below this value, the optimizer stops.
          * @param steplen minimum step length */
        void setMinStepLength(const Scalar steplen)
        {
            minStepLen_ = steplen;
        }

        /** Set the minimum gradient length.
          * If the gradient length falls below this value, the optimizer stops.
          * @param gradlen minimum gradient length */
        void setMinGradientLength(const Scalar gradlen)
        {
            minGradLen_ = gradlen;
        }

        /** Set the minimum squared error.
          * If the error falls below this value, the optimizer stops.
          * @param error minimum error */
        void setMinError(const Scalar error)
        {
            minError_ = error;
        }

        /** Set the level of verbosity to print status information after each
          * iteration.
          * Set to 0 to deacticate any output.
          * @param verbosity level of verbosity */
        void setVerbosity(const Index verbosity)
        {
            verbosity_ = verbosity;
        }

        Result minimize(const Vector &initialGuess)
        {
            finiteDifferences_.setErrorFunction(
                [this](const Vector &xval, Vector &fval)
                { Matrix tmp; this->errorFunction_(xval, fval, tmp); });
            stepSize_.setErrorFunction(
                [this](const Vector &xval, Vector &fval, Matrix &jacobian)
                { this->evaluateErrorFunction(xval, fval, jacobian); });

            Vector xval = initialGuess;
            Vector fval;
            Matrix jacobian;
            Vector gradient;
            Scalar gradLen = minGradLen_ + 1;
            Scalar stepSize;
            Scalar error = minError_ + 1;
            Vector step = Vector::Zero(xval.size());
            Scalar stepLen = minStepLen_ + 1;
            bool callbackResult = true;

            Index iterations = 0;
            while((maxIt_ <= 0 || iterations < maxIt_) &&
                gradLen >= minGradLen_ &&
                stepLen >= minStepLen_ &&
                error >= minError_ &&
                callbackResult)
            {
                xval -= step;
                evaluateErrorFunction(xval, fval, jacobian);
                error = fval.squaredNorm() / 2;
                gradient = jacobian.transpose() * fval;
                gradLen = gradient.norm();

                calculateStep(xval, fval, jacobian, gradient, step);

                // update step according to step size
                stepSize = stepSize_(xval, fval, jacobian, gradient, step);
                step *= stepSize;
                stepLen = step.norm();
                // evaluate callback and save its result
                callbackResult = callback_(iterations + 1, xval, fval, jacobian,
                    gradient, step);

                if(verbosity_ > 0)
                {
                    std::stringstream ss;
                    ss << "it=" << std::setfill('0')
                        << std::setw(4) << iterations
                        << std::fixed << std::showpoint << std::setprecision(6)
                        << "    stepsize=" << stepSize
                        << "    steplen=" << stepLen
                        << "    gradlen=" << gradLen;

                    if(verbosity_ > 1)
                        ss << "    callback=" << (callbackResult ? "true" : "false");

                    ss << "    error=" << error;

                    if(verbosity_ > 2)
                        ss << "    fval=" << vector2str(fval);
                    if(verbosity_ > 3)
                        ss << "    xval=" << vector2str(xval);
                    if(verbosity_ > 4)
                        ss << "    step=" << vector2str(step);
                    std::cout << ss.str() << std::endl;
                }

                ++iterations;
            }

            Result result;
            result.xval = xval;
            result.fval = fval;
            result.error = error;
            result.iterations = iterations;
            result.converged = stepLen < minStepLen_ ||
                gradLen < minGradLen_ ||
                error < minError_;

            return result;
        }

        virtual void calculateStep(const Vector &xval,
            const Vector &fval,
            const Matrix &jacobian,
            const Vector &gradient,
            Vector &step) = 0;

    };

    template<typename Scalar,
        typename ErrorFunction,
        typename StepSize=BarzilaiBorwein<Scalar>,
        typename Callback=NoCallback<Scalar>,
        typename FiniteDifferences=CentralDifferences<Scalar>>
    class GradientDescent : public LeastSquaresAlgorithm<Scalar, ErrorFunction,
        StepSize, Callback, FiniteDifferences>
    {
    public:
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;

        GradientDescent()
            : LeastSquaresAlgorithm<Scalar, ErrorFunction,
                StepSize, Callback, FiniteDifferences>()
        { }

        void calculateStep(const Vector &,
            const Vector &,
            const Matrix &,
            const Vector &gradient,
            Vector &step) override
        {
            step = gradient;
        }

    };

    template<typename Scalar,
        typename ErrorFunction,
        typename StepSize=ArmijoBacktracking<Scalar>,
        typename Callback=NoCallback<Scalar>,
        typename FiniteDifferences=CentralDifferences<Scalar>,
        typename Solver=DenseSVDSolver<Scalar>>
    class GaussNewton : public LeastSquaresAlgorithm<Scalar, ErrorFunction,
        StepSize, Callback, FiniteDifferences>
    {
    public:
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;

    public:
        GaussNewton()
            : LeastSquaresAlgorithm<Scalar, ErrorFunction,
                StepSize, Callback, FiniteDifferences>()
        { }

        void calculateStep(const Vector &,
            const Vector &,
            const Matrix &jacobian,
            const Vector &gradient,
            Vector &step) override
        {
            Solver solver;

            Matrix A = jacobian.transpose() * jacobian;
            solver(A, gradient, step);
        }
    };

    template<typename Scalar,
        typename ErrorFunction,
        typename Callback=NoCallback<Scalar>,
        typename FiniteDifferences=CentralDifferences<Scalar>,
        typename Solver=DenseSVDSolver<Scalar>>
    class LevenbergMarquardt : public LeastSquaresAlgorithm<Scalar, ErrorFunction,
        ConstantStepSize<Scalar>, Callback, FiniteDifferences>
    {
    public:
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
    private:
        Scalar increase_;
        Scalar decrease_;
        Scalar lambda_;
        Index maxItLM_;

    public:
        LevenbergMarquardt()
            : LeastSquaresAlgorithm<Scalar, ErrorFunction,
                ConstantStepSize<Scalar>, Callback, FiniteDifferences>(),
                increase_(static_cast<Scalar>(2)),
                decrease_(static_cast<Scalar>(0.5)),
                lambda_(static_cast<Scalar>(1)),
                maxItLM_(0)
        { }

        /** Set the initial gradient descent factor of levenberg marquardt.
          * @param lambda gradient descent factor */
        void setLambda(const Scalar lambda)
        {
            lambda_ = lambda;
        }

        /** Set maximum iterations of the levenberg marquardt optimization.
          * Set to 0 or negative for infinite iterations.
          * @param iterations maximum iterations for lambda search */
        void setMaxIterationsLM(const Index iterations)
        {
            maxItLM_ = iterations;
        }

        /** Set the increase factor for the lambda damping.
          * Make sure the value is greater than 1.
          * @param increase factor for increasing lambda */
        void setLambdaIncrease(const Scalar increase)
        {
            assert(increase > static_cast<Scalar>(1));
            increase_ = increase;
        }

        /** Set the decrease factor for the lambda damping.
          * Make sure the value is in (0, 1).
          * @param increase factor for increasing lambda */
        void setLambdaDecrease(const Scalar decrease)
        {
            assert(decrease < static_cast<Scalar>(1));
            assert(decrease > static_cast<Scalar>(0));
            decrease_ = decrease;
        }

        void calculateStep(const Vector &xval,
            const Vector &fval,
            const Matrix &jacobian,
            const Vector &gradient,
            Vector &step) override
        {
            Solver solver;
            Scalar error = fval.squaredNorm() / 2;
            Scalar errorN = error + 1;

            Vector xvalN;
            Vector fvalN;
            Matrix jacobianN;

            Matrix jacobianSq = jacobian.transpose() * jacobian;
            Matrix A;

            Index iterations = 0;
            while((maxItLM_ <= 0 || iterations < maxItLM_) &&
                errorN > error)
            {
                A = jacobianSq;
                // add identity matrix
                for(Index i = 0; i < A.rows(); ++i)
                    A(i, i) += lambda_;

                solver(A, gradient, step);

                xvalN = xval - step;
                this->errorFunction_(xvalN, fvalN, jacobianN);
                errorN = fvalN.squaredNorm() / 2;

                if(errorN > error)
                    lambda_ *= increase_;
                else
                    lambda_ *= decrease_;

                ++iterations;
            }
        }
    };

    /** Implementation of Powell's Dogleg Method. */
    template<typename Scalar,
        typename ErrorFunction,
        typename Callback=NoCallback<Scalar>,
        typename FiniteDifferences=CentralDifferences<Scalar>,
        typename Solver=DenseSVDSolver<Scalar>>
    class DoglegMethod : public LeastSquaresAlgorithm<Scalar, ErrorFunction,
        ConstantStepSize<Scalar>, Callback, FiniteDifferences>
    {
    public:
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;

    private:
        Scalar radius_;
        Scalar maxRadius_;
        Scalar radiusEps_;
        Scalar acceptFitness_;
        Index maxItTR_;

        Scalar calulateModelFitness(const Vector &xval,
            const Vector &fval,
            const Vector &gradient,
            const Matrix &hessian,
            const Vector &step)
        {
            Scalar error = fval.squaredNorm() / 2;

            // evaluate the error function at the new position
            Vector xvalNext = xval + step;
            Vector fvalNext;
            Matrix jacobianNext;
            this->errorFunction_(xvalNext, fvalNext, jacobianNext);
            // compute the actual new error
            Scalar nextError = fvalNext.squaredNorm() / 2;
            // compute the new error by the model
            Scalar modelError = error + gradient.dot(step) + step.dot(hessian * step) / 2;

            return (error - nextError) / (error - modelError);
        }

    public:
        DoglegMethod()
            : LeastSquaresAlgorithm<Scalar, ErrorFunction,
                ConstantStepSize<Scalar>, Callback, FiniteDifferences>(),
                radius_(1),
                maxRadius_(static_cast<Scalar>(2)),
                radiusEps_(static_cast<Scalar>(1e-6)),
                acceptFitness_(static_cast<Scalar>(0.25)),
                maxItTR_(0)
        { }

        /** Set maximum iterations of the trust region radius search.
          * Set to 0 or negative for infinite iterations.
          * @param iterations maximum iterations for radius search */
        void setMaxIterationsTR(const Index iterations)
        {
            maxItTR_ = iterations;
        }

        /** Set the minimum fitness value at which a model is accepted.
          * The model fitness is computed as follows:
          *
          * fitness = f(xval) - f(xval + step) / m(0) - m(step)
          *
          * Where f(x) is the objective error function and m(x) is the
          * model function describe by the trust region method.
          *
          * @param fitness minimum fitness for step acceptance */
        void setAcceptanceFitness(const Scalar fitness)
        {
            acceptFitness_ = fitness;
        }

        /** Set the comparison epsilon on how close the step should be
          * to the trust region radius to trigger an increase of the radius.
          * Should usually be picked low, e.g. 1e-8.
          * @param eps comparison epsilon for radius increase */
        void setRaidusEps(const Scalar eps)
        {
            radiusEps_ = eps;
        }

        void calculateStep(const Vector &xval,
            const Vector &fval,
            const Matrix &jacobian,
            const Vector &gradient,
            Vector &step) override
        {
            // approximate hessian
            Matrix hessian = jacobian.transpose() * jacobian;

            // compute the full newton step
            Vector fullStep;
            Solver solver;
            solver(hessian, gradient, fullStep);
            fullStep = -fullStep;

            // precompute the full step length
            Scalar fullStepLenSq = fullStep.squaredNorm();

            // compute the cauchy step
            Scalar gradientLenSq = gradient.squaredNorm();
            Scalar curvature = gradient.dot(hessian * gradient);
            Vector cauchyStep = -(gradientLenSq / curvature) * gradient;
            Scalar cauchyStepLenSq = cauchyStep.squaredNorm();

            // compute step diff
            Vector diffStep = fullStep - cauchyStep;
            Scalar diffLenSq = diffStep.squaredNorm();
            Scalar diffFac = cauchyStep.dot(diffStep) / diffLenSq;

            Scalar modelFitness = acceptFitness_ - 1;
            Index iteration = 0;

            // keep computing while the model fitness is bad
            while(modelFitness < acceptFitness_
                && (maxItTR_ <= 0 || iteration < maxItTR_))
            {
                Scalar radiusSq = radius_ * radius_;

                // if the full step is within the trust region simply
                // use it, it provides a good minimizer
                if(fullStepLenSq <= radiusSq)
                {
                    step = fullStep;
                }
                else
                {
                    // if the cauchy step lies outside the trust region
                    // go towards it until the trust region boundary
                    if(cauchyStepLenSq >= radiusSq)
                    {
                        step = (radius_ / std::sqrt(cauchyStepLenSq)) * cauchyStep;
                    }
                    else
                    {
                        Scalar secondTerm = std::sqrt(diffFac * diffFac + (radiusSq + cauchyStepLenSq) / diffLenSq);
                        Scalar scale1 = -diffFac - secondTerm;
                        Scalar scale2 = -diffFac + secondTerm;

                        step = cauchyStep + std::max(scale1, scale2) * (fullStep - cauchyStep);
                    }
                }

                // compute the model fitness to determine the update scheme for
                // the trust region radius
                modelFitness = calulateModelFitness(xval, fval, gradient, hessian, step);

                Scalar stepLen = step.norm();

                // if the model fitness is really bad reduce the radius!
                if(modelFitness < static_cast<Scalar>(0.25))
                    radius_ = static_cast<Scalar>(0.25) * stepLen;
                // if the model fitness is very good then increase it
                else if(modelFitness > static_cast<Scalar>(0.75) && std::abs(stepLen - radius_) < radiusEps_)
                {
                    // use the double radius
                    radius_ = 2 * radius_;
                    // maintain radius border if configured
                    if(maxRadius_ > 0 && radius_ > maxRadius_)
                        radius_ = maxRadius_;
                }

                ++iteration;
            }

            step = -step;
        }
    };
}

#endif
