/*******************************************************
 * Copyright (C) 2020, RAM-LAB, Hong Kong University of Science and Technology
 *
 * This file is part of M-LOAM (https://ram-lab.com/file/jjiao/m-loam).
 * If you use this code, please cite the respective publications as
 * listed on the above websites.
 *
 * Licensed under the GNU General Public License v3.0;
 * you may not use this file except in compliance with the License.
 *
 * Author: Jianhao JIAO (jiaojh1994@gmail.com)
 *******************************************************/

#pragma once

#include <cmath>
#include <cassert>
#include <cstring>
#include <ceres/ceres.h>
#include <ceres/rotation.h>

#include <Eigen/Eigen>
#include <Eigen/Dense>

template <typename Derived>
static Eigen::Quaternion<typename Derived::Scalar> deltaQ(const Eigen::MatrixBase<Derived> &theta)
{
    typedef typename Derived::Scalar Scalar_t;
    Eigen::Quaternion<Scalar_t> dq;
    Eigen::Matrix<Scalar_t, 3, 1> half_theta = theta;
    half_theta /= static_cast<Scalar_t>(2.0);
    dq.w() = static_cast<Scalar_t>(1.0);
    dq.x() = half_theta.x();
    dq.y() = half_theta.y();
    dq.z() = half_theta.z();
    return dq;
}
template <typename Derived>
static Eigen::Matrix<typename Derived::Scalar, 3, 3> skewSymmetric(const Eigen::MatrixBase<Derived> &q)
{
    Eigen::Matrix<typename Derived::Scalar, 3, 3> ans;
    ans << typename Derived::Scalar(0), -q(2), q(1),
        q(2), typename Derived::Scalar(0), -q(0),
        -q(1), q(0), typename Derived::Scalar(0);
    return ans;
}

// pure odom planar factor
class LidarPureOdomPlaneNormFactor : public ceres::SizedCostFunction<1, 7, 7, 7>
{

public:
	LidarPureOdomPlaneNormFactor(const Eigen::Vector3d &point,
								 const Eigen::Vector4d &coeff,
								 const double &sqrt_info = 1.0)
		: point_(point),
		  coeff_(coeff),
		  sqrt_info_(sqrt_info) {}

	// residual = sum(w^(T) * (R * p + t) + d)
	bool Evaluate(double const *const *param, double *residuals, double **jacobians) const
    {
		Eigen::Quaterniond Q_pivot(param[0][6], param[0][3], param[0][4], param[0][5]);
		Eigen::Vector3d t_pivot(param[0][0], param[0][1], param[0][2]);
		Eigen::Quaterniond Q_i(param[1][6], param[1][3], param[1][4], param[1][5]);
		Eigen::Vector3d t_i(param[1][0], param[1][1], param[1][2]);
		Eigen::Quaterniond Q_ext(param[2][6], param[2][3], param[2][4], param[2][5]);
		Eigen::Vector3d t_ext(param[2][0], param[2][1], param[2][2]);

		Eigen::Quaterniond Q_pi = Q_pivot.conjugate() * Q_i;
		Eigen::Vector3d t_pi = Q_pivot.conjugate() * (t_i - t_pivot);
		Eigen::Quaterniond Q_ext_pi = Q_pi * Q_ext;
		Eigen::Vector3d t_ext_pi = Q_pi * t_ext + t_pi;

		Eigen::Vector3d w(coeff_(0), coeff_(1), coeff_(2));
		const double d = coeff_(3);
		const double r = w.dot(Q_ext_pi * point_ + t_ext_pi) + d;
		residuals[0] = sqrt_info_ * r;

		// jacobians: 3x7
        if (jacobians)
        {
            Eigen::Matrix3d Rp = Q_pivot.toRotationMatrix();
			Eigen::Matrix3d Ri = Q_i.toRotationMatrix();
            Eigen::Matrix3d Rext = Q_ext.toRotationMatrix();
            if (jacobians[0])
            {
                Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > jacobian_pose_pivot(jacobians[0]);

                Eigen::Matrix<double, 1, 6> jaco_pivot;
				jaco_pivot.leftCols<3>() = -w.transpose() * Rp.transpose();
				jaco_pivot.rightCols<3>() = w.transpose() * (Rp.transpose() *
															 skewSymmetric(Ri * Rext * point_ + Ri * t_ext + t_i - t_pivot));

                jacobian_pose_pivot.leftCols<6>() = sqrt_info_ * jaco_pivot;
                jacobian_pose_pivot.rightCols<1>().setZero();
            }

            if (jacobians[1])
            {
                Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > jacobian_pose_i(jacobians[1]);

                Eigen::Matrix<double, 1, 6> jaco_i;
				jaco_i.leftCols<3>() = w.transpose() * Rp.transpose();
				jaco_i.rightCols<3>() = -w.transpose() * Rp.transpose() *
										Ri * skewSymmetric(Rext * point_ + t_ext);

                jacobian_pose_i.leftCols<6>() = sqrt_info_ * jaco_i;
                jacobian_pose_i.rightCols<1>().setZero();
            }

            if (jacobians[2])
            {
                Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > jacobian_pose_ex(jacobians[2]);

				Eigen::Matrix<double, 1, 6> jaco_ex;
				jaco_ex.leftCols<3>() = w.transpose() * Rp.transpose() * Ri;
				jaco_ex.rightCols<3>() = -w.transpose() * Rp.transpose() * Ri * skewSymmetric(Rext * point_);

                jacobian_pose_ex.leftCols<6>() = sqrt_info_ * jaco_ex;
                jacobian_pose_ex.rightCols<1>().setZero();
            }
        }
        return true;
    }

	// TODO: check if derived jacobian == perturbation on the raw function
    void check(double **param)
    {
        double *res = new double[1];
        double **jaco = new double *[3];
        jaco[0] = new double[1 * 7];
        jaco[1] = new double[1 * 7];
        jaco[2] = new double[1 * 7];
        Evaluate(param, res, jaco);
		std::cout << "[LidarPureOdomPlaneNormFactor] check begins" << std::endl;
        std::cout << "analytical:" << std::endl;

        std::cout << res[0] << std::endl;
        std::cout << Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> >(jaco[0]) << std::endl;
        std::cout << Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> >(jaco[1]) << std::endl;
        std::cout << Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> >(jaco[2]) << std::endl;

		delete[] jaco[0];
		delete[] jaco[1];
		delete[] jaco[2];
		delete[] jaco;
		delete[] res;

		Eigen::Quaterniond Q_pivot(param[0][6], param[0][3], param[0][4], param[0][5]);
		Eigen::Vector3d t_pivot(param[0][0], param[0][1], param[0][2]);
		Eigen::Quaterniond Q_i(param[1][6], param[1][3], param[1][4], param[1][5]);
		Eigen::Vector3d t_i(param[1][0], param[1][1], param[1][2]);
		Eigen::Quaterniond Q_ext(param[2][6], param[2][3], param[2][4], param[2][5]);
		Eigen::Vector3d t_ext(param[2][0], param[2][1], param[2][2]);

		Eigen::Quaterniond Q_pi = Q_pivot.conjugate() * Q_i;
		Eigen::Vector3d t_pi = Q_pivot.conjugate() * (t_i - t_pivot);
		Eigen::Quaterniond Q_ext_pi = Q_pi * Q_ext;
		Eigen::Vector3d t_ext_pi = Q_pi * t_ext + t_pi;

		Eigen::Vector3d w(coeff_(0), coeff_(1), coeff_(2));
        double d = coeff_(3);
		double r = w.dot(Q_ext_pi * point_ + t_ext_pi) + d;
        r *= sqrt_info_;

        std::cout << "perturbation:" << std::endl;
        std::cout << r << std::endl;

        const double eps = 1e-6;
        Eigen::Matrix<double, 1, 18> num_jacobian;

		// add random perturbation
		for (int k = 0; k < 18; k++)
		{
			Eigen::Quaterniond Q_pivot(param[0][6], param[0][3], param[0][4], param[0][5]);
			Eigen::Vector3d t_pivot(param[0][0], param[0][1], param[0][2]);
			Eigen::Quaterniond Q_i(param[1][6], param[1][3], param[1][4], param[1][5]);
			Eigen::Vector3d t_i(param[1][0], param[1][1], param[1][2]);
			Eigen::Quaterniond Q_ext(param[2][6], param[2][3], param[2][4], param[2][5]);
			Eigen::Vector3d t_ext(param[2][0], param[2][1], param[2][2]);
			int a = k / 3, b = k % 3;
			Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps;

			if (a == 0)
				t_pivot += delta;
			else if (a == 1)
				Q_pivot = Q_pivot * deltaQ(delta);
			else if (a == 2)
				t_i += delta;
			else if (a == 3)
				Q_i = Q_i * deltaQ(delta);
			else if (a == 4)
				t_ext += delta;
			else if (a == 5)
				Q_ext = Q_ext * deltaQ(delta);

			Eigen::Quaterniond Q_pi = Q_pivot.conjugate() * Q_i;
			Eigen::Vector3d t_pi = Q_pivot.conjugate() * (t_i - t_pivot);
			Eigen::Quaterniond Q_ext_pi = Q_pi * Q_ext;
			Eigen::Vector3d t_ext_pi = Q_pi * t_ext + t_pi;

			Eigen::Vector3d w(coeff_(0), coeff_(1), coeff_(2));
	        double d = coeff_(3);
			double tmp_r = w.dot(Q_ext_pi * point_ + t_ext_pi) + d;
	        tmp_r *= sqrt_info_;
            num_jacobian(k) = (tmp_r - r) / eps;
        }
        std::cout << num_jacobian.block<1, 6>(0, 0) << std::endl;
        std::cout << num_jacobian.block<1, 6>(0, 6) << std::endl;
        std::cout << num_jacobian.block<1, 6>(0, 12) << std::endl;
    }

private:
	const Eigen::Vector3d point_;
	const Eigen::Vector4d coeff_;
	const double sqrt_info_;
};

// pure odom planar factor
class LidarPureOdomEdgeFactor : public ceres::SizedCostFunction<1, 7, 7, 7>
{
public:
	LidarPureOdomEdgeFactor(const Eigen::Vector3d &point,
							const Eigen::VectorXd &coeff,
							const double &sqrt_info = 1.0)
		: point_(point),
		  coeff_(coeff),
		  sqrt_info_(sqrt_info) {}

	// residual = sum(w^(T) * (R * p + t) + d)
	bool Evaluate(double const *const *param, double *residuals, double **jacobians) const
    {
		Eigen::Quaterniond Q_pivot(param[0][6], param[0][3], param[0][4], param[0][5]);
		Eigen::Vector3d t_pivot(param[0][0], param[0][1], param[0][2]);
		Eigen::Quaterniond Q_i(param[1][6], param[1][3], param[1][4], param[1][5]);
		Eigen::Vector3d t_i(param[1][0], param[1][1], param[1][2]);
		Eigen::Quaterniond Q_ext(param[2][6], param[2][3], param[2][4], param[2][5]);
		Eigen::Vector3d t_ext(param[2][0], param[2][1], param[2][2]);

		Eigen::Quaterniond Q_pi = Q_pivot.conjugate() * Q_i;
		Eigen::Vector3d t_pi = Q_pivot.conjugate() * (t_i - t_pivot);
		Eigen::Quaterniond Q_ext_pi = Q_pi * Q_ext;
		Eigen::Vector3d t_ext_pi = Q_pi * t_ext + t_pi;

		Eigen::Vector3d lpa(coeff_(0), coeff_(1), coeff_(2));
		Eigen::Vector3d lpb(coeff_(3), coeff_(4), coeff_(5));
		Eigen::Vector3d lp = Q_ext_pi * point_ + t_ext_pi;

        Eigen::Vector3d nu = (lp - lpa).cross(lp - lpb);
        Eigen::Vector3d de = lpa - lpb;
        residuals[0] = sqrt_info_ * nu.norm() / de.norm();

		// jacobians: 1x21
        if (jacobians)
        {
            Eigen::Matrix3d Rp = Q_pivot.toRotationMatrix();
			Eigen::Matrix3d Ri = Q_i.toRotationMatrix();
            Eigen::Matrix3d Rext = Q_ext.toRotationMatrix();

			Eigen::Matrix<double, 1, 3> eta = 1.0 / de.norm() * nu.normalized().transpose();
			Eigen::Vector3d ba = lp - lpa;
			Eigen::Vector3d bb = lp - lpb;

            if (jacobians[0])
            {
                Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > jacobian_pose_pivot(jacobians[0]);

                Eigen::Matrix<double, 1, 6> jaco_pivot;
				jaco_pivot.leftCols<3>() = - eta * skewSymmetric(ba - bb) * Rp.transpose();
				jaco_pivot.rightCols<3>() = eta * skewSymmetric(ba - bb) *
					skewSymmetric(Rp.transpose() * (Ri * Rext * point_ + Ri * t_ext + t_i - t_pivot));

				jacobian_pose_pivot.setZero();
                jacobian_pose_pivot.leftCols<6>() = sqrt_info_ * jaco_pivot;
            }

            if (jacobians[1])
            {
                Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > jacobian_pose_i(jacobians[1]);

                Eigen::Matrix<double, 1, 6> jaco_i;
				jaco_i.leftCols<3>() = eta * skewSymmetric(ba - bb) * Rp.transpose();
				jaco_i.rightCols<3>() = - eta * skewSymmetric(ba - bb) * Rp.transpose() * Ri *
					skewSymmetric(Rext * point_ + t_ext);

				jacobian_pose_i.setZero();
                jacobian_pose_i.leftCols<6>() = sqrt_info_ * jaco_i;
            }

            if (jacobians[2])
            {
                Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > jacobian_pose_ex(jacobians[2]);

				Eigen::Matrix<double, 1, 6> jaco_ex;
				jaco_ex.leftCols<3>() = eta * skewSymmetric(ba - bb) * Rp.transpose() * Ri;
				jaco_ex.rightCols<3>() = - eta * skewSymmetric(ba - bb) * Rp.transpose() * Ri *
					(Rext * skewSymmetric(point_) + skewSymmetric(t_ext));

				jacobian_pose_ex.setZero();
                jacobian_pose_ex.leftCols<6>() = sqrt_info_ * jaco_ex;
            }
        }
        return true;
    }

	// TODO: check if derived jacobian == perturbation on the raw function
    void check(double **param)
    {
        double *res = new double[1];
        double **jaco = new double *[3];
        jaco[0] = new double[1 * 7];
        jaco[1] = new double[1 * 7];
        jaco[2] = new double[1 * 7];
        Evaluate(param, res, jaco);
		std::cout << "[LidarPureOdomEdgeFactor] check begins" << std::endl;
        std::cout << "analytical:" << std::endl;

        std::cout << res[0] << std::endl;
        std::cout << Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> >(jaco[0]) << std::endl;
        std::cout << Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> >(jaco[1]) << std::endl;
        std::cout << Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> >(jaco[2]) << std::endl;

		delete[] jaco[0];
		delete[] jaco[1];
		delete[] jaco[2];
		delete[] jaco;
		delete[] res;

		Eigen::Quaterniond Q_pivot(param[0][6], param[0][3], param[0][4], param[0][5]);
		Eigen::Vector3d t_pivot(param[0][0], param[0][1], param[0][2]);
		Eigen::Quaterniond Q_i(param[1][6], param[1][3], param[1][4], param[1][5]);
		Eigen::Vector3d t_i(param[1][0], param[1][1], param[1][2]);
		Eigen::Quaterniond Q_ext(param[2][6], param[2][3], param[2][4], param[2][5]);
		Eigen::Vector3d t_ext(param[2][0], param[2][1], param[2][2]);

		Eigen::Quaterniond Q_pi = Q_pivot.conjugate() * Q_i;
		Eigen::Vector3d t_pi = Q_pivot.conjugate() * (t_i - t_pivot);
		Eigen::Quaterniond Q_ext_pi = Q_pi * Q_ext;
		Eigen::Vector3d t_ext_pi = Q_pi * t_ext + t_pi;

		Eigen::Vector3d lpa(coeff_(0), coeff_(1), coeff_(2));
		Eigen::Vector3d lpb(coeff_(3), coeff_(4), coeff_(5));
		Eigen::Vector3d lp = Q_ext_pi * point_ + t_ext_pi;

        Eigen::Vector3d nu = (lp - lpa).cross(lp - lpb);
        Eigen::Vector3d de = lpa - lpb;
        double r = sqrt_info_ * nu.norm() / de.norm();

        std::cout << "perturbation:" << std::endl;
        std::cout << r << std::endl;

        const double eps = 1e-6;
        Eigen::Matrix<double, 1, 18> num_jacobian;

		// add random perturbation
		for (int k = 0; k < 18; k++)
		{
			Eigen::Quaterniond Q_pivot(param[0][6], param[0][3], param[0][4], param[0][5]);
			Eigen::Vector3d t_pivot(param[0][0], param[0][1], param[0][2]);
			Eigen::Quaterniond Q_i(param[1][6], param[1][3], param[1][4], param[1][5]);
			Eigen::Vector3d t_i(param[1][0], param[1][1], param[1][2]);
			Eigen::Quaterniond Q_ext(param[2][6], param[2][3], param[2][4], param[2][5]);
			Eigen::Vector3d t_ext(param[2][0], param[2][1], param[2][2]);
			int a = k / 3, b = k % 3;
			Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps;

			if (a == 0)
				t_pivot += delta;
			else if (a == 1)
				Q_pivot = Q_pivot * deltaQ(delta);
			else if (a == 2)
				t_i += delta;
			else if (a == 3)
				Q_i = Q_i * deltaQ(delta);
			else if (a == 4)
				t_ext += delta;
			else if (a == 5)
				Q_ext = Q_ext * deltaQ(delta);

			Eigen::Quaterniond Q_pi = Q_pivot.conjugate() * Q_i;
			Eigen::Vector3d t_pi = Q_pivot.conjugate() * (t_i - t_pivot);
			Eigen::Quaterniond Q_ext_pi = Q_pi * Q_ext;
			Eigen::Vector3d t_ext_pi = Q_pi * t_ext + t_pi;

			Eigen::Vector3d lpa(coeff_(0), coeff_(1), coeff_(2));
			Eigen::Vector3d lpb(coeff_(3), coeff_(4), coeff_(5));
			Eigen::Vector3d lp = Q_ext_pi * point_ + t_ext_pi;

			Eigen::Vector3d nu = (lp - lpa).cross(lp - lpb);
			Eigen::Vector3d de = lpa - lpb;
			double tmp_r = sqrt_info_ * nu.norm() / de.norm();
            num_jacobian(k) = (tmp_r - r) / eps;
        }
        std::cout << num_jacobian.block<1, 6>(0, 0) << std::endl;
        std::cout << num_jacobian.block<1, 6>(0, 6) << std::endl;
        std::cout << num_jacobian.block<1, 6>(0, 12) << std::endl;
    }

private:
	const Eigen::Vector3d point_;
	const Eigen::VectorXd coeff_;
	const double sqrt_info_;
};

// pure odom edge factor using auto diff
class LidarPureOdomEdgeFactorAuto
{
public:
	LidarPureOdomEdgeFactorAuto(const Eigen::Vector3d &point,
							    const Eigen::VectorXd &coeff,
							    const double &sqrt_info = 1.0)
		: point_(point),
		  coeff_(coeff),
		  sqrt_info_(sqrt_info) {}

	template <typename T>
	bool operator()(const T *param1, const T *param2, const T *param3, T *residuals) const
	{
		Eigen::Quaternion<T> Q_pivot(param1[6], param1[3], param1[4], param1[5]);
		Eigen::Matrix<T, 3, 1> t_pivot(param1[0], param1[1], param1[2]);
		Eigen::Quaternion<T> Q_i(param2[6], param2[3], param2[4], param2[5]);
		Eigen::Matrix<T, 3, 1> t_i(param2[0], param2[1], param2[2]);
		Eigen::Quaternion<T> Q_ext(param3[6], param3[3], param3[4], param3[5]);
		Eigen::Matrix<T, 3, 1> t_ext(param3[0], param3[1], param3[2]);

		Eigen::Quaternion<T> Q_pi = Q_pivot.conjugate() * Q_i;
		Eigen::Matrix<T, 3, 1> t_pi = Q_pivot.conjugate() * (t_i - t_pivot);
		Eigen::Quaternion<T> Q_ext_pi = Q_pi * Q_ext;
		Eigen::Matrix<T, 3, 1> t_ext_pi = Q_pi * t_ext + t_pi;

		Eigen::Matrix<T, 3, 1> cp(T(point_.x()), T(point_.y()), T(point_.z()));
		Eigen::Matrix<T, 3, 1> lpa(T(coeff_(0)), T(coeff_(1)), T(coeff_(2)));
		Eigen::Matrix<T, 3, 1> lpb(T(coeff_(3)), T(coeff_(4)), T(coeff_(5)));
		Eigen::Matrix<T, 3, 1> lp = Q_ext_pi * cp + t_ext_pi;

		Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);
		Eigen::Matrix<T, 3, 1> de = lpa - lpb;
		// residuals[0] = T(sqrt_info_) * nu.x() / de.norm();
		// residuals[1] = T(sqrt_info_) * nu.y() / de.norm();
		// residuals[2] = T(sqrt_info_) * nu.z() / de.norm();
		residuals[0] = T(sqrt_info_) * nu.norm() / de.norm();

		return true;
	}

	static ceres::CostFunction *Create(const Eigen::Vector3d &point,
									   const Eigen::VectorXd &coeff,
									   const double sqrt_info)
	{
		return (new ceres::AutoDiffCostFunction<
				LidarPureOdomEdgeFactorAuto, 1, 7, 7, 7>(
			new LidarPureOdomEdgeFactorAuto(point, coeff, sqrt_info)));
	}

private:
	const Eigen::Vector3d point_;
	const Eigen::VectorXd coeff_;
	const double sqrt_info_;
};

//


#include <cmath>

#include <functional>

#include <Eigen/Core>

#include "Tudat/Astrodynamics/Propulsion/costateBasedThrustGuidance.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/modifiedEquinoctialElementConversions.h"

namespace tudat
{

namespace propulsion
{

//! Constructor
MeeCostateBasedThrustGuidance::MeeCostateBasedThrustGuidance(
        const std::function< Eigen::Vector6d( ) > thrustingBodyStateFunction,
        const std::function< Eigen::Vector6d( ) > centralBodyStateFunction,
        const std::function< double( ) > centralBodyGravitationalParameterFunction,
        std::function< Eigen::VectorXd( const double ) > costateFunction,
        const std::function< Eigen::Vector3d( ) > bodyFixedForceDirection )
    : BodyFixedForceDirectionGuidance( bodyFixedForceDirection ),
      thrustingBodyStateFunction_( thrustingBodyStateFunction ),
      centralBodyStateFunction_( centralBodyStateFunction ),
      centralBodyGravitationalParameterFunction_( centralBodyGravitationalParameterFunction ),
      costateFunction_( costateFunction ){ }

//! Function to update the force direction to the current time.
void MeeCostateBasedThrustGuidance::updateForceDirection( const double time )
{
    if( !( time == currentTime_ ) )
    {
        Eigen::VectorXd costates_ = costateFunction_( time );

        // Get the current state in cartesian coordinates and keplerian elements, and some convenient parameters
        Eigen::Vector6d currentState = thrustingBodyStateFunction_( ) - centralBodyStateFunction_( );
        double centralBodyGravitationalParameter = centralBodyGravitationalParameterFunction_( );

        // Obtain ModifiedEquinoctial elements, flag of 0 indicates that singularity occurs at 180 deg inclination.
        Eigen::Vector6d modifiedEquinoctialElements =
                orbital_element_conversions::convertCartesianToModifiedEquinoctialElements(
                    currentState, centralBodyGravitationalParameter, 0 );

        // Optimal control laws local variables declared for clarity
        double auxiliaryParameterW = ( 1.0 + modifiedEquinoctialElements( 1 ) * cos( modifiedEquinoctialElements( 5 ) )
                     + modifiedEquinoctialElements( 2 ) * sin( modifiedEquinoctialElements( 5 ) ) );
        double auxiliaryParameterSSquared = 1.0 + modifiedEquinoctialElements( 3 ) * modifiedEquinoctialElements( 3 )
                + modifiedEquinoctialElements( 4 ) * modifiedEquinoctialElements( 4 );

        // Local variables for al constant terms for the calculation of pitch angle
        double Lap = costates_( 0 ) * 2.0 * modifiedEquinoctialElements( 0 ) / auxiliaryParameterW;
        double Laf1 = costates_( 1 )  * sin( modifiedEquinoctialElements( 5 ) ) ;
        double Laf2 = costates_( 1 ) / auxiliaryParameterW *
                ( ( auxiliaryParameterW + 1.0 ) * cos( modifiedEquinoctialElements( 5 ) )
                                             + modifiedEquinoctialElements( 1 ) ) ;
        double Lag1 = costates_( 2 ) * cos( modifiedEquinoctialElements( 5 ) );
        double Lag2 = costates_( 2 ) / auxiliaryParameterW *
                ( ( auxiliaryParameterW + 1.0 ) * sin( modifiedEquinoctialElements( 5 ) )
                                             + modifiedEquinoctialElements( 2 ) );

        // Calculate pitch angle, NOTE: denomitator ommitted since it is not relevant for the atan2 function,
        // since both denominators are the same.
        double thrustAngleAlpha = std::atan2( -Laf1+Lag1, -Lap-Laf2-Lag2);

        // Local variables for al constant terms for the calculation of yaw angle
        double Lbp = costates_( 0 ) * 2.0 * modifiedEquinoctialElements( 0 ) * cos( thrustAngleAlpha) / auxiliaryParameterW;
        double Lbf1 = costates_( 1 )  * sin( modifiedEquinoctialElements( 5 ) ) * sin( thrustAngleAlpha );
        double Lbf2 = costates_( 1 ) / auxiliaryParameterW *
                ( ( auxiliaryParameterW + 1.0 ) * cos( modifiedEquinoctialElements( 5 ) )
                                             + modifiedEquinoctialElements( 1 ) ) * cos( thrustAngleAlpha );
        double Lbf3 = costates_( 1 ) / auxiliaryParameterW * ( modifiedEquinoctialElements( 2 ) *(
                                                 modifiedEquinoctialElements( 3 ) * sin( modifiedEquinoctialElements( 5 ) )
                                                 - modifiedEquinoctialElements( 4 ) * cos( modifiedEquinoctialElements( 5 ) ) ) );

        double Lbg1 = costates_( 2 ) * cos( modifiedEquinoctialElements( 5 ) ) * sin( thrustAngleAlpha);
        double Lbg2 = costates_( 2 ) / auxiliaryParameterW *
                ( ( auxiliaryParameterW + 1.0 ) * sin( modifiedEquinoctialElements( 5 ) )
                                             + modifiedEquinoctialElements( 2 ) ) * cos( thrustAngleAlpha);
        double Lbg3 = costates_( 2 ) / auxiliaryParameterW * ( modifiedEquinoctialElements( 1 ) *(
                                                 modifiedEquinoctialElements( 3 ) * sin( modifiedEquinoctialElements( 5 ) )
                                                 - modifiedEquinoctialElements( 4 ) * cos( modifiedEquinoctialElements( 5 ) ) ) );
        double Lbh = costates_( 3 ) * auxiliaryParameterSSquared *
                cos( modifiedEquinoctialElements( 5 ) ) / ( 2.0 * auxiliaryParameterW );
        double Lbk = costates_( 4 ) * auxiliaryParameterSSquared *
                sin( modifiedEquinoctialElements( 5 ) ) / ( 2.0 * auxiliaryParameterW );

        // Calculate yaw angle, NOTE: denomitator ommitted since it is not relevant for the atan2 function,
        // since both denominators are the same.
        double thrustAngleBeta = std::atan2( Lbf3 - Lbg3 - Lbh - Lbk, - Lbp - Lbf1 - Lbf2 + Lbg1 - Lbg2 );

        // Calculate thrust direction
        currentForceDirection_ = reference_frames::getVelocityBasedLvlhToInertialRotation(
                    currentState, Eigen::Vector6d::Zero( ), false ) *
                ( ( Eigen::Vector3d( ) <<
                    cos( thrustAngleAlpha ) * cos( thrustAngleBeta ), sin( thrustAngleAlpha ) * cos( thrustAngleBeta ) ,
                    sin( thrustAngleBeta )  ).finished( ).normalized( ) );
        currentTime_ = time;
    }

}

} // namespace propulsion

} // namespace tudat


#include <string>
#include <sstream>
#include <iostream>
#include <map>
#include <regex>
#include <fstream>
#include <boost/algorithm/string.hpp>

using namespace std;

int seat_id(string line) {
    boost::trim(line);

    int row_min = 0;
    int row_max = 127;
    int col_min = 0;
    int col_max = 7;
    for (char c : line) {
        if (c == 'F') {
            row_max = row_min + (row_max - row_min) / 2;
        } else if (c == 'B') {
            row_min = row_min + (row_max - row_min) / 2 + 1;
        } else if (c == 'L') {
            col_max = col_min + (col_max - col_min) / 2;
        } else if (c == 'R') {
            col_min = col_min + (col_max - col_min) / 2 + 1;
        }
    }

    return row_min * 8 + col_min;
}

int main() {
    ifstream file ("2020/5.txt");
    if (!file.is_open()) {
        cout << "Failed to open file: " << strerror(errno) << endl;
        return -1;
    }

    map<int, bool> found;
    int max_id = 0;

    string line;
    while (getline(file, line, '\n')) {
        int id = seat_id(line);

        if (id > max_id) {
            max_id = id;
        }
        found[id] = true;

        //cout << line << " " << id << endl;
    }

    cout << "Answer 5.1: " << max_id << endl;

    for (int i = 0; i < max_id; i++) {
        if (!found[i] && found[i - 1] && found[i + 1]) {
            cout << "Answer 5.2: " << i << endl;
            break;
        }
    }

    file.close();
}


// Copyright (C) 2008  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "optimization_test_functions.h"
#include <dlib/optimization.h>
#include <dlib/statistics.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>
#include "../stl_checked.h"
#include "../array.h"
#include "../rand.h"

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.optimization");

// ----------------------------------------------------------------------------------------

    bool approx_equal (
        double a,
        double b
    )
    {
        return std::abs(a - b) < 100*std::numeric_limits<double>::epsilon();
    }

// ----------------------------------------------------------------------------------------

    long total_count = 0;

    template <typename T>
    double apq ( const T& x)
    {
        DLIB_ASSERT(x.nr() > 1 && x.nc() == 1,"");
        COMPILE_TIME_ASSERT(is_matrix<T>::value);
        double temp = 0;
        for (long r = 0; r < x.nr(); ++r)
        {
            temp += (r+1)*x(r)*x(r);
        }

        ++total_count;

        return temp + 1/100.0*(x(0) + x(x.nr()-1))*(x(0) + x(x.nr()-1));
    }

    template <typename T>
    T der_apq ( const T& x)
    {
        DLIB_ASSERT(x.nr() > 1 && x.nc() == 1,"");
        COMPILE_TIME_ASSERT(is_matrix<T>::value);
        T temp(x.nr());
        for (long r = 0; r < x.nr(); ++r)
        {
            temp(r) = 2*(r+1)*x(r) ;
        }

        temp(0) += 1/50.0*(x(0) + x(x.nr()-1));
        temp(x.nr()-1) += 1/50.0*(x(0) + x(x.nr()-1));

        ++total_count;

        return temp;
    }

// ----------------------------------------------------------------------------------------

    // Rosenbrock's function.  minimum at (1,1)
    double rosen ( const matrix<double,2,1>& x)
    {
        ++total_count;
        return 100*pow(x(1) - x(0)*x(0),2) + pow(1 - x(0),2);
    }

    matrix<double,2,1> der_rosen ( const matrix<double,2,1>& x)
    {
        ++total_count;
        matrix<double,2,1> res;
        res(0) = -400*x(0)*(x(1)-x(0)*x(0)) - 2*(1-x(0));
        res(1) = 200*(x(1)-x(0)*x(0));
        return res;
    }

// ----------------------------------------------------------------------------------------

    // negative of Rosenbrock's function.  minimum at (1,1)
    double neg_rosen ( const matrix<double,2,1>& x)
    {
        ++total_count;
        return -(100*pow(x(1) - x(0)*x(0),2) + pow(1 - x(0),2));
    }

    matrix<double,2,1> der_neg_rosen ( const matrix<double,2,1>& x)
    {
        ++total_count;
        matrix<double,2,1> res;
        res(0) = -400*x(0)*(x(1)-x(0)*x(0)) - 2*(1-x(0));
        res(1) = 200*(x(1)-x(0)*x(0));
        return -res;
    }

// ----------------------------------------------------------------------------------------

    double simple ( const matrix<double,2,1>& x)
    {
        ++total_count;
        return 10*x(0)*x(0) + x(1)*x(1);
    }

    matrix<double,2,1> der_simple ( const matrix<double,2,1>& x)
    {
        ++total_count;
        matrix<double,2,1> res;
        res(0) = 20*x(0);
        res(1) = 2*x(1);
        return res;
    }

// ----------------------------------------------------------------------------------------

    double powell ( const matrix<double,4,1>& x)
    {
        ++total_count;
        return pow(x(0) + 10*x(1),2) +
            pow(std::sqrt(5.0)*(x(2) - x(3)),2) +
            pow((x(1) - 2*x(2))*(x(1) - 2*x(2)),2) +
            pow(std::sqrt(10.0)*(x(0) - x(3))*(x(0) - x(3)),2);
    }

// ----------------------------------------------------------------------------------------

// a simple function with a minimum at zero
    double single_variable_function ( double x)
    {
        ++total_count;
        return 3*x*x + 5;
    }

// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------

    void test_apq (
        const matrix<double,0,1> p
    )
    {
        typedef matrix<double,0,1> T;
        const double eps = 1e-12;
        const double minf = -10;
        matrix<double,0,1> x(p.nr()), opt(p.nr());
        set_all_elements(opt, 0);
        double val = 0;

        if (p.size() < 20)
            dlog << LINFO << "testing with apq and the start point: " << trans(p);
        else
            dlog << LINFO << "testing with apq and a big vector with " << p.size() << " components.";

        // don't use bfgs on really large vectors
        if (p.size() < 20)
        {
            total_count = 0;
            x = p;
            val = find_min(bfgs_search_strategy(),
                     objective_delta_stop_strategy(eps),
                     wrap_function(apq<T>), wrap_function(der_apq<T>), x, minf);
            DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
            DLIB_TEST(approx_equal(val , apq(x)));
            dlog << LINFO << "find_min() bgfs: got apq in " << total_count;

            total_count = 0;
            x = p;
            find_min(bfgs_search_strategy(),
                     gradient_norm_stop_strategy(),
                     wrap_function(apq<T>), wrap_function(der_apq<T>), x, minf);
            DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
            dlog << LINFO << "find_min() bgfs(gn): got apq in " << total_count;
        }

        if (p.size() < 100)
        {
            total_count = 0;
            x = p;
            val=find_min_bobyqa(wrap_function(apq<T>), x, 2*x.size()+1,
                            uniform_matrix<double>(x.size(),1,-1e100),
                            uniform_matrix<double>(x.size(),1,1e100),
                            (max(abs(x))+1)/10,
                            1e-6,
                            10000);
            DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
            DLIB_TEST(approx_equal(val , apq(x)));
            dlog << LINFO << "find_min_bobyqa(): got apq in " << total_count;
        }

        total_count = 0;
        x = p;
        val=find_min(lbfgs_search_strategy(10),
                 objective_delta_stop_strategy(eps),
                 wrap_function(apq<T>), wrap_function(der_apq<T>), x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
        DLIB_TEST(approx_equal(val , apq(x)));
        dlog << LINFO << "find_min() lbgfs-10: got apq in " << total_count;

        total_count = 0;
        x = p;
        val=find_min(lbfgs_search_strategy(1),
                 objective_delta_stop_strategy(eps),
                 wrap_function(apq<T>), wrap_function(der_apq<T>), x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
        DLIB_TEST(approx_equal(val , apq(x)));
        dlog << LINFO << "find_min() lbgfs-1: got apq in " << total_count;

        total_count = 0;
        x = p;
        val=find_min(cg_search_strategy(),
                 objective_delta_stop_strategy(eps),
                 wrap_function(apq<T>), wrap_function(der_apq<T>), x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
        DLIB_TEST(approx_equal(val , apq(x)));
        dlog << LINFO << "find_min() cg: got apq in " << total_count;

        // don't do approximate derivative tests if the input point is really long
        if (p.size() < 20)
        {
            total_count = 0;
            x = p;
            val=find_min(bfgs_search_strategy(),
                     objective_delta_stop_strategy(eps),
                     wrap_function(apq<T>), derivative(wrap_function(apq<T>)), x, minf);
            DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
            DLIB_TEST(approx_equal(val , apq(x)));
            dlog << LINFO << "find_min() bfgs: got apq/noder in " << total_count;

            total_count = 0;
            x = p;
            val=find_min(cg_search_strategy(),
                     objective_delta_stop_strategy(eps),
                     wrap_function(apq<T>), derivative(wrap_function(apq<T>)), x, minf);
            DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
            DLIB_TEST(approx_equal(val , apq(x)));
            dlog << LINFO << "find_min() cg: got apq/noder in " << total_count;

            total_count = 0;
            x = p;
            val=find_min_using_approximate_derivatives(bfgs_search_strategy(),
                                                   objective_delta_stop_strategy(eps),
                                                   wrap_function(apq<T>), x, minf);
            DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
            DLIB_TEST(approx_equal(val , apq(x)));
            dlog << LINFO << "find_min() bfgs: got apq/noder2 in " << total_count;

            total_count = 0;
            x = p;
            val=find_min_using_approximate_derivatives(lbfgs_search_strategy(10),
                                                   objective_delta_stop_strategy(eps),
                                                   wrap_function(apq<T>), x, minf);
            DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
            dlog << LINFO << "find_min() lbfgs-10: got apq/noder2 in " << total_count;

            total_count = 0;
            x = p;
            val=find_min_using_approximate_derivatives(cg_search_strategy(),
                                                   objective_delta_stop_strategy(eps),
                                                   wrap_function(apq<T>), x, minf);
            DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
            DLIB_TEST(approx_equal(val , apq(x)));
            dlog << LINFO << "find_min() cg: got apq/noder2 in " << total_count;
        }
    }

    void test_powell (
        const matrix<double,4,1> p
    )
    {
        const double eps = 1e-15;
        const double minf = -1;
        matrix<double,4,1> x, opt;
        opt(0) = 0;
        opt(1) = 0;
        opt(2) = 0;
        opt(3) = 0;

        double val = 0;

        dlog << LINFO << "testing with powell and the start point: " << trans(p);

        /*
        total_count = 0;
        x = p;
        val=find_min(bfgs_search_strategy(),
                 objective_delta_stop_strategy(eps),
                 powell, derivative(powell,1e-8), x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-2),opt-x);
        DLIB_TEST(approx_equal(val , powell(x)));
        dlog << LINFO << "find_min() bfgs: got powell/noder in " << total_count;

        total_count = 0;
        x = p;
        val=find_min(cg_search_strategy(),
                 objective_delta_stop_strategy(eps),
                 powell, derivative(powell,1e-9), x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-2),opt-x);
        DLIB_TEST(approx_equal(val , powell(x)));
        dlog << LINFO << "find_min() cg: got powell/noder in " << total_count;
        */

        total_count = 0;
        x = p;
        val=find_min_using_approximate_derivatives(bfgs_search_strategy(),
                                               objective_delta_stop_strategy(eps),
                                               powell, x, minf, 1e-10);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-1),opt-x);
        DLIB_TEST(approx_equal(val , powell(x)));
        dlog << LINFO << "find_min() bfgs: got powell/noder2 in " << total_count;

        total_count = 0;
        x = p;
        val=find_min_using_approximate_derivatives(lbfgs_search_strategy(4),
                                               objective_delta_stop_strategy(eps),
                                               powell, x, minf, 1e-10);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-1),opt-x);
        DLIB_TEST(approx_equal(val , powell(x)));
        dlog << LINFO << "find_min() lbfgs-4: got powell/noder2 in " << total_count;

        total_count = 0;
        x = p;
        val=find_min_using_approximate_derivatives(lbfgs_search_strategy(4),
                                               gradient_norm_stop_strategy(),
                                               powell, x, minf, 1e-10);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-1),opt-x);
        DLIB_TEST(approx_equal(val , powell(x)));
        dlog << LINFO << "find_min() lbfgs-4(gn): got powell/noder2 in " << total_count;

        total_count = 0;
        x = p;
        val=find_min_using_approximate_derivatives(cg_search_strategy(),
                                               objective_delta_stop_strategy(eps),
                                               powell, x, minf, 1e-10);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-1),opt-x);
        DLIB_TEST(approx_equal(val , powell(x)));
        dlog << LINFO << "find_min() cg: got powell/noder2 in " << total_count;

        total_count = 0;
        x = p;
        val=find_min_bobyqa(powell, x, 2*x.size()+1,
                        uniform_matrix<double>(x.size(),1,-1e100),
                        uniform_matrix<double>(x.size(),1,1e100),
                        (max(abs(x))+1)/10,
                        1e-8,
                        10000);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-3),opt-x);
        DLIB_TEST(approx_equal(val , powell(x)));
        dlog << LINFO << "find_min_bobyqa(): got powell in " << total_count;

    }

    void test_simple (
        const matrix<double,2,1> p
    )
    {
        const double eps = 1e-12;
        const double minf = -10000;
        matrix<double,2,1> x, opt;
        opt(0) = 0;
        opt(1) = 0;
        double val = 0;

        dlog << LINFO << "testing with simple and the start point: " << trans(p);

        total_count = 0;
        x = p;
        val=find_min(bfgs_search_strategy(),
                 objective_delta_stop_strategy(eps),
                 simple, der_simple, x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
        DLIB_TEST(approx_equal(val , simple(x)));
        dlog << LINFO << "find_min() bfgs: got simple in " << total_count;

        total_count = 0;
        x = p;
        val=find_min(bfgs_search_strategy(),
                 gradient_norm_stop_strategy(),
                 simple, der_simple, x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
        DLIB_TEST(approx_equal(val , simple(x)));
        dlog << LINFO << "find_min() bfgs(gn): got simple in " << total_count;

        total_count = 0;
        x = p;
        val=find_min(lbfgs_search_strategy(3),
                 objective_delta_stop_strategy(eps),
                 simple, der_simple, x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
        DLIB_TEST(approx_equal(val , simple(x)));
        dlog << LINFO << "find_min() lbfgs-3: got simple in " << total_count;

        total_count = 0;
        x = p;
        val=find_min(cg_search_strategy(),
                 objective_delta_stop_strategy(eps),
                 simple, der_simple, x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
        DLIB_TEST(approx_equal(val , simple(x)));
        dlog << LINFO << "find_min() cg: got simple in " << total_count;

        total_count = 0;
        x = p;
        val=find_min(bfgs_search_strategy(),
                 objective_delta_stop_strategy(eps),
                 simple, derivative(simple), x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
        DLIB_TEST(approx_equal(val , simple(x)));
        dlog << LINFO << "find_min() bfgs: got simple/noder in " << total_count;

        total_count = 0;
        x = p;
        val=find_min(lbfgs_search_strategy(8),
                 objective_delta_stop_strategy(eps),
                 simple, derivative(simple), x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
        DLIB_TEST(approx_equal(val , simple(x)));
        dlog << LINFO << "find_min() lbfgs-8: got simple/noder in " << total_count;

        total_count = 0;
        x = p;
        val=find_min(cg_search_strategy(),
                 objective_delta_stop_strategy(eps),
                 simple, derivative(simple), x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
        DLIB_TEST(approx_equal(val , simple(x)));
        dlog << LINFO << "find_min() cg: got simple/noder in " << total_count;

        total_count = 0;
        x = p;
        val=find_min_using_approximate_derivatives(bfgs_search_strategy(),
                                               objective_delta_stop_strategy(eps),
                                               simple, x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
        DLIB_TEST(approx_equal(val , simple(x)));
        dlog << LINFO << "find_min() bfgs: got simple/noder2 in " << total_count;

        total_count = 0;
        x = p;
        val=find_min_using_approximate_derivatives(lbfgs_search_strategy(6),
                                               objective_delta_stop_strategy(eps),
                                               simple, x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
        DLIB_TEST(approx_equal(val , simple(x)));
        dlog << LINFO << "find_min() lbfgs-6: got simple/noder2 in " << total_count;

        total_count = 0;
        x = p;
        val=find_min_using_approximate_derivatives(cg_search_strategy(),
                                               objective_delta_stop_strategy(eps),
                                               simple, x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
        DLIB_TEST(approx_equal(val , simple(x)));
        dlog << LINFO << "find_min() cg: got simple/noder2 in " << total_count;

        total_count = 0;
        x = p;
        val=find_min_bobyqa(simple, x, 2*x.size()+1,
                        uniform_matrix<double>(x.size(),1,-1e100),
                        uniform_matrix<double>(x.size(),1,1e100),
                        (max(abs(x))+1)/10,
                        1e-6,
                        10000);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
        DLIB_TEST(approx_equal(val , simple(x)));
        dlog << LINFO << "find_min_bobyqa(): got simple in " << total_count;

    }

    void test_rosen (
        const matrix<double,2,1> p
    )
    {
        const double eps = 1e-15;
        const double minf = -10;
        matrix<double,2,1> x, opt;
        opt(0) = 1;
        opt(1) = 1;

        double val = 0;

        dlog << LINFO << "testing with rosen and the start point: " << trans(p);

        total_count = 0;
        x = p;
        val=find_min(bfgs_search_strategy(),
                 objective_delta_stop_strategy(eps),
                 rosen, der_rosen, x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-7),opt-x);
        DLIB_TEST(approx_equal(val , rosen(x)));
        dlog << LINFO << "find_min() bfgs: got rosen in " << total_count;

        total_count = 0;
        x = p;
        val=find_min(bfgs_search_strategy(),
                 gradient_norm_stop_strategy(),
                 rosen, der_rosen, x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-7),opt-x);
        DLIB_TEST(approx_equal(val , rosen(x)));
        dlog << LINFO << "find_min() bfgs(gn): got rosen in " << total_count;

        total_count = 0;
        x = p;
        val=find_min(lbfgs_search_strategy(20),
                 objective_delta_stop_strategy(eps),
                 rosen, der_rosen, x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-7),opt-x);
        DLIB_TEST(approx_equal(val , rosen(x)));
        dlog << LINFO << "find_min() lbfgs-20: got rosen in " << total_count;

        total_count = 0;
        x = p;
        val=find_min(cg_search_strategy(),
                 objective_delta_stop_strategy(eps),
                 rosen, der_rosen, x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-7),opt-x);
        DLIB_TEST(approx_equal(val , rosen(x)));
        dlog << LINFO << "find_min() cg: got rosen in " << total_count;

        total_count = 0;
        x = p;
        val=find_min(bfgs_search_strategy(),
                 objective_delta_stop_strategy(eps),
                 rosen, derivative(rosen,1e-5), x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-4),opt-x);
        DLIB_TEST(approx_equal(val , rosen(x)));
        dlog << LINFO << "find_min() bfgs: got rosen/noder in " << total_count;

        total_count = 0;
        x = p;
        val=find_min(lbfgs_search_strategy(5),
                 objective_delta_stop_strategy(eps),
                 rosen, derivative(rosen,1e-5), x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-4),opt-x);
        DLIB_TEST(approx_equal(val , rosen(x)));
        dlog << LINFO << "find_min() lbfgs-5: got rosen/noder in " << total_count;

        total_count = 0;
        x = p;
        val=find_min(cg_search_strategy(),
                 objective_delta_stop_strategy(eps),
                 rosen, derivative(rosen,1e-5), x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-4),opt-x);
        DLIB_TEST(approx_equal(val , rosen(x)));
        dlog << LINFO << "find_min() cg: got rosen/noder in " << total_count;

        total_count = 0;
        x = p;
        val=find_min_using_approximate_derivatives(cg_search_strategy(),
                                               objective_delta_stop_strategy(eps),
                                               rosen, x, minf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-4),opt-x);
        DLIB_TEST(approx_equal(val , rosen(x)));
        dlog << LINFO << "find_min() cg: got rosen/noder2 in " << total_count;

        if (max(abs(p)) < 1000)
        {
            total_count = 0;
            x = p;
            val=find_min_bobyqa(rosen, x, 2*x.size()+1,
                            uniform_matrix<double>(x.size(),1,-1e100),
                            uniform_matrix<double>(x.size(),1,1e100),
                            (max(abs(x))+1)/10,
                            1e-6,
                            10000);
            DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
            DLIB_TEST(approx_equal(val , rosen(x)));
            dlog << LINFO << "find_min_bobyqa(): got rosen in " << total_count;
        }
    }

    void test_neg_rosen (
        const matrix<double,2,1> p
    )
    {
        const double eps = 1e-15;
        const double maxf = 10;
        matrix<double,2,1> x, opt;
        opt(0) = 1;
        opt(1) = 1;

        double val = 0;

        dlog << LINFO << "testing with neg_rosen and the start point: " << trans(p);

        total_count = 0;
        x = p;
        val=find_max(
            bfgs_search_strategy(),
            objective_delta_stop_strategy(eps), neg_rosen, der_neg_rosen, x, maxf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-7),opt-x);
        DLIB_TEST(approx_equal(val , neg_rosen(x)));
        dlog << LINFO << "find_max() bfgs: got neg_rosen in " << total_count;

        total_count = 0;
        x = p;
        val=find_max(
            lbfgs_search_strategy(5),
            objective_delta_stop_strategy(eps), neg_rosen, der_neg_rosen, x, maxf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-7),opt-x);
        DLIB_TEST(approx_equal(val , neg_rosen(x)));
        dlog << LINFO << "find_max() lbfgs-5: got neg_rosen in " << total_count;

        total_count = 0;
        x = p;
        val=find_max(
            lbfgs_search_strategy(5),
            objective_delta_stop_strategy(eps), neg_rosen, derivative(neg_rosen), x, maxf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-7),opt-x);
        DLIB_TEST(approx_equal(val , neg_rosen(x)));
        dlog << LINFO << "find_max() lbfgs-5: got neg_rosen/noder in " << total_count;

        total_count = 0;
        x = p;
        val=find_max_using_approximate_derivatives(
            cg_search_strategy(),
            objective_delta_stop_strategy(eps), neg_rosen, x, maxf);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-7),opt-x);
        DLIB_TEST(approx_equal(val , neg_rosen(x)));
        dlog << LINFO << "find_max() cg: got neg_rosen/noder2 in " << total_count;

        total_count = 0;
        x = p;
        val=find_max_bobyqa(neg_rosen, x, 2*x.size()+1,
                        uniform_matrix<double>(x.size(),1,-1e100),
                        uniform_matrix<double>(x.size(),1,1e100),
                        (max(abs(x))+1)/10,
                        1e-6,
                        10000);
        DLIB_TEST_MSG(dlib::equal(x,opt, 1e-5),opt-x);
        DLIB_TEST(approx_equal(val , neg_rosen(x)));
        dlog << LINFO << "find_max_bobyqa(): got neg_rosen in " << total_count;
    }

// ----------------------------------------------------------------------------------------

    void test_single_variable_function (
        const double p
    )
    {
        const double eps = 1e-7;

        dlog << LINFO << "testing with single_variable_function and the start point: " << p;
        double out, x;

        total_count = 0;
        x = p;
        out = find_min_single_variable(single_variable_function, x, -1e100, 1e100, eps, 1000);
        DLIB_TEST_MSG(std::abs(out-5) < 1e-6, out-5);
        DLIB_TEST_MSG(std::abs(x) < 1e-6, x);
        dlog << LINFO << "find_min_single_variable(): got single_variable_function in " << total_count;

        total_count = 0;
        x = p;
        out = -find_max_single_variable(negate_function(single_variable_function), x, -1e100, 1e100, eps, 1000);
        DLIB_TEST_MSG(std::abs(out-5) < 1e-6, out-5);
        DLIB_TEST_MSG(std::abs(x) < 1e-6, x);
        dlog << LINFO << "find_max_single_variable(): got single_variable_function in " << total_count;

        if (p > 0)
        {
            total_count = 0;
            x = p;
            out = find_min_single_variable(single_variable_function, x, -1e-4, 1e100, eps, 1000);
            DLIB_TEST_MSG(std::abs(out-5) < 1e-6, out-5);
            DLIB_TEST_MSG(std::abs(x) < 1e-6, x);
            dlog << LINFO << "find_min_single_variable(): got single_variable_function in " << total_count;

            if (p > 3)
            {
                total_count = 0;
                x = p;
                out = -find_max_single_variable(negate_function(single_variable_function), x, 3, 1e100, eps, 1000);
                DLIB_TEST_MSG(std::abs(out - (3*3*3+5)) < 1e-6, out-(3*3*3+5));
                DLIB_TEST_MSG(std::abs(x-3) < 1e-6, x);
                dlog << LINFO << "find_max_single_variable(): got single_variable_function in " << total_count;
            }
        }

        if (p < 0)
        {
            total_count = 0;
            x = p;
            out = find_min_single_variable(single_variable_function, x, -1e100, 1e-4, eps, 1000);
            DLIB_TEST_MSG(std::abs(out-5) < 1e-6, out-5);
            DLIB_TEST_MSG(std::abs(x) < 1e-6, x);
            dlog << LINFO << "find_min_single_variable(): got single_variable_function in " << total_count;

            if (p < -3)
            {
                total_count = 0;
                x = p;
                out = find_min_single_variable(single_variable_function, x, -1e100, -3, eps, 1000);
                DLIB_TEST_MSG(std::abs(out - (3*3*3+5)) < 1e-6, out-(3*3*3+5));
                DLIB_TEST_MSG(std::abs(x+3) < 1e-6, x);
                dlog << LINFO << "find_min_single_variable(): got single_variable_function in " << total_count;
            }
        }

    }

// ----------------------------------------------------------------------------------------

    void optimization_test (
    )
    /*!
        ensures
            - runs tests on the optimization stuff compliance with the specs
    !*/
    {
        matrix<double,0,1> p;

        print_spinner();

        p.set_size(2);

        // test with single_variable_function
        test_single_variable_function(0);
        test_single_variable_function(1);
        test_single_variable_function(-10);
        test_single_variable_function(-100);
        test_single_variable_function(900.53);

        // test with the rosen function
        p(0) = 9;
        p(1) = -4.9;
        test_rosen(p);
        test_neg_rosen(p);

        p(0) = 0;
        p(1) = 0;
        test_rosen(p);

        p(0) = 5323;
        p(1) = 98248;
        test_rosen(p);

        // test with the simple function
        p(0) = 1;
        p(1) = 1;
        test_simple(p);

        p(0) = 0.5;
        p(1) = -9;
        test_simple(p);

        p(0) = 645;
        p(1) = 839485;
        test_simple(p);

        print_spinner();

        // test with the apq function
        p.set_size(5);

        p(0) = 1;
        p(1) = 1;
        p(2) = 1;
        p(3) = 1;
        p(4) = 1;
        test_apq(p);

        p(0) = 1;
        p(1) = 2;
        p(2) = 3;
        p(3) = 4;
        p(4) = 5;
        test_apq(p);

        p(0) = 1;
        p(1) = 2;
        p(2) = -3;
        p(3) = 4;
        p(4) = 5;
        test_apq(p);

        print_spinner();

        p(0) = 1;
        p(1) = 2324;
        p(2) = -3;
        p(3) = 4;
        p(4) = 534534;
        test_apq(p);

        p.set_size(10);
        p(0) = 1;
        p(1) = 2;
        p(2) = -3;
        p(3) = 4;
        p(4) = 5;
        p(5) = 1;
        p(6) = 2;
        p(7) = -3;
        p(8) = 4;
        p(9) = 5;
        test_apq(p);

        // test apq with a big vector
        p.set_size(500);
        dlib::rand rnd;
        for (long i = 0; i < p.size(); ++i)
        {
            p(i) = rnd.get_random_double()*20 - 10;
        }
        test_apq(p);

        print_spinner();

        // test with the powell function
        p.set_size(4);

        p(0) = 3;
        p(1) = -1;
        p(2) = 0;
        p(3) = 1;
        test_powell(p);

        {
            matrix<double,2,1> m;
            m(0) = -0.43;
            m(1) = 0.919;
            DLIB_TEST(dlib::equal(der_rosen(m) , derivative(rosen)(m),1e-5));

            DLIB_TEST_MSG(std::abs(derivative(make_line_search_function(rosen,m,m))(0) -
                                  make_line_search_function(derivative(rosen),m,m)(0)) < 1e-5,"");
            DLIB_TEST_MSG(std::abs(derivative(make_line_search_function(rosen,m,m))(1) -
                                  make_line_search_function(derivative(rosen),m,m)(1)) < 1e-5,"");

            DLIB_TEST_MSG(std::abs(derivative(make_line_search_function(rosen,m,m))(0) -
                                  make_line_search_function(der_rosen,m,m)(0)) < 1e-5,"");
            DLIB_TEST_MSG(std::abs(derivative(make_line_search_function(rosen,m,m))(1) -
                                  make_line_search_function(der_rosen,m,m)(1)) < 1e-5,"");
        }
        {
            matrix<double,2,1> m;
            m(0) = 1;
            m(1) = 2;
            DLIB_TEST(dlib::equal(der_rosen(m) , derivative(rosen)(m),1e-5));

            DLIB_TEST_MSG(std::abs(derivative(make_line_search_function(rosen,m,m))(0) -
                                  make_line_search_function(derivative(rosen),m,m)(0)) < 1e-5,"");
            DLIB_TEST_MSG(std::abs(derivative(make_line_search_function(rosen,m,m))(1) -
                                  make_line_search_function(derivative(rosen),m,m)(1)) < 1e-5,"");

            DLIB_TEST_MSG(std::abs(derivative(make_line_search_function(rosen,m,m))(0) -
                                  make_line_search_function(der_rosen,m,m)(0)) < 1e-5,"");
            DLIB_TEST_MSG(std::abs(derivative(make_line_search_function(rosen,m,m))(1) -
                                  make_line_search_function(der_rosen,m,m)(1)) < 1e-5,"");
        }

        {
            matrix<double,2,1> m;
            m = 1,2;
            DLIB_TEST(std::abs(neg_rosen(m) - negate_function(rosen)(m) ) < 1e-16);
        }

    }

    template <typename der_funct, typename T>
    double unconstrained_gradient_magnitude (
        const der_funct& grad,
        const T& x,
        const T& lower,
        const T& upper
    )
    {
        T g = grad(x);

        double unorm = 0;

        for (long i = 0; i < g.size(); ++i)
        {
            if (lower(i) < x(i) && x(i) < upper(i))
                unorm += g(i)*g(i);
            else if (x(i) == lower(i) && g(i) < 0)
                unorm += g(i)*g(i);
            else if (x(i) == upper(i) && g(i) > 0)
                unorm += g(i)*g(i);
        }

        return unorm;
    }

    template <typename der_funct, typename T>
    double unconstrained_gradient_magnitude_neg_funct (
        const der_funct& grad,
        const T& x,
        const T& lower,
        const T& upper
    )
    {
        T g = grad(x);

        double unorm = 0;

        for (long i = 0; i < g.size(); ++i)
        {
            if (lower(i) < x(i) && x(i) < upper(i))
                unorm += g(i)*g(i);
            else if (x(i) == lower(i) && g(i) > 0)
                unorm += g(i)*g(i);
            else if (x(i) == upper(i) && g(i) < 0)
                unorm += g(i)*g(i);
        }

        return unorm;
    }

    template <typename search_strategy_type>
    double test_bound_solver_neg_rosen (dlib::rand& rnd, search_strategy_type search_strategy)
    {
        using namespace dlib::test_functions;
        print_spinner();
        matrix<double,2,1> starting_point, lower, upper, x;

        // pick random bounds
        lower = rnd.get_random_gaussian()+1, rnd.get_random_gaussian()+1;
        upper = rnd.get_random_gaussian()+1, rnd.get_random_gaussian()+1;
        while (upper(0) < lower(0)) upper(0) = rnd.get_random_gaussian()+1;
        while (upper(1) < lower(1)) upper(1) = rnd.get_random_gaussian()+1;

        starting_point = rnd.get_random_double()*(upper(0)-lower(0))+lower(0),
                       rnd.get_random_double()*(upper(1)-lower(1))+lower(1);

        dlog << LINFO << "lower: "<< trans(lower);
        dlog << LINFO << "upper: "<< trans(upper);
        dlog << LINFO << "starting: "<< trans(starting_point);

        x = starting_point;
        double val = find_max_box_constrained(
            search_strategy,
            objective_delta_stop_strategy(1e-16, 500),
            neg_rosen, der_neg_rosen, x,
            lower,
            upper
        );

        DLIB_TEST_MSG(std::abs(val - neg_rosen(x)) < 1e-11, std::abs(val - neg_rosen(x)));
        dlog << LINFO << "neg_rosen solution:\n" << x;

        dlog << LINFO << "neg_rosen gradient: "<< trans(der_neg_rosen(x));
        const double gradient_residual = unconstrained_gradient_magnitude_neg_funct(der_neg_rosen, x, lower, upper);
        dlog << LINFO << "gradient_residual: "<< gradient_residual;

        return gradient_residual;
    }

    template <typename search_strategy_type>
    double test_bound_solver_rosen (dlib::rand& rnd, search_strategy_type search_strategy)
    {
        using namespace dlib::test_functions;
        print_spinner();
        matrix<double,2,1> starting_point, lower, upper, x;

        // pick random bounds and sometimes put the upper bound at zero so we can have
        // a test where the optimal value has a bound active at 0 so make sure this case
        // works properly.
        if (rnd.get_random_double() > 0.2)
        {
            lower = rnd.get_random_gaussian()+1, rnd.get_random_gaussian()+1;
            upper = rnd.get_random_gaussian()+1, rnd.get_random_gaussian()+1;
            while (upper(0) < lower(0)) upper(0) = rnd.get_random_gaussian()+1;
            while (upper(1) < lower(1)) upper(1) = rnd.get_random_gaussian()+1;
        }
        else
        {
            upper = 0,0;
            if (rnd.get_random_double() > 0.5)
                upper(0) = -rnd.get_random_double();
            if (rnd.get_random_double() > 0.5)
                upper(1) = -rnd.get_random_double();

            lower = rnd.get_random_double()+1, rnd.get_random_double()+1;
            lower = upper - lower;
        }
        const bool pick_uniform_bounds = rnd.get_random_double() > 0.9;
        if (pick_uniform_bounds)
        {
            double x = rnd.get_random_gaussian()*2;
            double y = rnd.get_random_gaussian()*2;
            lower = min(x,y);
            upper = max(x,y);
        }

        starting_point = rnd.get_random_double()*(upper(0)-lower(0))+lower(0),
                       rnd.get_random_double()*(upper(1)-lower(1))+lower(1);

        dlog << LINFO << "lower: "<< trans(lower);
        dlog << LINFO << "upper: "<< trans(upper);
        dlog << LINFO << "starting: "<< trans(starting_point);

        x = starting_point;
        double val;
        if (!pick_uniform_bounds)
        {
            val = find_min_box_constrained(
                search_strategy,
                objective_delta_stop_strategy(1e-16, 500),
                rosen, der_rosen, x,
                lower,
                upper
            );
        }
        else
        {
            val = find_min_box_constrained(
                search_strategy,
                objective_delta_stop_strategy(1e-16, 500),
                rosen, der_rosen, x,
                lower(0),
                upper(0)
            );
        }

        DLIB_TEST_MSG(std::abs(val - rosen(x)) < 1e-11, std::abs(val - rosen(x)));
        dlog << LINFO << "rosen solution:\n" << x;

        dlog << LINFO << "rosen gradient: "<< trans(der_rosen(x));
        const double gradient_residual = unconstrained_gradient_magnitude(der_rosen, x, lower, upper);
        dlog << LINFO << "gradient_residual: "<< gradient_residual;

        return gradient_residual;
    }

    template <typename search_strategy_type>
    double test_bound_solver_brown (dlib::rand& rnd, search_strategy_type search_strategy)
    {
        using namespace dlib::test_functions;
        print_spinner();
        matrix<double,4,1> starting_point(4), lower(4), upper(4), x;

        const matrix<double,0,1> solution = brown_solution();

        // pick random bounds
        lower = rnd.get_random_gaussian(), rnd.get_random_gaussian(), rnd.get_random_gaussian(), rnd.get_random_gaussian();
        lower = lower*10 + solution;
        upper = rnd.get_random_gaussian(), rnd.get_random_gaussian(), rnd.get_random_gaussian(), rnd.get_random_gaussian();
        upper = upper*10 + solution;
        for (int i = 0; i < lower.size(); ++i)
        {
            if (upper(i) < lower(i))
                swap(upper(i),lower(i));
        }

        starting_point = rnd.get_random_double()*(upper(0)-lower(0))+lower(0),
                       rnd.get_random_double()*(upper(1)-lower(1))+lower(1),
                       rnd.get_random_double()*(upper(2)-lower(2))+lower(2),
                       rnd.get_random_double()*(upper(3)-lower(3))+lower(3);

        dlog << LINFO << "lower: "<< trans(lower);
        dlog << LINFO << "upper: "<< trans(upper);
        dlog << LINFO << "starting: "<< trans(starting_point);

        x = starting_point;
        double val = find_min_box_constrained(
            search_strategy,
            objective_delta_stop_strategy(1e-16, 500),
            brown, brown_derivative, x,
            lower,
            upper
        );

        DLIB_TEST(std::abs(val - brown(x)) < 1e-14);
        dlog << LINFO << "brown solution:\n" << x;
        return unconstrained_gradient_magnitude(brown_derivative, x, lower, upper);
    }

    template <typename search_strategy_type>
    void test_box_constrained_optimizers(search_strategy_type search_strategy)
    {
        dlib::rand rnd;
        running_stats<double> rs;

        dlog << LINFO << "test find_min_box_constrained() on rosen";
        for (int i = 0; i < 10000; ++i)
            rs.add(test_bound_solver_rosen(rnd, search_strategy));
        dlog << LINFO << "mean rosen gradient: " << rs.mean();
        dlog << LINFO << "max rosen gradient:  " << rs.max();
        DLIB_TEST(rs.mean() < 1e-12);
        DLIB_TEST(rs.max() < 1e-9);

        dlog << LINFO << "test find_min_box_constrained() on brown";
        rs.clear();
        for (int i = 0; i < 1000; ++i)
            rs.add(test_bound_solver_brown(rnd, search_strategy));
        dlog << LINFO << "mean brown gradient: " << rs.mean();
        dlog << LINFO << "max brown gradient:  " << rs.max();
        dlog << LINFO << "min brown gradient:  " << rs.min();
        DLIB_TEST(rs.mean() < 4e-5);
        DLIB_TEST_MSG(rs.max() < 3e-2, rs.max());
        DLIB_TEST(rs.min() < 1e-10);

        dlog << LINFO << "test find_max_box_constrained() on neg_rosen";
        rs.clear();
        for (int i = 0; i < 1000; ++i)
            rs.add(test_bound_solver_neg_rosen(rnd, search_strategy));
        dlog << LINFO << "mean neg_rosen gradient: " << rs.mean();
        dlog << LINFO << "max neg_rosen gradient:  " << rs.max();
        DLIB_TEST(rs.mean() < 1e-12);
        DLIB_TEST(rs.max() < 1e-9);

    }

    void test_poly_min_extract_2nd()
    {
        double off;

        off = 0.0; DLIB_TEST(std::abs( poly_min_extrap(off*off, -2*off, (1-off)*(1-off)) - off) < 1e-13);
        off = 0.1; DLIB_TEST(std::abs( poly_min_extrap(off*off, -2*off, (1-off)*(1-off)) - off) < 1e-13);
        off = 0.2; DLIB_TEST(std::abs( poly_min_extrap(off*off, -2*off, (1-off)*(1-off)) - off) < 1e-13);
        off = 0.3; DLIB_TEST(std::abs( poly_min_extrap(off*off, -2*off, (1-off)*(1-off)) - off) < 1e-13);
        off = 0.4; DLIB_TEST(std::abs( poly_min_extrap(off*off, -2*off, (1-off)*(1-off)) - off) < 1e-13);
        off = 0.5; DLIB_TEST(std::abs( poly_min_extrap(off*off, -2*off, (1-off)*(1-off)) - off) < 1e-13);
        off = 0.6; DLIB_TEST(std::abs( poly_min_extrap(off*off, -2*off, (1-off)*(1-off)) - off) < 1e-13);
        off = 0.8; DLIB_TEST(std::abs( poly_min_extrap(off*off, -2*off, (1-off)*(1-off)) - off) < 1e-13);
        off = 0.9; DLIB_TEST(std::abs( poly_min_extrap(off*off, -2*off, (1-off)*(1-off)) - off) < 1e-13);
        off = 1.0; DLIB_TEST(std::abs( poly_min_extrap(off*off, -2*off, (1-off)*(1-off)) - off) < 1e-13);
    }

    void test_solve_trust_region_subproblem_bounded()
    {
        print_spinner();
        matrix<double> H(2,2);
        H = 1, 0,
        0, 1;
        matrix<double,0,1> g, lower, upper, p, true_p;
        g = {0, 0};

        double radius = 0.5;
        lower = {0.5, 0};
        upper = {10, 10};

        solve_trust_region_subproblem_bounded(H,g, radius, p,  0.001, 500, lower, upper);
        true_p = { 0.5, 0};
        DLIB_TEST_MSG(length(p-true_p) < 1e-12, p);

    }

// ----------------------------------------------------------------------------------------

    class optimization_tester : public tester
    {
    public:
        optimization_tester (
        ) :
            tester ("test_optimization",
                    "Runs tests on the optimization component.")
        {}

        void perform_test (
        )
        {
            dlog << LINFO << "test_box_constrained_optimizers(bfgs_search_strategy())";
            test_box_constrained_optimizers(bfgs_search_strategy());
            dlog << LINFO << "test_box_constrained_optimizers(lbfgs_search_strategy(5))";
            test_box_constrained_optimizers(lbfgs_search_strategy(5));
            test_poly_min_extract_2nd();
            optimization_test();
            test_solve_trust_region_subproblem_bounded();
        }
    } a;

}


/* Copyright (c) 2019 Kjetil Olsen Lye, ETH Zurich
 * MIT License
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <boost/test/unit_test.hpp>
#include <fbm/fbm.hpp>
#include <fbm/generate_normal_random.hpp>

BOOST_AUTO_TEST_SUITE(FBB2DTests)

BOOST_AUTO_TEST_CASE(BridgeZero2D) {
    const int N = 64;
    auto X = fbm::generate_normal_random((N - 1) * (N - 1));

    auto fBm = fbm::fractional_brownian_bridge_2d(0.5, N, X);

    BOOST_TEST((N + 1) * (N + 1) == fBm.size());

    for (size_t i = 0; i < N + 1; ++i) {
        BOOST_TEST(0.0 == fBm[i]);
        BOOST_TEST(0.0 == fBm[i + N * (N + 1)]);
        BOOST_TEST(0.0 == fBm[(N + 1) * i]);
        BOOST_TEST(0.0 == fBm[(N + 1) * i + N]);

    }
}

BOOST_AUTO_TEST_CASE(BridgeZero2DNotSidesToZero) {
    const int N = 64;
    auto X = fbm::generate_normal_random((N) * (N));

    auto fBm = fbm::fractional_brownian_bridge_2d(0.5, N, X, false);

    BOOST_TEST((N + 1) * (N + 1) == fBm.size());

    BOOST_TEST(fBm[0] == 0);
    BOOST_TEST(fBm[N] == 0);
    BOOST_TEST(fBm[N * (N + 1)] == 0);
    BOOST_TEST(fBm[N * (N + 1) + N] == 0);

    for (size_t i = 1; i < N; ++i) {
        BOOST_TEST(0.0 != fBm[i]);
        BOOST_TEST(0.0 != fBm[(N + 1) * i]);
    }
}

BOOST_AUTO_TEST_CASE(SmallSizeTwo) {
    // Check that we can create small sizes
    int N = 2;
    auto X = fbm::generate_normal_random((N - 1) * (N - 1));

    auto fBm = fbm::fractional_brownian_bridge_2d(0.5, N, X);

    BOOST_TEST(fBm.size() == 9);

}

BOOST_AUTO_TEST_CASE(SmallSizeOne) {
    // Check that we can create small sizes
    int N = 1;
    auto X = fbm::generate_normal_random((N - 1) * (N - 1));

    auto fBm = fbm::fractional_brownian_bridge_2d(0.5, N, X);

    BOOST_TEST(fBm.size() == 4);

    for (auto d : fBm) {
        BOOST_TEST(d == 0);
    }

}

BOOST_AUTO_TEST_CASE(Nested) {

    //return;
    std::vector<int> resolutions = {8, 16, 32, 64, 128};

    const int N = 2 * resolutions.back();
    auto X = fbm::generate_normal_random((N - 1) * (N - 1));

    for (auto resolution : resolutions) {

        auto fBm = fbm::fractional_brownian_bridge_2d(0.5, resolution, X);
        auto fBm_fine = fbm::fractional_brownian_bridge_2d(0.5, 2 * resolution, X);

        for (int i = 0; i < resolution; ++i) {
            for (int j = 0; j < resolution; ++j) {

                BOOST_TEST(fBm[i * (resolution + 1) + j] == fBm_fine[2 * i *
                        (2 * resolution + 1) + 2 * j]);
            }
        }
    }

}

BOOST_AUTO_TEST_CASE(NestedNotSetSidesToZero) {

    //return;
    std::vector<int> resolutions = {8, 16, 32, 64, 128};

    const int N = 2 * resolutions.back();
    auto X = fbm::generate_normal_random((N) * (N));

    for (auto resolution : resolutions) {

        auto fBm = fbm::fractional_brownian_bridge_2d(0.5, resolution, X, false);
        auto fBm_fine = fbm::fractional_brownian_bridge_2d(0.5, 2 * resolution, X,
                false);

        for (int i = 0; i < resolution; ++i) {
            for (int j = 0; j < resolution; ++j) {

                BOOST_TEST(fBm[i * (resolution + 1) + j] == fBm_fine[2 * i *
                        (2 * resolution + 1) + 2 * j]);
            }
        }
    }

}

BOOST_AUTO_TEST_CASE(ThrowsOnNonePowerOfTwo) {
    std::vector<int> non_powers = {3, 5, 6, 12};

    for (auto non_power : non_powers) {
        auto X = fbm::generate_normal_random((non_power - 1));
        //Boost check throw wouldn't work

        try {
            auto fBm = fbm::fractional_brownian_bridge_2d(0.5, non_power, X);
        } catch (std::runtime_error& e) {
            continue;
        }

        BOOST_TEST(false);
    }
}

BOOST_AUTO_TEST_CASE(ThrowsOnNonPositive) {
    std::vector<int> non_positives = {-10, -4, 0};
    auto X = fbm::generate_normal_random((10 - 1));

    for (auto non_positive : non_positives) {

        //Boost check throw wouldn't work

        try {
            auto fBm = fbm::fractional_brownian_bridge_2d(0.5, non_positive, X);
        } catch (std::runtime_error& e) {
            continue;
        }

        BOOST_TEST(false);
    }
}

BOOST_AUTO_TEST_CASE(NonZeroInterior) {
    // We generate a fake random vector with constnat value 1.0
    // This should guarantee that we never get any zero values in the interior.
    const int N = 64;

    std::vector<double> X(N * N, 1);

    auto fBm = fbm::fractional_brownian_bridge_2d(0.5, N, X);

    BOOST_TEST((N + 1) * (N + 1) == fBm.size());

    for (size_t i = 0; i < N + 1; ++i) {
        BOOST_TEST(0.0 == fBm[i]);
        BOOST_TEST(0.0 == fBm[i + N * (N + 1)]);
        BOOST_TEST(0.0 == fBm[(N + 1) * i]);
        BOOST_TEST(0.0 == fBm[(N + 1) * i + N]);

    }

    for (size_t i = 1; i < N; ++i) {
        for (size_t j = 1; j < N; ++j) {
            BOOST_TEST(0.0 != fBm[i + j * (N + 1)]);
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

typedef tiny::MathTypes<float> MT;
typedef MT::vector3_type       V;
typedef MT::real_type          T;

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(inside_triangle_test)
{
  V const p0 = V::make(0.0, 0.0, 0.0);
  V const p1 = V::make(1.0, 0.0, 0.0);
  V const p2 = V::make(0.0, 1.0, 0.0);

  geometry::Triangle<V> const A = geometry::make_triangle(p0,p1,p2);

  BOOST_CHECK( geometry::is_valid(A) == true );

  // Close to corners are allways inside
  {
    V const k0 = V::make(0.0001, 0.0001, 0.0);
    V const k1 = V::make(0.9999, 0.0, 0.0);
    V const k2 = V::make(0.0, 0.9999, 0.0);

    bool test0 = geometry::inside_triangle( k0, A, true );
    bool test1 = geometry::inside_triangle( k1, A, true );
    bool test2 = geometry::inside_triangle( k2, A, true );

    BOOST_CHECK( test0 );
    BOOST_CHECK( test1 );
    BOOST_CHECK( test2 );

    test0 = geometry::inside_triangle( k0, A, false );
    test1 = geometry::inside_triangle( k1, A, false );
    test2 = geometry::inside_triangle( k2, A, false );

    BOOST_CHECK( test0 );
    BOOST_CHECK( test1 );
    BOOST_CHECK( test2 );

  }
  // Midpoint is allways inside
  {
    bool const test0 = geometry::inside_triangle( (p0+p1+p2)/3.0 , A, true );
    bool const test1 = geometry::inside_triangle( (p0+p1+p2)/3.0 , A, false );

    BOOST_CHECK( test0 );
    BOOST_CHECK( test1 );
  }
  // Some arbitary points
  {
    V const k0 = V::make( 0.2,  0.2,  0.0);  // inside and on plane
    V const k1 = V::make( 0.2,  0.2,  0.2);  // inside and above plane
    V const k2 = V::make( 0.2,  0.2, -0.2); // inside and below plane
    V const k3 = V::make(-1.0, -1.0,  0.0);   // outside and on plane
    V const k4 = V::make(-1.0, -1.0,  0.2);  // outside and above plane
    V const k5 = V::make(-1.0, -1.0, -0.2); // outside and below plane

    bool test0 = geometry::inside_triangle( k0, A, true );
    bool test1 = geometry::inside_triangle( k1, A, true );
    bool test2 = geometry::inside_triangle( k2, A, true );
    bool test3 = geometry::inside_triangle( k3, A, true );
    bool test4 = geometry::inside_triangle( k4, A, true );
    bool test5 = geometry::inside_triangle( k5, A, true );

    BOOST_CHECK( test0 );
    BOOST_CHECK( !test1 );
    BOOST_CHECK( !test2 );
    BOOST_CHECK( !test3 );
    BOOST_CHECK( !test4 );
    BOOST_CHECK( !test5 );

    test0 = geometry::inside_triangle( k0, A, false );
    test1 = geometry::inside_triangle( k1, A, false );
    test2 = geometry::inside_triangle( k2, A, false );
    test3 = geometry::inside_triangle( k3, A, false );
    test4 = geometry::inside_triangle( k4, A, false );
    test5 = geometry::inside_triangle( k5, A, false );

    BOOST_CHECK( test0 );
    BOOST_CHECK( test1 );
    BOOST_CHECK( test2 );
    BOOST_CHECK( !test3 );
    BOOST_CHECK( !test4 );
    BOOST_CHECK( !test5 );
  }
  // Different outside tests -- trying to generate all different cases....
  {
    V const k0 = V::make( 0.5, -0.1, 0.0);
    V const k1 = V::make( 1.0,  1.0, 0.0);
    V const k2 = V::make(-0.1,  0.5, 0.0);
    V const k3 = V::make(-1.0, -1.0, 0.0);
    V const k4 = V::make( 0.1,  1.0, 0.0);
    V const k5 = V::make( 1.0,  0.1, 0.0);

    bool const test0 = geometry::inside_triangle( k0, A, false );
    bool const test1 = geometry::inside_triangle( k1, A, false );
    bool const test2 = geometry::inside_triangle( k2, A, false );
    bool const test3 = geometry::inside_triangle( k3, A, false );
    bool const test4 = geometry::inside_triangle( k4, A, false );
    bool const test5 = geometry::inside_triangle( k5, A, false );

    BOOST_CHECK( !test0 );
    BOOST_CHECK( !test1 );
    BOOST_CHECK( !test2 );
    BOOST_CHECK( !test3 );
    BOOST_CHECK( !test4 );
    BOOST_CHECK( !test5 );
  }
}

BOOST_AUTO_TEST_SUITE_END();


/**
 * @author      Mahdi Maghrebi <mahdi.maghrebi@nih.gov>
 * This code is an implementation of UMAP algorithm for dimension reduction.
 * The reference paper is “UMAP: Uniform Manifold Approximation and Projection for Dimension Reduction“, by McInnes et al., 2018 (https://arxiv.org/abs/1802.03426)
 * Jan 2020
 */

#include <vector>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <list>
#include <string>
#include <math.h>
#include <fstream>
#include <float.h>
#include <boost/filesystem.hpp>
#include "KNN_OpenMP_Code.h"
#include "highDComputes.h"
#include "Initialization.h"
#include "LMOptimization.h"
#include "SGD.h"
#include <exception>
#include <sstream>
#include <omp.h>
#include <Eigen/Sparse>

using namespace std;
using namespace Eigen;

int main(int argc, char ** argv) {
	/**
	 * The errors and informational messages are outputted to the log file
	 */
	ofstream logFile;
	string logFileName="Setting.txt";
	logFile.open(logFileName);
	/**
	 * The input parameters are read from command line which are as follow.
	 * filePath: The full path to the input file containig the dataset.
	 * outputPath: The full path to the output csv file containing the coordinates of data in the embedding space.
	 * K: K in K-NN that means the desired number of Nearest Neighbours to be computed.
	 * sampleRate: The rate at which we do sampling. This parameter plays a key role in the performance.
	 * This parameter is a trades-off between the performance and the accuracy of the results.
	 * Values closer to 1 provides more accurate results but the execution takes longer.
	 * DimLowSpace: Dimension of Low-D or embedding space (usually 1,2,or 3).
	 * randomInitializing: Defining the Method for Initialization of data in low-D space
	 * nepochs: is the number of training epochs to be used in optimizing. Larger values result in more accurate embeddings
	 * mindist defines how tight the points are from each other in Low-D space
	 * distanceMetric is the metric to compute the distance between the points in high-D space, by deafult should be euclidean
	 * distanceV1 is the first optional variable needed for computing distance in some metrics
	 * distanceV2 is the second optional variable needed for computing distance in some metrics
	 * inputPathOptionalArray is the full path to the directory that contains a csv file of the optional array needed for computing distance in some metrics.
	 */
	string filePath, filePathOptionalArray="", outputPath, LogoutputPath, inputPath;
	int K,DimLowSpace,nepochs;
	float sampleRate,mindist,distanceV1=0,distanceV2=0;
	bool randomInitializing;
	string distanceMetric="euclidean";

	for (int i=1; i<argc;++i){
		if (string(argv[i])=="--inputPath") {
			inputPath=argv[i+1];

			if(!boost::filesystem::exists(inputPath) || !boost::filesystem::is_directory(inputPath))
			{
				logFile << "Incorrect input path";
				cout << "Incorrect input path";
				return 1;
			}

			const std::string ext = ".csv";
			boost::filesystem::recursive_directory_iterator it(inputPath);
			boost::filesystem::recursive_directory_iterator endit;

			bool fileFound = false;
			while(it != endit) {
				if(boost::filesystem::is_regular_file(*it) && it->path().extension() == ext){
					fileFound = true;
					filePath = it->path().string();
					break;
				}
				++it;
			}
			if (!fileFound){
				logFile << "CSV file is not found in the input path";
				cout << "CSV file is not found in the input path";
				return 1;
			}
		}
		else if (string(argv[i])=="--K") K=atoi(argv[i+1]);
		else if (string(argv[i])=="--sampleRate") sampleRate=stof(argv[i+1]);
		else if (string(argv[i])=="--mindist") mindist=stof(argv[i+1]);
		else if (string(argv[i])=="--DimLowSpace") DimLowSpace=atoi(argv[i+1]);
		else if (string(argv[i])=="--randomInitializing") {
			std::stringstream ss(argv[i+1]);
			ss >> std::boolalpha >> randomInitializing;
		}
		else if (string(argv[i])=="--outputPath"){
			boost::filesystem::path p(argv[i+1]);

			if(!boost::filesystem::exists(p) || !boost::filesystem::is_directory(p))
			{
				logFile << "Incorrect output path";
				cout << "Incorrect output path";
				return 1;
			}

			LogoutputPath=argv[i+1];
			boost::filesystem::path joinedPath = p / boost::filesystem::path("ProjectedData_EmbeddedSpace.csv");
			outputPath = joinedPath.string();

		}
		else if (string(argv[i])=="--nepochs") nepochs=atoi(argv[i+1]);
		else if (string(argv[i])=="--distanceMetric") distanceMetric=argv[i+1];
		else if (string(argv[i])=="--distanceV1") distanceV1=stof(argv[i+1]);
		else if (string(argv[i])=="--distanceV2") distanceV2=stof(argv[i+1]);
		else if (string(argv[i])=="--inputPathOptionalArray") {
			string inputPathOptionalArray=argv[i+1];

			if(!boost::filesystem::exists(inputPathOptionalArray) || !boost::filesystem::is_directory(inputPathOptionalArray))
			{
				logFile << "Incorrect input path";
				cout << "Incorrect input path";
				return 1;
			}

			const std::string ext = ".csv";
			boost::filesystem::recursive_directory_iterator it(inputPathOptionalArray);
			boost::filesystem::recursive_directory_iterator endit;

			bool fileFound = false;
			while(it != endit) {
				if(boost::filesystem::is_regular_file(*it) && it->path().extension() == ext){
					fileFound = true;
					filePathOptionalArray = it->path().string();
					break;
				}
				++it;
			}
			if (!fileFound){
				logFile << "CSV file is not found in the input path";
				cout << "CSV file is not found in the input path";
				return 1;
			}
		}

	}
	logFile<<"------------The following Input Arguments were read------------"<<endl;
	logFile<<"The full path to the input file: "<< filePath<<endl;
	logFile<<"The full path to the output file: "<< outputPath<<endl;
	logFile<<"The desired number of NN to be computed: "<< K <<endl;
	logFile<<"The sampleRate(The rate at which we do sampling): "<< sampleRate <<endl;
	logFile<<"The Dimension of Low-D Space: "<< DimLowSpace <<endl;
	logFile << std::boolalpha;
	logFile<<"Random Initialization of Points in Low-D Space: "<< randomInitializing <<endl;
	logFile<<"The number of training epochs: "<< nepochs <<endl;
	logFile<<"The chosen mindist parameter: "<< mindist <<endl;
	logFile<<"The metric to compute the distance between the points in high-D space: "<< distanceMetric <<endl;
	logFile<<"The optional variable 1 for the distance: "<< distanceV1 <<endl;
	logFile<<"The optional variable 2 for the distance: "<< distanceV2 <<endl;
	logFile<<"The full path to optional array for the distance metric computation: "<< filePathOptionalArray <<endl;

	cout<<"------------The following Input Arguments were read------------"<<endl;
	cout<<"The full path to the input file: "<< filePath<<endl;
	cout<<"The full path to the output file: "<< outputPath<<endl;
	cout<<"The desired number of NN to be computed: "<< K <<endl;
	cout<<"The sampleRate(The rate at which we do sampling): "<< sampleRate <<endl;
	cout<<"The Dimension of Low-D Space: "<< DimLowSpace <<endl;
	cout << std::boolalpha;
	cout<<"Random Initialization of Points in Low-D Space: "<< randomInitializing <<endl;
	cout<<"The number of training epochs: "<< nepochs <<endl;
	cout<<"The chosen mindist parameter: "<< mindist <<endl;
	cout<<"The metric to compute the distance between the points in high-D space: "<< distanceMetric <<endl;
	cout<<"The optional variable 1 for the distance: "<< distanceV1 <<endl;
	cout<<"The optional variable 2 for the distance: "<< distanceV2 <<endl;
	cout<<"The full path to optional array for the distance metric computation: "<< filePathOptionalArray <<endl;
	/**
	 * Size of Dataset without the header (i.e.(#Rows in dataset)-1).
	 */
	string cmd="wc -l "+filePath;
	string outputCmd = exec(cmd.c_str());
	const int N=stoi(outputCmd.substr(0, outputCmd.find(" ")))-1;
	logFile<<"The Dimension of Dataset Records (Number of Rows in inputfile w/o header ): "<< N <<endl;
	cout<<"The Dimension of Dataset Records (Number of Rows in inputfile w/o header ): "<< N <<endl;
	/**
	 * Dimension of Dataset (#Columns)
	 */
	int Dim;
	string cmd2="head -n 1 "+ filePath + " |tr '\\,' '\\n' |wc -l ";
	Dim = stoi(exec(cmd2.c_str()));
	logFile<<"The Dimension of Dataset Features(Number of Columns in inputfile): "<< Dim <<endl;
	cout<<"The Dimension of Dataset Features(Number of Columns in inputfile): "<< Dim <<endl;

	if (K > N) {
		logFile<<" The desired number of NN has exceeded the size of dataset "<<endl;
		cout<<" The desired number of NN has exceeded the size of dataset "<<endl;
		return 1;
	}

	logFile<<"------------END of INPUT READING------------"<< endl;
	cout<<"------------END of INPUT READING------------"<< endl;

	/**
	 * Query about the number of available CPU processors and set it as OpenMP threads
	 */
	int nProcessors = omp_get_num_procs();
    omp_set_num_threads(nProcessors-1);
	cout <<"Total Number of Processes in the OpenMP Parallel Region = "<< nProcessors-1 <<endl;

	srand(17);

	/**
	 * convThreshold: Convergance Threshold of K-NN. A fixed integer is used here instead of delta*N*K.
	 */
	const int convThreshold=5;
	/**
	 * indices of K-NN for each data point
	 */
	int** B_Index = new int*[N];
	for (int i = 0; i < N; ++i) { B_Index[i] = new int[K]; }
	/**
	 * corresponding distance for K-NN indices stored in B_Index
	 */
	double** B_Dist = new double*[N];
	for (int i = 0; i < N; ++i) { B_Dist[i] = new double[K]; }
	/**
	 * Compute K-NN following the algorithm for shared-memory K-NN
	 * @param filePath The full path to the input file containig the dataset.
	 * @param N Size of Dataset without the header (i.e.(#Rows in dataset)-1).
	 * @param Dim Dimension of Dataset (#Columns)
	 * @param K the desired number of Nearest Neighbours to be computed
	 * @param sampleRate The rate at which we do sampling
	 * @param convThreshold Convergance Threshold
	 * @param logFile The errors and informational messages are outputted to the log file
	 * @param distanceMetric is the metric to compute the distance between the points in high-D space, by deafult should be euclidean
	 * @param distanceV1 is the first optional variable needed for computing distance in some metrics
	 * @param distanceV2 is the second optional variable needed for computing distance in some metrics
	 * @param filePathOptionalArray The full path to optional array for the distance metric computation
	 * @return B_Index indices of K-NN for each data point
	 * @return B_Dist corresponding distance for K-NN indices stored in B_Index
	 */
	computeKNNs(filePath, N, Dim, K, sampleRate, convThreshold,B_Index,B_Dist, logFile, distanceMetric, distanceV1, distanceV2,filePathOptionalArray);

	bool flag=false;
	for (int i = 0; i < N; ++i) {
		for (int j = 0; j < K; ++j) {
			if (B_Dist[i][j] < 0) {
				logFile<<"ALERT: A distance in high-D space was computed as negative, use this program with caution"<<endl;
				cout<<"ALERT: A distance in high-D space was computed as negative, use this program with caution"<<endl;
				flag=true;
				break;
			}
		}
		if (flag) break;
	}

	int* B_Index_Min = new int[N];
	double* B_Dist_Min = new double[N];
	/**
	 * Compute B_Index and B_Dist for the closest points (K-NNs)
	 * @param B_Index indices of K-NN for each data point
	 * @param B_Dist corresponding distance for K-NN indices stored in B_Index
	 * @param N Size of Dataset without the header (i.e.(#Rows in dataset)-1).
	 * @param K the desired number of Nearest Neighbours to be computed
	 * @return B_Index_Min B_Index for the closest point
	 * @return B_Dist_Min B_Dist for the corresponding B_Index_Min
	 */
	findMin(B_Index,B_Dist, N,K,B_Index_Min,B_Dist_Min);

	double* SigmaValues = new double[N];
	/**
	 * Compute SigmaValues for each data point (Smooth approximator to K-NN distance) iteratively
	 * @param B_Dist corresponding distance for K-NN indices stored in B_Index
	 * @param B_Dist_Min B_Dist for the corresponding B_Index_Min
	 * @param N Size of Dataset without the header (i.e.(#Rows in dataset)-1).
	 * @param K the desired number of Nearest Neighbours to be computed
	 * @return SigmaValues An array of Sigma Values for data
	 */
	findSigma(B_Dist, B_Dist_Min,SigmaValues, N, K);

	/**
	 * To save memory space, "Sparse Matrix" data structure is used here
	 * SparseMatrix by default is oriented column-major
	 */
	SparseMatrix<float> adjacencyMatrixA(N,N), adjacencyMatrixAT(N,N), graphSM(N,N);
	typedef Eigen::Triplet<float> T;
	std::vector<T> tripletList;
	tripletList.reserve(N*K);

	for (int i=0; i<N; ++i){
		for (int j=0; j<K; ++j){
			int point2=B_Index[i][j];
			float tmp=exp((B_Dist_Min[i]-B_Dist[i][j])/SigmaValues[i]);
			tripletList.push_back(T(i,point2,tmp));
		}
	}

	adjacencyMatrixA.setFromTriplets(tripletList.begin(), tripletList.end());
	adjacencyMatrixAT=adjacencyMatrixA.transpose();
	graphSM=adjacencyMatrixA+adjacencyMatrixAT;
	graphSM -=adjacencyMatrixA.cwiseProduct(adjacencyMatrixAT);

	float MaxWeight=0;
	for (int k=0; k<graphSM.outerSize(); ++k){
		float sum=0;
		for (SparseMatrix<float>::InnerIterator it(graphSM,k); it; ++it) {
			sum += it.value();
			if (it.value() > MaxWeight) MaxWeight=it.value();
		}
	}

	logFile<<"------------Setting Low-D Space Design------------"<<endl;
	cout<<"------------Setting Low-D Space Design------------"<<endl;

	/**
	 * embedding is the coordinates of the points in the low-D space
	 */
	double** embedding = new double*[N];
	for (int i = 0; i < N; ++i) { embedding[i] = new double[DimLowSpace]; }

	logFile<<"------------Starting Initialization in the Low-D Space------------"<<endl;
	cout<<"------------Starting Initialization in the Low-D Space------------"<<endl;
	/**
	 * Initializes the data points in low-D space
	 * @param randomInitializing the methodology for Initialization of data in low-D space
	 * @param logFile contains the errors and informational messages
	 * @param N Size of Dataset without the header (i.e.(#Rows in dataset)-1).
	 * @param graph contains undirected weights (similarities) in the form of a matrix of size NxN
	 * @param DimLowSpace Dimension of Low-D space
	 * @return embedding is the coordinates of the points in the low-D space
	 */
	Initialization (randomInitializing, embedding, logFile, N, graphSM, MaxWeight, DimLowSpace, nepochs);

	logFile<<"------------Starting Estimating Hyper-Parameters a and b ------------"<<endl;
	cout<<"------------Starting Estimating Hyper-Parameters a and b ------------"<<endl;
	/**
	 * Hyper-Parameters a and b which needs to be estimated by data fitting.
	 */
	float aValue, bValue;
	float spread=1.0;
	/**
	 *  Estimation of Hyper-Parameters a and b by curve fitting and using Levenberg-Marquardt solution
	 */
	estimateParameters(aValue, bValue, mindist, spread, logFile);

	logFile<<"The Estimated Values for a is "<< aValue << " and for b is "<< bValue <<endl;
	cout<<"The Estimated Values for a is "<< aValue << " and for b is "<< bValue <<endl;

	logFile<<"------------Starting Solution for Stochastic Gradient Descent (SGD)------------"<<endl;
	cout<<"------------Starting Solution for Stochastic Gradient Descent (SGD)------------"<<endl;
	/**
	 *  alpha is the initial learning rate for the SGD. alpha starts from 1 and decreases in each epoch iteration
	 */
	float alpha=1.0;
	/**
	 * epochs_per_sample is a vector of edges with the values proportional to the values in graph
	 * epochs_per_sample represents the epoch weight for edges where the edge with the highest similarity will get the value of 1
	 * and all other edges will get a proportional epoch weight scaled from it. epochs_per_sample is used as a measure to include an edge in
	 * SGD computations. The edge with the highest similarity will be used at every epoch iteration.
	 * head is a vector containing the head index of the edge
	 * tail is a vector containing the tail index of the edge
	 */
	vector<int> head, tail;
	vector<float> epochs_per_sample;

	for (int k=0; k<graphSM.outerSize(); ++k){
		for (SparseMatrix<float>::InnerIterator it(graphSM,k); it; ++it) {
		    if (it.value() <  MaxWeight/nepochs) continue;
			epochs_per_sample.push_back(MaxWeight/it.value());
			head.push_back(it.col());
			tail.push_back(it.row());
		}
	}

	/**
	 * This section was adopted from SGD implementation at https://github.com/lmcinnes/umap/blob/8f2ef23ec835cc5071fe6351a0da8313d8e75706/umap/layouts.py#L136
	 * edgeCounts is total number of edges in the high-D space graph
	 * epoch_of_next_sample is an index of the epoch state of the edges. If it is less than epoch index, we will use the edge in the computation
	 * epoch_of_next_negative_sample is an index of the epoch state of the edges for sampling from non-connected surrounding points.
	 * negative_sample_rate is the rate at which we sample from the non-connected surrounding points as compared to the connected edges.
	 * Increasing this value will result in greater repulsive force being applied, greater optimization cost, but slightly more accuracy.
	 */
	int edgeCounts=epochs_per_sample.size();
	const int negative_sample_rate=5;
	int n_neg_samples;
//Substituting with Vectors due to Stacksize run-time error
//	float epoch_of_next_sample[edgeCounts];
//	float epochs_per_negative_sample[edgeCounts];
//	float epoch_of_next_negative_sample[edgeCounts];
    vector<float> epoch_of_next_sample,epochs_per_negative_sample,epoch_of_next_negative_sample;

	for (int i = 0; i < edgeCounts; ++i) {
//		epoch_of_next_sample[i]=epochs_per_sample[i];
//		epochs_per_negative_sample[i]=epochs_per_sample[i]/negative_sample_rate;
//		epoch_of_next_negative_sample[i]=epochs_per_negative_sample[i];
        epoch_of_next_sample.push_back(epochs_per_sample[i]);
        epochs_per_negative_sample.push_back(epochs_per_sample[i]/negative_sample_rate);
        epoch_of_next_negative_sample.push_back(epochs_per_negative_sample[i]);
	}
	/**
	 *  move_other is equal to 1 if not embedding new previously unseen points to low-D space
	 */
	const int move_other=1;
	/**
	 *  dEpsilon is zero approximation in double precision
	 */
	const double dEpsilon=1e-14;
	double dist_squared;
	// The main training loop
	for (int n = 1; n < nepochs; ++n) {

		//Loop over all edges of the graph
		if (n%100 == 0){
			logFile << "SGD iteration = "<<n<<" from "<< nepochs <<endl;
			cout << "SGD iteration = "<<n<<" from "<< nepochs <<endl;
		}

		for (int i = 0; i < edgeCounts; ++i) {
			if (epoch_of_next_sample[i] <= n){

				int headIndex = head[i];
				int tailIndex = tail[i];

				dist_squared = rdist(embedding, DimLowSpace, headIndex, tailIndex);

				double grad_coeff;
				if (dist_squared<dEpsilon) grad_coeff=0;
				else {grad_coeff= -2.0*aValue*bValue*pow(dist_squared,bValue-1)/(1.0+aValue*pow(dist_squared,bValue)); }

                double grad_d;
				for (int jj = 0; jj < DimLowSpace; ++jj) {
					grad_d = alpha*clip(grad_coeff*(embedding[headIndex][jj]-embedding[tailIndex][jj]));
                    embedding[headIndex][jj] += grad_d;
					//if (move_other==1) 	{
						embedding[tailIndex][jj] -= grad_d;
					//}
				}

				epoch_of_next_sample[i] += epochs_per_sample[i];
				n_neg_samples = int((float(n) - epoch_of_next_negative_sample[i])/ epochs_per_negative_sample[i]);

				for (int ll = 0; ll < n_neg_samples; ++ll) {
					int randomIndex = rand() % N;
					if (randomIndex==headIndex) continue;

					dist_squared= rdist(embedding, DimLowSpace, headIndex, randomIndex);

					if (dist_squared < dEpsilon) grad_coeff=0;
					else{ grad_coeff = 2.0*bValue/((0.001+dist_squared)*(1.0+aValue*pow(dist_squared,bValue))); }

					for (int jj = 0; jj < DimLowSpace; ++jj) {
						if  (grad_coeff > 0) {
							embedding[headIndex][jj] += alpha*clip(grad_coeff*(embedding[headIndex][jj]-embedding[randomIndex][jj]));
						} else  {
							embedding[headIndex][jj] += alpha*4.0;
						}
					}
				}
				epoch_of_next_negative_sample[i] += (n_neg_samples * epochs_per_negative_sample[i]);
			}
		}
		alpha=1.0-((float)n)/nepochs;
	}

	logFile<<"------------Starting Outputing the Results------------"<<endl;
	cout<<"------------Starting Outputing the Results------------"<<endl;
	/**
	 * Output the coordinates of the projected data in the low-D space
	 */
	ofstream embeddedSpacefile;
	embeddedSpacefile.open(outputPath);

	for (int j = 0; j < DimLowSpace; ++j) {
		if (j != DimLowSpace-1) embeddedSpacefile<<"Dimension"<<j+1<<",";
		else embeddedSpacefile<<"Dimension"<<j+1<<endl;
	}

	for (int i = 0; i < N; ++i) {
		for (int j = 0; j < DimLowSpace; ++j) {
			if (j==DimLowSpace-1) {
				embeddedSpacefile<< embedding[i][j]<<endl;}
			else {embeddedSpacefile<< embedding[i][j]<<",";}
		}
	}

	embeddedSpacefile.close();
	logFile.close();
	/**
	 * copy Logfile to the file system which could be accessed outside the docker container
	 */
	string cmd3="cp "+ logFileName+"  "+LogoutputPath;
	// To remove the returning messages, we can switch to the following command
	//	string cmd3="cp "+ logFileName+"  "+LogoutputPath+ " 2>&1 /dev/null";
	string outputCmd3 = exec(cmd3.c_str());

	return 0;
}