#include "theia/sfm/estimators/estimate_absolute_pose_with_known_orientation.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ceres/rotation.h>
#include <vector>

#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/estimators/feature_correspondence_2d_3d.h"
#include "theia/sfm/pose/essential_matrix_utils.h"
#include "theia/sfm/pose/relative_pose_from_two_points_with_known_rotation.h"
#include "theia/sfm/pose/util.h"
#include "theia/sfm/triangulation/triangulation.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

// An estimator for computing the relative pose from 2 feature
// correspondences. The feature correspondences should be normalized by the
// focal length with the principal point at (0, 0).
class RelativePoseWithKnownOrientationEstimator
    : public Estimator<FeatureCorrespondence, Eigen::Vector3d> {
 public:
  RelativePoseWithKnownOrientationEstimator() {}

  // 2 correspondences are needed to determine the relative position.
  double SampleSize() const { return 2; }

  // Estimates candidate relative poses from correspondences.
  bool EstimateModel(const std::vector<FeatureCorrespondence>& correspondences,
                     std::vector<Eigen::Vector3d>* relative_positions) const {
    Eigen::Vector3d position;
    const Eigen::Vector2d rotated_features1[2] = {
        correspondences[0].feature1.point_, correspondences[1].feature1.point_};
    const Eigen::Vector2d rotated_features2[2] = {
        correspondences[0].feature2.point_, correspondences[1].feature2.point_};
    if (!RelativePoseFromTwoPointsWithKnownRotation(
            rotated_features1, rotated_features2, &position)) {
      return false;
    }

    relative_positions->emplace_back(position);
    return true;
  }

  // The error for a correspondences given an relative position. This is the
  // squared reprojection error.
  double Error(const FeatureCorrespondence& correspondence,
               const Eigen::Vector3d& relative_position) const {
    static const Eigen::Matrix3d rotation = Eigen::Matrix3d::Identity();
    return SquaredSampsonDistance(CrossProductMatrix(-relative_position),
                                  correspondence.feature1.point_,
                                  correspondence.feature2.point_);
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(RelativePoseWithKnownOrientationEstimator);
};

}  // namespace

bool EstimateRelativePoseWithKnownOrientation(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<FeatureCorrespondence>& rotated_correspondences,
    Eigen::Vector3d* relative_camera2_position,
    RansacSummary* ransac_summary) {
  RelativePoseWithKnownOrientationEstimator relative_pose_estimator;
  std::unique_ptr<
      SampleConsensusEstimator<RelativePoseWithKnownOrientationEstimator> >
      ransac = CreateAndInitializeRansacVariant(
          ransac_type, ransac_params, relative_pose_estimator);
  // Estimate the relative pose.
  return ransac->Estimate(
      rotated_correspondences, relative_camera2_position, ransac_summary);
}

}  // namespace theia


//  (C) Copyright Matt Borland 2021.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <cmath>
#include <cfloat>
#include <cstdint>
#include <limits>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/math/tools/config.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

template <typename T>
void test()
{
    constexpr bool test_val = boost::math::ccmath::isnan(T(0));
    static_assert(!test_val, "Not constexpr");

    if constexpr (std::numeric_limits<T>::has_quiet_NaN)
    {
        static_assert(boost::math::ccmath::isnan(std::numeric_limits<T>::quiet_NaN()), "Quiet NAN failed");
    }
    if constexpr (std::numeric_limits<T>::has_signaling_NaN)
    {
        static_assert(boost::math::ccmath::isnan(std::numeric_limits<T>::signaling_NaN()), "Signaling NAN failed");
    }
    static_assert(!boost::math::ccmath::isnan(std::numeric_limits<T>::infinity()), "Infininty failed");
    static_assert(!boost::math::ccmath::isnan(T(0)), "Real 0 failed");
}

// Only test on platforms that provide BOOST_MATH_IS_CONSTANT_EVALUATED
#ifndef BOOST_MATH_NO_CONSTEXPR_DETECTION
int main()
{
    test<float>();
    test<double>();

    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test<long double>();
    #endif

    #if defined(BOOST_HAS_FLOAT128) && !defined(BOOST_MATH_USING_BUILTIN_CONSTANT_P)
    test<boost::multiprecision::float128>();
    #endif

    test<int>();
    test<unsigned>();
    test<long>();
    test<std::int32_t>();
    test<std::int64_t>();
    test<std::uint32_t>();

    return boost::report_errors();
}
#else
int main()
{
    return 0;
}
#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testNonlinearOptimizer.cpp
 * @brief   Unit tests for NonlinearOptimizer class
 * @author  Frank Dellaert
 */

#include <tests/smallExample.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Matrix.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;

#include <iostream>

using namespace std;
using namespace gtsam;

const double tol = 1e-5;

using symbol_shorthand::X;
using symbol_shorthand::L;

/* ************************************************************************* */
TEST( NonlinearOptimizer, iterateLM )
{
	// really non-linear factor graph
  example::Graph fg(example::createReallyNonlinearFactorGraph());

	// config far from minimum
	Point2 x0(3,0);
	Values config;
	config.insert(X(1), x0);

	// normal iterate
	GaussNewtonParams gnParams;
	GaussNewtonOptimizer gnOptimizer(fg, config, gnParams);
	gnOptimizer.iterate();

	// LM iterate with lambda 0 should be the same
	LevenbergMarquardtParams lmParams;
	lmParams.lambdaInitial = 0.0;
	LevenbergMarquardtOptimizer lmOptimizer(fg, config, lmParams);
	lmOptimizer.iterate();

	CHECK(assert_equal(gnOptimizer.values(), lmOptimizer.values(), 1e-9));
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, optimize )
{
  example::Graph fg(example::createReallyNonlinearFactorGraph());

	// test error at minimum
	Point2 xstar(0,0);
	Values cstar;
	cstar.insert(X(1), xstar);
	DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);

	// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
	Point2 x0(3,3);
	Values c0;
	c0.insert(X(1), x0);
	DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);

	// optimize parameters
	Ordering ord;
	ord.push_back(X(1));

	// Gauss-Newton
	GaussNewtonParams gnParams;
	gnParams.ordering = ord;
	Values actual1 = GaussNewtonOptimizer(fg, c0, gnParams).optimize();
	DOUBLES_EQUAL(0,fg.error(actual1),tol);

	// Levenberg-Marquardt
	LevenbergMarquardtParams lmParams;
	lmParams.ordering = ord;
  Values actual2 = LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize();
  DOUBLES_EQUAL(0,fg.error(actual2),tol);

  // Dogleg
  DoglegParams dlParams;
  dlParams.ordering = ord;
  Values actual3 = DoglegOptimizer(fg, c0, dlParams).optimize();
  DOUBLES_EQUAL(0,fg.error(actual3),tol);
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleLMOptimizer )
{
	example::Graph fg(example::createReallyNonlinearFactorGraph());

	Point2 x0(3,3);
	Values c0;
	c0.insert(X(1), x0);

	Values actual = LevenbergMarquardtOptimizer(fg, c0).optimize();
	DOUBLES_EQUAL(0,fg.error(actual),tol);
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleGNOptimizer )
{
  example::Graph fg(example::createReallyNonlinearFactorGraph());

  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);

  Values actual = GaussNewtonOptimizer(fg, c0).optimize();
	DOUBLES_EQUAL(0,fg.error(actual),tol);
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleDLOptimizer )
{
  example::Graph fg(example::createReallyNonlinearFactorGraph());

  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);

  Values actual = DoglegOptimizer(fg, c0).optimize();
  DOUBLES_EQUAL(0,fg.error(actual),tol);
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, optimization_method )
{
  LevenbergMarquardtParams paramsQR;
  paramsQR.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_QR;
  LevenbergMarquardtParams paramsChol;
  paramsChol.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_CHOLESKY;

	example::Graph fg = example::createReallyNonlinearFactorGraph();

	Point2 x0(3,3);
	Values c0;
	c0.insert(X(1), x0);

	Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimize();
	DOUBLES_EQUAL(0,fg.error(actualMFQR),tol);

  Values actualMFChol = LevenbergMarquardtOptimizer(fg, c0, paramsChol).optimize();
  DOUBLES_EQUAL(0,fg.error(actualMFChol),tol);
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, Factorization )
{
	Values config;
	config.insert(X(1), Pose2(0.,0.,0.));
	config.insert(X(2), Pose2(1.5,0.,0.));

	NonlinearFactorGraph graph;
	graph.add(PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)));
	graph.add(BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)));

	Ordering ordering;
	ordering.push_back(X(1));
	ordering.push_back(X(2));

	LevenbergMarquardtOptimizer optimizer(graph, config, ordering);
	optimizer.iterate();

	Values expected;
	expected.insert(X(1), Pose2(0.,0.,0.));
	expected.insert(X(2), Pose2(1.,0.,0.));
	CHECK(assert_equal(expected, optimizer.values(), 1e-5));
}

/* ************************************************************************* */
TEST(NonlinearOptimizer, NullFactor) {

  example::Graph fg = example::createReallyNonlinearFactorGraph();

  // Add null factor
  fg.push_back(example::Graph::sharedFactor());

  // test error at minimum
  Point2 xstar(0,0);
  Values cstar;
  cstar.insert(X(1), xstar);
  DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);

  // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);
  DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);

  // optimize parameters
  Ordering ord;
  ord.push_back(X(1));

  // Gauss-Newton
  Values actual1 = GaussNewtonOptimizer(fg, c0, ord).optimize();
  DOUBLES_EQUAL(0,fg.error(actual1),tol);

  // Levenberg-Marquardt
  Values actual2 = LevenbergMarquardtOptimizer(fg, c0, ord).optimize();
  DOUBLES_EQUAL(0,fg.error(actual2),tol);

  // Dogleg
  Values actual3 = DoglegOptimizer(fg, c0, ord).optimize();
  DOUBLES_EQUAL(0,fg.error(actual3),tol);
}

/* ************************************************************************* */
TEST(NonlinearOptimizer, MoreOptimization) {

  NonlinearFactorGraph fg;
  fg.add(PriorFactor<Pose2>(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1)));
  fg.add(BetweenFactor<Pose2>(0, 1, Pose2(1,0,M_PI/2), noiseModel::Isotropic::Sigma(3,1)));
  fg.add(BetweenFactor<Pose2>(1, 2, Pose2(1,0,M_PI/2), noiseModel::Isotropic::Sigma(3,1)));

  Values init;
  init.insert(0, Pose2(3,4,-M_PI));
  init.insert(1, Pose2(10,2,-M_PI));
  init.insert(2, Pose2(11,7,-M_PI));

  Values expected;
  expected.insert(0, Pose2(0,0,0));
  expected.insert(1, Pose2(1,0,M_PI/2));
  expected.insert(2, Pose2(1,1,M_PI));

  // Try LM and Dogleg
  EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init).optimize()));
  EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize()));
}

/* ************************************************************************* */
int main() {
	TestResult tr;
	return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


// Copyright 2004 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_CIRCLE_LAYOUT_HPP
#define BOOST_GRAPH_CIRCLE_LAYOUT_HPP
#include <boost/config/no_tr1/cmath.hpp>
#include <boost/math/constants/constants.hpp>
#include <utility>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/topology.hpp>
#include <boost/static_assert.hpp>

namespace boost {
  /**
   * \brief Layout the graph with the vertices at the points of a regular
   * n-polygon.
   *
   * The distance from the center of the polygon to each point is
   * determined by the @p radius parameter. The @p position parameter
   * must be an Lvalue Property Map whose value type is a class type
   * containing @c x and @c y members that will be set to the @c x and
   * @c y coordinates.
   */
  template<typename VertexListGraph, typename PositionMap, typename Radius>
  void
  circle_graph_layout(const VertexListGraph& g, PositionMap position,
                      Radius radius)
  {
    BOOST_STATIC_ASSERT (property_traits<PositionMap>::value_type::dimensions >= 2);
    const double pi = boost::math::constants::pi<double>();

#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sin;
    using std::cos;
#endif // BOOST_NO_STDC_NAMESPACE

    typedef typename graph_traits<VertexListGraph>::vertices_size_type
      vertices_size_type;

    vertices_size_type n = num_vertices(g);

    vertices_size_type i = 0;
    double two_pi_over_n = 2. * pi / n;
    BGL_FORALL_VERTICES_T(v, g, VertexListGraph) {
      position[v][0] = radius * cos(i * two_pi_over_n);
      position[v][1] = radius * sin(i * two_pi_over_n);
      ++i;
    }
  }
} // end namespace boost

#endif // BOOST_GRAPH_CIRCLE_LAYOUT_HPP


/*
  test_stoch_euler.cpp : Sample program to demonstrate the working of stochastic Euler

  Copyright (C) 2015 Anup Gopalakrishna Pillai, Suhita Nadkarni Lab, IISER, Pune <anupgpillai@gmail.com>

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <iostream>
#include <vector>
#include <algorithm>
#include <fstream>
#include <string>

#include "det_model_hh_post.hpp"
#include "physical_constants.hpp"
#include "stl_vector_operation_functions.hpp"
#include "stoch_odeint_explicit_euler.hpp"

#include <boost/numeric/odeint.hpp> /*  Specifying the file within < > lets c++ search for it at -I path  */
#include <boost/range/iterator_range.hpp>
#include <boost/range/iterator.hpp>

namespace bno=boost::numeric::odeint;
namespace myode=stoch_odeint_explicit_euler;

typedef std::vector< double > state_type;
typedef bno::runge_kutta4 < state_type > boost_rk4_stepper_type;

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

int main(int argc, char *argv[])
{
  // initializes the 'classical' odeint stepper from BOOST
  boost_rk4_stepper_type boost_rk4;
  state_type X_hh = {0,1,0,-80.0E-03}; // {m,h,n,V}
  // initializes an HH object using the 'DET_MODEL_HH_POST' class
  DET_MODEL_HH_POST det_hhp(X_hh);
  //--------------------------------------

  // initializes the stochastic explicit euler stepper
  myode::STOCH_ODEINT_EXPLICIT_EULER< state_type > odeint_euler;
  // the vector s can be used to add noise to each each element of the state vector
  // '0' indictaes no noise added.
  // setting all elements of the s vector reduces the stepper to simple explicit Euler!
  std::vector<unsigned> s = {0,0,0,1}; // {m,h,n,V}
  // initialization of observer object : outputs to stdout
  null_observer my_observer;

  //--------------------------------------
  double t = 0.0;
  const double dt = 15E-06;
  const double tmax = 100E-03;
  // integrate_const can be used to compute all the time steps at once. Use an oberver of your choise, defaults to the null_oberver which outputs to stdout
  //integrate_const(odeint_euler,det_hhp, det_hhp.X,t,tmax,dt,s,my_observer);
  while (t <= tmax){
    odeint_euler.do_step( det_hhp,det_hhp.X,t,dt,s);
    //boost_rk4.do_step( det_hhp,det_hhp.X, t, dt);
    //---------------------------
    if( (t > 50E-03) && ( t < (54E-03 )) ){
      det_hhp.IExt = 20E-02;	// External current to trigger presynaptic spike
    }
    else{
      det_hhp.IExt = 0.0;
    }
    std::cout << t << " " << det_hhp.X << std::endl;
    t = t + dt;
  };
  return 0;
}


//
// Created by Heuer on 10.03.18.
//

#include <gtest/gtest.h>
#include <Eigen/Core>
#include "LebedevSphericalIntegration/GridCreator.h"
#include "sphere_lebedev_rule.h"

class AGridCreatorTest : public ::testing::Test {
public:
    Lebedev::GridCreator gridCreator;
    void SetUp() override {}
};

TEST_F(AGridCreatorTest, JBurkardtReferenceCheck) {

    for (const auto & order : Lebedev::allOrders) {

        gridCreator.changeGrid(order);
        auto nPts = gridCreator.totalNumberOfGridPoints();
        auto calculated = gridCreator.grid();

        double x[nPts], y[nPts], z[nPts], w[nPts];
        ld_by_order(nPts, x, y, z, w);

        Eigen::Matrix<double,Eigen::Dynamic,4> reference(nPts, 4);
        reference.col(0) = Eigen::VectorXd::Map(x, nPts);
        reference.col(1) = Eigen::VectorXd::Map(y, nPts);
        reference.col(2) = Eigen::VectorXd::Map(z, nPts);
        reference.col(3) = Eigen::VectorXd::Map(w, nPts);

        ASSERT_EQ(calculated.rows(),reference.rows());
        ASSERT_EQ(calculated.cols(),reference.cols());

        ASSERT_TRUE(calculated.isApprox(reference,0.0));
    }
}

TEST_F(AGridCreatorTest, JBurkardtReferenceCheckElementWise) {

    for (const auto & order : Lebedev::allOrders) {

        gridCreator.changeGrid(order);
        auto nPts = gridCreator.totalNumberOfGridPoints();
        auto calculated = gridCreator.grid();

        double x[nPts], y[nPts], z[nPts], w[nPts];
        ld_by_order(nPts, x, y, z, w);

        Eigen::Matrix<double,Eigen::Dynamic,4> reference(nPts, 4);
        reference.col(0) = Eigen::VectorXd::Map(x, nPts);
        reference.col(1) = Eigen::VectorXd::Map(y, nPts);
        reference.col(2) = Eigen::VectorXd::Map(z, nPts);
        reference.col(3) = Eigen::VectorXd::Map(w, nPts);

        ASSERT_EQ(calculated.rows(),reference.rows());
        ASSERT_EQ(calculated.cols(),reference.cols());

        for (int i = 0; i < nPts; ++i) {
            for (int j = 0; j < 4; ++j) {
                ASSERT_EQ(calculated.row(i)[j], reference.row(i)[j]);
            }
        }
    }
}

TEST_F(AGridCreatorTest, CorrectTotalNumberOfPoints) {

}


/**
  * \file test_solver_tools.cpp
  *
  * Tests for solver_tools
  */

#include<solver_tools.h>
#include<sstream>
#include<fstream>
#include<iostream>

#define BOOST_TEST_MODULE test_vector_tools
#include <boost/test/included/unit_test.hpp>

typedef solverTools::errorOut errorOut;
typedef solverTools::errorNode errorNode;
typedef solverTools::floatType floatType;
typedef solverTools::floatVector floatVector;
typedef solverTools::floatMatrix floatMatrix;
typedef solverTools::intVector intVector;
typedef solverTools::intMatrix intMatrix;

struct cout_redirect{
    cout_redirect( std::streambuf * new_buffer )
        : old( std::cout.rdbuf( new_buffer ) )
    { }

    ~cout_redirect( ) {
        std::cout.rdbuf( old );
    }

    private:
        std::streambuf * old;
};

struct cerr_redirect{
    cerr_redirect( std::streambuf * new_buffer )
        : old( std::cerr.rdbuf( new_buffer ) )
    { }

    ~cerr_redirect( ) {
        std::cerr.rdbuf( old );
    }

    private:
        std::streambuf * old;
};

errorOut nlFxn1( const floatVector &x, const floatMatrix &floatArgs, const intMatrix &intArgs,
                 floatVector &residual, floatMatrix &jacobian, floatMatrix &floatOuts,
                 intMatrix &intOuts ){
    /*!
     * A non-linear function for use in testing the solver. This function is a linear
     * function of the form
     *
     * \f$ R = \left [ x + 1, y - 5.6 \right ]\f$
     *
     * which has a solution at \f$\left ( -1, 5.6 \right )\f$
     *
     * The function also sets floatOuts to
     *
     * \f$\left [ \left [ -1 \right ], \left [ -1, -2, -3 \right ], \left [ 4, 5, 6 \right ] \right ]\f$
     *
     * And intOuts to
     *
     * \f$\left [ \left [ 1, 2, 8 \right ] \right ]\f$
     *
     * \param &x: The variable vector. Of size 2.
     * \param &floatArgs: Floating point arguments to the function. None expected.
     * \param &intArgs: Integer arguments to the function. None expected.
     * \param &residual: The residual vector output.
     * \param &jacobian: The jacobian output.
     * \param &floatOuts: Additional floating point outputs.
     * \param &intOuts: Additional integer outputs.
     */

    if ( x.size( ) != 2 ){
        return new errorNode( "nlFnx1", "x must have a size of 2" );
    }

    floatType x0 = -1;
    floatType y0 = 5.6;

    residual = { x[ 0 ] - x0, x[ 1 ] - y0 };
    jacobian = { { 1, 0 }, { 0, 1 } };

    floatOuts = { { -1 }, { -1, -2, -3 }, { 4, 5, 6 } };
    intOuts = { { 1, 2, 8 } };

    return NULL;
}

errorOut nlFxn1( const floatVector &x, const floatMatrix &floatArgs, const intMatrix &intArgs,
                 floatVector &residual ){
    /*!
     * A non-linear function for use in testing the solver. An overload that hides the jacobian,
     * floatOut, and intOut arrays.
     *
     * \param &x: The variable vector. Of size 2
     * \param &floatArgs: Floating point arguments to the function
     * \param &intArgs: Integer arguments to the function
     * \param &residual: The residual vector output.
     */

    floatMatrix Jtmp;
    floatMatrix fO;
    intMatrix iO;
    return nlFxn1( x, floatArgs, intArgs, residual, Jtmp, fO, iO );
}

errorOut nlFxn2( const floatVector &x, const floatMatrix &floatArgs, const intMatrix &intArgs,
                 floatVector &residual, floatMatrix &jacobian, floatMatrix &floatOuts,
                 intMatrix &intOuts ){
    /*!
     * A non-linear function for use in testing the solver. A polynomial function of the form
     *
     * \f$ R = \left [ ( x - 1 ) ( x - 7 ) y, ( y - 1 ) ( x - 3 ) z, x y z \right ]\f$
     *
     * The function also sets floatOuts to
     *
     * \f$\left [ \left [ -1 \right ], \left [ -1, -2, -3 \right ], \left [ 4, 5, 6 \right ] \right ]\f$
     *
     * And intOuts to
     *
     * \f$\left [ \left [ 1, 2, 8 \right ] \right ]\f$
     *
     * \param &x: The variable vector. Of size 3.
     * \param &floatArgs: Floating point arguments to the function. Unused.
     * \param &intArgs: Integer arguments to the function. Unused.
     * \param &residual: The residual vector output.
     * \param &jacobian: The jacobian output.
     * \param &floatOuts: Additional floating point outputs.
     * \param &intOuts: Additional integer outputs.
     */

    if ( x.size( ) != 3 ){
        return new errorNode( "nlFxn2", "x must have a size of 3" );
    }

    residual = { ( x[ 0 ] - 1 ) * ( x[ 0 ] - 7 ) * x[ 1 ], ( x[ 1 ] - 1 ) * ( x[ 0 ] - 3 ) * x[ 2 ], x[ 0 ] * x[ 1 ] * x[ 2 ] };
    jacobian = { { ( x[ 0 ] - 7 ) * x[ 1 ] + ( x[ 0 ] - 1 ) * x[ 1 ], ( x[ 0 ] - 1 ) * ( x[ 0 ] - 7 ), 0 },
                 {   ( x[ 1 ] - 1 ) * x[ 2 ],    ( x[ 0 ] - 3 ) * x[ 2 ], ( x[ 1 ] - 1 ) * ( x[ 0 ] - 3 ) },
                 {   x[ 1 ] * x[ 2 ],    x[ 0 ] * x[ 2 ], x[ 0 ] * x[ 1 ] } };

    floatOuts = { { -1 }, { -1, -2, -3 }, { 4, 5, 6 } };
    intOuts = { { 1, 2, 8 } };

    return NULL;
}

errorOut nlFxn2( const floatVector &x, const floatMatrix &floatArgs, const intMatrix &intArgs,
                 floatVector &residual ){
    /*!
     * A non-linear function for use in testing the solver. The same as the previously overloaded function
     * except this function obfuscates the computation of the Jacobian, the floatOuts, and the intOuts.
     *
     * \param &x: The variable vector
     * \param &floatArgs: Floating point arguments to the function
     * \param &intArgs: Integer arguments to the function
     * \param &residual: The residual vector output.
     */

    floatMatrix Jtmp;
    floatMatrix fO;
    intMatrix iO;
    return nlFxn2( x, floatArgs, intArgs, residual, Jtmp, fO, iO );
}

errorOut nlFxn3( const floatVector &x, const floatMatrix &floatArgs, const intMatrix &intArgs,
                 floatVector &residual, floatMatrix &jacobian, floatMatrix &floatOuts,
                 intMatrix &intOuts ){
    /*!
     * A non-linear function for use in testing the solver which will
     * require the use of the line-search algorithm. The function is of the form
     *
     * \f$R = exp( -x ) - 1\f$
     *
     * The function also sets floatOuts to
     *
     * \f$\left [ \left [ -1 \right ], \left [ -1, -2, -3 \right ], \left [ 4, 5, 6 \right ] \right ]\f$
     *
     * And intOuts to
     *
     * \f$\left [ \left [ 1, 2 8 \right ] \right ]\f$
     *
     * \param &x: The variable vector. Size 1.
     * \param &floatArgs: Floating point arguments to the function
     * \param &intArgs: Integer arguments to the function
     * \param &residual: The residual vector output.
     * \param &jacobian: The jacobian output.
     * \param &floatOuts: Additional floating point outputs.
     * \param &intOuts: Additional integer outputs.
     */

    residual = { std::exp( -x[ 0 ] ) - 1 };
    jacobian = { { -std::exp( -x[ 0 ] ) } };
    floatOuts = { { -1 }, { -1, -2, -3 }, { 4, 5, 6 } };
    intOuts = { { 1, 2, 8 } };
    return NULL;
}

errorOut nlFxn3( const floatVector &x, const floatMatrix &floatArgs, const intMatrix &intArgs,
                 floatVector &residual ){
    /*!
     * A non-linear function for use in testing the solver which will
     * require the use of the line-search algorithm. The same as the overloaded nlFxn3 except
     * this obfuscates the jacobian, the floatOuts, and the intOuts.
     *
     * \param &x: The variable vector. Size 1.
     * \param &floatArgs: Floating point arguments to the function
     * \param &intArgs: Integer arguments to the function
     * \param &residual: The residual vector output.
     */
    floatMatrix Jtmp;
    floatMatrix fO;
    intMatrix iO;
    return nlFxn3( x, floatArgs, intArgs, residual, Jtmp, fO, iO );
}

errorOut nlFxn4( const floatVector &x, const floatMatrix &floatArgs, const intMatrix &intArgs,
                 floatVector &residual, floatMatrix &jacobian, floatMatrix &floatOuts,
                 intMatrix &intOuts ){
    /*!
     * A non-linear function for use in testing the solver which will
     * require the use of the line-search algorithm. The function is of the form
     *
     * \f$R = tanh( x )\f$
     *
     * \param &x: The variable vector. Of size 1.
     * \param &floatArgs: Floating point arguments to the function. Unused.
     * \param &intArgs: Integer arguments to the function. Unused.
     * \param &residual: The residual vector output.
     * \param &jacobian: The jacobian output.
     * \param &floatOuts: Additional floating point outputs. Unused.
     * \param &intOuts: Additional integer outputs. Unused.
     */

    residual = { std::tanh( x[ 0 ] ) };
    jacobian = { { ( std::cosh( x[ 0 ] ) * std::cosh( x[ 0 ] ) - std::sinh( x[ 0 ] ) * std::sinh( x[ 0 ] ) ) / ( std::cosh( x[ 0 ] ) * std::cosh( x[ 0 ] ) ) } };
    return NULL;
}

errorOut nlFxn5( const floatVector &x, const floatMatrix &floatArgs, const intMatrix &intArgs,
                 floatVector &residual, floatMatrix &jacobian, floatMatrix &floatOuts,
                 intMatrix &intOuts ){
    /*!
     * A non-linear function for use in testing the solver which will require
     * the use of the bounded homotopy solver. This function performs error checking on the
     * floatArgs, intArgs, floatOuts, and intOuts arrays. The function is of the form.
     *
     *  \f$ R = \left [ ( x - 1 ) ( x + 1 ) ( x + 1 ) ( x - 0.25 ) ( x + 0.1 ) \right ]\f$
     *
     * The expected values for `floatArgs` are:
     *
     * \f$ floatArgs = \left [ \left [ 0.1, 0.2, 0.3, 0.4 \right ], \left [ -0.01, -0.02 \right ] \right ] \f$
     *
     * The expected values for `intArgs` are
     *
     * \f$ intArgs = \left [ \left [ -1, -2, -3 \right ], \left [ 5, 4, 3, 2 \right ], \left [ 8, 9, 9 \right ] \right ]\f$
     *
     * The expected incoming values for `floatOuts` are
     *
     * \f$ floatOuts = \left [ \left [ 0, 1, 2 \right ], \left [ 7, -6 \right ], \left [ 0.24, 0.25 \right ] \right ]\f$
     *
     * The expected incoming values for `intOuts` are
     *
     * \f$ intOuts = \left [ \left [ 1, 2, 3 \right ], \left [ -5, 6, 7, 8 \right ] \right ] \f$
     *
     * The function currently throws an error if the expected values are not provided. `floatOuts` is
     * updated to
     *
     * \f$ floatOuts = \left [ \left [ 0.1, 1.1, 2.1 \right ], \left [ 7, -6 \right ], \left [ 0, 1, 2 \right ] \right ]\f$
     *
     * `intOuts` is updated to
     *
     * \f$ intOuts = \left [ \left [ -1, 0, 1 \right ], \left [ 1, 2, 3 \right ], \left [ -5, 6, 7, 8 \right ] \right ] \f$
     *
     * \param &x: The variable vector. One value.
     * \param &floatArgs: Floating point arguments to the function
     * \param &intArgs: Integer arguments to the function
     * \param &residual: The residual vector output.
     * \param &jacobian: The jacobian output.
     * \param &floatOuts: Additional floating point outputs.
     * \param &intOuts: Additional integer outputs.
     */

    //floatArgs answers
    floatVector answer1 = { .1, .2, .3, .4 };
    floatVector answer2 = { -0.01, -0.02 };

    //IntArgs answers
    intVector answer3 = { -1, -2, -3 };
    intVector answer4 = { 5, 4, 3, 2 };
    intVector answer5 = { 8, 9, 9 };

    //floatOuts answers
    floatVector answer6 = { 0, 1, 2 };
    floatVector answer7 = { 7, -6 };
    floatVector answer8 = { .24, .25 };

    //intOuts answers
    intVector   answer9  = { 1, 2, 3 };
    intVector   answer10 = { -5, 6, 7, 8 };

    //x tests
    if ( x.size( ) != 1 ){
        return new errorNode( "nlFxn5", "The x vector should have a size of 1" );
    }

    //floatArgs tests
    if ( floatArgs.size( ) != 2 ){
        return new errorNode( "nlFxn5", "The floatArgs matrix should have two values" );
    }

    if ( !vectorTools::fuzzyEquals( floatArgs[ 0 ], answer1 ) ){
        return new errorNode( "nlFxn5", "The first value of floatArgs should be { 0.1, 0.2, 0.3, 0.4 }" );
    }

    if ( !vectorTools::fuzzyEquals( floatArgs[ 1 ], answer2 ) ){
        return new errorNode( "nlFxn5", "The second value of floatArgs should be { -0.01, -0.02 }" );
    }

    //intArgs tests
    if ( intArgs.size( ) != 3 ){
        return new errorNode( "nlFxn5", "The intArgs matrix should have three values" );
    }

    if ( !vectorTools::fuzzyEquals( intArgs[ 0 ], answer3 ) ){
        return new errorNode( "nlFxn5", "The first value of intargs should be { -1, -2, -3 }" );
    }

    if ( !vectorTools::fuzzyEquals( intArgs[ 1 ], answer4 ) ){
        return new errorNode( "nlFxn5", "The second value of intargs should be { 5, 4, 3, 2 }" );
    }

    if ( !vectorTools::fuzzyEquals( intArgs[ 2 ], answer5 ) ){
        return new errorNode( "nlFxn5", "The third value of intargs should be { 8, 9, 9 }" );
    }

    //floatOuts tests
    if ( floatOuts.size( ) != 3 ){
        return new errorNode( "nlFxn5", "The floatOuts matrix should have three values" );
    }

    if ( !vectorTools::fuzzyEquals( floatOuts[ 0 ], answer6 ) ){
        return new errorNode( "nlFxn5", "The first values in the floatOuts should be { 0, 1, 2 }" );
    }

    if ( !vectorTools::fuzzyEquals( floatOuts[ 1 ], answer7 ) ){
        return new errorNode( "nlFxn5", "The second values in the floatOuts should be { 7, -6 }" );
    }

    if ( !vectorTools::fuzzyEquals( floatOuts[ 2 ], answer8 ) ){
        return new errorNode( "nlFxn5", "The third values in the floatOuts should be { 0.24, 0.25 }" );
    }

    //intOuts tests
    if ( intOuts.size( ) != 2 ){
        return new errorNode( "nlFxn5", "The intOuts matrix must have a size of 2" );
    }

    if ( !vectorTools::fuzzyEquals( intOuts[ 0 ], answer9 ) ){
        return new errorNode( "nlFxn5", "The first values in the intOuts should be { 1, 2, 3 }" );
    }

    if ( !vectorTools::fuzzyEquals( intOuts[ 1 ], answer10 ) ){
        return new errorNode( "nlFxn5", "The second values in the intOuts should be { -5, 6, 7, 8 }" );
    }

    residual = { ( x[ 0 ] - 1. ) * ( x[ 0 ] + 1 ) * ( x[ 0 ] - 0.25 ) * ( x[ 0 ] + 0.1 ) };

    jacobian = { {  ( x[ 0 ] + 1. ) * ( x[ 0 ] - 0.25 ) * ( x[ 0 ] + 0.1  )
                  + ( x[ 0 ] - 1. ) * ( x[ 0 ] - 0.25 ) * ( x[ 0 ] + 0.1  )
                  + ( x[ 0 ] - 1. ) * ( x[ 0 ] + 1.   ) * ( x[ 0 ] + 0.1  )
                  + ( x[ 0 ] - 1. ) * ( x[ 0 ] + 1.   ) * ( x[ 0 ] - 0.25 ) } };

    floatOuts = { floatOuts[ 0 ] + 0.1, floatOuts[ 1 ], floatOuts[ 0 ] };
    intOuts = { intOuts[ 0 ] - 2, intOuts[ 0 ], intOuts[ 1 ] };

    return NULL;
}

errorOut nlFxn6( const floatVector &x, const floatMatrix &floatArgs, const intMatrix &intArgs,
                 floatVector &residual, floatMatrix &jacobian, floatMatrix &floatOuts,
                 intMatrix &intOuts ){
    /*!
     * A non-linear function for use in testing the solver which will require
     * the use of the bounded homotopy solver. The function is of the form
     *
     * \f$R = \left [ \left ( x - 1 \right ) \left ( x + 1 \right ) \left ( x - 0.25 \right ) \left ( x + 0.1 \right ), \left ( y - 1 \right ) \left ( y - 1 \right ), \left ( x + 5 \right ) \left ( z + 1 \right ) \right ]\f$
     *
     * \param &x: The variable vector. Three values required.
     * \param &floatArgs: Floating point arguments to the function. Unused.
     * \param &intArgs: Integer arguments to the function. Unused.
     * \param &residual: The residual vector output.
     * \param &jacobian: The jacobian output.
     * \param &floatOuts: Additional floating point outputs. Unused.
     * \param &intOuts: Additional integer outputs. Unused.
     */

    floatType x1 = x[ 0 ];
    floatType x2 = x[ 1 ];
    floatType x3 = x[ 2 ];

    residual.resize( 3 );

    residual[ 0 ] = ( x1 - 1 )*( x1 + 1 )*( x1 - 0.25 )*( x1 + 0.1 );
    residual[ 1 ] = ( x2 - 1 ) * ( x2 - 1 );
    residual[ 2 ] = ( x1 + 5 ) * ( x3 + 1 );

    floatType dr1dx1 = ( x1 - 1 ) * ( x1 - 0.25 ) * ( x1 + 0.1 )
                     + ( x1 - 1 ) * ( x1 - 0.25 ) * ( x1 + 1 )
                     + ( x1 - 1 ) * ( x1 + 0.1 ) * ( x1 + 1 )
                     + ( x1 - 0.25 ) * ( x1 + 0.1 ) * ( x1 + 1 );

    floatType dr1dx2 = 0.;
    floatType dr1dx3 = 0.;

    floatType dr2dx1 = 0.;
    floatType dr2dx2 = 2 * ( x2 - 1 );
    floatType dr2dx3 = 0.;

    floatType dr3dx1 = x3 + 1;
    floatType dr3dx2 = 0.;
    floatType dr3dx3 = x1 + 5;

    jacobian = { { dr1dx1, dr1dx2, dr1dx3 },
                 { dr2dx1, dr2dx2, dr2dx3 },
                 { dr3dx1, dr3dx2, dr3dx3 } };

    return NULL;
}

errorOut nlFxn7( const floatVector &x, const floatMatrix &floatArgs, const intMatrix &intArgs,
                 floatVector &residual, floatMatrix &jacobian, floatMatrix &floatOuts,
                 intMatrix &intOuts ){
    /*!
     * A non-linear function for use in testing the solver which will require
     * the use of the bounded homotopy solver. The function is of the form
     *
     * \f$R = log( x )\f$
     *
     * \param &x: The variable vector. One value required.
     * \param &floatArgs: Floating point arguments to the function. Unused.
     * \param &intArgs: Integer arguments to the function. Unused.
     * \param &residual: The residual vector output.
     * \param &jacobian: The jacobian output.
     * \param &floatOuts: Additional floating point outputs. Unused.
     * \param &intOuts: Additional integer outputs. Unused.
     */

    if ( x.size( ) != 1 ){
        return new errorNode( "nlFxn7", "The x vector must have a size of 1" );
    }

    residual = { std::log( x[ 0 ] ) };

    jacobian = { { 1. / x[ 0 ] } };

    return NULL;
}

errorOut lagrangian1( const floatVector &x, const floatMatrix &floatArgs, const intMatrix &intArgs,
                      floatType &value, floatVector &gradient, floatMatrix &floatOuts, intMatrix &intOuts ){
    /*!
     * A lagrangian used to test the optimization tools. The function is of the form.
     *
     * \f$ L = ( x - 1 ) ( x + 3 )\f$
     *
     * `floatOuts` is updated to
     *
     * \f$floatOuts = \left [ \left [ 1, 2, 3 \right ], \left [ -0.4, -0.5, -0.6 \right ] \right ] \f$
     *
     * `intOuts` is updated to
     *
     * \f$intOuts = \left [ \left [ 5, 6, 7 \right ], \left [ 8 \right ], \left [ 9, 10 \right ] \right ]\f$
     *
     * \param &x: A vector of the variable to be solved. One value required.
     * \param &floatArgs: Additional floating point arguments to residual. Unused.
     * \param &intArgs: Additional integer arguments to the residual. Unused.
     * \param &value: The value of the Lagrangian
     * \param &gradient: The gradient of the Lagrangian
     * \param &floatOuts: Additional floating point values to return.
     * \param &intOuts: Additional integer values to return.
     */

    if ( x.size( ) != 1 ){
        return new errorNode( "lagrangian1", "The x vector must have a size of 1" );
    }

    value = ( x[ 0 ] - 1 ) * ( x[ 0 ] + 3 );
    gradient = { ( x[ 0 ] + 3 ) + ( x[ 0 ] - 1 ) };

    floatOuts = { { 1, 2, 3 }, { -0.4, -0.5, -0.6 } };
    intOuts = { { 5, 6, 7 }, { 8 }, { 9, 10 } };

    return NULL;
}

errorOut lagrangian2( const floatVector &x, const floatMatrix &floatArgs, const intMatrix &intArgs,
                      floatType &value, floatVector &gradient, floatMatrix &floatOuts, intMatrix &intOuts ){
    /*!
     * A lagrangian used to test the optimization tools
     *
     * \param &x: A vector of the variable to be solved.
     * \param &floatArgs: Additional floating point arguments to residual
     * \param &intArgs: Additional integer arguments to the residual
     * \param &value: The value of the Lagrangian
     * \param &gradient: The gradient of the Lagrangian
     * \param &floatOuts: Additional floating point values to return.
     * \param &intOuts: Additional integer values to return.
     */

    if ( x.size( ) != 3 ){
        return new errorNode( "lagrangian2", "The x vector must have a size of 3" );
    }

    if ( floatOuts.size( ) != 1 ){
        return new errorNode( "lagrangian2", "The floatOuts must have a size of 1" );
    }

    if ( intOuts.size( ) != 1 ){
        return new errorNode( "lagrangian2", "The intOuts must have a size of 1" );
    }

    if ( !vectorTools::fuzzyEquals( floatOuts[ 0 ], { 0.1, 0.2, 0.3, 0.4 } ) ){
        return new errorNode( "lagrangian2", "The first value of the floatOuts is incorrect" );
    }

    if ( !vectorTools::fuzzyEquals( intOuts[ 0 ], { -1, -2 } ) ){
        return new errorNode( "lagrangian2", "The first value of the intOuts is incorrect" );
    }

    floatType _x = x[ 0 ];
    floatType _y = x[ 1 ];
    floatType _L = x[ 2 ];

    value = _x + _y + _L * ( _x * _x + _y * _y - 1 );

    gradient = { 1 + 2 * _L * _x,
                 1 + 2 * _L * _y,
                 _x * _x + _y * _y - 1 };

    floatOuts = { { 1, 2, 3 }, { -0.4, -0.5, -0.6 }, { 7, 6, 5 } };
    intOuts = { { -4 }, { 5, 6, 7 }, { 8 }, { 9, 10 } };

    return NULL;
}

errorOut lagrangian3( const floatVector &x, const floatMatrix &floatArgs, const intMatrix &intArgs,
                      floatType &value, floatVector &gradient, floatMatrix &floatOuts, intMatrix &intOuts
                    ){
    /*!
     * A lagrangian used to test the optimization tools. The function is of the form
     *
     * \f$L = x^2 y + z * \left ( x^2 y^2 - 3 \right ) \f$
     *
     * `floatOuts` is expected to have an incoming value of
     *
     * \f$ floatOuts = \left [ \left [ 0.1, 0.2, 0.3, 0.4 \right ] \right ] \f$
     *
     * `intOuts` is expected to have an incoming value of
     *
     * \f$ intOuts = \left [ \left [ 0 \right ], \left [ -1, -2 \right ] \right ] \f$
     *
     * `floatOuts is updated to
     *
     * \f$ floatOuts = \left [ \left [ 1, 2, 3 \right ], \left [ -0.4, -0.5, -0.6 \right ], \left [ 7, 6, 5 \right ] \right ] \f$
     *
     * `intOuts` is updated to
     *
     * \f$ intOuts = \left [ \left [ -4 \right ], \left [ 5, 6, 7 \right ], \left [ 8 \right ], \left [ 9, 10 \right ] \right ] \f$
     *
     * \param &x: A vector of the variable to be solved.
     * \param &floatArgs: Additional floating point arguments to residual
     * \param &intArgs: Additional integer arguments to the residual
     * \param &value: The value of the Lagrangian
     * \param &gradient: The gradient of the Lagrangian
     * \param &floatOuts: Additional floating point values to return.
     * \param &intOuts: Additional integer values to return.
     */

    if ( x.size( ) != 3 ){
        return new errorNode( "lagrangian3", "The x vector must have a size of 3" );
    }

    if ( floatOuts.size( ) != 1 ){
        return new errorNode( "lagrangian3", "The floatOuts must have a size of 1" );
    }

    if ( intOuts.size( ) != 1 ){
        return new errorNode( "lagrangian3", "The intOuts must have a size of 1" );
    }

    if ( !vectorTools::fuzzyEquals( floatOuts[ 0 ], { 0.1, 0.2, 0.3, 0.4 } ) ){
        return new errorNode( "lagrangian3", "The first value of the floatOuts is incorrect" );
    }

    if ( !vectorTools::fuzzyEquals( intOuts[ 0 ], { -1, -2 } ) ){
        return new errorNode( "lagrangian3", "The first value of the intOuts is incorrect" );
    }

    floatType _x = x[ 0 ];
    floatType _y = x[ 1 ];
    floatType _L = x[ 2 ];

    value = _x * _x * _y + _L * ( _x * _x + _y * _y - 3 );

    gradient = { 2 * _x * _y + 2 * _L * _x,
                 _x * _x + 2 * _L * _y,
                 _x * _x + _y * _y - 3 };

    floatOuts = { { 1, 2, 3 }, { -0.4, -0.5, -0.6 }, { 7, 6, 5 } };
    intOuts = { { -4 }, { 5, 6, 7 }, { 8 }, { 9, 10 } };

    return NULL;
}

BOOST_AUTO_TEST_CASE( testCheckTolerance ){
    /*!
     * Test the tolerance checking function.
     */

    floatVector R   = {  1,   2, 3.00000, -4.0 };
    floatVector tol = { 1.5, 2.1, 3.00001,  4.1 };
    bool result;

    errorOut error = solverTools::checkTolerance( R, tol, result );

    BOOST_CHECK( ! error );

    BOOST_CHECK( result );

    tol[ 0 ] = .98;

    error = solverTools::checkTolerance( R, tol, result );

    BOOST_CHECK( ! error );

    BOOST_CHECK( ! result );

    tol[ 0 ] = 1.5;
    tol[ 3 ] = 3.8;
    error = solverTools::checkTolerance( R, tol, result );

    BOOST_CHECK( ! error );

    BOOST_CHECK( ! result );

    tol = { 1.6, 2.5 };

    error = solverTools::checkTolerance( R, tol, result );

    BOOST_CHECK( error );

}

BOOST_AUTO_TEST_CASE( testNewtonRaphson ){
    /*!
     * Tests of the Newton-Raphson solver
     *
     * \param &results: The output file
     */

    //The first test
    floatVector x0 = { 1.5, 6 };
    floatVector x;
    bool converged, fatalError;

    solverTools::stdFncNLFJ func;
    func = static_cast< solverTools::NonLinearFunctionWithJacobian >( nlFxn1 );

    floatMatrix floatOut;
    intMatrix intOut;
    errorOut error = solverTools::newtonRaphson( func, x0, x, converged, fatalError, floatOut, intOut, { }, { } );

    BOOST_CHECK( ! error );

    floatVector Rtmp;
    floatMatrix Jtmp;
    floatMatrix fO;
    intMatrix iO;
    error = nlFxn1( x, { }, { }, Rtmp, Jtmp, fO, iO );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( Rtmp, { 0, 0 } ) );

    //The second test
    x0 = { 1, 1, 1 };
    floatOut.clear( );
    intOut.clear( );
    fO.clear( );
    iO.clear( );

    func = static_cast< solverTools::NonLinearFunctionWithJacobian >( nlFxn2 );
    error = solverTools::newtonRaphson( func, x0, x, converged, fatalError, floatOut, intOut, { }, { } );

    BOOST_CHECK( ! error );

    error = nlFxn2( x, { }, { }, Rtmp, Jtmp, fO, iO );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( Rtmp, { 0, 0, 0 } ) );

    //The third test
    x0 = { 3 };
    floatOut.clear( );
    intOut.clear( );
    fO.clear( );
    iO.clear( );

    func = static_cast< solverTools::NonLinearFunctionWithJacobian >( nlFxn3 );
    error = solverTools::newtonRaphson( func, x0, x, converged, fatalError, floatOut, intOut, { }, { } );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( x, { 0 } ) );

    //The fourth test. Tests the bounded Newton method
    x0 = { 10. };
    floatOut.clear( );
    intOut.clear( );
    fO.clear( );
    iO.clear( );

    func = static_cast< solverTools::NonLinearFunctionWithJacobian >( nlFxn7 );

    solverTools::solverType linearSolver;
    floatMatrix J;

    intVector boundVariableIndices = { 0 };
    intVector boundSigns = { 0 };
    floatVector boundValues = { 1e-9 };
    floatMatrix Jexp = { { 1. } };

    error = solverTools::newtonRaphson( func, x0, x, converged, fatalError, floatOut, intOut, { }, { }, linearSolver, J );

    BOOST_CHECK( ! error );
    BOOST_CHECK( vectorTools::fuzzyEquals( x, { 1. } ) );

    BOOST_CHECK( vectorTools::fuzzyEquals( J, Jexp ) );

    //The fifth test: This test makes sure that when a Newton-Raphson iteration fails it
    //correctly returns a failure to converge.
    x0 = { -5. };
    floatOut.clear( );
    intOut.clear( );

    func = static_cast< solverTools::NonLinearFunctionWithJacobian >( nlFxn4 );

    error = solverTools::newtonRaphson( func, x0, x, converged, fatalError, floatOut, intOut, { }, { }, 5 );

    BOOST_CHECK( ! converged );

}

BOOST_AUTO_TEST_CASE( testFiniteDifference ){
    /*!
     * Test the finite difference jacobian calculator.
     */

    //The first test
    floatVector x0 = { 1.5, 6 };
    floatMatrix J;

    solverTools::stdFncNLF func;
    func = static_cast<solverTools::NonLinearFunction>( nlFxn1 );
    solverTools::finiteDifference( func, x0, J, { }, { } );

    floatVector Rtmp;
    floatMatrix result;
    floatMatrix floatOuts;
    intMatrix intOuts;
    nlFxn1( x0, { }, { }, Rtmp, result, floatOuts, intOuts );

    BOOST_CHECK( vectorTools::fuzzyEquals( J, result ) );

    //The second test
    x0 = { 1, 1, 1 };
    floatOuts.clear( );
    intOuts.clear( );
    func = static_cast<solverTools::NonLinearFunction>( nlFxn2 );
    solverTools::finiteDifference( func, x0, J, { }, { } );
    nlFxn2( x0, { }, { }, Rtmp, result, floatOuts, intOuts );

    BOOST_CHECK( vectorTools::fuzzyEquals( J, result ) );

}

BOOST_AUTO_TEST_CASE( testCheckJacobian ){
    /*!
     * Test the jacobian checking utility.
     */

    //The first test

    solverTools::stdFncNLFJ func;
    func = static_cast<solverTools::NonLinearFunctionWithJacobian>( nlFxn1 );
    bool isGood = false;
    floatType eps = 1e-6;
    floatType tolr = 1e-6;
    floatType tola = 1e-6;
    bool suppressOutput = true;

    floatVector x0 = { 0, 0 };
    errorOut error = solverTools::checkJacobian( func, x0, { }, { }, isGood, eps, tolr, tola, suppressOutput );

    BOOST_CHECK( ! error );

    BOOST_CHECK( isGood );

    //The second test
    solverTools::stdFncNLFJ badfunc;
    badfunc = [ & ]( const floatVector &x_, const floatMatrix &floatArgs_, const intMatrix &intArgs_,
                            floatVector &r, floatMatrix &j, floatMatrix &fO, intMatrix &iO ){
        errorOut e = func( x_, floatArgs_, intArgs_, r, j, fO, iO );
        j[ 0 ][ 1 ] = 0.1;
        return e;
    };

    error = solverTools::checkJacobian( badfunc, x0, { }, { }, isGood, eps, tolr, tola, suppressOutput );

    BOOST_CHECK( ! error );

    BOOST_CHECK( ! isGood );

}

BOOST_AUTO_TEST_CASE( testCheckLSCriteria ){
    /*!
     * Test the line search criteria
     */

    floatVector R  = { 1, 2, 3, 4, 5, 6 };
    floatVector Rp = { 2, 3, 4, 5, 6, 7 };
    bool result;

    solverTools::checkLSCriteria( R, Rp, result );

    BOOST_CHECK( result );

    R[ 0 ] = 100;

    solverTools::checkLSCriteria( R, Rp, result );

    BOOST_CHECK( ! result );

}

BOOST_AUTO_TEST_CASE( testHomotopySolver ){
    /*!
     * Test the Homotopy solver.
     */

    //The first test
    floatVector x0 = { 1.5, 6 };
    floatVector x;
    bool converged, fatalErrorFlag;
    floatMatrix floatOuts;
    intMatrix intOuts;

    solverTools::stdFncNLFJ func;
    func = static_cast< solverTools::NonLinearFunctionWithJacobian >( nlFxn1 );

    errorOut error = solverTools::homotopySolver( func, x0, x, converged, fatalErrorFlag, floatOuts, intOuts, { }, { } );

    BOOST_CHECK( ! error );

    floatVector Rtmp;
    floatMatrix Jtmp;
    floatMatrix fO;
    intMatrix iO;
    error = nlFxn1( x, { }, { }, Rtmp, Jtmp, fO, iO );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( Rtmp, { 0, 0 } ) );

    //The second test
    x0 = { 1, 1, 1 };
    floatOuts.clear( );
    intOuts.clear( );
    fO.clear( );
    iO.clear( );

    func = static_cast< solverTools::NonLinearFunctionWithJacobian >( nlFxn2 );
    error = solverTools::homotopySolver( func, x0, x, converged, fatalErrorFlag, floatOuts, intOuts, { }, { } );

    BOOST_CHECK( ! error );

    error = nlFxn2( x, { }, { }, Rtmp, Jtmp, fO, iO );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( Rtmp, { 0, 0, 0 } ) );

    //The third test
    x0 = { 3 };
    floatOuts.clear( );
    intOuts.clear( );
    fO.clear( );
    iO.clear( );

    func = static_cast< solverTools::NonLinearFunctionWithJacobian >( nlFxn3 );
    error = solverTools::homotopySolver( func, x0, x, converged, fatalErrorFlag, floatOuts, intOuts, { }, { },
                                         20, 1e-9, 1e-9, 1e-4, 5, 0.2, 0.01 );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( x, { 0 } ) );

    //The fourth test
    x0 = { 3 };
    floatOuts.clear( );
    intOuts.clear( );

    func = static_cast< solverTools::NonLinearFunctionWithJacobian >( nlFxn4 );

    error = solverTools::homotopySolver( func, x0, x, converged, fatalErrorFlag, floatOuts, intOuts, { }, { },
                                         20, 1e-9, 1e-9, 1e-4, 4, 1.0, 0.1 );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( x, { 0 } ) );

    //The fifth test ( hard bounds )
    x0 = { 10 };
    floatOuts.clear( );
    intOuts.clear( );

    solverTools::solverType linearSolver;
    floatMatrix J, Jexp;

    Jexp = { { 1 } };

    intVector variableIndices = { 0 };
    intVector barrierSigns = { 0 };
    floatVector barrierValues = { 1e-9 };

    func = static_cast< solverTools::NonLinearFunctionWithJacobian >( nlFxn7 );

    error = solverTools::homotopySolver( func, x0, x, converged, fatalErrorFlag, floatOuts, intOuts, { }, { },
                                         linearSolver, J, variableIndices, barrierSigns, barrierValues,
                                         false, 20, 1e-9, 1e-9, 1e-4, 4, 1.0, 0.1 );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( x, { 1 } ) );

    BOOST_CHECK( vectorTools::fuzzyEquals( J, Jexp ) );

}

BOOST_AUTO_TEST_CASE( test_aFxn ){
    /*!
     * Test the computation of the "\f$a\f$" parameter in the Barrier function.
     */

    floatType pseudoT = .72;
    floatType logAfxn = 5.2;

    floatType answer  = 42.26671935907283;

    floatType result;

    errorOut error = solverTools::aFxn( pseudoT, logAfxn, result );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( result, answer ) );

    floatType dadT;

    error = solverTools::aFxn( pseudoT, logAfxn, result, dadT );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( result, answer ) );

    floatType eps = 1e-6;
    floatType delta = eps * pseudoT + eps;

    floatType aP, aM;

    error = solverTools::aFxn( pseudoT + delta, logAfxn, aP );

    BOOST_CHECK( ! error );

    error = solverTools::aFxn( pseudoT - delta, logAfxn, aM );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( ( aP - aM ) / ( 2 * delta ), dadT ) );

}

BOOST_AUTO_TEST_CASE( test_computeBarrierFunction ){
    /*!
     * Test the computation of the boundary function
     */

    floatType x        = 0.4;
    floatType pseudoT  = 0.25;
    floatType logAmax  = 5;
    floatType b        = 0.14;

    floatType negativeSignAnswer = -0.5964638357684787;
    floatType positiveSignAnswer = 1.4780926435784547;

    floatType result;

    errorOut error = solverTools::computeBarrierFunction( x, pseudoT, logAmax, b, false, result );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( result, negativeSignAnswer ) );

    error = solverTools::computeBarrierFunction( x, pseudoT, logAmax, b, true, result );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( result, positiveSignAnswer ) );

    //Test the Jacobians
    floatType dbdx, dbdt;

    //Test the Jacobians when the sign is negative
    error = solverTools::computeBarrierFunction( x, pseudoT, logAmax, b, false, result, dbdx, dbdt );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( result, negativeSignAnswer ) );

    floatType eps = 1e-6;

    floatType dx = eps * fabs( x ) + eps;
    floatType dt = eps * fabs( pseudoT ) + eps;

    floatType bP, bM;

    error = solverTools::computeBarrierFunction( x + dx, pseudoT, logAmax, b, false, bP );

    BOOST_CHECK( ! error );

    error = solverTools::computeBarrierFunction( x - dx, pseudoT, logAmax, b, false, bM );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( ( bP - bM ) / ( 2 * dx ), dbdx ) );

    error = solverTools::computeBarrierFunction( x, pseudoT + dt, logAmax, b, false, bP );

    BOOST_CHECK( ! error );

    error = solverTools::computeBarrierFunction( x, pseudoT - dt, logAmax, b, false, bM );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( ( bP - bM ) / ( 2 * dt ), dbdt ) );

    //Test the Jacobians when the sign is positive
    error = solverTools::computeBarrierFunction( x, pseudoT, logAmax, b, true, result, dbdx, dbdt );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( result, positiveSignAnswer ) );

    error = solverTools::computeBarrierFunction( x + dx, pseudoT, logAmax, b, true, bP );

    BOOST_CHECK( ! error );

    error = solverTools::computeBarrierFunction( x - dx, pseudoT, logAmax, b, true, bM );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( ( bP - bM ) / ( 2 * dx ), dbdx ) );

    error = solverTools::computeBarrierFunction( x, pseudoT + dt, logAmax, b, true, bP );

    BOOST_CHECK( ! error );

    error = solverTools::computeBarrierFunction( x, pseudoT - dt, logAmax, b, true, bM );

    BOOST_CHECK( ! error );

    BOOST_CHECK( vectorTools::fuzzyEquals( ( bP - bM ) / ( 2 * dt ), dbdt ) );

}

BOOST_AUTO_TEST_CASE( test_computeBarrierHomotopyResidual ){
    /*!
     * Test the computation of the barrier homotopy residual
     */

    solverTools::stdFncNLFJ func;
    func = static_cast<solverTools::NonLinearFunctionWithJacobian>( nlFxn5 );

    floatVector x0 = { 0. };
    floatMatrix floatArgsDefault =
        {
            { 0.28 }, //The pseudo-time
            { 0.1 },  //The barrier value
            { 10. },  //The logAMax values
            { .1, .2, .3, .4 }, //Function Parameters
            { -0.01, -0.02 }
        };

    intMatrix intArgsDefault =
        {
            { 0 }, //Variable indices
            { 0 }, //Residual indices
            { 0 }, //Barrier signs
            { -1, -2, -3 }, //Function parameters
            { 5, 4, 3, 2 },
            { 8, 9, 9 }
        };

    floatVector residualResult;
    floatMatrix jacobian;

    floatMatrix floatOutsDefault =
        {
            { 0, 1, 2 },
            { 7, -6 },
            { .24, .25 }
        };

    intMatrix intOutsDefault =
        {
            { 1, 2, 3 },
            { -5, 6, 7, 8 },
        };

    floatMatrix floatArgs = floatArgsDefault;
    intMatrix   intArgs   = intArgsDefault;
    floatMatrix floatOuts = floatOutsDefault;
    intMatrix   intOuts   = intOutsDefault;

    floatVector residualAnswer = { 0.2775586103363596 };

#ifdef DEBUG_MODE
    debugMap DEBUG;
#endif

    errorOut error = solverTools::computeBarrierHomotopyResidual( func, x0, floatArgs, intArgs, residualResult, jacobian,
                                                                  floatOuts, intOuts
#ifdef DEBUG_MODE
                                                                  , DEBUG
#endif
                                                                );

    BOOST_CHECK( ! error  );

    BOOST_CHECK( vectorTools::fuzzyEquals( residualAnswer, residualResult ) );

    //Check that the non-homotopy outputs are as expected.
    BOOST_CHECK( floatOuts.size( ) == 4 );

    BOOST_CHECK( vectorTools::fuzzyEquals( floatOuts[ 1 ], floatOutsDefault[ 0 ] + 0.1 ) );

    BOOST_CHECK( vectorTools::fuzzyEquals( floatOuts[ 2 ], floatOutsDefault[ 1 ] ) );

    BOOST_CHECK( vectorTools::fuzzyEquals( floatOuts[ 3 ], floatOutsDefault[ 0 ] ) );

    BOOST_CHECK( intOuts.size( ) == 3 );

    BOOST_CHECK( vectorTools::fuzzyEquals( intOuts[ 0 ], intOutsDefault[ 0 ] - 2 ) );

    BOOST_CHECK( vectorTools::fuzzyEquals( intOuts[ 1 ], intOutsDefault[ 0 ] ) );

    BOOST_CHECK( vectorTools::fuzzyEquals( intOuts[ 2 ], intOutsDefault[ 1 ] ) );

    //Test the Jacobians
    floatType eps = 1e-6;

    //Test drdx
    floatVector dx = eps * x0 + eps;

    floatVector rP, rM;
    floatMatrix JP, JM;

    floatOuts = floatOutsDefault;
    intOuts   = intOutsDefault;

    error = solverTools::computeBarrierHomotopyResidual( func, x0 + dx, floatArgs, intArgs, rP, JP,
                                                         floatOuts, intOuts );

    BOOST_CHECK( ! error  );

    floatOuts = floatOutsDefault;
    intOuts   = intOutsDefault;

    error = solverTools::computeBarrierHomotopyResidual( func, x0 - dx, floatArgs, intArgs, rM, JM,
                                                         floatOuts, intOuts );

    BOOST_CHECK( ! error  );

    floatVector gradCol = ( rP - rM ) / ( 2 * dx[ 0 ] );

    BOOST_CHECK( vectorTools::fuzzyEquals( gradCol, jacobian[ 0 ] ) );

    //test drdt
    eps = 1e-7;
    floatType dt = eps * floatArgsDefault[ 0 ][ 0 ] + eps;

    floatArgs = floatArgsDefault;
    floatArgs[ 0 ][ 0 ] += dt;

    floatOuts = floatOutsDefault;
    intOuts   = intOutsDefault;

    error = solverTools::computeBarrierHomotopyResidual( func, x0, floatArgs, intArgs, rP, JP,
                                                         floatOuts, intOuts );

    BOOST_CHECK( ! error  );

    floatArgs = floatArgsDefault;
    floatArgs[ 0 ][ 0 ] -= dt;

    floatOuts = floatOutsDefault;
    intOuts   = intOutsDefault;

    error = solverTools::computeBarrierHomotopyResidual( func, x0, floatArgs, intArgs, rM, JM,
                                                         floatOuts, intOuts );

    BOOST_CHECK( ! error  );

    gradCol = ( rP - rM ) / ( 2 * dt );

    BOOST_CHECK( vectorTools::fuzzyEquals( gradCol, floatOuts[ 0 ], 1e-5 ) );

}

BOOST_AUTO_TEST_CASE( test_computeBarrierHomotopyResidual2 ){
    /*!
     * Test for the computation of the barrier homotopy residual.
     */

    solverTools::stdFncNLFJ func;
    func = static_cast<solverTools::NonLinearFunctionWithJacobian>( nlFxn6 );

    floatVector          x = { 0.15, 0.1, -1.2 };
    floatType   pseudoTime = 0.24;

    floatVector logAMaxVals = { 10, 6 };
    floatVector bvals       = { 0.1, -1.1 };

    intVector   variableIndices = { 0, 2 };
    intVector   residualIndices = { 2, 1 };
    intVector   signs           = { 0, 1 };

    floatMatrix floatArgs = { { pseudoTime }, bvals, logAMaxVals };
    intMatrix   intArgs   = { variableIndices, residualIndices, signs };

    floatMatrix floatOuts = { { } };
    intMatrix   intOuts   = { { } };

    floatVector residualAnswer = { 0.0244375 ,  0.53651154, -0.97499937 };

    floatVector residualResult;
    floatMatrix jacobian;

    errorOut error = solverTools::computeBarrierHomotopyResidual( func, x, floatArgs, intArgs, residualResult, jacobian,
                                                                  floatOuts, intOuts );

    BOOST_CHECK( ! error  );

    BOOST_CHECK( vectorTools::fuzzyEquals( residualResult, residualAnswer ) );

    //Tests of the Jacobians

    //Test the Jacobian w.r.t. x
    floatType eps = 1e-7;
    for ( unsigned int i = 0; i < x.size( ); i++ ){
        floatVector delta( x.size( ), 0 );
        delta[ i ] = eps * fabs( x[ i ] ) + eps;

        floatVector rP, rM;

        error = solverTools::computeBarrierHomotopyResidual( func, x + delta, floatArgs, intArgs, rP, jacobian,
                                                             floatOuts, intOuts );

        BOOST_CHECK( ! error  );

        error = solverTools::computeBarrierHomotopyResidual( func, x - delta, floatArgs, intArgs, rM, jacobian,
                                                             floatOuts, intOuts );

        BOOST_CHECK( ! error  );

        floatVector gradCol = ( rP - rM ) / ( 2 * delta[ i ] );

        for ( unsigned int j = 0; j < gradCol.size( ); j++ ){
            BOOST_CHECK( vectorTools::fuzzyEquals( gradCol[ j ], jacobian[ j ][ i ] ) );
        }
    }

    //Test the Jacobian w.r.t. t
    floatType dt = eps * fabs( pseudoTime ) + eps;

    floatVector rP, rM;

    floatArgs[ 0 ][ 0 ] = pseudoTime + dt;

    error = solverTools::computeBarrierHomotopyResidual( func, x, floatArgs, intArgs, rP, jacobian,
                                                         floatOuts, intOuts );

    BOOST_CHECK( ! error  );

    floatArgs[ 0 ][ 0 ] = pseudoTime - dt;

    error = solverTools::computeBarrierHomotopyResidual( func, x, floatArgs, intArgs, rM, jacobian,
                                                         floatOuts, intOuts );

    BOOST_CHECK( ! error  );

    floatVector gradCol = ( rP - rM ) / ( 2 * dt );

    BOOST_CHECK( vectorTools::fuzzyEquals( gradCol, floatOuts[ 0 ] ) );

}

BOOST_AUTO_TEST_CASE( test_barrierHomotopySolver ){
    /*!
     * Test the barrier Homotopy solver. This solver enables the addition of
     * bounds to a non-linear solve which can help prevent solutions from being
     * pulled into undesirable domains without having to resort to computing the
     * Hessian of the residual function as would be required for optimization
     * based techniques.
     */

    solverTools::stdFncNLFJ func;
    func = static_cast<solverTools::NonLinearFunctionWithJacobian>( nlFxn5 );

    floatVector barrierValues = { 0.1 };
    floatVector logAMaxValues = { 10. };
    floatMatrix floatArgsDefault =
        {
            { .1, .2, .3, .4 },
            { -0.01, -0.02 }
        };

    intVector variableIndices = { 0 };
    intVector residualIndices = { 0 };
    intVector barrierSigns    = { 0 };

    intMatrix intArgsDefault =
        {
            { -1, -2, -3 },
            { 5, 4, 3, 2 },
            { 8, 9, 9 }
        };

    floatVector residualResult;
    floatMatrix jacobian;

    floatMatrix floatOutsDefault =
        {
            { 0, 1, 2 },
            { 7, -6 },
            { .24, .25 }
        };

    intMatrix intOutsDefault =
        {
            { 1, 2, 3 },
            { -5, 6, 7, 8 },
        };

    floatMatrix floatArgs = floatArgsDefault;
    intMatrix   intArgs   = intArgsDefault;
    floatMatrix floatOuts = floatOutsDefault;
    intMatrix   intOuts   = intOutsDefault;

    floatType dt = 0.1;
    floatVector x0 = { 0. };
    bool implicitRefine = false;

    bool convergeFlag, fatalErrorFlag;

    floatVector result;
    floatVector answer = { 0.25 };

    errorOut error = solverTools::barrierHomotopySolver( func, dt, x0, variableIndices, residualIndices, barrierSigns,
                                                         barrierValues, logAMaxValues, floatArgs, intArgs,
                                                         implicitRefine, result, convergeFlag, fatalErrorFlag,
                                                         floatOuts, intOuts,
                                                         20, 1e-9, 1e-9, 1e-4, 5, true );

    BOOST_CHECK( ! error  );

    BOOST_CHECK( vectorTools::fuzzyEquals( answer, result ) );

    floatOuts = floatOutsDefault;
    intOuts = intOutsDefault;

    solverTools::solverType linearSolver;

    error = solverTools::barrierHomotopySolver( func, dt, x0, variableIndices, residualIndices, barrierSigns,
                                                         barrierValues, logAMaxValues, floatArgs, intArgs,
                                                         implicitRefine, result, convergeFlag, fatalErrorFlag,
                                                         floatOuts, intOuts, linearSolver, jacobian,
                                                         20, 1e-9, 1e-9, 1e-4, 5, true );

    BOOST_CHECK( ! error  );

    BOOST_CHECK( vectorTools::fuzzyEquals( answer, result ) );

    floatMatrix jacobianResult;

    floatOuts = floatOutsDefault;
    intOuts = intOutsDefault;

    error = func( result, floatArgs, intArgs, residualResult, jacobianResult, floatOuts, intOuts );

    BOOST_CHECK( ! error  );

    BOOST_CHECK( vectorTools::fuzzyEquals( jacobian, jacobianResult ) );

    x0 = { 0. };
    implicitRefine = true;

    floatOuts = floatOutsDefault;
    intOuts = intOutsDefault;

    error = solverTools::barrierHomotopySolver( func, dt, x0, variableIndices, residualIndices, barrierSigns,
                                                barrierValues, logAMaxValues, floatArgs, intArgs,
                                                implicitRefine, result, convergeFlag, fatalErrorFlag,
                                                floatOuts, intOuts,
                                                20, 1e-9, 1e-9, 1e-4, 5, true );

    BOOST_CHECK( ! error  );

    BOOST_CHECK( vectorTools::fuzzyEquals( answer, result ) );

    floatOuts = floatOutsDefault;
    intOuts = intOutsDefault;

    error = solverTools::barrierHomotopySolver( func, dt, x0, variableIndices, residualIndices, barrierSigns,
                                                barrierValues, logAMaxValues, floatArgs, intArgs,
                                                implicitRefine, result, convergeFlag, fatalErrorFlag,
                                                floatOuts, intOuts, linearSolver, jacobian,
                                                20, 1e-9, 1e-9, 1e-4, 5, true );

    BOOST_CHECK( ! error  );

    BOOST_CHECK( vectorTools::fuzzyEquals( answer, result ) );

    floatOuts = floatOutsDefault;
    intOuts = intOutsDefault;

    error = func( result, floatArgs, intArgs, residualResult, jacobianResult, floatOuts, intOuts );

    BOOST_CHECK( ! error  );

    BOOST_CHECK( vectorTools::fuzzyEquals( jacobian, jacobianResult ) );

}

BOOST_AUTO_TEST_CASE( test_applyBoundaryLimitation ){
    /*!
     * Test of the application of the boundary conditions.
     *
     * \param &results: The output file
     */

    floatVector x0 = { 1.0,  2.0, 3.0, -1.0, -2.0, -3.0 };
    floatVector dxDefault = { 0.1, -0.5, 1.0,  0.1,  2.1, -0.5 };

    intVector variableIndices = {    0,    3, 4   };
    intVector barrierSigns    = {    1,    0, 1   };
    floatVector barrierValues = { 1.05, -1.0, 0.0 };

    floatVector dx = dxDefault;

    floatVector xAnswer1 = { 1.05, 1.75, 3.5, -0.95, -0.95, -3.25 };
    floatVector xAnswer2 = { 1.033333, 1.833333, 3.333333, -1, -1.3, -3.1666667 };
    floatVector xAnswer3 = { 1.05, 1.5, 4, -0.8, 0.0, -3.5 };

    errorOut error = solverTools::applyBoundaryLimitation( x0, variableIndices, barrierSigns, barrierValues, dx );

    BOOST_CHECK( ! error  );

    BOOST_CHECK( vectorTools::fuzzyEquals( x0 + dx, xAnswer1 ) );

    dx = dxDefault;
    x0[ 3 ] = -0.9;
    dx[ 3 ] = -0.3;

    error = solverTools::applyBoundaryLimitation( x0, variableIndices, barrierSigns, barrierValues, dx );

    BOOST_CHECK( ! error  );

    BOOST_CHECK( vectorTools::fuzzyEquals( x0 + dx, xAnswer2 ) );

    dx = dxDefault;
    error = solverTools::applyBoundaryLimitation( x0, variableIndices, barrierSigns, barrierValues, dx, 1e-9, 1e-9, true );

    BOOST_CHECK( ! error  );

    BOOST_CHECK( vectorTools::fuzzyEquals( x0 + dx, xAnswer3 ) );

}

BOOST_AUTO_TEST_CASE( test_BFGS ){
    /*!
     * Test of the BFGS optimization algorithm.
     */

    solverTools::stdFncLagrangianG func;
    func = static_cast<solverTools::LagrangianFunctionWithGradient>( lagrangian1 );

    solverTools::floatVector x0 = { 0. };
    solverTools::floatVector x;

    bool convergeFlag, fatalErrorFlag;
    solverTools::floatMatrix floatArgs, floatOuts;
    solverTools::intMatrix intArgs, intOuts;

    floatVector xAnswer = { -1 };
    floatMatrix floatOutsAnswer = { { 1, 2, 3 }, { -0.4, -0.5, -0.6 } };
    intMatrix intOutsAnswer = { { 5, 6, 7 }, { 8 }, { 9, 10 } };

    errorOut error = solverTools::BFGS( func, x0, x, convergeFlag, fatalErrorFlag,
                                        floatOuts, intOuts, floatArgs, intArgs );

    BOOST_CHECK( ! error  );

    BOOST_CHECK( vectorTools::fuzzyEquals( x, xAnswer ) );

    BOOST_CHECK( vectorTools::fuzzyEquals( floatOuts, floatOutsAnswer ) );

    BOOST_CHECK( vectorTools::fuzzyEquals( intOuts, intOutsAnswer ) );

}

BOOST_AUTO_TEST_CASE( test_BFGS2 ){
    /*!
     * Test of the BFGS optimization algorithm.
     */

    solverTools::stdFncLagrangianG func;
    func = static_cast<solverTools::LagrangianFunctionWithGradient>( lagrangian2 );

    solverTools::floatVector x0 = { 0., 0., 0. };
    solverTools::floatVector x;

    bool convergeFlag, fatalErrorFlag;
    solverTools::floatMatrix floatArgs, floatOuts;
    solverTools::intMatrix intArgs, intOuts;

    floatOuts = { { .1, .2, .3, .4 } };
    intOuts = { { -1, -2 } };

    floatVector xAnswer = { -std::sqrt( 2. ) / 2, -std::sqrt( 2. ) / 2 };
    floatMatrix floatOutsAnswer = { { 1, 2, 3 }, { -0.4, -0.5, -0.6 }, { 7, 6, 5 } };
    intMatrix intOutsAnswer = { { -4 }, { 5, 6, 7 }, { 8 }, { 9, 10 } };

    errorOut error = solverTools::BFGS( func, x0, x, convergeFlag, fatalErrorFlag,
                                        floatOuts, intOuts, floatArgs, intArgs,
                                        20, 1e-9, 1e-9, 1e-4, 5, true
                                      );

    BOOST_CHECK( ! error  );

    BOOST_CHECK( vectorTools::fuzzyEquals( { x[ 0 ], x[ 1 ] }, xAnswer ) );

    BOOST_CHECK( vectorTools::fuzzyEquals( floatOuts, floatOutsAnswer ) );

    BOOST_CHECK( vectorTools::fuzzyEquals( intOuts, intOutsAnswer ) );

}

BOOST_AUTO_TEST_CASE( test_homotopyBFGS ){
    /*!
     * Test of the homotopy BFGS optimization algorithm.
     */

    solverTools::stdFncLagrangianG func;
    func = static_cast<solverTools::LagrangianFunctionWithGradient>( lagrangian1 );

    solverTools::floatVector x0 = { 0. };
    solverTools::floatVector x;

    bool convergeFlag, fatalErrorFlag;
    solverTools::floatMatrix floatArgs, floatOuts;
    solverTools::intMatrix intArgs, intOuts;

    floatVector xAnswer = { -1 };
    floatMatrix floatOutsAnswer = { { 1, 2, 3 }, { -0.4, -0.5, -0.6 } };
    intMatrix intOutsAnswer = { { 5, 6, 7 }, { 8 }, { 9, 10 } };

    errorOut error = solverTools::homotopyBFGS( func, x0, x, convergeFlag, fatalErrorFlag,
                                                floatOuts, intOuts, floatArgs, intArgs,
                                                100, 1e-9, 1e-9, 1e-4, 10, 1.0, 0.1, true, 1.0
                                              );

    BOOST_CHECK( ! error  );

    BOOST_CHECK( vectorTools::fuzzyEquals( x, xAnswer ) );

    BOOST_CHECK( vectorTools::fuzzyEquals( floatOuts, floatOutsAnswer ) );

    BOOST_CHECK( vectorTools::fuzzyEquals( intOuts, intOutsAnswer ) );

}

BOOST_AUTO_TEST_CASE( test_homotopyBFGS2 ){
    /*!
     * Test of the BFGS optimization algorithm.
     */

    solverTools::stdFncLagrangianG func;
    func = static_cast<solverTools::LagrangianFunctionWithGradient>( lagrangian2 );

    solverTools::floatVector x0 = { 0., 0., 0. };
    solverTools::floatVector x;

    bool convergeFlag, fatalErrorFlag;
    solverTools::floatMatrix floatArgs, floatOuts;
    solverTools::intMatrix intArgs, intOuts;

    floatOuts = { { .1, .2, .3, .4 } };
    intOuts = { { -1, -2 } };

    floatVector xAnswer = { -std::sqrt( 2. ) / 2, -std::sqrt( 2. ) / 2 };
    floatMatrix floatOutsAnswer = { { 1, 2, 3 }, { -0.4, -0.5, -0.6 }, { 7, 6, 5 } };
    intMatrix intOutsAnswer = { { -4 }, { 5, 6, 7 }, { 8 }, { 9, 10 } };

    errorOut error = solverTools::homotopyBFGS( func, x0, x, convergeFlag, fatalErrorFlag,
                                                floatOuts, intOuts, floatArgs, intArgs,
                                                100, 1e-9, 1e-9, 1e-4, 10, 1.0, 0.1, true, 1.0
                                              );

    BOOST_CHECK( ! error  );

    BOOST_CHECK( vectorTools::fuzzyEquals( { x[ 0 ], x[ 1 ] }, xAnswer ) );

    BOOST_CHECK( vectorTools::fuzzyEquals( floatOuts, floatOutsAnswer ) );

    BOOST_CHECK( vectorTools::fuzzyEquals( intOuts, intOutsAnswer ) );

}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <Eigen/Dense>
#include <dpMM/basemeasure.hpp>
#include <dpMM/sphere.hpp>

using namespace Eigen;

template<typename T>
class UnifSphere : public BaseMeasure<T>
{
public:

  uint32_t D_; // dimensionality of the space in which the sphere lies
  Sphere<T> S_;

  UnifSphere(uint32_t D);
  ~UnifSphere();

  virtual baseMeasureType getBaseMeasureType() const {return(UNIF_SPHERE); }

  virtual BaseMeasure<T>* copy();

  virtual T logLikelihood(const Matrix<T,Dynamic,1>& x) const;
  virtual T logLikelihood(const Matrix<T,Dynamic,Dynamic>& x, uint32_t i) const
    {return logLikelihood(x.col(i));};
  virtual void posterior(const Matrix<T,Dynamic,Dynamic>& x,
      const VectorXu& z, uint32_t k);
  virtual T logPdfUnderPrior() const;
  void print() const {cout<<"Unif Sphere in D="<<D_<<endl;};
  virtual uint32_t getDim() const {return(D_);};
private:

};

typedef UnifSphere<double> UnifSphered;
typedef UnifSphere<float> UnifSpheref;

// ---------------------------------------------------------------------------
template<typename T>
UnifSphere<T>::UnifSphere(uint32_t D)
  : D_(D), S_(D)
{};
template<typename T>
UnifSphere<T>::~UnifSphere()
{};

template<typename T>
BaseMeasure<T>* UnifSphere<T>::copy()
{
  return new UnifSphere<T>(D_);
};

template<typename T>
T UnifSphere<T>::logLikelihood(const Matrix<T,Dynamic,1>& x) const
{
  return - S_.logSurfaceArea();
};

template<typename T>
void UnifSphere<T>::posterior(const Matrix<T,Dynamic,Dynamic>& x,
    const VectorXu& z, uint32_t k)
{
  // nothing seince we have no priors
};

template<typename T>
T UnifSphere<T>::logPdfUnderPrior() const
{
  return 0.0;
};


#include <iostream>
#include <list>
#include <numeric>
#include <unordered_map>

#include <boost/optional.hpp>

#include "node.h"
#include "graph.h"

// Route between two nodes
namespace rbn
{
    bool routeExists(g::IntGraph const& graph, g::IntVertex::Ptr const& start, g::IntVertex::Ptr const& end)
    {
        if (start == end)
            return true;

        // Just for sure mark all nodes as unvisited
        for (auto && n : graph.verticies())
            n.second->setState(g::IntVertex::Unvisited);

        std::queue<g::IntVertex::Ptr> verteciesQueue;

        start->setState(g::IntVertex::Visiting);
        verteciesQueue.push(start);

        g::IntVertex::Ptr visitingVertex;
        while (!verteciesQueue.empty()) {
            visitingVertex = verteciesQueue.front();
            verteciesQueue.pop();

            if (visitingVertex) {
                for (auto && v : visitingVertex->linkedVertices()) {
                    if (v->state() == g::IntVertex::Unvisited) {
                        if (v == end)
                            return true;
                        else {
                            v->setState(g::IntVertex::Visiting);
                            verteciesQueue.push(v);
                        }
                    }
                }

                visitingVertex->setState(g::IntVertex::Visited);
            }
        }

        return false;
    }
}

// Minimal tree (create minimal tree from sorted array with unique elements)
namespace bst
{
    using namespace Tree;

    namespace details
    {
        IntNodePtr createMinimalBSTImpl(std::vector<int> const& array, int from, int to)
        {
            if (to < from)
                return nullptr;

            int mid = (from + to) / 2;
            auto node = std::make_shared<IntNode>(array.at(mid));
            node->setLeftChild(createMinimalBSTImpl(array, from, mid - 1));
            node->setRightChild(createMinimalBSTImpl(array, mid + 1, to));
            return node;
        }
    }

    IntNodePtr createMinimalBST(std::vector<int> const& array)
    {
        return details::createMinimalBSTImpl(array, 0, array.size() - 1);
    }
}

// List of depth. For binary tree, algorithm to create a linked list of all nodes on each depth
// using preorder traversal algorithm
namespace lod
{
    using namespace Tree;
    using NodesList = std::list<IntNodePtr>;
    using NodesListsArray = std::vector<NodesList>;

    namespace details
    {
        void createLevelLinkedListImpl(IntNodePtr const& root, NodesListsArray & lists, std::size_t level)
        {
            if (!root)
                return;

            // Check if level not in the list
            if (lists.size() == level) {
                lists.push_back(NodesList());
                lists.back().push_back(root);
            } else
                lists[level].push_back(root);

            createLevelLinkedListImpl(root->mLeftChild, lists, level + 1);
            createLevelLinkedListImpl(root->mRightChild, lists, level + 1);
        }
    }

    NodesListsArray createLevelLinkedList(IntNodePtr const& root)
    {
        NodesListsArray lists;
        details::createLevelLinkedListImpl(root, lists, 0);
        return lists;
    }
}

// Check if tree is balanced (difference between subtrees no more than one)
namespace bt
{
    using namespace Tree;

    namespace details
    {
        static const int ERROR_TAG = std::numeric_limits<int>::min();

        int checkHeight(IntNodePtr const& root)
        {
            if (!root)
                return -1;

            int leftHeight = checkHeight(root->mLeftChild);
            if (leftHeight == ERROR_TAG)
                return ERROR_TAG; // Propagate error to the top

            int rightHeight = checkHeight(root->mRightChild);
            if (rightHeight == ERROR_TAG)
                return ERROR_TAG; // Propagate error to the top

            if (std::abs(leftHeight - rightHeight) > 1)
                return ERROR_TAG; // Pass error back
            else
                return std::max(leftHeight, rightHeight) + 1;
        }
    }

    bool isBalanced(IntNodePtr const& root)
    {
        return details::checkHeight(root) != details::ERROR_TAG;
    }
}

// Check if tree is a valid BST
namespace vbst
{
    using namespace Tree;

    namespace details
    {
        using Int = boost::optional<int>;
        bool checkBSTImpl(IntNodePtr const& n, Int min = Int(), Int max = Int())
        {
            if (!n)
                return true;

            if ((min && n->mKey <= min) || (max && n->mKey > max))
                return false;

            if (!checkBSTImpl(n->mLeftChild, min, n->mKey) ||
                !checkBSTImpl(n->mRightChild, n->mKey, max))
                return false;

            return true;
        }
    }

    bool checkBST(IntNodePtr const& root)
    {
        return details::checkBSTImpl(root);
    }
}

// Successor (find "next" node)
namespace sr
{
    using namespace Tree;

    namespace details
    {
        IntNodePtr leftMostChild(IntNodePtr node)
        {
            if (!node)
                return nullptr;

            while (node->mLeftChild)
                node = node->mLeftChild;

            return node;
        }
    }

    IntNodePtr successor(IntNodePtr const& node)
    {
        if (!node)
            return nullptr;

        // If there are right child just return leftmost child shich will be the next node
        if (node->mRightChild)
            return details::leftMostChild(node->mRightChild);
        else {
            auto current = node;
            auto parent  = node->parent();

            // Go up until we're on left node instead of right
            while (parent && parent->mLeftChild != current) {
                current = parent;
                parent  = parent->parent();
            }

            return parent;
        }
    }
}

// Topological sort.
// Given list of projects and list of dependencies (pairs second depends on first). Find bild order
// or error is there is no valid build order.
namespace ts
{
    using namespace g;
    using ProjectsVector     = std::vector<std::string>;
    using DependenciesVector = std::vector<std::pair<std::string, std::string>>;

    namespace details
    {
        using Project = Vertex<std::string>;
        using OrderedProjects = std::list<Project::Ptr>;

        bool doDFS(Project::Ptr const& project, OrderedProjects & projects)
        {
            // Cycle detected
            if (project->state() == Project::Visiting)
                return false;

            if (project->state() == Project::Unvisited) {
                project->setState(Project::Visiting);

                for (auto && child : project->linkedVertices())
                    if (!doDFS(child, projects))
                        return false; // Propagate cycle

                project->setState(Project::Visited);
                projects.push_front(project);
            }

            return true;
        }

        decltype(auto) fillGraph(ProjectsVector const& projects,
                                 DependenciesVector const& dependencies)
        {
            auto graph = std::make_unique<StrGraph>();

            for (auto && project : projects)
                graph->addVertex(project);

            // Second depends on first
            for (auto && edge : dependencies)
                graph->addEdge(edge.second, edge.first);

            return graph;
        }
    }

    ProjectsVector orderedProjects(ProjectsVector     const& projects,
                                   DependenciesVector const& dependencies)
    {
        auto graph = details::fillGraph(projects, dependencies);

        details::OrderedProjects op;
        for (auto && project : graph->verticies())
            if (project.second->state() == details::Project::Unvisited)
                if (!details::doDFS(project.second, op))
                    return ProjectsVector();

        ProjectsVector result(op.size());
        std::transform(op.begin(), op.end(), result.begin(), [](auto && p) { return p->data(); });
        return result;
    }
}

// Find first common ancestor for two nodes
namespace fca
{
    using namespace Tree;

    namespace details
    {
        IntNodePtr doUpBy(IntNodePtr node, int depth)
        {
            while (depth > 0 && node) {
                node = node->parent();
                --depth;
            }

            return node;
        }
    }

    IntNodePtr commonAncestor(IntNodePtr const& f, IntNodePtr const& s)
    {
        assert(f && s);
        int delta = f->depth() - s->depth();
        IntNodePtr shallower = delta > 0 ? s : f;
        IntNodePtr deeper    = delta > 0 ? f : s;

        // Up deeper node
        deeper = details::doUpBy(deeper, std::abs(delta));

        // Search for intersection
        while (deeper != shallower && deeper && shallower) {
            deeper = deeper->parent();
            shallower = shallower->parent();
        }

        return !shallower || !deeper ? nullptr : shallower;
    }
}

// Given a BST which created by traversing array left to right,
// generate all arrays may lead to this tree
namespace bstsq
{
    using List = std::list<int>;
    using VectorLists = std::vector<List>;

    using namespace Tree;

    namespace details
    {
        void mergeLists(List & first, List & second, VectorLists & results, List & prefix);

        void addHeadToPrefixAndMerge(List & first, List & second, VectorLists & results,
                                     List & prefix, List & listForTakingHead)
        {
            // Take head
            int head = listForTakingHead.front();
            listForTakingHead.pop_front();
            prefix.push_back(head);

            mergeLists(first, second, results, prefix);

            // Restore head
            prefix.pop_back();
            listForTakingHead.push_front(head);
        }

        // Merge lists in all possible ways
        void mergeLists(List & first, List & second, VectorLists & results, List & prefix)
        {
            // If one list is empty, add remainder to the prefix and store result
            if (first.empty() || second.empty()) {
                List result(prefix);
                std::copy(first.begin(), first.end(), std::back_inserter(result));
                std::copy(second.begin(), second.end(), std::back_inserter(result));
                results.push_back(result);
                return;
            }

            // Recurse go through first and second lists
            addHeadToPrefixAndMerge(first, second, results, prefix, first /*listForTakingHead*/);
            addHeadToPrefixAndMerge(first, second, results, prefix, second /*listForTakingHead*/);
        }
    }

    VectorLists allSequences(IntNodePtr const& root)
    {
        VectorLists result;

        if (!root) {
            result.push_back(List());
            return result;
        }

        List prefix;
        prefix.push_back(root->mKey);

        // Recurse on left and right subtrees
        auto leftSeq  = allSequences(root->mLeftChild);
        auto rightSeq = allSequences(root->mRightChild);

        // Merge all list from the left and right sides
        for (auto && left : leftSeq) {
            for (auto && right : rightSeq) {
                VectorLists merged;
                details::mergeLists(left, right, merged, prefix);
                std::copy(merged.begin(), merged.end(), std::back_inserter(result));
            }
        }

        return result;
    }
}

// There are two large binary trees (T1 and T2), check if T2 is sutree of T1. T1 is bigger.
namespace ct
{
    using namespace Tree;

    namespace details
    {
        bool matchTree(IntNodePtr const& n1, IntNodePtr const& n2)
        {
            if (!n1 && !n2)
                return true; // nothing left
            else if (!n1 || !n2)
                return false; // one of them is empty, don't match
            else if (n1->mKey != n2->mKey)
                return false; // different data
            else // Recurse compare subtrees
                return matchTree(n1->mLeftChild, n2->mLeftChild) &&
                       matchTree(n1->mRightChild, n2->mRightChild);
        }

        bool subTree(IntNodePtr const& n1, IntNodePtr const& n2)
        {
            if (!n1)
                return false; // big tree is empty
            else if (n1->mKey == n2->mKey && matchTree(n1, n2))
                return true;

            return subTree(n1->mLeftChild, n2) || subTree(n1->mRightChild, n2);
        }
    }

    bool containsTree(IntNodePtr const& t1, IntNodePtr const& t2)
    {
      // The empty tree is always a subtree :)
      return !t2 || details::subTree(t1, t2);
    }
}

// Given binary tree and sum. Count the number of pathes to get given sum. Paths should traveling
// only from parent nodes to child.
namespace sp
{
    using namespace Tree;

    namespace details
    {
        using PathCount = std::unordered_map<int, int>;

        void incVal(PathCount & pathCount, int key, int delta)
        {
            int newCount = pathCount[key] + delta;
            if (newCount == 0)
                pathCount.erase(key); // Reduce space
            else
                pathCount[key] = delta;
        }

        int countPathsWithSumImpl(IntNodePtr const& node, int targetSum, int runnigSum,
                                  PathCount & pathCount)
        {
            if (!node)
                return 0; // Base case

            // Count paths with with sum ending at the current node
            runnigSum += node->mKey;
            int sum = runnigSum - targetSum;
            int totalPaths = pathCount[sum];

            // One additional path starts at root
            if (runnigSum == targetSum)
                ++totalPaths;

            // Increment pathCount, recurse, then decrement
            incVal(pathCount, runnigSum, 1);
            totalPaths += countPathsWithSumImpl(node->mLeftChild, targetSum, runnigSum, pathCount);
            totalPaths += countPathsWithSumImpl(node->mRightChild, targetSum, runnigSum, pathCount);
            incVal(pathCount, runnigSum, -1);

            return totalPaths;
        }
    }

    int countPathWithSum(IntNodePtr const& node, int sum)
    {
        details::PathCount pathCount;
        return details::countPathsWithSumImpl(node, sum, 0, pathCount);
    }
}

int main(int /*argc*/, char */*argv*/[])
{
    // 1
//    g::IntGraph graph;
//    auto first = graph.addVertex(1);
//    auto second = graph.addVertex(2);
//    auto third = graph.addVertex(3);
//    auto fourth = graph.addVertex(4);

//    graph.addEdge(1, 2);
//    graph.addEdge(1, 3);
//    graph.addEdge(2, 3);
//    graph.addEdge(3, 4);

//    std::cout << std::boolalpha << rbn::routeExists(graph, fourth, first) << std::endl;

//    graph.dump("/home/vt4a2h/Projects/alg/graph.dot");
//    system("cd /home/vt4a2h/Projects/alg/ && dot graph.dot -Tsvg > graph.svg ");

    // 2
//    try {
//        std::vector<int> v {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
//        auto tree = bst::createMinimalBST(v);
//        tree->dump("/home/vt4a2h/Projects/alg/graph.dot");
//        system("cd /home/vt4a2h/Projects/alg/ && dot graph.dot -Tsvg > graph.svg ");
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 3
//    try {
//        std::vector<int> v {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
//        auto tree = bst::createMinimalBST(v);

//        auto lists = lod::createLevelLinkedList(tree);
//        for (auto && list : lists) {
//            for (auto && n : list) {
//                std::cout << n->mKey << "\t";
//            }
//            std::cout << std::endl;
//        }
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 4
//    try {
//        std::vector<int> v {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
//        auto tree = bst::createMinimalBST(v);

//        // Must be balanced
//        std::cout << "Is balanced: " << std::boolalpha << bt::isBalanced(tree) << std::endl;

//        // Disbalance somehow
//        Tree::IntNodePtr min = tree->mLeftChild;
//        while (min->mLeftChild)
//            min = min->mLeftChild;

//        std::cout << "Min element: " << min->mKey << std::endl;

//        min->makeLeftChild(-1)->makeLeftChild(-2);

//        std::cout << "Is balanced: " << std::boolalpha << bt::isBalanced(tree) << std::endl;
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 5
//    try {
//        std::vector<int> v {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
//        auto tree = bst::createMinimalBST(v);

//        std::cout << "Is BST: " << std::boolalpha << vbst::checkBST(tree) << std::endl;

//        Tree::IntNodePtr min = tree->mLeftChild;
//        while (min->mLeftChild)
//            min = min->mLeftChild;
//        min->makeLeftChild(100)->makeLeftChild(200);

//        std::cout << "Is BST: " << std::boolalpha << bt::isBalanced(tree) << std::endl;
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 6
//    try {
//        std::vector<int> v {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
//        auto tree = bst::createMinimalBST(v);

//        Tree::IntNodePtr min = tree->mLeftChild;
//        while (min->mLeftChild)
//            min = min->mLeftChild;

//        // 1 -> 2
//        std::cout << min->mKey << " -> " << sr::successor(min)->mKey << std::endl;

//        // 2 -> 3
//        auto two = min->parent();
//        std::cout << two->mKey << " -> " << sr::successor(two)->mKey << std::endl;

//        // 3 -> 4
//        auto three = sr::successor(min)->mRightChild;
//        std::cout << three->mKey << " -> " << sr::successor(three)->mKey << std::endl;

//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 7
//    ts::ProjectsVector pv {"a", "b", "c", "d", "e", "f"};
//    ts::DependenciesVector dv {{"a", "d"}, {"f", "b"}, {"b", "d"}, {"f", "a"}, {"d", "c"}};
//    try {
//        auto orderedProjects = ts::orderedProjects(pv, dv);
//        for (auto && p : orderedProjects)
//            std::cout << p << "\t";
//        std::cout << std::endl;
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 8
//    try {
//        std::vector<int> v {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
//        auto tree = bst::createMinimalBST(v);

//        Tree::IntNodePtr min = tree->mLeftChild;
//        while (min->mLeftChild)
//            min = min->mLeftChild;

//        Tree::IntNodePtr someNode = min->parent()->parent()->mRightChild->mRightChild;

//        min = min->makeRightChild(200)->makeLeftChild(300);

//        tree->dump("/home/vt4a2h/Projects/alg/fca_graph.dot");
//        system("cd /home/vt4a2h/Projects/alg/ && dot fca_graph.dot -Tsvg > fca_graph.svg ");

//        if (auto ca = fca::commonAncestor(min, someNode))
//            std::cout << "First common ancestor: " << ca->mKey << std::endl;
//        else
//            std::cout << "No common ancestor here." << std::endl;
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 9
    // TODO: revise
//    try {
//        std::vector<int> v {1, 2, 3, 4, 5};
//        auto tree = bst::createMinimalBST(v);

//        auto allSequences = bstsq::allSequences(tree);
//        for (auto && s : allSequences) {
//            for (auto && e : s) {
//                std::cout << e << "\t";
//            }
//            std::cout << std::endl;
//        }

//        tree->dump("/home/vt4a2h/Projects/alg/bstsq_graph.dot");
//        system("cd /home/vt4a2h/Projects/alg/ && dot bstsq_graph.dot -Tsvg > bstsq_graph.svg ");
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 10
//    try {
//        std::vector<int> v1 {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
//        auto tree1 = bst::createMinimalBST(v1);

//        auto tree2 = std::make_shared<Tree::IntNode>(2);
//        tree2->makeLeftChild(1);
//        tree2->makeRightChild(3)->makeRightChild(4);

//        tree1->dump("/home/vt4a2h/Projects/alg/tree1.dot");
//        system("cd /home/vt4a2h/Projects/alg/ && dot tree1.dot -Tsvg > tree1.svg ");

//        tree2->dump("/home/vt4a2h/Projects/alg/tree2.dot");
//        system("cd /home/vt4a2h/Projects/alg/ && dot tree2.dot -Tsvg > tree2.svg ");

//        std::cout << std::boolalpha << ct::containsTree(tree1, tree2) << std::endl;
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 11
    // Implement method for getting random node from BST (nodes should be equally likelly to be chosen)
//    try {
//        srand(time(NULL));

//        auto tree = std::make_shared<Tree::IntNode>(4);
//        auto two = tree->insertInOrder(2);
//        two->insertInOrder(1);
//        two->insertInOrder(3);

//        auto six = tree->insertInOrder(6);
//        six->insertInOrder(5);
//        six->insertInOrder(7);

//        tree->dump("/home/vt4a2h/Projects/alg/ub.dot");
//        system("cd /home/vt4a2h/Projects/alg/ && dot ub.dot -Tsvg > ub.svg ");

//        for (int i = 1; i <= 7; ++i)
//            std::cout << tree->randomNode()->mKey << "\t";
//        std::cout << std::endl;
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 12 // TODO: revise
    try {
        std::vector<int> v {1, 2, 3, 4, 5};
        auto tree = bst::createMinimalBST(v);

        std::cout << sp::countPathWithSum(tree, 3) << std::endl;
    } catch (std::exception const& e) {
        std::cout << e.what() << std::endl;
    }

    return 0;
}