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

#ifndef LSQCPP_LSQCPP_HPP_
#define LSQCPP_LSQCPP_HPP_

#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <iostream>
#include <iomanip>
#include <functional>

namespace lsqcpp
{
    using Index = Eigen::MatrixXd::Index;

    /// Parametrization container for jacobian estimation using finite differences.
    /// This class holds parameters for the finite differences operation, such as the
    /// number of threads to be used and the numerical espilon.
    template<typename _Scalar>
    struct FiniteDifferencesParameter
    {
    public:
        using Scalar = _Scalar;

        static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Finite differences only supports non-integer scalars.");

        FiniteDifferencesParameter() = default;

        FiniteDifferencesParameter(const Scalar eps)
            : _eps(eps)
        { }

        FiniteDifferencesParameter(const Scalar eps, const int threads)
            : _eps(eps), _threads(threads)
        { }

        /// Sets the numerical espilon that is used as step between two sucessive function evaluations.
        /// @param eps numerical epsilon
        void setEpsilon(const Scalar eps)
        {
            _eps = eps;
        }

        /// Returns the numerical epsilon, which is used for jacobian approximation.
        /// @return numerical epsilon
        Scalar epsilon() const
        {
            return _eps;
        }

        /// Sets the number of threads which should be used to compute finite differences
        /// (OMP only).
        /// Set to 0 or negative for auto-detection of a suitable number.
        /// Each dimension of the input vector can be handled independently.
        /// @param threads number of threads
        void setThreads(const int threads)
        {
            _threads = threads;
        }

        /// Returns the number of threads which should be used to compute finite differences
        /// (OMP only).
        /// @return threads number of threads
        int threads() const
        {
            return _threads;
        }

    private:
        Scalar _eps = std::sqrt(Eigen::NumTraits<Scalar>::epsilon());
        int _threads = int{1};
    };

    /// 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.
    struct ForwardDifferences
    {
        template<typename Scalar, typename Objective, typename I, typename O, typename J>
        void operator()(const Eigen::MatrixBase<I> &xval,
                        const Eigen::MatrixBase<O> &fval,
                        const Objective &objective,
                        const FiniteDifferencesParameter<Scalar>& param,
                        Eigen::MatrixBase<J> &jacobian) const
        {
            static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Finite differences only supports non-integer scalars.");

            using InputVector = typename Eigen::MatrixBase<I>::PlainMatrix;
            using OutputVector = typename Eigen::MatrixBase<O>::PlainMatrix;

            // noop in fixed size problems
            jacobian.derived().resize(fval.size(), xval.size());

            #pragma omp parallel for num_threads(param.threads())
            for(Index i = 0; i < xval.size(); ++i)
            {
                InputVector xvalNext = xval;
                OutputVector fvalNext;
                xvalNext(i) += param.epsilon();
                objective(xvalNext, fvalNext);

                jacobian.col(i) = (fvalNext - fval) / param.epsilon();
            }
        }
    };

    /// 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.
    struct BackwardDifferences
    {
        template<typename Scalar, typename Objective, typename I, typename O, typename J>
        void operator()(const Eigen::MatrixBase<I> &xval,
                        const Eigen::MatrixBase<O> &fval,
                        const Objective &objective,
                        const FiniteDifferencesParameter<Scalar>& param,
                        Eigen::MatrixBase<J> &jacobian) const
        {
            static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Finite differences only supports non-integer scalars.");

            using InputVector = typename Eigen::MatrixBase<I>::PlainMatrix;
            using OutputVector = typename Eigen::MatrixBase<O>::PlainMatrix;

            // noop in fixed size problems
            jacobian.derived().resize(fval.size(), xval.size());

            #pragma omp parallel for num_threads(param.threads())
            for(Index i = 0; i < xval.size(); ++i)
            {
                InputVector xvalNext = xval;
                OutputVector fvalNext;
                xvalNext(i) -= param.epsilon();
                objective(xvalNext, fvalNext);

                jacobian.col(i) = (fval - fvalNext) / param.epsilon();
            }
        }
    };

    /// 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.
    struct CentralDifferences
    {
        template<typename Scalar, typename Objective, typename I, typename O, typename J>
        void operator()(const Eigen::MatrixBase<I> &xval,
                        const Eigen::MatrixBase<O> &fval,
                        const Objective &objective,
                        const FiniteDifferencesParameter<Scalar>& param,
                        Eigen::MatrixBase<J> &jacobian) const
        {
            static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Finite differences only supports non-integer scalars.");

            using InputVector = typename Eigen::MatrixBase<I>::PlainMatrix;
            using OutputVector = typename Eigen::MatrixBase<O>::PlainMatrix;

            // noop in fixed size problems
            jacobian.derived().resize(fval.size(), xval.size());

            #pragma omp parallel for num_threads(param.threads())
            for(Index i = 0; i < xval.size(); ++i)
            {
                InputVector xvalNext = xval;
                OutputVector fvalA;
                OutputVector fvalB;

                xvalNext(i) = xval(i) - param.epsilon() / Scalar{2};
                objective(xvalNext, fvalA);

                xvalNext(i) = xval(i) + param.epsilon() / Scalar{2};
                objective(xvalNext, fvalB);

                jacobian.col(i) = (fvalB - fvalA) / param.epsilon();
            }
        }
    };

    /// Solves for dense linear equation systems using the Jacobi SVD method.
    struct DenseSVDSolver
    {
        /// Solves the linear equation system Ax = b and stores the result in x.
        /// @param A linear equation system matrix
        /// @param b linear equation system vector
        /// @param x computed result
        /// @return true if successful, false otherwise
        template<typename M, typename B, typename X>
        bool operator()(const Eigen::MatrixBase<M> &A,
                        const Eigen::MatrixBase<B> &b,
                        Eigen::MatrixBase<X> &x) const
        {
            using Solver = Eigen::JacobiSVD<M, Eigen::FullPivHouseholderQRPreconditioner>;
            auto solver = Solver(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
            x = solver.solve(b);
            return true;
        }
    };

    /// Solver for dense linear equation systems, which a positive semi-definite, using the Cholesky decomposition.
    struct DenseCholeskySolver
    {
        /// Solves the linear equation system Ax = b and stores the result in x.
        /// @param A linear equation system matrix
        /// @param b linear equation system vector
        /// @param x computed result
        /// @return true if successful, false otherwise
        template<typename M, typename B, typename X>
        bool operator()(const Eigen::MatrixBase<M> &A,
                        const Eigen::MatrixBase<B> &b,
                        Eigen::MatrixBase<X> &x) const
        {
            using Solver = Eigen::LDLT<M>;

            Solver decomp;
            decomp.compute(A);

            if(!decomp.isPositive())
                return false;

            x = decomp.solve(b);
            return true;
        }
    };

    /// Generic class for refining a computed newton step.
    /// The method parameter determines how the step is actually refined, e.g
    /// ArmijoBacktracking, DoglegMethod or WolfeBacktracking.
    template<typename Scalar, int Inputs, int Outputs, typename Method>
    class NewtonStepRefiner { };

    /// Newton step refinement method which applies a constant scaling factor to the newton step.
    struct ConstantStepFactor { };

    /// Parametrization class for the ConstantStepFactor refinement method.
    template<typename _Scalar>
    class ConstantStepFactorParameter
    {
    public:
        using Scalar = _Scalar;

        ConstantStepFactorParameter() = default;

        ConstantStepFactorParameter(const Scalar factor)
            : _factor(factor)
        { }
        /// Sets the constant scaling factor which is applied to the newton step.
        /// @param factor constant newton step scaling factor
        void setFactor(const Scalar factor)
        {
            _factor = factor;
        }

        /// Returns the constant scaling factor which is applied to the newton step.
        /// @return constant newton step scaling factor
        Scalar factor() const
        {
            return _factor;
        }
    private:
        Scalar _factor = Scalar{1};
    };

    template<typename _Scalar, int _Inputs, int _Outputs>
    class NewtonStepRefiner<_Scalar, _Inputs, _Outputs, ConstantStepFactor>
    {
    public:
        using Scalar = _Scalar;
        static constexpr int Inputs = _Inputs;
        static constexpr int Outputs = _Outputs;
        using Method = ConstantStepFactor;
        using Parameter = ConstantStepFactorParameter<Scalar>;

        static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Step refinement only supports non-integer scalars");

        using InputVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using OutputVector = Eigen::Matrix<Scalar, Outputs, 1>;
        using JacobiMatrix = Eigen::Matrix<Scalar, Outputs, Inputs>;
        using GradientVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using StepVector = Eigen::Matrix<Scalar, Inputs, 1>;

        NewtonStepRefiner(const Parameter &param)
            : _param(param)
        { }

        /// Refines the given newton step and scales it by a constant factor.
        /// @param step newton step which is scaled.
        template<typename Objective>
        void operator()(const InputVector &,
                        const OutputVector &,
                        const JacobiMatrix &,
                        const GradientVector &,
                        const Objective &,
                        StepVector &step) const
        {
            step *= _param.factor();
        }
    private:
        const Parameter &_param;
    };

    /// Applies Barzilai-Borwein (BB) refinemnt to the newton step.
    /// 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.
    struct BarzilaiBorwein
    {
        enum class Mode
        {
            Direct,
            Inverse
        };
    };

    /// Parametrization class for the BarzilaiBorwein refinement method.
    template<typename _Scalar>
    class BarzilaiBorweinParameter
    {
    public:
        using Scalar = _Scalar;

        BarzilaiBorweinParameter() = default;

        BarzilaiBorweinParameter(const BarzilaiBorwein::Mode mode)
            : _mode(mode)
        { }

        BarzilaiBorweinParameter(const Scalar constStep)
            : _constStep(constStep)
        { }

        BarzilaiBorweinParameter(const BarzilaiBorwein::Mode mode, const Scalar constStep)
            : _mode(mode), _constStep(constStep)
        { }

        /// Sets the BarzilaiBorwein operation mode.
        /// @param mode mode
        void setMode(const BarzilaiBorwein::Mode mode)
        {
            _mode = mode;
        }

        /// Returns the BarzilaiBorwein operation mode.
        /// @return mode
        BarzilaiBorwein::Mode mode() const
        {
            return _mode;
        }

        /// Sets the constant step size, which is used when the refiner was not initialized yet.
        /// @param stepSize constant step size
        void setConstantStepSize(const Scalar stepSize)
        {
            _constStep = stepSize;
        }

        /// Returns he constant step size, which is used when the refiner was not initialized yet.
        /// @return constant step size
        Scalar constantStepSize() const
        {
            return _constStep;
        }
    private:
        BarzilaiBorwein::Mode _mode = BarzilaiBorwein::Mode::Direct;
        Scalar _constStep = static_cast<Scalar>(1e-2);
    };

    template<typename _Scalar, int _Inputs, int _Outputs>
    class NewtonStepRefiner<_Scalar, _Inputs, _Outputs, BarzilaiBorwein>
    {
    public:
        using Scalar = _Scalar;
        static constexpr int Inputs = _Inputs;
        static constexpr int Outputs = _Outputs;
        using Method = BarzilaiBorwein;
        using Parameter = BarzilaiBorweinParameter<Scalar>;

        static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Step refinement only supports non-integer scalars");

        using InputVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using OutputVector = Eigen::Matrix<Scalar, Outputs, 1>;
        using JacobiMatrix = Eigen::Matrix<Scalar, Outputs, Inputs>;
        using GradientVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using StepVector = Eigen::Matrix<Scalar, Inputs, 1>;

        NewtonStepRefiner(const Parameter &param)
            : _param(param)
        {
            _lastXval.setZero();
            _lastStep.setZero();
        }

        template<typename Objective>
        void operator()(const InputVector &xval,
            const OutputVector &,
            const JacobiMatrix &,
            const GradientVector &,
            const Objective &,
            StepVector &step)
        {
            auto stepSize = Scalar{0};
            if(_lastXval.sum() == Scalar{0})
            {
                stepSize = (Scalar{1} / step.norm()) * _param.constantStepSize();
            }
            else
            {
                switch(_param.mode())
                {
                case BarzilaiBorwein::Mode::Direct:
                    stepSize = directStep(xval, step);
                    break;
                case BarzilaiBorwein::Mode::Inverse:
                    stepSize = inverseStep(xval, step);
                    break;
                default:
                    assert(false);
                    break;
                }
            }

            _lastStep = step;
            _lastXval = xval;

            step *= stepSize;
        }
    private:
        const Parameter &_param;
        InputVector _lastXval = {};
        StepVector _lastStep = {};

        Scalar directStep(const InputVector &xval,
                          const StepVector &step) const
        {
            const auto sk = xval - _lastXval;
            const auto yk = step - _lastStep;
            const auto num = sk.dot(sk);
            const auto denom = sk.dot(yk);

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

        Scalar inverseStep(const InputVector &xval,
                           const StepVector &step) const
        {
            const auto sk = xval - _lastXval;
            const auto yk = step - _lastStep;
            const auto num = sk.dot(yk);
            const auto denom = yk.dot(yk);

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

    /// Step refinement method performs Armijo Linesearch with backtracking.
    /// The line search 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
    struct ArmijoBacktracking { };

    /// Parametrization class for the ArmijoBacktracking refinement method.
    template<typename _Scalar>
    class ArmijoBacktrackingParameter
    {
    public:
        using Scalar = _Scalar;

        ArmijoBacktrackingParameter() = default;

        ArmijoBacktrackingParameter(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)
        {
            assert(decrease > static_cast<Scalar>(0));
            assert(decrease < static_cast<Scalar>(1));
            assert(c1 > static_cast<Scalar>(0));
            assert(c1 < static_cast<Scalar>(0.5));
        }

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

        /// Returns the decreasing factor for backtracking.
        /// The value should always lie within (0, 1).
        /// @return backtracking decrease
        Scalar backtrackingDecrease() const
        {
            return _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;
        }

        /// Returns the the relaxation constant for the Armijo condition (see class description).
        /// The value should always lie within (0, 0.5).
        /// @return armijo constant
        Scalar armijoConstant() const
        {
            return _c1;
        }

        /// 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 >= 0);
            assert(minStep < maxStep);
            _minStep = minStep;
            _maxStep = maxStep;
        }

        /// Returns the minimum bound for the step size during linesearch.
        /// @return minimum step size bound
        Scalar minimumStepBound() const
        {
            return _minStep;
        }

        /// Returns the maximum bound for the step size during linesearch.
        /// @return maximum step size bound
        Scalar maximumStepBound() const
        {
            return _maxStep;
        }

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

        /// Returns the maximum number of iterations.
        /// A value of 0 or negative means infinite iterations.
        /// @return maximum number of iterations
        Index maximumIterations() const
        {
            return _maxIt;
        }
    private:
        Scalar _decrease = static_cast<Scalar>(0.8);
        Scalar _c1 = static_cast<Scalar>(1e-4);
        Scalar _minStep = static_cast<Scalar>(1e-10);
        Scalar _maxStep = static_cast<Scalar>(1);
        Index _maxIt = Index{0};
    };

    template<typename _Scalar, int _Inputs, int _Outputs>
    class NewtonStepRefiner<_Scalar, _Inputs, _Outputs, ArmijoBacktracking>
    {
    public:
        using Scalar = _Scalar;
        static constexpr int Inputs = _Inputs;
        static constexpr int Outputs = _Outputs;
        using Method = ArmijoBacktracking;
        using Parameter = ArmijoBacktrackingParameter<Scalar>;

        static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Step refinement only supports non-integer scalars");

        using InputVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using OutputVector = Eigen::Matrix<Scalar, Outputs, 1>;
        using JacobiMatrix = Eigen::Matrix<Scalar, Outputs, Inputs>;
        using GradientVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using StepVector = Eigen::Matrix<Scalar, Inputs, 1>;

        NewtonStepRefiner(const Parameter &param)
            : _param(param)
        { }

        template<typename Objective>
        void operator()(const InputVector &xval,
                        const OutputVector &fval,
                        const JacobiMatrix &,
                        const GradientVector &gradient,
                        const Objective &objective,
                        StepVector &step) const
        {
            JacobiMatrix jacobianN;
            InputVector xvalN;
            OutputVector fvalN;

            const auto error = static_cast<Scalar>(0.5) * fval.squaredNorm();
            const auto stepGrad = gradient.dot(step);
            bool armijoCondition = false;
            auto stepSize = _param.maximumStepBound() / _param.backtrackingDecrease();

            auto iterations = Index{0};
            while((_param.maximumIterations() <= Index{0} || iterations < _param.maximumIterations()) &&
                   stepSize * _param.backtrackingDecrease() >= _param.minimumStepBound() &&
                   !armijoCondition)
            {
                stepSize = _param.backtrackingDecrease() * stepSize;
                xvalN = xval - stepSize * step;
                objective(xvalN, fvalN, jacobianN);

                const auto errorN = static_cast<Scalar>(0.5) * fvalN.squaredNorm();

                armijoCondition = errorN <= error + _param.armijoConstant() * stepSize * stepGrad;

                ++iterations;
            }

            step *= stepSize;
        }
    private:
        const Parameter &_param;
    };

    /// Step refinement method which performs Wolfe Linesearch with backtracking.
    /// The line search 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
    struct WolfeBacktracking { };

    /// Parametrization class for the WolfeBacktracking refinement method.
    template<typename _Scalar>
    class WolfeBacktrackingParameter
    {
    public:
        using Scalar = _Scalar;

        WolfeBacktrackingParameter() = default;

        WolfeBacktrackingParameter(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)
        {
            assert(decrease > static_cast<Scalar>(0));
            assert(decrease < static_cast<Scalar>(1));
            assert(c1 > static_cast<Scalar>(0));
            assert(c1 < static_cast<Scalar>(0.5));
            assert(c1 < c2);
            assert(c2 < static_cast<Scalar>(1));
        }

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

        /// Returns the decreasing factor for backtracking.
        /// The value should always lie within (0, 1).
        /// @return backtracking decrease
        Scalar backtrackingDecrease() const
        {
            return _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;
        }

        /// Returns the the relaxation constant for the Armijo condition (see class description).
        /// The value should always lie within (0, 0.5).
        /// @return armijo constant
        Scalar armijoConstant() const
        {
            return _c1;
        }

        /// Returns the the relaxation constant for the Wolfe condition (see class description).
        /// @return wolfe constant
        Scalar wolfeConstant() const
        {
            return _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 >= Scalar{0});
            assert(minStep < maxStep);
            _minStep = minStep;
            _maxStep = maxStep;
        }

        /// Returns the minimum bound for the step size during linesearch.
        /// @return minimum step size bound
        Scalar minimumStepBound() const
        {
            return _minStep;
        }

        /// Returns the maximum bound for the step size during linesearch.
        /// @return maximum step size bound
        Scalar maximumStepBound() const
        {
            return _maxStep;
        }

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

        /// Returns the maximum number of iterations.
        /// A value of 0 or negative means infinite iterations.
        /// @return maximum number of iterations
        Index maximumIterations() const
        {
            return _maxIt;
        }
    private:
        Scalar _decrease = static_cast<Scalar>(0.8);
        Scalar _c1 = static_cast<Scalar>(1e-4);
        Scalar _c2 = static_cast<Scalar>(0.9);
        Scalar _minStep = static_cast<Scalar>(1e-10);
        Scalar _maxStep = static_cast<Scalar>(1.0);
        Index _maxIt = 0;
    };

    template<typename _Scalar, int _Inputs, int _Outputs>
    class NewtonStepRefiner<_Scalar, _Inputs, _Outputs, WolfeBacktracking>
    {
    public:
        using Scalar = _Scalar;
        static constexpr int Inputs = _Inputs;
        static constexpr int Outputs = _Outputs;
        using Method = WolfeBacktracking;
        using Parameter = WolfeBacktrackingParameter<Scalar>;

        static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Step refinement only supports non-integer scalars");

        using InputVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using OutputVector = Eigen::Matrix<Scalar, Outputs, 1>;
        using JacobiMatrix = Eigen::Matrix<Scalar, Outputs, Inputs>;
        using GradientVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using StepVector = Eigen::Matrix<Scalar, Inputs, 1>;

        NewtonStepRefiner(const Parameter &param)
            : _param(param)
        {}

        template<typename Objective>
        void operator()(const InputVector &xval,
                        const OutputVector &fval,
                        const JacobiMatrix &,
                        const GradientVector &gradient,
                        const Objective &objective,
                        StepVector &step) const
        {
            auto stepSize = _param.maximumStepBound() / _param.backtrackingDecrease();
            JacobiMatrix jacobianN;
            GradientVector gradientN;
            InputVector xvalN;
            OutputVector fvalN;

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

            Index iterations = 0;
            while((_param.maximumIterations() <= 0 || iterations < _param.maximumIterations()) &&
                   stepSize * _param.backtrackingDecrease() >= _param.minimumStepBound() &&
                   !(armijoCondition && wolfeCondition))
            {
                stepSize = _param.backtrackingDecrease() * stepSize;
                xvalN = xval - stepSize * step;
                objective(xvalN, fvalN, jacobianN);
                Scalar errorN = fvalN.squaredNorm() / 2;
                gradientN = jacobianN.transpose() * fvalN;

                armijoCondition = errorN <= error + _param.armijoConstant() * stepSize * stepGrad;
                wolfeCondition = gradientN.dot(step) >= _param.wolfeConstant() * stepGrad;

                ++iterations;
            }

            step *= stepSize;
        }
    private:
        const Parameter &_param;
    };

    /// Step refinement method which implements Powell' Dogleg Method.
    struct DoglegMethod { };

    /// Parametrization class for the DoglegMethod refinement method.
    template<typename _Scalar>
    class DoglegMethodParameter
    {
    public:
        using Scalar = _Scalar;

        DoglegMethodParameter() = default;

        DoglegMethodParameter(const Scalar initRadius,
                              const Scalar maxRadius,
                              const Scalar radiusEps,
                              const Scalar acceptFitness,
                              const Index iterations)
            : _initialRadius(initRadius), _maxRadius(maxRadius), _radiusEps(radiusEps),
            _acceptFitness(acceptFitness), _maxIt(iterations)
        { }

        /// 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 setMaximumIterations(const Index iterations)
        {
            _maxIt = iterations;
        }

        /// Returns the maximum iterations of the trust region radius search.
        /// @return maximum iterations for radius search
        Index maximumIterations() const
        {
            return _maxIt;
        }

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

        /// Returns the minimum fitness value at which a model is accepted.
        /// @return minimum fitness for step acceptance
        Scalar acceptanceFitness() const
        {
            return _acceptFitness;
        }

        /// Sets 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 setRadiusEpsilon(const Scalar eps)
        {
            _radiusEps = eps;
        }

        /// Returns the comparison epsilon on how close the step should be
        /// to the trust region radius to trigger an increase of the radius.
        /// @return comparison epsilon for radius increase
        Scalar radiusEpsilon() const
        {
            return _radiusEps;
        }

        /// Sets the maximum radius that is used for trust region search.
        /// @param radius maximum trust region radius
        void setMaximumRadius(const Scalar radius)
        {
            _initialRadius = std::min(_initialRadius, radius);
            _maxRadius = radius;
        }

        /// Returns the maximum radius that is used for trust region search.
        /// @return maximum trust region radius
        Scalar maximumRadius() const
        {
            return _maxRadius;
        }

        /// Sets the radius that is initially used by the dogleg method.
        /// The radius must be greater than zero.
        /// @param radius the initial radius that should be used.
        void setInitialRadius(const Scalar radius)
        {
            assert(radius > 0);
            _initialRadius = radius;
        }

        /// Returns the radius that is initially used by the dogleg method.
        /// @return the initial radius
        Scalar initialRadius() const
        {
            return _initialRadius;
        }

    private:
        Scalar _initialRadius = {1};
        Scalar _maxRadius = Scalar{2};
        Scalar _radiusEps = static_cast<Scalar>(1e-6);
        Scalar _acceptFitness = static_cast<Scalar>(0.25);
        Index _maxIt = 0;
    };

    /// Implementation of Powell's Dogleg Method.
    template<typename _Scalar, int _Inputs, int _Outputs>
    class NewtonStepRefiner<_Scalar, _Inputs, _Outputs, DoglegMethod>
    {
    public:
        using Scalar = _Scalar;
        static constexpr int Inputs = _Inputs;
        static constexpr int Outputs = _Outputs;
        using Method = DoglegMethod;
        using Parameter = DoglegMethodParameter<Scalar>;

        static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Step refinement only supports non-integer scalars");

        using InputVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using OutputVector = Eigen::Matrix<Scalar, Outputs, 1>;
        using JacobiMatrix = Eigen::Matrix<Scalar, Outputs, Inputs>;
        using HessianMatrix = Eigen::Matrix<Scalar, Inputs, Inputs>;
        using GradientVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using StepVector = Eigen::Matrix<Scalar, Inputs, 1>;

        NewtonStepRefiner(const Parameter &param)
            : _param(param), _radius(param.initialRadius())
        { }

        template<typename Objective>
        void operator()(const InputVector &xval,
                        const OutputVector &fval,
                        const JacobiMatrix &jacobian,
                        const GradientVector &gradient,
                        const Objective &objective,
                        StepVector &step)
        {
            // approximate hessian
            const HessianMatrix hessian = jacobian.transpose() * jacobian;

            // precompute the full step length
            const StepVector fullStep = step;
            const auto fullStepLenSq = fullStep.squaredNorm();

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

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

            auto modelFitness = _param.acceptanceFitness() - Scalar{1};
            Index iteration = 0;

            // keep computing while the model fitness is bad
            while(modelFitness < _param.acceptanceFitness() &&
                  (_param.maximumIterations() <= Index{0} || iteration < _param.maximumIterations()))
            {
                const auto 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
                    {
                        const auto secondTerm = std::sqrt(diffFac * diffFac + (radiusSq + cauchyStepLenSq) / diffLenSq);
                        const auto scale1 = -diffFac - secondTerm;
                        const auto 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, objective);

                const auto 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) < _param.radiusEpsilon())
                {
                    // use the double radius
                    _radius = 2 * _radius;
                    // maintain radius border if configured
                    if(_param.maximumRadius() > 0 && _radius > _param.maximumRadius())
                        _radius = _param.maximumRadius();
                }

                ++iteration;
            }
        }

    private:
        const Parameter &_param;
        Scalar _radius = Scalar{1};

        template<typename Objective>
        Scalar calulateModelFitness(const InputVector &xval,
                                    const OutputVector &fval,
                                    const GradientVector &gradient,
                                    const HessianMatrix &hessian,
                                    const StepVector &step,
                                    const Objective &objective) const
        {
            const Scalar error = fval.squaredNorm() / Scalar{2};

            // evaluate the error function at the new position
            InputVector xvalNext = xval + step;
            OutputVector fvalNext;
            JacobiMatrix jacobianNext;
            objective(xvalNext, fvalNext, jacobianNext);
            // compute the actual new error
            const Scalar nextError = fvalNext.squaredNorm() / Scalar{2};
            // compute the new error by the model
            const Scalar modelError = error + gradient.dot(step) + step.dot(hessian * step) / Scalar{2};

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

    /// Parameterization class for the GradientDescent step calculation method.
    struct GradientDescentParameter {};

    /// Gradient descent step calculation method.
    struct GradientDescentMethod
    {
        using Parameter = GradientDescentParameter;

        GradientDescentMethod(const Parameter &)
        { }

        /// Computes the newton step as the gradient of the objective function.
        /// @param gradient gradient of the objective function
        /// @param step computed newton step
        /// @return true on success, otherwise false
        template<typename I, typename O, typename J, typename G, typename Objective, typename S>
        bool operator()(const Eigen::MatrixBase<I>&,
                        const Eigen::MatrixBase<O> &,
                        const Eigen::MatrixBase<J> &,
                        const Eigen::MatrixBase<G> &gradient,
                        const Objective &,
                        Eigen::MatrixBase<S>& step) const
        {
            step = gradient;
            return true;
        }
    };

    /// Parameterization class for the GaussNewton step calculation method.
    struct GaussNewtonParameter {};

    /// Gradient descent step calculation method.
    template<typename _Solver=DenseSVDSolver>
    struct GaussNewtonMethod
    {
        using Parameter = GaussNewtonParameter;
        using Solver = _Solver;

        GaussNewtonMethod(const Parameter &)
        { }

        /// Computes the newton step from a linearized approximation of the objective function.
        /// @param gradient gradient of the objective function
        /// @param step computed newton step
        /// @return true on success, otherwise false
        template<typename I, typename O, typename J, typename G, typename Objective, typename S>
        bool operator()(const Eigen::MatrixBase<I> &,
                        const Eigen::MatrixBase<O> &,
                        const Eigen::MatrixBase<J> &jacobian,
                        const Eigen::MatrixBase<G> &gradient,
                        const Objective &,
                        Eigen::MatrixBase<S>& step) const
        {
            Solver solver;
            const auto A = jacobian.transpose() * jacobian;
            using Matrix = typename decltype(A)::PlainMatrix;
            return solver(Matrix(A), gradient, step);
        }
    };

    /// Parameterization class for the LevenbergMarquardt step calculation method.
    template<typename _Scalar>
    class LevenbergMarquardtParameter
    {
    public:
        using Scalar = _Scalar;

        LevenbergMarquardtParameter() = default;

        LevenbergMarquardtParameter(const Scalar initialLambda,
                                    const Scalar increase,
                                    const Scalar decrease,
                                    const Index iterations)
            : _initialLambda(initialLambda), _increase(increase), _decrease(decrease), _maxIt(iterations)
        {
            assert(initialLambda > Scalar{0});
            assert(decrease < Scalar{1});
            assert(decrease > Scalar{0});
            assert(increase > Scalar{1});
        }

        /// Sets the initial gradient descent factor of levenberg marquardt.
        /// @param lambda gradient descent factor
        void setInitialLambda(const Scalar lambda)
        {
            assert(lambda > Scalar{0});
            _initialLambda = lambda;
        }

        /// Returns the initial gradient descent factor of levenberg marquardt.
        /// @return lambda gradient descent factor
        Scalar initialLambda() const
        {
            return _initialLambda;
        }

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

        /// Returns the maximum iterations of the levenberg marquardt optimization.
        /// @return maximum iterations for lambda search
        Index maximumIterations() const
        {
            return _maxIt;
        }

        /// Sets the increase factor for the lambda damping.
        /// The value has to be greater than 1.
        /// @param increase factor for increasing lambda
        void setIncrease(const Scalar increase)
        {
            assert(increase > Scalar{1});
            _increase = increase;
        }

        /// Returns the increase factor for the lambda damping.
        /// @return increase factor for increasing lambda
        Scalar increase() const
        {
            return _increase;
        }

        /// Sets the decrease factor for the lambda damping.
        /// The value has to be in (0, 1).
        /// @param decrease factor for decreasing lambda
        void setDecrease(const Scalar decrease)
        {
            assert(decrease < Scalar{1});
            assert(decrease > Scalar{0});
            _decrease = decrease;
        }

        /// Returns the decrease factor for the lambda damping.
        /// @return factor for increasing lambda
        Scalar decrease() const
        {
            return _decrease;
        }
    private:
        Scalar _initialLambda = Scalar{1};
        Scalar _increase = static_cast<Scalar>(2);
        Scalar _decrease = static_cast<Scalar>(0.5);
        Index _maxIt = 0;
    };

    /// Step refinement method which implements the Levenberg Marquardt method.
    template<typename _Scalar, typename _Solver=DenseSVDSolver>
    class LevenbergMarquardtMethod
    {
    public:
        using Scalar = _Scalar;
        using Solver = _Solver;
        using Parameter = LevenbergMarquardtParameter<Scalar>;

        LevenbergMarquardtMethod(const Parameter &param)
            : _param(param), _lambda(param.initialLambda())
        { }

        /// Computes the newton step from a linearized approximation of the objective function.
        /// @param gradient gradient of the objective function
        /// @param step computed newton step
        /// @return true on success, otherwise false
        template<typename I, typename O, typename J, typename G, typename Objective, typename S>
        bool operator()(const Eigen::MatrixBase<I> &xval,
                        const Eigen::MatrixBase<O> &fval,
                        const Eigen::MatrixBase<J> &jacobian,
                        const Eigen::MatrixBase<G> &gradient,
                        const Objective &objective,
                        Eigen::MatrixBase<S>& step)
        {
            using InputVector = typename Eigen::MatrixBase<I>::PlainMatrix;
            using OutputVector = typename Eigen::MatrixBase<O>::PlainMatrix;
            using JacobiMatrix = typename Eigen::MatrixBase<J>::PlainMatrix;
            using SystemMatrix = typename decltype(jacobian.transpose() * jacobian)::PlainMatrix;

            const auto error = fval.squaredNorm() / 2;
            const SystemMatrix jacobianSq = jacobian.transpose() * jacobian;

            Scalar errorN = error + 1;
            SystemMatrix A;
            InputVector xvalN;
            OutputVector fvalN;
            JacobiMatrix jacobianN;
            Solver solver;

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

                const auto ret = solver(A, gradient, step);
                if(!ret)
                    return false;

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

                if(errorN > error)
                    _lambda *= _param.increase();
                else
                    _lambda *= _param.decrease();

                ++iterations;
            }

            return true;
        }

    private:
        Parameter _param = {};
        Scalar _lambda = Scalar{1};
    };

    namespace internal
    {
        template<bool ComputesJacobian>
        struct ObjectiveEvaluator
        { };

        template<>
        struct ObjectiveEvaluator<true>
        {
            template<typename InputVector, typename Objective, typename FiniteDifferences, typename Params, typename OutputVector, typename JacobiMatrix>
            void operator()(const InputVector &xval,
                            const Objective &objective,
                            const FiniteDifferences&,
                            const Params &,
                            OutputVector &fval,
                            JacobiMatrix &jacobian) const
            {
                objective(xval, fval, jacobian);
            }
        };

        template<>
        struct ObjectiveEvaluator<false>
        {
            template<typename InputVector, typename Objective, typename FiniteDifferences, typename Params, typename OutputVector, typename JacobiMatrix>
            void operator()(const InputVector &xval,
                            const Objective &objective,
                            const FiniteDifferences &finiteDifferences,
                            const Params &param,
                            OutputVector &fval,
                            JacobiMatrix &jacobian) const
            {
                objective(xval, fval);
                finiteDifferences(xval, fval, objective, param, jacobian);
            }
        };

        template <typename T>
        bool almostZero(T x)
        {
            return std::abs(x) < std::numeric_limits<T>::epsilon();
        }

        template<typename Derived>
        std::string makePrettyVector(const Eigen::MatrixBase<Derived> &vec)
        {
            assert(vec.cols() == 1);

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

            return ss2.str();
        }
    }

    /// Core class for least squares algorithms.
    /// It implements the whole optimization strategy except the step
    /// calculation and its refinement.
    template<typename _Scalar,
             int _Inputs,
             int _Outputs,
             typename _Objective,
             typename _StepMethod,
             typename _RefineMethod,
             typename _FiniteDifferencesMethod>
    class LeastSquaresAlgorithm
    {
    public:
        using Scalar = _Scalar;
        constexpr static auto Inputs = _Inputs;
        constexpr static auto Outputs = _Outputs;
        using Objective = _Objective;

        using StepMethod = _StepMethod;
        using StepMethodParameter = typename StepMethod::Parameter;

        using StepRefiner = NewtonStepRefiner<Scalar, Inputs, Outputs, _RefineMethod>;
        using StepRefinerParameter = typename StepRefiner::Parameter;

        using FiniteDifferencesMethod = _FiniteDifferencesMethod;

        using InputVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using OutputVector = Eigen::Matrix<Scalar, Outputs, 1>;
        using JacobiMatrix = Eigen::Matrix<Scalar, Outputs, Inputs>;
        using HessianMatrix = Eigen::Matrix<Scalar, Inputs, Inputs>;
        using GradientVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using StepVector = Eigen::Matrix<Scalar, Inputs, 1>;

        using Callback = std::function<bool(const Index,
                                            const InputVector&,
                                            const OutputVector&,
                                            const JacobiMatrix&,
                                            const GradientVector&,
                                            const StepVector&)>;

        /// Result data returned after optimization.
        struct Result
        {
            InputVector xval = {};
            OutputVector fval = {};
            Scalar error = Scalar{-1};
            Index iterations = -1;
            bool converged = false;
            bool succeeded = false;
        };

        LeastSquaresAlgorithm() = default;

        /// 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)
        {
            _finiteDifferencesParam.setThreads(threads);
        }

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

        /// Sets the parameters for the newton step calculation method, e.g. GaussNewton, LevenbergMarquardt, etc.
        /// @param param parameters that should be set.
        void setMethodParameters(const StepMethodParameter &param)
        {
            _methodParam = param;
        }

        /// Returns the parameters for the newton step calculation method.
        /// @return step calculation parameters
        const StepMethodParameter &methodParameters() const
        {
            return _methodParam;
        }

        /// Sets the parameters for the newton step refinement method, e.g. ArmijoBacktracking, DoglegMethod, etc.
        /// @param param parameters that should be set.
        void setRefinementParameters(const StepRefinerParameter &param)
        {
            _refinerParam = param;
        }

        /// Returns the parameters for the newton step refinement method.
        /// @return step calculation parameters
        const StepRefinerParameter &refinementParameters() const
        {
            return _refinerParam;
        }

        /// Sets the instance values of the custom objective function.
        /// Should be used if the objective function requires custom data parameters.
        /// @param objective instance that should be copied
        void setObjective(const Objective &objective)
        {
            _objective = objective;
        }

        /// Sets the instance functor for iteration callbacks.
        /// @param refiner instance that should be copied
        template<typename T>
        void setCallback(const T &callback)
        {
            _callback = callback;
        }

        /// Sets the maximum number of iterations.
        /// Set to 0 or negative for infinite iterations.
        /// @param iterations maximum number of iterations
        void setMaximumIterations(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 setMinimumStepLength(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 setMinimumGradientLength(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 setMinimumError(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;
        }

        /// Sets the output stream that is used to print the optimization progress.
        /// @param output the output stream
        void setOutputStream(std::ostream &output)
        {
            _output = &output;
        }

        /// Minimizes the configured objective starting at the given initial guess.
        Result minimize(const InputVector &initialGuess)
        {
            InputVector xval = initialGuess;
            OutputVector fval;
            JacobiMatrix jacobian;
            GradientVector gradient;
            StepVector step = StepVector::Zero(xval.size());

            auto gradLen = _minGradLen + 1;
            auto error = _minError + 1;
            auto stepLen = _minStepLen + 1;
            bool callbackResult = true;
            bool succeeded = true;

            // create objective lambda function which performs automatically numerical estimation
            // of the gradient if necessary.
            const auto objective = [this](const InputVector &xval, OutputVector &fval, JacobiMatrix &jacobian)
            {
                const auto evaluator = internal::ObjectiveEvaluator<Objective::ComputesJacobian>();
                evaluator(xval, this->_objective, FiniteDifferencesMethod(), this->_finiteDifferencesParam, fval, jacobian);
            };

            auto stepMethod = StepMethod(_methodParam);
            auto stepRefiner = StepRefiner(_refinerParam);

            Index iterations = 0;
            while((_maxIt <= 0 || iterations < _maxIt) &&
                   gradLen >= _minGradLen &&
                   stepLen >= _minStepLen &&
                   error >= _minError &&
                   callbackResult)
            {
                xval -= step;
                objective(xval, fval, jacobian);

                error = fval.squaredNorm() / 2;
                gradient = jacobian.transpose() * fval;
                gradLen = gradient.norm();

                // compute the full newton step according to the current method
                if(!stepMethod(xval, fval, jacobian, gradient, objective, step))
                {
                    succeeded = false;
                    break;
                }

                // refine the step according to the current refiner
                stepRefiner(xval, fval, jacobian, gradient, objective, step);
                stepLen = step.norm();

                // evaluate callback if available
                if(_callback)
                {
                    callbackResult = _callback(iterations + 1, xval, fval, jacobian, gradient, step);
                }

                if(_verbosity > 0)
                {
                    using internal::makePrettyVector;

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

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

                    ss << "    error=" << error;

                    if(_verbosity > 2)
                        ss << "    xval=" << makePrettyVector(xval);
                    if(_verbosity > 3)
                        ss << "    step=" << makePrettyVector(step);
                    if(_verbosity > 4)
                        ss << "    fval=" << makePrettyVector(fval);
                    (*_output) << ss.str() << std::endl;
                }

                ++iterations;
            }

            Result result;
            result.xval = xval;
            result.fval = fval;
            result.error = error;
            result.iterations = iterations;
            result.succeeded = succeeded;
            result.converged = stepLen < _minStepLen ||
                gradLen < _minGradLen ||
                error < _minError;

            return result;
        }

    private:
        Objective _objective = {};
        Callback _callback = {};

        StepMethodParameter _methodParam = {};
        StepRefinerParameter _refinerParam = {};
        FiniteDifferencesParameter<Scalar> _finiteDifferencesParam = {};

        Index _maxIt = 0;
        Scalar _minStepLen = static_cast<Scalar>(1e-9);
        Scalar _minGradLen = static_cast<Scalar>(1e-9);
        Scalar _minError = Scalar{0};
        Index _verbosity = 0;
        std::ostream *_output = &std::cout;
    };

    /// General Gauss Newton algorithm.
    template<typename Scalar,
             int Inputs,
             int Outputs,
             typename Objective,
             typename RefineMethod=ConstantStepFactor,
             typename Solver=DenseSVDSolver,
             typename FiniteDifferencesMethod=CentralDifferences>
    using GaussNewton = LeastSquaresAlgorithm<Scalar,
                                              Inputs,
                                              Outputs,
                                              Objective,
                                              GaussNewtonMethod<Solver>,
                                              RefineMethod,
                                              FiniteDifferencesMethod>;

    /// Gauss Newton algorithm with dynamic problem size.
    template<typename Scalar,
             typename Objective,
             typename RefineMethod=ConstantStepFactor,
             typename Solver=DenseSVDSolver,
             typename FiniteDifferencesMethod=CentralDifferences>
    using GaussNewtonX = GaussNewton<Scalar,
                                     Eigen::Dynamic,
                                     Eigen::Dynamic,
                                     Objective,
                                     RefineMethod,
                                     Solver,
                                     FiniteDifferencesMethod>;

    /// General Gradient Descent algorithm.
    template<typename Scalar,
             int Inputs,
             int Outputs,
             typename Objective,
             typename RefineMethod=ConstantStepFactor,
             typename FiniteDifferencesMethod=CentralDifferences>
    using GradientDescent = LeastSquaresAlgorithm<Scalar,
                                                  Inputs,
                                                  Outputs,
                                                  Objective,
                                                  GradientDescentMethod,
                                                  RefineMethod,
                                                  FiniteDifferencesMethod>;

    /// Gradient Descent algorithm with dynamic problem size.
    template<typename Scalar,
             typename Objective,
             typename RefineMethod=ConstantStepFactor,
             typename FiniteDifferencesMethod=CentralDifferences>
    using GradientDescentX = GradientDescent<Scalar,
                                             Eigen::Dynamic,
                                             Eigen::Dynamic,
                                             Objective,
                                             RefineMethod,
                                             FiniteDifferencesMethod>;

    /// General Levenberg Marquardt algorithm.
    template<typename Scalar,
             int Inputs,
             int Outputs,
             typename Objective,
             typename Solver=DenseSVDSolver,
             typename FiniteDifferencesMethod=CentralDifferences>
    using LevenbergMarquardt = LeastSquaresAlgorithm<Scalar,
                                              Inputs,
                                              Outputs,
                                              Objective,
                                              LevenbergMarquardtMethod<Scalar, Solver>,
                                              ConstantStepFactor,
                                              FiniteDifferencesMethod>;

    template<typename Scalar,
             typename Objective,
             typename Solver=DenseSVDSolver,
             typename FiniteDifferencesMethod=CentralDifferences>
    using LevenbergMarquardtX = LevenbergMarquardt<Scalar,
                                     Eigen::Dynamic,
                                     Eigen::Dynamic,
                                     Objective,
                                     Solver,
                                     FiniteDifferencesMethod>;

    namespace parameter
    {
        /// Encodes a 3x3 rotation matrix into a 3-vector in angle axis representation using the rodrigues' formula.
        /// This parametrization is minimal, free of singularities and gimbal lock.
        /// @note https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
        template<typename Derived>
        Eigen::Matrix<typename Eigen::MatrixBase<Derived>::Scalar, 3, 1> encodeRotation(const Eigen::MatrixBase<Derived> &rotation)
        {
            using Scalar = typename Eigen::MatrixBase<Derived>::Scalar;
            using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
            assert(rotation.rows() == 3);
            assert(rotation.cols() == 3);

            const auto theta = std::acos((rotation.trace() - Scalar{1}) / Scalar{2});

            if(internal::almostZero(theta))
            {
                return Vector3::Zero();
            }
            else
            {
                const auto fac = theta / (Scalar{2} * std::sin(theta));

                return fac * Vector3(rotation(2, 1) - rotation(1, 2),
                                     rotation(0, 2) - rotation(2, 0),
                                     rotation(1, 0) - rotation(0, 1));
            }

        }

        /// Decodes a 3-vector in angle axis representation into a 3x3 rotation matrix using the rodrigues' formula.
        /// @note https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
        template<typename Derived>
        Eigen::Matrix<typename Eigen::MatrixBase<Derived>::Scalar, 3, 3> decodeRotation(const Eigen::MatrixBase<Derived> &rotation)
        {
            using Scalar = typename Eigen::MatrixBase<Derived>::Scalar;
            using Matrix3 = Eigen::Matrix<Scalar, 3, 3>;
            using Vector3 = Eigen::Matrix<Scalar, 3, 1>;

            assert(rotation.rows() == 3);
            assert(rotation.cols() == 1);

            const auto theta = rotation.norm();
            if(internal::almostZero(theta))
            {
                return Matrix3::Identity();
            }
            else
            {
                const Vector3 w = rotation / theta;

                const auto s = std::sin(theta);
                const auto c = std::cos(theta);

                Matrix3 K;
                K << 0, -w.z(), w.y(),
                    w.z(), 0,  -w.x(),
                    -w.y(), w.x(), 0;

                // Matrix3 result;
                // result << c + wx * wx * (1 - c),      wx * wy * (1 - c) - wz * s, wy * s + wx * wz * (1 - c),
                //           wz * s + wx * wy * (1 - c), c + wy * wy * (1 - c), -wx * s + wy * wz * (1 - c),
                //           -wy * s + wx * wz * (1 - c), wx * s + wy * wz * (1 - c), c + wz * wz * (1 - c);

                return Matrix3::Identity() + s * K + (1 - c) * (w * w.transpose() - Matrix3::Identity());
            }
        }
    }
}

#endif


/*
[begin_description]
Modification of the implicit Euler method, works with the MTL4 matrix library only.
[end_description]

Copyright 2012-2013 Andreas Angelopoulos
Copyright 2012-2013 Karsten Ahnert
Copyright 2012-2013 Mario Mulansky

Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/

#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_IMPLICIT_EULER_MTL4_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_IMPLICIT_EULER_MTL4_HPP_INCLUDED

#include <utility>

#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>

#include <boost/numeric/odeint/external/mtl4/mtl4_resize.hpp>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template< class ValueType , class Resizer = initially_resizer >
class implicit_euler_mtl4
{

public:

    typedef ValueType value_type;
    typedef value_type time_type;
    typedef mtl::dense_vector<value_type> state_type;

    typedef state_wrapper< state_type > wrapped_state_type;
    typedef state_type deriv_type;
    typedef state_wrapper< deriv_type > wrapped_deriv_type;
    typedef mtl::compressed2D< value_type > matrix_type;
    typedef state_wrapper< matrix_type > wrapped_matrix_type;

    typedef Resizer resizer_type;
    typedef stepper_tag stepper_category;

    typedef implicit_euler_mtl4< ValueType , Resizer > stepper_type;

    implicit_euler_mtl4( const value_type epsilon = 1E-6 )
        : m_epsilon( epsilon ) , m_resizer() ,
          m_dxdt() , m_x() ,
          m_identity() , m_jacobi()
    { }

    template< class System >
    void do_step( System system , state_type &x , time_type t , time_type dt )
    {
        typedef typename odeint::unwrap_reference< System >::type system_type;
        typedef typename odeint::unwrap_reference< typename system_type::first_type >::type deriv_func_type;
        typedef typename odeint::unwrap_reference< typename system_type::second_type >::type jacobi_func_type;
        system_type &sys = system;
        deriv_func_type &deriv_func = sys.first;
        jacobi_func_type &jacobi_func = sys.second;

        m_resizer.adjust_size( x , detail::bind(
                                   &stepper_type::template resize_impl< state_type > , detail::ref( *this ) , detail::_1 ) );

        m_identity.m_v = 1;

        t += dt;
        m_x.m_v = x;

        deriv_func( x , m_dxdt.m_v , t );
        jacobi_func( x , m_jacobi.m_v , t );

        m_dxdt.m_v *= -dt;

        m_jacobi.m_v *= dt;
        m_jacobi.m_v -= m_identity.m_v ;

        // using ilu_0 preconditioning -incomplete LU factorisation
        // itl::pc::diagonal<matrix_type,double> L(m_jacobi.m_v);
        itl::pc::ilu_0<matrix_type> L( m_jacobi.m_v );

        solve( m_jacobi.m_v , m_x.m_v , m_dxdt.m_v , L );
        x+= m_x.m_v;

    }

    template< class StateType >
    void adjust_size( const StateType &x )
    {
        resize_impl( x );
    }

private:

    /*
      Applying approximate iterative linear solvers
      default solver is Biconjugate gradient stabilized method
      itl::bicgstab(A, x, b, L, iter);
    */
    template < class LinearOperator, class HilbertSpaceX, class HilbertSpaceB, class Preconditioner>
    void solve(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
               const Preconditioner& L, int max_iteractions =500)
    {
        // Termination criterion: r < 1e-6 * b or N iterations
        itl::basic_iteration< double > iter( b , max_iteractions , 1e-6 );
        itl::bicgstab( A , x , b , L , iter );

    }

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized = false;
        resized |= adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_x , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_identity , x , typename is_resizeable<matrix_type>::type() );
        resized |= adjust_size_by_resizeability( m_jacobi , x , typename is_resizeable<matrix_type>::type() );
        return resized;
    }

private:

    value_type m_epsilon;
    resizer_type m_resizer;
    wrapped_deriv_type m_dxdt;
    wrapped_state_type m_x;
    wrapped_matrix_type m_identity;
    wrapped_matrix_type m_jacobi;
};

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_IMPLICIT_EULER_MTL4_HPP_INCLUDED


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the deep learning tools from the dlib C++
    Library.  I'm assuming you have already read the dnn_introduction_ex.cpp, the
    dnn_introduction2_ex.cpp and the dnn_introduction3_ex.cpp examples.  In this example
    program we are going to show how one can train a YOLO detector.  In particular, we will train
    the YOLOv3 model like the one introduced in this paper:
    "YOLOv3: An Incremental Improvement" by Joseph Redmon and  Ali Farhadi.

    This example program will work with any imglab dataset, such as:
        - faces: http://dlib.net/files/data/dlib_face_detection_dataset-2016-09-30.tar.gz
        - vehicles: http://dlib.net/files/data/dlib_rear_end_vehicles_v1.tar
    Just uncompress the dataset and give the directory containing the training.xml and testing.xml
    files as an argument to this program.
*/

#include <dlib/cmd_line_parser.h>
#include <dlib/data_io.h>
#include <dlib/dnn.h>
#include <dlib/gui_widgets.h>
#include <dlib/image_io.h>
#include <tools/imglab/src/metadata_editor.h>

using namespace std;
using namespace dlib;

// In the darknet namespace we define:
// - the network architecture: DarkNet53 backbone and detection head for YOLO.
// - a helper function to setup the detector: change the number of classes, etc.
namespace darknet
{
    // backbone tags
    template <typename SUBNET> using btag8 = add_tag_layer<8008, SUBNET>;
    template <typename SUBNET> using btag16 = add_tag_layer<8016, SUBNET>;
    template <typename SUBNET> using bskip8 = add_skip_layer<btag8, SUBNET>;
    template <typename SUBNET> using bskip16 = add_skip_layer<btag16, SUBNET>;
    // neck tags
    template <typename SUBNET> using ntag8 = add_tag_layer<6008, SUBNET>;
    template <typename SUBNET> using ntag16 = add_tag_layer<6016, SUBNET>;
    template <typename SUBNET> using ntag32 = add_tag_layer<6032, SUBNET>;
    template <typename SUBNET> using nskip8 = add_skip_layer<ntag8, SUBNET>;
    template <typename SUBNET> using nskip16 = add_skip_layer<ntag16, SUBNET>;
    template <typename SUBNET> using nskip32 = add_skip_layer<ntag32, SUBNET>;
    // head tags
    template <typename SUBNET> using htag8 = add_tag_layer<7008, SUBNET>;
    template <typename SUBNET> using htag16 = add_tag_layer<7016, SUBNET>;
    template <typename SUBNET> using htag32 = add_tag_layer<7032, SUBNET>;
    template <typename SUBNET> using hskip8 = add_skip_layer<htag8, SUBNET>;
    template <typename SUBNET> using hskip16 = add_skip_layer<htag16, SUBNET>;
    // yolo tags
    template <typename SUBNET> using ytag8 = add_tag_layer<4008, SUBNET>;
    template <typename SUBNET> using ytag16 = add_tag_layer<4016, SUBNET>;
    template <typename SUBNET> using ytag32 = add_tag_layer<4032, SUBNET>;

    template <template <typename> class ACT, template <typename> class BN>
    struct def
    {
        template <long nf, long ks, int s, typename SUBNET>
        using conblock = ACT<BN<add_layer<con_<nf, ks, ks, s, s, ks / 2, ks / 2>, SUBNET>>>;

        template <long nf, typename SUBNET>
        using residual = add_prev1<conblock<nf, 3, 1, conblock<nf / 2, 1, 1, tag1<SUBNET>>>>;

        template <long nf, long factor, typename SUBNET>
        using conblock5 = conblock<nf, 1, 1,
                          conblock<nf * factor, 3, 1,
                          conblock<nf, 1, 1,
                          conblock<nf * factor, 3, 1,
                          conblock<nf, 1, 1, SUBNET>>>>>;

        template <typename SUBNET> using res_64 = residual<64, SUBNET>;
        template <typename SUBNET> using res_128 = residual<128, SUBNET>;
        template <typename SUBNET> using res_256 = residual<256, SUBNET>;
        template <typename SUBNET> using res_512 = residual<512, SUBNET>;
        template <typename SUBNET> using res_1024 = residual<1024, SUBNET>;

        template <typename INPUT>
        using backbone53 = repeat<4, res_1024, conblock<1024, 3, 2,
                    btag16<repeat<8, res_512,  conblock<512, 3, 2,
                     btag8<repeat<8, res_256,  conblock<256, 3, 2,
                           repeat<2, res_128,  conblock<128, 3, 2,
                                     res_64<   conblock<64, 3, 2,
                                               conblock<32, 3, 1,
                                               INPUT>>>>>>>>>>>>>;

        // This is the layer that will be passed to the loss layer to get the detections from the network.
        // The main thing to pay attention to when defining the YOLO output layer is that it should be
        // a tag layer, followed by a sigmoid layer and a 1x1 convolution.  The tag layer should be unique
        // in the whole network definition, as the loss layer will use it to get the outputs.  The number of
        // filters in the convolutional layer should be (1 + 4 + num_classes) * num_anchors at that output.
        // The 1 corresponds to the objectness in the loss layer and the 4 to the bounding box coordinates.
        template <long num_classes, long nf, template <typename> class YTAG, template <typename> class NTAG, typename SUBNET>
        using yolo = YTAG<sig<con<3 * (num_classes + 5), 1, 1, 1, 1,
                     conblock<nf, 3, 1,
                NTAG<conblock5<nf / 2, 2,
                     SUBNET>>>>>>;

        template <long num_classes>
        using yolov3 = yolo<num_classes, 256, ytag8, ntag8,
                       concat2<htag8, btag8,
                 htag8<upsample<2, conblock<128, 1, 1,
                       nskip16<
                       yolo<num_classes, 512, ytag16, ntag16,
                       concat2<htag16, btag16,
                htag16<upsample<2, conblock<256, 1, 1,
                       nskip32<
                       yolo<num_classes, 1024, ytag32, ntag32,
                       backbone53<input_rgb_image>>>>>>>>>>>>>>;

    };

    using yolov3_train_type = loss_yolo<ytag8, ytag16, ytag32, def<leaky_relu, bn_con>::yolov3<80>>;
    using yolov3_infer_type = loss_yolo<ytag8, ytag16, ytag32, def<leaky_relu, affine>::yolov3<80>>;

    void setup_detector(yolov3_train_type& net, const yolo_options& options)
    {
        // remove bias from bn inputs
        disable_duplicative_biases(net);
        // setup leaky relus
        visit_computational_layers(net, [](leaky_relu_& l) { l = leaky_relu_(0.1); });
        // enlarge the batch normalization stats window
        set_all_bn_running_stats_window_sizes(net, 1000);
        // set the number of filters for detection layers (they are located after the tag and sig layers)
        const long nfo1 = options.anchors.at(tag_id<ytag8>::id).size() * (options.labels.size() + 5);
        const long nfo2 = options.anchors.at(tag_id<ytag16>::id).size() * (options.labels.size() + 5);
        const long nfo3 = options.anchors.at(tag_id<ytag32>::id).size() * (options.labels.size() + 5);
        layer<ytag8, 2>(net).layer_details().set_num_filters(nfo1);
        layer<ytag16, 2>(net).layer_details().set_num_filters(nfo2);
        layer<ytag32, 2>(net).layer_details().set_num_filters(nfo3);
    }
}

// In this example, YOLO expects square images, and we choose to transform them by letterboxing them.
rectangle_transform preprocess_image(const matrix<rgb_pixel>& image, matrix<rgb_pixel>& output, const long image_size)
{
    return rectangle_transform(inv(letterbox_image(image, output, image_size)));
}

// YOLO outputs the bounding boxes in the coordinate system of the input (letterboxed) image, so we need to convert them
// back to the original image.
void postprocess_detections(const rectangle_transform& tform, std::vector<yolo_rect>& detections)
{
    for (auto& d : detections)
        d.rect = tform(d.rect);
}

int main(const int argc, const char** argv)
try
{
    command_line_parser parser;
    parser.add_option("size", "image size for training (default: 416)", 1);
    parser.add_option("learning-rate", "initial learning rate (default: 0.001)", 1);
    parser.add_option("batch-size", "mini batch size (default: 8)", 1);
    parser.add_option("burnin", "learning rate burnin steps (default: 1000)", 1);
    parser.add_option("patience", "number of steps without progress (default: 10000)", 1);
    parser.add_option("workers", "number of worker threads to load data (default: 4)", 1);
    parser.add_option("gpus", "number of GPUs to run the training on (default: 1)", 1);
    parser.add_option("test", "test the detector with a threshold (default: 0.01)", 1);
    parser.add_option("visualize", "visualize data augmentation instead of training");
    parser.add_option("map", "compute the mean average precision");
    parser.add_option("anchors", "Do nothing but compute <arg1> anchor boxes using K-Means and print their shapes.", 1);
    parser.set_group_name("Help Options");
    parser.add_option("h", "alias of --help");
    parser.add_option("help", "display this message and exit");
    parser.parse(argc, argv);
    if (parser.number_of_arguments() == 0 || parser.option("h") || parser.option("help"))
    {
        parser.print_options();
        cout << "Give the path to a folder containing the training.xml file." << endl;
        return 0;
    }
    const double learning_rate = get_option(parser, "learning-rate", 0.001);
    const size_t patience = get_option(parser, "patience", 10000);
    const size_t batch_size = get_option(parser, "batch-size", 8);
    const size_t burnin = get_option(parser, "burnin", 1000);
    const size_t image_size = get_option(parser, "size", 416);
    const size_t num_workers = get_option(parser, "workers", 4);
    const size_t num_gpus = get_option(parser, "gpus", 1);

    const string data_directory = parser[0];
    const string sync_file_name = "yolov3_sync";

    image_dataset_metadata::dataset dataset;
    image_dataset_metadata::load_image_dataset_metadata(dataset, data_directory + "/training.xml");
    cout << "# images: " << dataset.images.size() << endl;
    std::map<string, size_t> labels;
    size_t num_objects = 0;
    for (const auto& im : dataset.images)
    {
        for (const auto& b : im.boxes)
        {
            labels[b.label]++;
            ++num_objects;
        }
    }
    cout << "# labels: " << labels.size() << endl;

    yolo_options options;
    color_mapper string_to_color;
    for (const auto& label : labels)
    {
        cout <<  " - " << label.first << ": " << label.second;
        cout << " (" << (100.0*label.second)/num_objects << "%)\n";
        options.labels.push_back(label.first);
        string_to_color(label.first);
    }

    // If the default anchor boxes don't fit your data well you should recompute them.
    // Here's a simple way to do it using K-Means clustering.  Note that the approach
    // shown below is suboptimal, since it doesn't group the bounding boxes by size.
    // Grouping the bounding boxes by size and computing the K-Means on each group
    // would make more sense, since each stride of the network is meant to output
    // boxes at a particular size, but that is very specific to the network architecture
    // and the dataset itself.
    if (parser.option("anchors"))
    {
        const auto num_clusers = std::stoul(parser.option("anchors").argument());
        std::vector<dpoint> samples;
        // First we need to rescale the bounding boxes to match the image size at training time.
        for (const auto& image_info : dataset.images)
        {
            const auto scale = image_size / std::max<double>(image_info.width, image_info.height);
            for (const auto& box : image_info.boxes)
            {
                dpoint sample(box.rect.width(), box.rect.height());
                samples.push_back(sample*scale);
            }
        }
        // Now we can compute K-Means clustering
        randomize_samples(samples);
        cout << "Computing anchors for " << samples.size() << " samples" << endl;
        std::vector<dpoint> anchors;
        pick_initial_centers(num_clusers, anchors, samples);
        find_clusters_using_kmeans(samples, anchors);
        std::sort(anchors.begin(), anchors.end(), [](const dpoint& a, const dpoint& b){ return prod(a) < prod(b); });
        for (const dpoint& c : anchors)
            cout << round(c(0)) << 'x' << round(c(1)) << endl;
        // And check the average IoU of the newly computed anchor boxes and the training samples.
        double average_iou = 0;
        for (const dpoint& s : samples)
        {
            drectangle sample = centered_drect(dpoint(0, 0), s.x(), s.y());
            double best_iou = 0;
            for (const dpoint& a : anchors)
            {
                drectangle anchor = centered_drect(dpoint(0, 0), a.x(), a.y());
                best_iou = std::max(best_iou, box_intersection_over_union(sample, anchor));
            }
            average_iou += best_iou;
        }
        cout << "Average IoU: " << average_iou / samples.size() << endl;
        return EXIT_SUCCESS;
    }

    // When computing the objectness loss in YOLO, predictions that do not have an IoU
    // with any ground truth box of at least options.iou_ignore_threshold, will be
    // treated as not capable of detecting an object, an therefore incur loss.
    // Similarly, predictions above this threshold are considered correct predictions
    // by the loss.  Typical settings for this threshold are in the range 0.5 to 0.7.
    options.iou_ignore_threshold = 0.7;
    // By setting this to a value < 1, we are telling the model to update all the predictions
    // as long as the anchor box has an IoU > 0.2 with a ground truth.
    options.iou_anchor_threshold = 0.2;
    // These are the anchors computed on COCO dataset, presented in the YOLOv3 paper.
    options.add_anchors<darknet::ytag8>({{10, 13}, {16, 30}, {33, 23}});
    options.add_anchors<darknet::ytag16>({{30, 61}, {62, 45}, {59, 119}});
    options.add_anchors<darknet::ytag32>({{116, 90}, {156, 198}, {373, 326}});
    darknet::yolov3_train_type net(options);
    darknet::setup_detector(net, options);

    // The training process can be unstable at the beginning.  For this reason, we exponentially
    // increase the learning rate during the first burnin steps.
    const matrix<double> learning_rate_schedule = learning_rate * pow(linspace(1e-12, 1, burnin), 4);

    // In case we have several GPUs, we can tell the dnn_trainer to make use of them.
    std::vector<int> gpus(num_gpus);
    iota(gpus.begin(), gpus.end(), 0);
    // We initialize the trainer here, as it will be used in several contexts, depending on the
    // arguments passed the the program.
    dnn_trainer<darknet::yolov3_train_type> trainer(net, sgd(0.0005, 0.9), gpus);
    trainer.be_verbose();
    trainer.set_mini_batch_size(batch_size);
    trainer.set_learning_rate_schedule(learning_rate_schedule);
    trainer.set_synchronization_file(sync_file_name, chrono::minutes(15));
    cout << trainer;

    // If the training has started and a synchronization file has already been saved to disk,
    // we can re-run this program with the --test option and a confidence threshold to see
    // how the training is going.
    if (parser.option("test"))
    {
        if (!file_exists(sync_file_name))
        {
            cout << "Could not find file " << sync_file_name << endl;
            return EXIT_FAILURE;
        }
        const double threshold = get_option(parser, "test", 0.01);
        image_window win;
        matrix<rgb_pixel> image, resized;
        for (const auto& im : dataset.images)
        {
            win.clear_overlay();
            load_image(image, data_directory + "/" + im.filename);
            win.set_title(im.filename);
            win.set_image(image);
            const auto tform = preprocess_image(image, resized, image_size);
            auto detections = net.process(resized, threshold);
            postprocess_detections(tform, detections);
            cout << "# detections: " << detections.size() << endl;
            for (const auto& det : detections)
            {
                win.add_overlay(det.rect, string_to_color(det.label), det.label);
                cout << det.label << ": " << det.rect << " " << det.detection_confidence << endl;
            }
            cin.get();
        }
        return EXIT_SUCCESS;
    }

    // If the training has started and a synchronization file has already been saved to disk,
    // we can re-run this program with the --map option to compute the mean average precision
    // on the test set.
    if (parser.option("map"))
    {
        image_dataset_metadata::dataset dataset;
        image_dataset_metadata::load_image_dataset_metadata(dataset, data_directory + "/testing.xml");
        if (!file_exists(sync_file_name))
        {
            cout << "Could not find file " << sync_file_name << endl;
            return EXIT_FAILURE;
        }
        matrix<rgb_pixel> image, resized;
        std::map<std::string, std::vector<std::pair<double, bool>>> hits;
        std::map<std::string, unsigned long> missing;
        for (const auto& label : options.labels)
        {
            hits[label] = std::vector<std::pair<double, bool>>();
            missing[label] = 0;
        }
        cout << "computing mean average precision for " << dataset.images.size() << " images..." << endl;
        for (size_t i = 0; i < dataset.images.size(); ++i)
        {
            const auto& im = dataset.images[i];
            load_image(image, data_directory + "/" + im.filename);
            const auto tform = preprocess_image(image, resized, image_size);
            auto dets = net.process(resized, 0.005);
            postprocess_detections(tform, dets);
            std::vector<bool> used(dets.size(), false);
            // true positives: truths matched by detections
            for (size_t t = 0; t < im.boxes.size(); ++t)
            {
                bool found_match = false;
                for (size_t d = 0; d < dets.size(); ++d)
                {
                    if (used[d])
                        continue;
                    if (im.boxes[t].label == dets[d].label &&
                        box_intersection_over_union(drectangle(im.boxes[t].rect), dets[d].rect) > 0.5)
                    {
                        used[d] = true;
                        found_match = true;
                        hits.at(dets[d].label).emplace_back(dets[d].detection_confidence, true);
                        break;
                    }
                }
                // false negatives: truths not matched
                if (!found_match)
                    missing.at(im.boxes[t].label)++;
            }
            // false positives: detections not matched
            for (size_t d = 0; d < dets.size(); ++d)
            {
                if (!used[d])
                    hits.at(dets[d].label).emplace_back(dets[d].detection_confidence, false);
            }
            cout << "progress: " << i << '/' << dataset.images.size() << "\t\t\t\r" << flush;
        }
        double map = 0;
        for (auto& item : hits)
        {
            std::sort(item.second.rbegin(), item.second.rend());
            const double ap = average_precision(item.second, missing[item.first]);
            cout << rpad(item.first + ": ", 16, " ") << ap * 100 << '%' << endl;
            map += ap;
        }
        cout << rpad(string("mAP: "), 16, " ") << map / hits.size() * 100 << '%' << endl;
        return EXIT_SUCCESS;
    }

    // Create some data loaders which will load the data, and perform some data augmentation.
    dlib::pipe<std::pair<matrix<rgb_pixel>, std::vector<yolo_rect>>> train_data(1000);
    const auto loader = [&dataset, &data_directory, &train_data, &image_size](time_t seed)
    {
        dlib::rand rnd(time(nullptr) + seed);
        matrix<rgb_pixel> image, rotated;
        std::pair<matrix<rgb_pixel>, std::vector<yolo_rect>> temp;
        random_cropper cropper;
        cropper.set_seed(time(nullptr) + seed);
        cropper.set_chip_dims(image_size, image_size);
        cropper.set_max_object_size(0.9);
        cropper.set_min_object_size(10, 10);
        cropper.set_max_rotation_degrees(10);
        cropper.set_translate_amount(0.5);
        cropper.set_randomly_flip(true);
        cropper.set_background_crops_fraction(0);
        cropper.set_min_object_coverage(0.8);
        while (train_data.is_enabled())
        {
            const auto idx = rnd.get_random_32bit_number() % dataset.images.size();
            load_image(image, data_directory + "/" + dataset.images[idx].filename);
            for (const auto& box : dataset.images[idx].boxes)
                temp.second.emplace_back(box.rect, 1, box.label);

            // We alternate between augmenting the full image and random cropping
            if (rnd.get_random_double() > 0.5)
            {
                rectangle_transform tform = rotate_image(
                    image,
                    rotated,
                    rnd.get_double_in_range(-5 * pi / 180, 5 * pi / 180),
                    interpolate_bilinear());
                for (auto& box : temp.second)
                    box.rect = tform(box.rect);

                tform = letterbox_image(rotated, temp.first, image_size);
                for (auto& box : temp.second)
                    box.rect = tform(box.rect);

                if (rnd.get_random_double() > 0.5)
                {
                    tform = flip_image_left_right(temp.first);
                    for (auto& box : temp.second)
                        box.rect = tform(box.rect);
                }
            }
            else
            {
                std::vector<yolo_rect> boxes = temp.second;
                cropper(image, boxes, temp.first, temp.second);
            }
            disturb_colors(temp.first, rnd);
            train_data.enqueue(temp);
        }
    };

    std::vector<thread> data_loaders;
    for (size_t i = 0; i < num_workers; ++i)
        data_loaders.emplace_back([loader, i]() { loader(i + 1); });

    // It is always a good idea to visualize the training samples.  By passing the --visualize
    // flag, we can see the training samples that will be fed to the dnn_trainer.
    if (parser.option("visualize"))
    {
        image_window win;
        while (true)
        {
            std::pair<matrix<rgb_pixel>, std::vector<yolo_rect>> temp;
            train_data.dequeue(temp);
            win.clear_overlay();
            win.set_image(temp.first);
            for (const auto& r : temp.second)
            {
                auto color = string_to_color(r.label);
                // make semi-transparent and cross-out the ignored boxes
                if (r.ignore)
                {
                    color.alpha = 128;
                    win.add_overlay(r.rect.tl_corner(), r.rect.br_corner(), color);
                    win.add_overlay(r.rect.tr_corner(), r.rect.bl_corner(), color);
                }
                win.add_overlay(r.rect, color, r.label);
            }
            cout << "Press enter to visualize the next training sample.";
            cin.get();
        }
    }

    std::vector<matrix<rgb_pixel>> images;
    std::vector<std::vector<yolo_rect>> bboxes;

    // The main training loop, that we will reuse for the warmup and the rest of the training.
    const auto train = [&images, &bboxes, &train_data, &trainer]()
    {
        images.clear();
        bboxes.clear();
        pair<matrix<rgb_pixel>, std::vector<yolo_rect>> temp;
        while (images.size() < trainer.get_mini_batch_size())
        {
            train_data.dequeue(temp);
            images.push_back(move(temp.first));
            bboxes.push_back(move(temp.second));
        }
        trainer.train_one_step(images, bboxes);
    };

    cout << "training started with " << burnin << " burn-in steps" << endl;
    while (trainer.get_train_one_step_calls() < burnin)
        train();

    cout << "burn-in finished" << endl;
    trainer.get_net();
    trainer.set_learning_rate(learning_rate);
    trainer.set_min_learning_rate(learning_rate * 1e-3);
    trainer.set_learning_rate_shrink_factor(0.1);
    trainer.set_iterations_without_progress_threshold(patience);
    cout << trainer << endl;

    while (trainer.get_learning_rate() >= trainer.get_min_learning_rate())
        train();

    cout << "training done" << endl;

    trainer.get_net();
    train_data.disable();
    for (auto& worker : data_loaders)
        worker.join();

    // Before saving the network, we can assign it to the "infer" version, so that it won't
    // perform batch normalization with batch sizes larger than one, as usual.  Moreover,
    // we can also fuse the batch normalization (affine) layers into the convolutional
    // layers, so that the network can run a bit faster.  Notice that, after fusing the
    // layers, the network can no longer be used for training, so you should save the
    // yolov3_train_type network if you plan to further train or finetune the network.
    darknet::yolov3_infer_type inet(net);
    fuse_layers(inet);

    serialize("yolov3.dnn") << inet;
    return EXIT_SUCCESS;
}
catch (const std::exception& e)
{
    cout << e.what() << endl;
    return EXIT_FAILURE;
}


/*!
@file
Defines `boost::hana::Group::laws`.

@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_GROUP_LAWS_HPP
#define BOOST_HANA_GROUP_LAWS_HPP

#include <boost/hana/group/group.hpp>

#include <boost/hana/comparable/comparable.hpp>
#include <boost/hana/core/datatype.hpp>
#include <boost/hana/logical/logical.hpp>
#include <boost/hana/monoid/monoid.hpp>
#include <boost/hana/searchable/searchable.hpp>

namespace boost { namespace hana {
    struct Group::laws {
        template <typename GroupObjects>
        static constexpr auto check(GroupObjects objs) {
            return all(objs, [=](auto x) {
                using G = datatype_t<decltype(x)>;
                return and_(
                    equal(plus(x, negate(x)), zero<G>),
                    equal(plus(negate(x), x), zero<G>)
                );
            });
        }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_GROUP_LAWS_HPP