/*
 [auto_generated]
 boost/numeric/odeint/iterator/const_step_time_iterator.hpp

 [begin_description]
 Iterator for iterating throught the solution of an ODE with constant step size. The dereferences types containes also the time.
 [end_description]

 Copyright 2012-2013 Karsten Ahnert
 Copyright 2013 Mario Mulansky

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

#ifndef BOOST_NUMERIC_ODEINT_ITERATOR_CONST_STEP_TIME_ITERATOR_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_ITERATOR_CONST_STEP_TIME_ITERATOR_HPP_INCLUDED

#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/util/stepper_traits.hpp>
#include <boost/numeric/odeint/iterator/detail/ode_iterator_base.hpp>
#include <boost/numeric/odeint/iterator/impl/const_step_iterator_impl.hpp>

namespace boost {
namespace numeric {
namespace odeint {

    /* use the const_step_iterator_impl with the right tags */
    template< class Stepper , class System , class State
#ifndef DOXYGEN_SKIP
        , class StepperTag = typename base_tag< typename traits::stepper_category< Stepper >::type >::type
#endif
    >
    class const_step_time_iterator : public const_step_iterator_impl<
            const_step_time_iterator< Stepper , System , State , StepperTag > ,
            Stepper , System , State , detail::ode_state_time_iterator_tag , StepperTag
        >
    {
        typedef typename traits::time_type< Stepper >::type time_type;
        typedef const_step_time_iterator< Stepper , System , State , StepperTag > iterator_type;

    public:
        const_step_time_iterator( Stepper stepper , System sys , State &s , time_type t_start , time_type t_end , time_type dt )
            : const_step_iterator_impl< iterator_type , Stepper , System , State , detail::ode_state_time_iterator_tag , StepperTag >( stepper , sys , s , t_start , t_end , dt )
        {}

        const_step_time_iterator( Stepper stepper , System sys , State &s )
            : const_step_iterator_impl< iterator_type , Stepper , System , State , detail::ode_state_time_iterator_tag , StepperTag >( stepper , sys , s )
        {}
    };

    template< class Stepper , class System , class State >
    const_step_time_iterator< Stepper , System , State > make_const_step_time_iterator_begin(
        Stepper stepper ,
        System system ,
        State &x ,
        typename traits::time_type< Stepper >::type t_start ,
        typename traits::time_type< Stepper >::type t_end ,
        typename traits::time_type< Stepper >::type dt )
    {
        return const_step_time_iterator< Stepper , System , State >( stepper , system , x , t_start , t_end , dt );
    }

    template< class Stepper , class System , class State >
    const_step_time_iterator< Stepper , System , State > make_const_step_time_iterator_end(
        Stepper stepper ,
        System system ,
        State &x )
    {
        return const_step_time_iterator< Stepper , System , State >( stepper , system , x );
    }

    template< class Stepper , class System , class State >
    std::pair< const_step_time_iterator< Stepper , System , State > , const_step_time_iterator< Stepper , System , State > >
    make_const_step_time_range(
        Stepper stepper ,
        System system ,
        State &x ,
        typename traits::time_type< Stepper >::type t_start ,
        typename traits::time_type< Stepper >::type t_end ,
        typename traits::time_type< Stepper >::type dt )
    {
        return std::make_pair(
            const_step_time_iterator< Stepper , System , State >( stepper , system , x , t_start , t_end , dt ) ,
            const_step_time_iterator< Stepper , System , State >( stepper , system , x ) );
    }

    /**
     * \class const_step_time_iterator
     *
     * \brief ODE Iterator with constant step size. The value type of this iterator is a std::pair containing state and time.
     *
     * Implements an iterator representing the solution of an ODE from t_start
     * to t_end evaluated at steps with constant step size dt.
     * After each iteration the iterator dereferences to a pair containing
     * state and time at the next time point t+dt..
     * This iterator can be used with Steppers and
     * DenseOutputSteppers and it always makes use of the all the given steppers
     * capabilities. A for_each over such an iterator range behaves similar to
     * the integrate_const routine.
     *
     * const_step_time_iterator is a model of single-pass iterator.
     *
     * The value type of this iterator is a pair with the state type and time type of the stepper.
     *
     * \tparam Stepper The stepper type which should be used during the iteration.
     * \tparam System The type of the system function (ODE) which should be solved.
     * \tparam State The state type of the ODE.
     */

    /**
     * \fn make_const_step_time_iterator_begin( Stepper stepper , System system , State &x ,
        typename traits::time_type< Stepper >::type t_start ,
        typename traits::time_type< Stepper >::type t_end ,
        typename traits::time_type< Stepper >::type dt )
     *
     * \brief Factory function for const_step_time_iterator. Constructs a begin iterator.
     *
     * \param stepper The stepper to use during the iteration.
     * \param system The system function (ODE) to solve.
     * \param x The initial state. const_step_time_iterator stores a reference of s and changes its value during the iteration.
     * \param t_start The initial time.
     * \param t_end The end time, at which the iteration should stop.
     * \param dt The initial time step.
     * \returns The const step time iterator.
     */

    /**
     * \fn make_const_step_time_iterator_end( Stepper stepper , System system , State &x )
     * \brief Factory function for const_step_time_iterator. Constructs a end iterator.
     *
     * \param stepper The stepper to use during the iteration.
     * \param system The system function (ODE) to solve.
     * \param x The initial state. const_step_time_iterator store a reference of s and changes its value during the iteration.
     * \returns The const step time iterator.
     */

    /**
     * \fn make_const_step_time_range( Stepper stepper , System system , State &x ,
        typename traits::time_type< Stepper >::type t_start ,
        typename traits::time_type< Stepper >::type t_end ,
        typename traits::time_type< Stepper >::type dt)
     *
     * \brief Factory function to construct a single pass range of const_step_time_iterator. A range is here a pair of const_step_time_iterator.
     *
     * \param stepper The stepper to use during the iteration.
     * \param system The system function (ODE) to solve.
     * \param x The initial state. const_step_time_iterator stores a reference of s and changes its value during the iteration.
     * \param t The initial time.
     * \param t_end The end time, at which the iteration should stop.
     * \param dt The initial time step.
     * \returns The const step time range.
     */

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

#endif // BOOST_NUMERIC_ODEINT_ITERATOR_CONST_STEP_TIME_ITERATOR_HPP_INCLUDED


// Copyright 2017 Victor Smirnov
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#pragma once

#include <boost/assert.hpp>

#include <vector>
#include <memory>
#include <type_traits>
#include <stdint.h>

namespace dumbo {
namespace v1 {
namespace reactor {

template <typename T>
class RingBuffer {
    std::vector<T> buffer_;

    int64_t tail_{};
    int64_t head_{};

public:
    RingBuffer(size_t size): buffer_{size}
    {}

    size_t capacity() const {
        return buffer_.size() - available();
    }

    int capacity_i() const {
        return (int)(buffer_.size() - available());
    }

    size_t available() const {
        return head_ - tail_;
    }

    size_t ring_size() const {
        return buffer_.size();
    }

    const T& pop_back()
    {
        BOOST_ASSERT(head_ > tail_);
        return buffer_[idx(tail_++)];
    }

    void push_front(const T& value)
    {
        BOOST_ASSERT(available() < ring_size());
        buffer_[idx(head_++)] = value;
    }

private:
    size_t idx(size_t i) const
    {
        return i & (buffer_.size() - 1);
    }
};

}}}


#include "algorithm.h"
#include "data_point.h"
#include "debug.h"
#include "experiments.h"
#include "key.h"
#include "mock_classical_session.h"
#include "options.h"
#include "random.h"
#include "report.h"
#include "reconciliation.h"
#include "series.h"
#include <boost/filesystem.hpp>
#include <cerrno>
#include <deque>
#include <fstream>
#include <iostream>
#include <mutex>
#include <thread>
#include <vector>

static std::deque<Serie> series_queue;
static std::mutex series_mutex;
static std::mutex count_mutex;
static int total_nr_data_points = 0;
static int data_points_nr = 0;

void fatal_perror(const std::string& message)
{
    {
        // Take mutex to avoid error messages from multiple threads
        std::lock_guard<std::mutex> guard(series_mutex);
        std::perror(message.c_str());
        exit(1);
    }
}

void one_data_point_run(DataPoint& data_point, const std::string& algorithm_name, unsigned long key_size,
                        double error_rate)
{
    const Cascade::Algorithm* algorithm = Cascade::Algorithm::get_by_name(algorithm_name);
    assert(algorithm);
    Cascade::Key correct_key(key_size);
    Cascade::MockClassicalSession classical_session(correct_key, algorithm->cache_shuffles);
    Cascade::Key noisy_key = correct_key;
    noisy_key.apply_noise(error_rate);
    int actual_bit_errors = correct_key.nr_bits_different(noisy_key);
    data_point.actual_bit_errors.record_value(actual_bit_errors);
    double actual_bit_error_rate = double(actual_bit_errors) / double(key_size);
    data_point.actual_bit_error_rate.record_value(actual_bit_error_rate);
    int remaining_bit_errors = 0;
    {
        // New scope to make sure reconciliation destructor is run before we reporting bit errors.
        Cascade::Reconciliation reconciliation(*algorithm, classical_session, noisy_key, error_rate,
                                               &correct_key);
        reconciliation.reconcile();
        data_point.record_reconciliation_stats(reconciliation.get_stats());
        remaining_bit_errors = correct_key.nr_bits_different(reconciliation.get_reconciled_key());
        data_point.remaining_bit_errors.record_value(remaining_bit_errors);
    }
    double remaining_bit_error_rate = double(remaining_bit_errors) / double(key_size);
    data_point.remaining_bit_error_rate.record_value(remaining_bit_error_rate);
    if (remaining_bit_errors > 0)
        data_point.remaining_frame_error_rate.record_value(1.0);
    else
        data_point.remaining_frame_error_rate.record_value(0.0);
    DEBUG("remaining_bit_errors=" << remaining_bit_errors);
}

void produce_one_data_point(const std::string& algorithm, unsigned long key_size, double error_rate, int runs,
                            std::ofstream& data_file)
{
    {
        std::lock_guard<std::mutex> guard(count_mutex);
        data_points_nr += 1;
        REPORT(data_points_nr << "/" << total_nr_data_points
               << " algorithm=" << algorithm
               << " key_size=" << key_size
               << " error_rate=" << error_rate
               << " runs=" << runs);
    }

    DataPoint data_point(algorithm, key_size, error_rate);
    for(int run = 0; run < runs; ++run) {
        one_data_point_run(data_point, algorithm, key_size, error_rate);
    }

    data_file << data_point.to_json() << std::endl;
    if (!data_file.good())
        fatal_perror("Could write data point to file");
}

std::string serie_file_name(const Serie& serie)
{
    std::stringstream file_name("data__");
    file_name << "algorithm=" << serie.algorithm << ";";
    if (serie.key_sizes.size() == 1) {
        file_name << "key_size=" << serie.key_sizes[0] << ";";
    } else {
        file_name << "key_size=vary;";
    }
    if (serie.error_rates.size() == 1) {
        file_name << "error_rate=" << serie.error_rates[0];
    } else {
        file_name << "error_rate=vary";
    }
    return file_name.str();
}

void produce_one_serie(const Serie& serie, const Options& options)
{
    boost::filesystem::path data_file_name = "data__" + serie.name;
    if (!options.output_directory.empty())
        data_file_name = options.output_directory / data_file_name;
    std::ofstream data_file;
    data_file.open(data_file_name.string());
    if (!data_file.good()) {
        fatal_perror("Could not open file " + data_file_name.string() + " for writing");
    }
    for (auto key_size: serie.key_sizes) {
        for (auto error_rate: serie.error_rates) {
            produce_one_data_point(serie.algorithm, key_size, error_rate, serie.runs, data_file);
        }
    }
    data_file.close();
}

void serie_worker(const Options& options)
{
    while (true) {
        Serie serie;
        {
            std::lock_guard<std::mutex> guard(series_mutex);
            if (series_queue.empty()) {
                break;
            }
            serie = series_queue.front();
            series_queue.pop_front();
        }
        produce_one_serie(serie, options);
    }
}

void compute_total_nr_data_points(Series& series)
{
    total_nr_data_points = 0;
    for (Serie& serie: series.series) {
        total_nr_data_points += serie.key_sizes.size() * serie.error_rates.size();
    }
}

void run_all_series_multi_threaded(Series& series, const Options& options) {
    // Put all series on the queue. We don't need any locking because we have not started the
    // workers yet.
    for (Serie& serie: series.series) {
        series_queue.push_back(serie);
    }

    // Start the workers.
    std::vector<std::thread> worker_threads;
    int nr_workers = std::thread::hardware_concurrency();
    for (int worker_nr = 0; worker_nr < nr_workers; ++worker_nr) {
        std::thread worker_thread = std::thread(serie_worker, options);
        worker_threads.push_back(std::move(worker_thread));
    }

    // Wait for all of the workers to complete.
    for (auto& worker_thread: worker_threads) {
        worker_thread.join();
    }
}

void run_all_series_single_threaded(Series& series, const Options& options) {
    for (Serie& serie: series.series) {
        produce_one_serie(serie, options);
    }
}

int main(int argc, char** argv)
{
    Options options;
    options.parse(argc, argv);

    if (options.seed_is_set)
        Cascade::set_random_uint32_seed(options.seed);
    Experiments experiments(options.experiments_file);
    Series series(experiments, options.max_runs);
    compute_total_nr_data_points(series);
    if (options.multi_processing) {
        run_all_series_multi_threaded(series, options);
    } else {
        run_all_series_single_threaded(series, options);
    }
    return 0;
}


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/

#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/vectors/VectorBase.hpp"
#include "kindr/phys_quant/PhysicalType.hpp"

namespace kindr {

/*! \class Vector
 * \brief Vector in n-dimensional-space.
 *
 * This class implements a vector in n-dimensional-space.
 * More precisely an interface to store and access the coordinates of a vector of a point in n-dimensional-space is provided.
 * \tparam PhysicalType_    Physical type of the vector.
 * \tparam PrimType_        Primitive type of the coordinates.
 * \tparam Dimension_       Dimension of the vector.
 * \ingroup vectors
 */
template<enum PhysicalType PhysicalType_, typename PrimType_, int Dimension_>
class Vector : public VectorBase<Vector<PhysicalType_, PrimType_, Dimension_> >, private Eigen::Matrix<PrimType_, Dimension_, 1> {
 private:
  /*! \brief The base type.
   */
  typedef VectorBase<Vector<PhysicalType_, PrimType_, Dimension_> > Base;

  /*! \brief The size if the vector has dynamic dimension (must be equal to Eigen::Dynamic).
   */
  static constexpr int DynamicDimension = -1;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /*! \brief The implementation type.
   *
   *  The implementation type is always an Eigen object.
   */
  typedef Eigen::Matrix<PrimType_, Dimension_, 1> Implementation;

  /*! \brief The primitive type of the coordinates.
   */
  typedef PrimType_ Scalar;

  /*! \brief The dimension of the vector.
   */
  static constexpr int Dimension = Dimension_;

  /*! \brief Default constructor for static sized vectors which initializes all components with zero.
   */
  template<int DimensionCopy_ = Dimension_>
  Vector(typename std::enable_if<DimensionCopy_ != DynamicDimension>::type* = nullptr)
    : Implementation(Implementation::Zero()) {
  }

  /*! \brief Default constructor for dynamic sized vectors.
   */
  template<int DimensionCopy_ = Dimension_>
  Vector(typename std::enable_if<DimensionCopy_ == DynamicDimension>::type* = nullptr)
    : Implementation() {
  }

  /*! \brief Constructor using other vector with generic type.
   *  \param other   Vector<OtherPhysicalType_, OtherPrimType_, Dimension_>
   */
  template<enum PhysicalType OtherPhysicalType_, typename OtherPrimType_>
  explicit Vector(const Vector<OtherPhysicalType_, OtherPrimType_, Dimension_>& other)
    : Implementation(other.toImplementation().template cast<PrimType_>()) {
  }

  /*! \brief Constructor of a dynamic vector using a static vector.
   *  \param other   Vector<OtherPhysicalType_, OtherPrimType_, Dimension_>
   */
  template<int DimensionOther_, int DimensionCopy_ = Dimension_>
  Vector(const Vector<PhysicalType_, PrimType_, DimensionOther_>& other, typename std::enable_if<DimensionCopy_ == DynamicDimension>::type* = nullptr)
    : Implementation(other.toImplementation()) {
  }

  /*! \brief Constructor using Eigen::Matrix.
   *  \param other   Eigen::Matrix<PrimType_,Dimension_,1>
   */
  explicit Vector(const Implementation& other)
    : Implementation(other) {
  }

  /*! \brief Constructor using three scalars.
   *  \param x x-Component
   *  \param y y-Component
   *  \param z z-Component
   */
  template<int DimensionCopy_ = Dimension_>
  Vector(Scalar x, Scalar y, Scalar z, typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr)
    : Implementation(x,y,z) {
  }

  /*! \brief Get zero element.
   * \returns zero element
   */
  static Vector<PhysicalType_, PrimType_, Dimension_> Zero() {
    return Vector<PhysicalType_, PrimType_, Dimension_>(Implementation::Zero());
  }

  /*! \brief Sets all components of the vector to zero.
   * \returns reference
   */
  Vector<PhysicalType_, PrimType_, Dimension_>& setZero() {
    Implementation::setZero();
    return *this;
  }

  /*! \brief Get random element.
   * \returns random element
   */
  static Vector<PhysicalType_, PrimType_, Dimension_> Random() {
    return Vector<PhysicalType_, PrimType_, Dimension_>(Implementation::Random());
  }

  /*! \brief Sets all components of the vector to random.
   * \returns reference
   */
  Vector<PhysicalType_, PrimType_, Dimension_>& setRandom() {
    Implementation::setRandom();
    return *this;
  }

  /*! \brief Get the unity vector in x.
   * \returns the unity vector in x
   */
  static Vector<PhysicalType_, PrimType_, Dimension_> UnitX() {
    return Vector<PhysicalType_, PrimType_, Dimension_>(Implementation::UnitX());
  }

  /*! \brief Get the unity vector in y.
   * \returns the unity vector in y
   */
  static Vector<PhysicalType_, PrimType_, Dimension_> UnitY() {
    return Vector<PhysicalType_, PrimType_, Dimension_>(Implementation::UnitY());
  }

  /*! \brief Get the unity vector in z.
   * \returns the unity vector in z
   */
  static Vector<PhysicalType_, PrimType_, Dimension_> UnitZ() {
    return Vector<PhysicalType_, PrimType_, Dimension_>(Implementation::UnitZ());
  }

  /*! \brief Set values.
   */
  using Implementation::operator<<;

  /*! \brief Get/set values.
   */
  using Implementation::operator();

  /*!\brief Get the head of the vector (copy)
   * \returns the head of the vector (copy)
   */
  template<int DimensionOutput_>
  Vector<PhysicalType_, PrimType_, DimensionOutput_> getHead() const {
    return Vector<PhysicalType_, PrimType_, DimensionOutput_>(this->toImplementation().template head<DimensionOutput_>());
  }

  /*!\brief Get the head of the vector (copy)
   * \returns the head of the vector (copy)
   */
  Vector<PhysicalType_, PrimType_, DynamicDimension> getHead(int length) const {
    return Vector<PhysicalType_, PrimType_, DynamicDimension>(this->toImplementation().head(length));
  }

  /*!\brief Get the tail of the vector (copy)
   * \returns the tail of the vector (copy)
   */
  template<int DimensionOutput_>
  Vector<PhysicalType_, PrimType_, DimensionOutput_> getTail() const {
    return Vector<PhysicalType_, PrimType_, DimensionOutput_>(this->toImplementation().template tail<DimensionOutput_>());
  }

  /*!\brief Get the tail of the vector (copy)
   * \returns the tail of the vector (copy)
   */
  Vector<PhysicalType_, PrimType_, DynamicDimension> getTail(int length) const {
    return Vector<PhysicalType_, PrimType_, DynamicDimension>(this->toImplementation().tail(length));
  }

  /*!\brief Get a segment of the vector (copy)
   * \returns a segment of the vector (copy)
   */
  template<int DimensionOutput_>
  Vector<PhysicalType_, PrimType_, DimensionOutput_> getSegment(int start) const {
    return Vector<PhysicalType_, PrimType_, DimensionOutput_>(this->toImplementation().template segment<DimensionOutput_>(start));
  }

  /*!\brief Get a segment of the vector (copy)
   * \returns a segment of the vector (copy)
   */
  Vector<PhysicalType_, PrimType_, DynamicDimension> getSegment(int start, int length) const {
    return Vector<PhysicalType_, PrimType_, DynamicDimension>(this->toImplementation().segment(start, length));
  }

  /*!\brief Set the head of the vector
   */
  template<int DimensionInput_>
  void setHead(const Vector<PhysicalType_, PrimType_, DimensionInput_> & input) {
    this->toImplementation().template head<DimensionInput_>() = input.toImplementation();
  }

  /*!\brief Set the tail of the vector
   */
  template<int DimensionInput_>
  void setTail(const Vector<PhysicalType_, PrimType_, DimensionInput_> & input) {
    this->toImplementation().template tail<DimensionInput_>() = input.toImplementation();
  }

  /*!\brief Set a segment of the vector
   */
  template<int DimensionInput_>
  void setSegment(int start, const Vector<PhysicalType_, PrimType_, DimensionInput_> & input) {
    this->toImplementation().template segment<DimensionInput_>(start) = input.toImplementation();
  }

  /*!\brief Get x-coordinate of the vector (copy)
   * \returns the x-coordinate of the vector (copy)
   */
  template<int DimensionCopy_ = Dimension_>
  Scalar x(typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr) const {
    return this->toImplementation().x();
  }

  /*!\brief Get x-coordinate of the vector (reference)
   * \returns the x-coordinate of the vector (reference)
   */
  template<int DimensionCopy_ = Dimension_>
  Scalar& x(typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr) {
    return this->toImplementation().x();
  }

  /*!\brief Get y-coordinate of the vector (copy)
   * \returns the y-coordinate of the vector (copy)
   */
  template<int DimensionCopy_ = Dimension_>
  Scalar y(typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr) const {
    return this->toImplementation().y();
  }

  /*!\brief Get y-coordinate of the vector (reference)
   * \returns the y-coordinate of the vector (reference)
   */
  template<int DimensionCopy_ = Dimension_>
  Scalar& y(typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr) {
    return this->toImplementation().y();
  }

  /*!\brief Get z-coordinate of the vector (copy)
   * \returns the z-coordinate of the vector (copy)
   */
  template<int DimensionCopy_ = Dimension_>
  Scalar z(typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr) const {
    return this->toImplementation().z();
  }

  /*!\brief Get z-coordinate of the vector (reference)
   * \returns the z-coordinate of the vector (reference)
   */
  template<int DimensionCopy_ = Dimension_>
  Scalar& z(typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr) {
    return this->toImplementation().z();
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation (recommended only for advanced users)
   */
  inline Implementation& toImplementation() {
    return static_cast<Implementation&>(*this);
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation (recommended only for advanced users)
   */
  inline const Implementation& toImplementation() const {
    return static_cast<const Implementation&>(*this);
  }

  /*! \brief Cast to Eigen::Matrix<PrimType_, Dimension_, 1>.
   *  \returns Eigen::Matrix<PrimType_, Dimension_, 1>
   */
  inline Implementation& vector() {
    return static_cast<Implementation&>(*this);
  }

  /*! \brief Cast to Eigen::Matrix<PrimType_, Dimension_, 1>.
   *  \returns Eigen::Matrix<PrimType_, Dimension_, 1>
   */
  inline const Implementation& vector() const {
    return static_cast<const Implementation&>(*this);
  }

  /*! \brief Assignment operator.
   * \param other   other vector
   * \returns reference
   */
  Vector<PhysicalType_, PrimType_, Dimension_> & operator=(const Vector<PhysicalType_, PrimType_, Dimension_>& other) { // (The assignment of a static to a dynamic vector does not work because the amount of parameters must be one and SFINAE leads to two parameters. Workaround: cast the static vector into a dynamic one, then assign.)
    this->toImplementation() = other.toImplementation();
    return *this;
  }

  /*! \brief Addition of two vectors.
   * \param other   other vector
   * \returns sum
   */
  Vector<PhysicalType_, PrimType_, Dimension_> operator+(const Vector<PhysicalType_, PrimType_, Dimension_>& other) const {
    return Vector<PhysicalType_, PrimType_, Dimension_>(this->toImplementation() + other.toImplementation());
  }

  /*! \brief Subtraction of two vectors.
   * \param other   other vector
   * \returns difference
   */
  Vector<PhysicalType_, PrimType_, Dimension_> operator-(const Vector<PhysicalType_, PrimType_, Dimension_>& other) const {
    return Vector<PhysicalType_, PrimType_, Dimension_>(this->toImplementation() - other.toImplementation());
  }

  /*! \brief Multiplies vector with a scalar.
   * \param factor   factor
   * \returns product
   */
  template<typename PrimTypeFactor_>
  Vector<PhysicalType_, PrimType_, Dimension_> operator*(PrimTypeFactor_ factor) const {
    return Vector<PhysicalType_, PrimType_, Dimension_>(this->toImplementation()*(PrimType_)factor);
  }

  /*! \brief Divides vector by a scalar.
   * \param divisor   divisor
   * \returns quotient
   */
  template<typename PrimTypeDivisor_>
  Vector<PhysicalType_, PrimType_, Dimension_> operator/(PrimTypeDivisor_ divisor) const {
    return Vector<PhysicalType_, PrimType_, Dimension_>(this->toImplementation()/(PrimType_)divisor);
  }

  /*! \brief Addition and assignment of two vectors.
   * \param other   other vector
   * \returns reference
   */
  Vector<PhysicalType_, PrimType_, Dimension_>& operator+=(const Vector<PhysicalType_, PrimType_, Dimension_>& other) {
    this->toImplementation() += other.toImplementation();
    return *this;
  }

  /*! \brief Subtraction and assignment of two vectors.
   * \param other   other vector
   * \returns reference
   */
  Vector<PhysicalType_, PrimType_, Dimension_>& operator-=(const Vector<PhysicalType_, PrimType_, Dimension_>& other) {
    this->toImplementation() -= other.toImplementation();
    return *this;
  }

  /*! \brief Multiplication with a scalar and assignment.
   * \param factor   factor
   * \returns reference
   */
  template<typename PrimTypeFactor_>
  Vector<PhysicalType_, PrimType_, Dimension_>& operator*=(PrimTypeFactor_ factor) {
    this->toImplementation() *= (PrimType_)factor;
    return *this;
  }

  /*! \brief Division by a scalar and assignment.
   * \param divisor   divisor
   * \returns reference
   */
  template<typename PrimTypeDivisor_>
  Vector<PhysicalType_, PrimType_, Dimension_>& operator/=(PrimTypeDivisor_ divisor) {
    this->toImplementation() /= (PrimType_)divisor;
    return *this;
  }

  /*! \brief Negation of a vector.
   * \returns negative vector
   */
  Vector<PhysicalType_, PrimType_, Dimension_> operator-() const {
    return Vector<PhysicalType_, PrimType_, Dimension_>(-this->toImplementation());
  }

  /*! \brief Comparison operator.
   * \param other   other vector
   * \returns true if equal
   */
  bool operator==(const Vector<PhysicalType_, PrimType_, Dimension_>& other) const {
    return this->toImplementation() == other.toImplementation();
  }

  /*! \brief Comparison operator.
   * \param other   other vector
   * \returns true if unequal
   */
  bool operator!=(const Vector<PhysicalType_, PrimType_, Dimension_>& other) const {
    return this->toImplementation() != other.toImplementation();
  }

  /*! \brief Comparison function.
   * \param other   other vector
   * \param tol   tolerance
   * \returns true if similar within tolerance
   */
  bool isSimilarTo(const Vector<PhysicalType_, PrimType_, Dimension_>& other, Scalar tol) const {
    if((*this - other).abs().max() < tol) {
      return true;
    } else {
      return false;
    }
  }

  /*! \brief Norm of the vector.
   *  \returns norm.
   */
  Scalar norm() const {
    return this->toImplementation().norm();
  }

  /*! \brief Squared norm of the vector.
   *  \returns norm.
   */
  Scalar squaredNorm() const {
    return this->toImplementation().squaredNorm();
  }

  /*! \brief Normalizes the vector.
   *  \returns reference.
   */
  Vector<PhysicalType_, PrimType_, Dimension_>& normalize() {
    this->toImplementation().normalize();
    return *this;
  }

  /*! \brief Get a normalized version of the vector.
   *  \returns normalized vector.
   */
  Vector<PhysicalType_, PrimType_, Dimension_> normalized() const {
    return Vector<PhysicalType_, PrimType_, Dimension_>(this->toImplementation().normalized());
  }

  /*! \brief Dot product with other vector.
   *  \param other   other vector
   *  \returns dot product.
   */
  template<enum PhysicalType PhysicalTypeOther_>
  Scalar dot(const Vector<PhysicalTypeOther_, PrimType_, Dimension_>& other) const {
    return this->toImplementation().dot(other.toImplementation());
  }

  /*! \brief Cross product with other vector.
   *  \param other   other vector
   *  \returns cross product.
   */
  template<enum PhysicalType PhysicalTypeOther_, int DimensionCopy_ = Dimension_>
  typename internal::MultiplicationReturnTypeTrait<Vector<PhysicalType_, PrimType_, Dimension_>, Vector<PhysicalTypeOther_, PrimType_, Dimension_>>::ReturnType
  cross(const Vector<PhysicalTypeOther_, PrimType_, Dimension_>& other, typename std::enable_if<DimensionCopy_ == 3>::type* = nullptr) const {
    return typename internal::MultiplicationReturnTypeTrait<Vector<PhysicalType_, PrimType_, Dimension_>, Vector<PhysicalTypeOther_, PrimType_, Dimension_>>::ReturnType(this->toImplementation().cross(other.toImplementation()));
  }

  /*! \brief Projects this vector (a) on the other vector (b).
   *  The result  is
   *    proj = b/|b| * |a| * cos(angle) = b/|b| * |a| * a.b / |a| / |b|,
   *  which is computed by
   *           a.b
   *  proj = ------- * a
   *          |a|*|a|
   *  \param other   other vector
   *  \returns projected vector.
   */
  template<enum PhysicalType PhysicalTypeOther_>
  Vector<PhysicalType_, PrimType_, Dimension_> projectOn(const Vector<PhysicalTypeOther_, PrimType_, Dimension_>& other) const {
    return other * (this->dot(other)/other.squaredNorm());
  }

  /*! \brief Elementwise product with other vector.
   *  \param other   other vector
   *  \returns elementwise product.
   */
  template<enum PhysicalType PhysicalTypeOther_>
  typename internal::MultiplicationReturnTypeTrait<Vector<PhysicalType_, PrimType_, Dimension_>, Vector<PhysicalTypeOther_, PrimType_, Dimension_>>::ReturnType
  elementwiseMultiplication(const Vector<PhysicalTypeOther_, PrimType_, Dimension_>& other) const {
    return typename internal::MultiplicationReturnTypeTrait<Vector<PhysicalType_, PrimType_, Dimension_>, Vector<PhysicalTypeOther_, PrimType_, Dimension_>>::ReturnType(this->toImplementation().cwiseProduct(other.toImplementation()));
  }

  /*! \brief Elementwise division by other vector.
   *  \param other   other vector
   *  \returns elementwise quotient.
   */
  template<enum PhysicalType PhysicalTypeOther_>
  typename internal::DivisionReturnTypeTrait<Vector<PhysicalType_, PrimType_, Dimension_>, Vector<PhysicalTypeOther_, PrimType_, Dimension_>>::ReturnType
  elementwiseDivision(const Vector<PhysicalTypeOther_, PrimType_, Dimension_>& other) const {
    return typename internal::DivisionReturnTypeTrait<Vector<PhysicalType_, PrimType_, Dimension_>, Vector<PhysicalTypeOther_, PrimType_, Dimension_>>::ReturnType(this->toImplementation().cwiseQuotient(other.toImplementation()));
  }

  /*! \brief Absolute components.
   *  \returns absolute components.
   */
  Vector<PhysicalType_, PrimType_, Dimension_> abs() const {
    return Vector<PhysicalType_, PrimType_, Dimension_>(this->toImplementation().cwiseAbs());
  }

  /*! \brief Maximum of the components.
   *  \returns maximum.
   */
  Scalar max() const {
    return this->toImplementation().maxCoeff();
  }

  /*! \brief Minimum of the components.
   *  \returns minimum.
   */
  Scalar min() const {
    return this->toImplementation().minCoeff();
  }

  /*! \brief Sum of the components.
   *  \returns sum.
   */
  Scalar sum() const {
    return this->toImplementation().sum();
  }

  /*! \brief Mean of the components.
   *  \returns mean.
   */
  Scalar mean() const {
    return this->toImplementation().mean();
  }

  /*! \brief Used for printing the object with std::cout.
   *  \returns std::stream object
   */
  friend std::ostream& operator << (std::ostream& out, const Vector<PhysicalType_, PrimType_, Dimension_>& vector) {
    out << vector.transpose();
    return out;
  }
};

/*! \brief Multiplies a vector with a scalar.
 * \param factor   factor
 * \returns product
 */
template<enum PhysicalType PhysicalType_, typename PrimTypeFactor_, typename PrimType_, int Dimension_>
Vector<PhysicalType_, PrimType_, Dimension_> operator*(PrimTypeFactor_ factor, const Vector<PhysicalType_, PrimType_, Dimension_>& vector) {
  return vector*(PrimType_)factor;
}

namespace internal {

/*! \brief Gets the primitive type of the vector
 */
template<enum PhysicalType PhysicalType_, typename PrimType_, int Dimension_>
class get_scalar<Vector<PhysicalType_, PrimType_, Dimension_>> {
 public:
  typedef PrimType_ Scalar;
};

/*! \brief Gets the dimension of the vector
 */
template<enum PhysicalType PhysicalType_, typename PrimType_, int Dimension_>
class get_dimension<Vector<PhysicalType_, PrimType_, Dimension_>> {
 public:
  static constexpr int Dimension = Dimension_;
};

/*! \brief Gets the return type of a multiplication
 */
template<enum PhysicalType PhysicalType1_, enum PhysicalType PhysicalType2_, typename PrimType_, int Dimension_>
class MultiplicationReturnTypeTrait<Vector<PhysicalType1_, PrimType_, Dimension_>, Vector<PhysicalType2_, PrimType_, Dimension_>>
{
 public:
  typedef Vector<PhysicalType::Typeless, PrimType_, Dimension_> ReturnType;
};

/*! \brief Gets the return type of a multiplication
 */
template<enum PhysicalType PhysicalType1_, enum PhysicalType PhysicalType2_, typename PrimType_, int Dimension_>
class DivisionReturnTypeTrait<Vector<PhysicalType1_, PrimType_, Dimension_>, Vector<PhysicalType2_, PrimType_, Dimension_>>
{
 public:
  typedef Vector<PhysicalType::Typeless, PrimType_, Dimension_> ReturnType;
};

/*! \brief Specializes multiplication and division traits for the triple (factor1 != factor2)
 */
#define KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(FACTOR1, FACTOR2, PRODUCT) \
    template<typename PrimType_, int Dimension_> \
    class MultiplicationReturnTypeTrait<Vector<PhysicalType::FACTOR1, PrimType_, Dimension_>, Vector<PhysicalType::FACTOR2, PrimType_, Dimension_>> \
    { \
     public: \
      typedef Vector<PhysicalType::PRODUCT, PrimType_, Dimension_> ReturnType; \
    }; \
    template<typename PrimType_, int Dimension_> \
    class MultiplicationReturnTypeTrait<Vector<PhysicalType::FACTOR2, PrimType_, Dimension_>, Vector<PhysicalType::FACTOR1, PrimType_, Dimension_>> \
    { \
     public: \
      typedef Vector<PhysicalType::PRODUCT, PrimType_, Dimension_> ReturnType; \
    }; \
    template<typename PrimType_, int Dimension_> \
    class DivisionReturnTypeTrait<Vector<PhysicalType::PRODUCT, PrimType_, Dimension_>, Vector<PhysicalType::FACTOR1, PrimType_, Dimension_>> \
    { \
     public: \
      typedef Vector<PhysicalType::FACTOR2, PrimType_, Dimension_> ReturnType; \
    }; \
    template<typename PrimType_, int Dimension_> \
    class DivisionReturnTypeTrait<Vector<PhysicalType::PRODUCT, PrimType_, Dimension_>, Vector<PhysicalType::FACTOR2, PrimType_, Dimension_>> \
    { \
     public: \
      typedef Vector<PhysicalType::FACTOR1, PrimType_, Dimension_> ReturnType; \
    };

/*! \brief Specializes multiplication and division traits for the triple (factor1 == factor2)
 */
#define KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_B(FACTOR1AND2, PRODUCT) \
    template<typename PrimType_, int Dimension_> \
    class MultiplicationReturnTypeTrait<Vector<PhysicalType::FACTOR1AND2, PrimType_, Dimension_>, Vector<PhysicalType::FACTOR1AND2, PrimType_, Dimension_>> \
    { \
     public: \
      typedef Vector<PhysicalType::PRODUCT, PrimType_, Dimension_> ReturnType; \
    }; \
    template<typename PrimType_, int Dimension_> \
    class DivisionReturnTypeTrait<Vector<PhysicalType::PRODUCT, PrimType_, Dimension_>, Vector<PhysicalType::FACTOR1AND2, PrimType_, Dimension_>> \
    { \
     public: \
      typedef Vector<PhysicalType::FACTOR1AND2, PrimType_, Dimension_> ReturnType; \
    };

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_B(Typeless, Typeless)

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Time, Time)

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Jerk, Jerk)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Acceleration, Acceleration)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Velocity, Velocity)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Position, Position) // Position/Position = Typeless
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Force, Force)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Momentum, Momentum)

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, AngularJerk, AngularJerk)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, AngularAcceleration, AngularAcceleration)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, AngularVelocity, AngularVelocity)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Angle, Angle)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, Torque, Torque)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Typeless, AngularMomentum, AngularMomentum)

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Position, AngularJerk, Jerk)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Position, AngularAcceleration, Acceleration)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Position, AngularVelocity, Velocity)
//KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Position, Angle, Position) // Position/Position = Angle -> ambiguous, explicit cast to Angle if needed
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Position, Force, Torque)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Position, Momentum, AngularMomentum)

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, Jerk, Acceleration)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, Acceleration, Velocity)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, Velocity, Position)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, Force, Momentum)

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, AngularJerk, AngularAcceleration)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, AngularAcceleration, AngularVelocity)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, AngularVelocity, Angle)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Time, Torque, AngularMomentum)

KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Mass, Acceleration, Force)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Mass, Velocity, Momentum)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Inertia, AngularAcceleration, Torque)
KINDR_SPECIALIZE_PHYS_QUANT_RETURN_TYPE_A(Inertia, AngularVelocity, AngularMomentum)

} // namespace internal
} // namespace kindr


#ifndef EIGENCUDA_H_
#define EIGENCUDA_H_

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cublas_v2.h>
#include <curand.h>
#include <iostream>
#include <memory>
#include <sstream>
#include <vector>

/*
 * \brief Perform Tensor-matrix multiplications in a GPU
 *
 * The `CudaPipeline` class handles the allocation and deallocation of arrays on
 * the GPU.
 */

namespace eigencuda {

cudaError_t checkCuda(cudaError_t result);

using Index = Eigen::Index;
Index count_available_gpus();

class CudaMatrix {
 public:
  Index size() const { return _rows * _cols; };
  Index rows() const { return _rows; };
  Index cols() const { return _cols; };
  double *data() const { return _data.get(); };

  CudaMatrix(const Eigen::MatrixXd &matrix, const cudaStream_t &stream);

  // Allocate memory in the GPU for a matrix
  CudaMatrix(Index nrows, Index ncols, const cudaStream_t &stream);

  // Convert A Cudamatrix to an EigenMatrix
  operator Eigen::MatrixXd() const;

  void copy_to_gpu(const Eigen::MatrixXd &A);

 private:
  // Unique pointer with custom delete function
  using Unique_ptr_to_GPU_data = std::unique_ptr<double, void (*)(double *)>;

  Unique_ptr_to_GPU_data alloc_matrix_in_gpu(size_t size_arr) const;

  void throw_if_not_enough_memory_in_gpu(size_t requested_memory) const;

  size_t size_matrix() const { return this->size() * sizeof(double); }

  // Attributes of the matrix in the device
  Unique_ptr_to_GPU_data _data{nullptr,
                               [](double *x) { checkCuda(cudaFree(x)); }};
  cudaStream_t _stream = nullptr;
  Index _rows;
  Index _cols;
};

}  // namespace eigencuda

#endif  // EIGENCUDA_H_


#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>
#include <boost/algorithm/string/split.hpp>

#include <string>
#include <vector>
#include <memory>

#include "../../include/ast.hpp"

#include "test_setup.hpp"

namespace data = boost::unit_test::data;
namespace ast = rhea::ast;

namespace {
    // Datasets

    // Tests
    BOOST_AUTO_TEST_SUITE (AST_Binop)

    BOOST_AUTO_TEST_CASE (binop_ast)
    {
        ast::expression_ptr l = std::make_unique<ast::Integer>(42);
        ast::expression_ptr r = std::make_unique<ast::Integer>(69);
        auto op = ast::BinaryOperators::Add;

        auto node = std::make_unique<ast::BinaryOp>(op, std::move(l), std::move(r));

        BOOST_TEST_MESSAGE("Testing AST Node " << node->to_string());

        BOOST_TEST((node->op == ast::BinaryOperators::Add));

        auto node_l = dynamic_cast<ast::Integer*>(node->left.get());
        auto node_r = dynamic_cast<ast::Integer*>(node->right.get());
        BOOST_TEST(node_l->value + node_r->value == 42 + 69);
    }

    BOOST_AUTO_TEST_SUITE_END ()

    BOOST_AUTO_TEST_SUITE (AST_TernaryOp)

    BOOST_AUTO_TEST_CASE (ternary_ast)
    {
        ast::expression_ptr c = std::make_unique<ast::Integer>(42);
        ast::expression_ptr t = std::make_unique<ast::Boolean>(true);
        ast::expression_ptr f = std::make_unique<ast::Boolean>(false);

        auto node = std::make_unique<ast::TernaryOp>(std::move(c),std::move(t),std::move(f));

        BOOST_TEST_MESSAGE("Testing AST Node " << node->to_string());

        auto node_c = dynamic_cast<ast::Integer*>(node->condition.get());
        auto node_t = dynamic_cast<ast::Boolean*>(node->true_branch.get());
        auto node_f = dynamic_cast<ast::Boolean*>(node->false_branch.get());

        BOOST_TEST(node_c->value == 42);
        BOOST_TEST(node_t->value == !node_f->value);
    }

    BOOST_AUTO_TEST_SUITE_END ()

    BOOST_AUTO_TEST_SUITE (AST_UnaryOp)

    BOOST_AUTO_TEST_CASE (unary_ast)
    {
        ast::expression_ptr o = std::make_unique<ast::Integer>(42);

        auto node = std::make_unique<ast::UnaryOp>(ast::UnaryOperators::Minus, std::move(o));

        BOOST_TEST_MESSAGE("Testing AST Node " << node->to_string());

        BOOST_TEST((node->op == ast::UnaryOperators::Minus));
        BOOST_TEST((node->to_string() == "(UnaryOp,1,(Integral,42,0))"));
    }

    BOOST_AUTO_TEST_SUITE_END ()
}


// Copyright (C) 2012  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.
#include <dlib/optimization.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>

#include "tester.h"

namespace
{

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

    logger dlog("test.parse");

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

    const unsigned long DET = 0;
    const unsigned long N   = 1;
    const unsigned long V   = 2;
    const unsigned long NP  = 3;
    const unsigned long VP  = 4;
    const unsigned long S   = 5;
    const unsigned long B   = 6;
    const unsigned long G   = 7;
    const unsigned long A   = 8;

    typedef unsigned long tags;

    template <bool has_glue_term>
    void user_defined_ruleset (
        const std::vector<tags>& words,
        const constituent<tags>& c,
        std::vector<std::pair<tags,double> >& possible_ids
    )
    {
        DLIB_TEST(c.begin < c.k && c.k < c.end && c.end <= words.size());
        DLIB_TEST(possible_ids.size() == 0);

        if (c.left_tag == NP && c.right_tag == VP)      possible_ids.push_back(make_pair(S,log(0.80)));
        else if (c.left_tag == DET && c.right_tag == N) possible_ids.push_back(make_pair(NP,log(0.30)));
        else if (c.left_tag == VP && c.right_tag == A) possible_ids.push_back(make_pair(VP,log(0.30)));
        else if (c.left_tag == V && c.right_tag == NP)
        {
            possible_ids.push_back(make_pair(VP,log(0.20)));
            possible_ids.push_back(make_pair(B,0.10));
        }
        else if (has_glue_term)
        {
            possible_ids.push_back(make_pair(G, log(0.01)));
        }
    }

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

    void dotest1()
    {
        print_spinner();
        dlog << LINFO << "in dotest1()";

        std::vector<std::string> words;
        std::vector<tags> sequence;
        for (int i = 0; i < 8; ++i)
        {
            sequence.push_back(DET);
            sequence.push_back(N);
            sequence.push_back(V);
            sequence.push_back(DET);
            sequence.push_back(N);
            sequence.push_back(A);

            words.push_back("The");
            words.push_back("flight");
            words.push_back("includes");
            words.push_back("a");
            words.push_back("meal");
            words.push_back("AWORD");
        }

        std::vector<parse_tree_element<tags> > parse_tree;

        find_max_parse_cky(sequence, user_defined_ruleset<true>, parse_tree);
        DLIB_TEST(parse_tree.size() != 0);

        std::vector<unsigned long> roots;
        find_trees_not_rooted_with_tag(parse_tree, G, roots);
        DLIB_TEST(roots.size() == 8);

        for (unsigned long i = 0; i < roots.size(); ++i)
        {
            dlog << LINFO << parse_tree_to_string(parse_tree, words, roots[i]);
            DLIB_TEST(parse_tree_to_string(parse_tree, words, roots[i]) == "[[The flight] [[includes [a meal]] AWORD]]");
            dlog << LINFO << parse_tree_to_string_tagged(parse_tree, words, roots[i]);
            DLIB_TEST(parse_tree_to_string_tagged(parse_tree, words, roots[i]) == "[5 [3 The flight] [4 [4 includes [3 a meal]] AWORD]]");
        }

        words.clear();
        sequence.clear();

        for (int i = 0; i < 2; ++i)
        {
            sequence.push_back(DET);
            sequence.push_back(N);
            sequence.push_back(V);
            sequence.push_back(DET);
            sequence.push_back(N);

            words.push_back("The");
            words.push_back("flight");
            words.push_back("includes");
            words.push_back("a");
            words.push_back("meal");
        }

        find_max_parse_cky(sequence, user_defined_ruleset<true>, parse_tree);
        DLIB_TEST(parse_tree.size() != 0);

        const std::string str1 = "[[[The flight] [includes [a meal]]] [[The flight] [includes [a meal]]]]";
        const std::string str2 = "[7 [5 [3 The flight] [4 includes [3 a meal]]] [5 [3 The flight] [4 includes [3 a meal]]]]";
        dlog << LINFO << parse_tree_to_string(parse_tree, words);
        DLIB_TEST(parse_tree_to_string(parse_tree, words) == str1);
        dlog << LINFO << parse_tree_to_string_tagged(parse_tree, words);
        DLIB_TEST(parse_tree_to_string_tagged(parse_tree, words) == str2);

        const std::string str3 = "[[The flight] [includes [a meal]]] [[The flight] [includes [a meal]]]";
        const std::string str4 = "[5 [3 The flight] [4 includes [3 a meal]]] [5 [3 The flight] [4 includes [3 a meal]]]";
        dlog << LINFO << parse_trees_to_string(parse_tree, words, G);
        DLIB_TEST(parse_trees_to_string(parse_tree, words, G) == str3);
        dlog << LINFO << parse_trees_to_string_tagged(parse_tree, words, G);
        DLIB_TEST(parse_trees_to_string_tagged(parse_tree, words, G) == str4);

        sequence.clear();
        find_max_parse_cky(sequence, user_defined_ruleset<true>, parse_tree);
        DLIB_TEST(parse_tree.size() == 0);
    }

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

    void dotest2()
    {
        print_spinner();
        dlog << LINFO << "in dotest2()";

        std::vector<std::string> words;
        std::vector<tags> sequence;
        for (int i = 0; i < 8; ++i)
        {
            sequence.push_back(DET);
            sequence.push_back(N);
            sequence.push_back(V);
            sequence.push_back(DET);
            sequence.push_back(N);

            words.push_back("The");
            words.push_back("flight");
            words.push_back("includes");
            words.push_back("a");
            words.push_back("meal");
        }

        std::vector<parse_tree_element<tags> > parse_tree;

        find_max_parse_cky(sequence, user_defined_ruleset<false>, parse_tree);
        DLIB_TEST(parse_tree.size() == 0);

        std::vector<unsigned long> roots;
        find_trees_not_rooted_with_tag(parse_tree, G, roots);
        DLIB_TEST(roots.size() == 0);

        words.clear();
        sequence.clear();

        for (int i = 0; i < 2; ++i)
        {
            sequence.push_back(DET);
            sequence.push_back(N);
            sequence.push_back(V);
            sequence.push_back(DET);
            sequence.push_back(N);

            words.push_back("The");
            words.push_back("flight");
            words.push_back("includes");
            words.push_back("a");
            words.push_back("meal");
        }

        find_max_parse_cky(sequence, user_defined_ruleset<false>, parse_tree);
        DLIB_TEST(parse_tree.size() == 0);

        sequence.clear();
        find_max_parse_cky(sequence, user_defined_ruleset<false>, parse_tree);
        DLIB_TEST(parse_tree.size() == 0);
    }

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

    class parse_tester : public tester
    {
    public:
        parse_tester (
        ) :
            tester ("test_parse",
                    "Runs tests on the parsing tools.")
        {}

        void perform_test (
        )
        {
            dotest1();
            dotest2();
        }
    } a;

}


/**********************************************************************************************************************
This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
Licensed under the BSD-2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#pragma once

#include <map>

#include <boost/property_tree/info_parser.hpp>

namespace ct {
namespace optcon {

//! GNMS Line Search Settings
/*!
 * \ingroup GNMS
 *
 * The Line Search Settings are part of the general settings struct and hold parameters to customize the line-search for the NLOptCon controller update.
 */
struct LineSearchSettings
{
    //! types of backtracking line-search
    enum TYPE
    {
        NONE = 0,      //! take full-step updates
        SIMPLE,        //! simple backtracking using cost or merit function
        ARMIJO,        //! backtracking including riccati matrix measure
        GOLDSTEIN,  //! backtracking including riccati matrix measure and defects
        NUM_TYPES
    };

    //! mappings for line-search types
    std::map<TYPE, std::string> lineSearchTypeToString = {{NONE, "NONE (take full-step updates with alpha=1.0)"},
        {SIMPLE, "Simple Backtracking with cost/merit"}, {ARMIJO, "ARMIJO-style Backtracking for single-shooting"},
        {GOLDSTEIN, "GOLDSTEIN backtracking using Riccati matrices"}};

    std::map<std::string, TYPE> stringToLineSearchType = {
        {"NONE", NONE}, {"SIMPLE", SIMPLE}, {"ARMIJO", ARMIJO}, {"GOLDSTEIN", GOLDSTEIN}};

    //! default constructor for the NLOptCon line-search settings
    LineSearchSettings()
        : type(NONE),
          adaptive(false),
          maxIterations(10),
          alpha_0(1.0),
          alpha_max(1.0),
          n_alpha(0.5),
          armijo_parameter(0.01),
          debugPrint(false)
    {
    }

    //! check if the currently set line-search parameters are meaningful
    bool parametersOk() const { return (alpha_0 > 0.0) && (n_alpha > 0.0) && (n_alpha < 1.0) && (alpha_max > 0.0); }
    TYPE type;            /*!< type of line search */
    bool adaptive;        /*!< Flag whether alpha_0 gets updated based on previous iteration */
    size_t maxIterations; /*!< Maximum number of iterations during line search */
    double alpha_0;       /*!< Initial step size for line search. Use 1 for step size as suggested by NLOptCon */
    double alpha_max;     /*!< Maximum step size for line search. This is the limit when adapting alpha_0. */
    double
        n_alpha; /*!< Factor by which the step size alpha gets scaled after each iteration. Usually 0.5 is a good value. */
    double armijo_parameter; /*!< "Control Parameter" in Armijo line search condition. */
    bool debugPrint;         /*!< Print out debug information during line-search*/

    //! print the current line search settings to console
    void print() const
    {
        std::cout << "Line Search Settings: " << std::endl;
        std::cout << "=====================" << std::endl;
        std::cout << "type:\t" << lineSearchTypeToString.at(type) << std::endl;
        std::cout << "adaptive:\t" << adaptive << std::endl;
        std::cout << "maxIter:\t" << maxIterations << std::endl;
        std::cout << "alpha_0:\t" << alpha_0 << std::endl;
        std::cout << "alpha_max:\t" << alpha_max << std::endl;
        std::cout << "n_alpha:\t" << n_alpha << std::endl;
        std::cout << "armijo_parameter:\t" << armijo_parameter << std::endl;
        std::cout << "debugPrint:\t" << debugPrint << std::endl;
        std::cout << "              =======" << std::endl;
        std::cout << std::endl;
    }

    //! load line search settings from file
    void load(const std::string& filename, bool verbose = true, const std::string& ns = "line_search")
    {
        if (verbose)
            std::cout << "Trying to load line search settings from " << filename << ": " << std::endl;

        boost::property_tree::ptree pt;
        boost::property_tree::read_info(filename, pt);

        std::string ls_type = pt.get<std::string>(ns + ".type");
        if (stringToLineSearchType.find(ls_type) != stringToLineSearchType.end())
        {
            type = stringToLineSearchType[ls_type];
        }
        else
        {
            std::cout << "Invalid line search type specified in config, should be one of the following:" << std::endl;

            for (auto it = stringToLineSearchType.begin(); it != stringToLineSearchType.end(); it++)
            {
                std::cout << it->first << std::endl;
            }

            exit(-1);
        }

        maxIterations = pt.get<size_t>(ns + ".maxIterations");
        alpha_0 = pt.get<double>(ns + ".alpha_0");
        n_alpha = pt.get<double>(ns + ".n_alpha");
        debugPrint = pt.get<bool>(ns + ".debugPrint");
        alpha_max = alpha_0;
        adaptive = false;

        try
        {
            armijo_parameter = pt.get<double>(ns + ".armijo_parameter");
        } catch (...)
        {
        }

        try
        {
            adaptive = pt.get<bool>(ns + ".adaptive");
        } catch (...)
        {
        }

        try
        {
            alpha_max = pt.get<double>(ns + ".alpha_max");
        } catch (...)
        {
        }

        if (verbose)
        {
            std::cout << "Loaded line search settings from " << filename << ": " << std::endl;
            print();
        }
    }
};

//! LQOC Solver settings
/*!
 * Settings for solving each linear-quadratic (constrained) sub-problem
 */
struct LQOCSolverSettings
{
public:
    LQOCSolverSettings() : lqoc_debug_print(false), num_lqoc_iterations(10) {}

    bool lqoc_debug_print;
    int num_lqoc_iterations;  //! number of allowed sub-iterations of LQOC solver per NLOC main iteration

    void print() const
    {
        std::cout << "======================= LQOCSolverSettings =====================" << std::endl;
        std::cout << "num_lqoc_iterations: \t" << num_lqoc_iterations << std::endl;
        std::cout << "lqoc_debug_print: \t" << lqoc_debug_print << std::endl;
    }

    void load(const std::string& filename, bool verbose = true, const std::string& ns = "lqoc_solver_settings")
    {
        if (verbose)
            std::cout << "Trying to load LQOCSolverSettings config from " << filename << ": " << std::endl;

        boost::property_tree::ptree pt;
        boost::property_tree::read_info(filename, pt);

        try
        {
            num_lqoc_iterations = pt.get<int>(ns + ".num_lqoc_iterations");
        } catch (...)
        {
        }
        try
        {
            lqoc_debug_print = pt.get<bool>(ns + ".lqoc_debug_print");
        } catch (...)
        {
        }
    }
};

/*!
 * \ingroup NLOptCon
 *
 * \brief Settings for the NLOptCon algorithm.
 */
class NLOptConSettings
{
public:
    //! algorithm types for solving the NLOC problem
    enum NLOCP_ALGORITHM
    {
        GNMS = 0,   //! Gauss-Newton Multiple Shooting (shooting interval = control interval)
        ILQR,       //! Classical iLQR (1 shooting interval equal to problem horizon)
        MS_ILQR,    //! multiple-shooting iLQR
        SS_OL,      //! Classical (open-loop) Single Shooting
        SS_CL,      //! Closed-loop single shooting
        GNMS_M_OL,  //! GNMS(M) with open-loop shooting
        GNMS_M_CL,  //! GNMS(M) with closed-loop shooting
        NUM_TYPES
    };

    //! the linear optimal control problem solver in the background
    enum LQOCP_SOLVER
    {
        GNRICCATI_SOLVER = 0,
        HPIPM_SOLVER = 1
    };

    using APPROXIMATION = typename core::SensitivityApproximationSettings::APPROXIMATION;

    //! NLOptCon Settings default constructor
    /*!
     * sets all settings to default values.
     */
    NLOptConSettings()
        : integrator(ct::core::IntegrationType::RK4),
          discretization(APPROXIMATION::BACKWARD_EULER),
          timeVaryingDiscretization(false),
          nlocp_algorithm(GNMS),
          lqocp_solver(GNRICCATI_SOLVER),
          loggingPrefix("alg"),
          epsilon(1e-5),
          dt(0.001),
          K_sim(1),                    //! by default, there is only one sub-integration step
          K_shot(1),                   //! by default the shot length is equal to the control length
          min_cost_improvement(1e-5),  //! cost needs to be at least 1e-5 better before we assume convergence
          maxDefectSum(1e-5),
          meritFunctionRho(0.0),
          meritFunctionRhoConstraints(1.0),
          max_iterations(100),
          fixedHessianCorrection(false),
          recordSmallestEigenvalue(false),
          nThreads(4),
          nThreadsEigen(4),
          lineSearchSettings(),
          debugPrint(false),
          printSummary(true),
          useSensitivityIntegrator(false),
          logToMatlab(false)
    {
    }

    ct::core::IntegrationType integrator;  //! which integrator to use during the NLOptCon forward rollout
    APPROXIMATION discretization;
    bool timeVaryingDiscretization;
    NLOCP_ALGORITHM nlocp_algorithm;  //! which nonlinear optimal control algorithm is to be used
    LQOCP_SOLVER lqocp_solver;        //! the solver for the linear-quadratic optimal control problem
    std::string loggingPrefix;        //! the prefix to be stored before the matfile name for logging
    double epsilon;                   //! Eigenvalue correction factor for Hessian regularization
    double dt;                        //! sampling time for the control input (seconds)
    int K_sim;                        //! number of sub-integration-steps
    int K_shot;                       //! duration of a shot as an integer multiple of dt
    double min_cost_improvement;      //! minimum cost improvement between two interations to assume convergence
    double maxDefectSum;              //! maximum sum of squared defects (assume covergence if lower than this number)
    double meritFunctionRho;          //! trade off between internal (defect)constraint violation and cost
    double meritFunctionRhoConstraints;  //! trade off between external (general and path) constraint violation and cost
    int max_iterations;  //! the maximum admissible number of NLOptCon main iterations \warning make sure to select this number high enough allow for convergence
    bool fixedHessianCorrection;    //! perform Hessian regularization by incrementing the eigenvalues by epsilon.
    bool recordSmallestEigenvalue;  //! save the smallest eigenvalue of the Hessian
    int nThreads;                   //! number of threads, for MP version
    size_t
        nThreadsEigen;  //! number of threads for eigen parallelization (applies both to MP and ST) Note. in order to activate Eigen parallelization, compile with '-fopenmp'
    LineSearchSettings lineSearchSettings;  //! the line search settings
    LQOCSolverSettings lqoc_solver_settings;
    bool debugPrint;
    bool printSummary;
    bool useSensitivityIntegrator;
    bool logToMatlab;  //! log to matlab (true/false)

    //! compute the number of discrete time steps for an arbitrary input time interval
    /*!
     * @param timeHorizon the time horizon of interest, e.g. overall optimal control time horizon or shot-length
     * @return the resulting number of steps, minimum 1 steps long
     *
     * \todo naming it K is confusing, should better be N.
     */
    int computeK(double timeHorizon) const
    {
        if (timeHorizon < 0.0)
        {
            throw std::runtime_error("time Horizon is negative");
        }
        return std::max(1, (int)std::lround(timeHorizon / dt));
    }

    //! compute the simulation timestep
    double getSimulationTimestep() const { return (dt / (double)K_sim); }

    //! return if this is a closed-loop shooting algorithm (or not)
    bool closedLoopShooting() const { return nlocAlgorithmToClosedLoopShooting.at(nlocp_algorithm); }

    //! return if this is a single-shooting algorithm (or not)
    bool isSingleShooting() const { return nlocAlgorithmToSingleShooting.at(nlocp_algorithm); }

    //! print the current NLOptCon settings to console
    void print() const
    {
        std::cout << "======================= NLOptCon Settings =====================" << std::endl;
        std::cout << "===============================================================" << std::endl;
        std::cout << "integrator: " << integratorToString.at(integrator) << std::endl;
        std::cout << "discretization: " << discretizationToString.at(discretization) << std::endl;
        std::cout << "time varying discretization: " << timeVaryingDiscretization << std::endl;
        std::cout << "nonlinear OCP algorithm: " << nlocAlgorithmToString.at(nlocp_algorithm) << std::endl;
        std::cout << "linear-quadratic OCP solver: " << lqocSolverToString.at(lqocp_solver) << std::endl;
        std::cout << "dt:\t" << dt << std::endl;
        std::cout << "K_sim:\t" << K_sim << std::endl;
        std::cout << "K_shot:\t" << K_shot << std::endl;
        std::cout << "maxIter:\t" << max_iterations << std::endl;
        std::cout << "min cost improvement:\t" << min_cost_improvement << std::endl;
        std::cout << "max defect sum:\t" << maxDefectSum << std::endl;
        std::cout << "merit function rho defects:\t" << meritFunctionRho << std::endl;
        std::cout << "merit function rho constraints:\t" << meritFunctionRhoConstraints << std::endl;
        std::cout << "fixedHessianCorrection:\t" << fixedHessianCorrection << std::endl;
        std::cout << "recordSmallestEigenvalue:\t" << recordSmallestEigenvalue << std::endl;
        std::cout << "epsilon:\t" << epsilon << std::endl;
        std::cout << "nThreads:\t" << nThreads << std::endl;
        std::cout << "nThreadsEigen:\t" << nThreadsEigen << std::endl;
        std::cout << "loggingPrefix:\t" << loggingPrefix << std::endl;
        std::cout << "debugPrint:\t" << debugPrint << std::endl;
        std::cout << "printSummary:\t" << printSummary << std::endl;
        std::cout << "useSensitivityIntegrator:\t" << useSensitivityIntegrator << std::endl;
        std::cout << "logToMatlab:\t" << logToMatlab << std::endl;
        std::cout << std::endl;

        lineSearchSettings.print();

        lqoc_solver_settings.print();

        std::cout << "===============================================================" << std::endl;
    }

    //! perform a quick check if the given NLOptCon settings fulfil minimum requirements
    /*!
     * \warning This check cannot guarantee that the control problem is well parameterized
     */
    bool parametersOk() const
    {
        if (dt <= 0)
        {
            std::cout << "Invalid parameter dt in NLOptConSettings, needs to be > 0. dt currently is " << dt
                      << std::endl;
            return false;
        }

        if (K_sim <= 0)
        {
            std::cout << "Invalid parameter K_sim in NLOptConSettings, needs to be >= 1. K_sim currently is " << K_sim
                      << std::endl;
            return false;
        }

        if (K_shot <= 0)
        {
            std::cout << "Invalid parameter K_shot in NLOptConSettings, needs to be >= 1. K_shot currently is "
                      << K_shot << std::endl;
            return false;
        }

        // TODO need thorough check if this is really the case ....
        if ((K_shot > 1) && (nlocp_algorithm == ILQR))
        {
            std::cout << "Invalid parameter: for iLQR K_shot needs to be 1. K_shot currently is " << K_shot
                      << std::endl;
            return false;
        }

        if (nThreads > 100 || nThreadsEigen > 100)
        {
            std::cout << "Number of threads should not exceed 100." << std::endl;
            return false;
        }
        return (lineSearchSettings.parametersOk());
    }

    //! load NLOptCon Settings from file
    /*!
     *
     * @param filename path to the settings file
     * @param verbose print out settings after reading them
     * @param ns (optional) namspace of parameter file
     */
    void load(const std::string& filename, bool verbose = true, const std::string& ns = "alg")
    {
        if (verbose)
            std::cout << "Trying to load NLOptCon config from " << filename << ": " << std::endl;

        boost::property_tree::ptree pt;
        boost::property_tree::read_info(filename, pt);

        try
        {
            epsilon = pt.get<double>(ns + ".epsilon");
        } catch (...)
        {
        }
        try
        {
            timeVaryingDiscretization = pt.get<bool>(ns + ".timeVaryingDiscretization");
        } catch (...)
        {
        }
        try
        {
            min_cost_improvement = pt.get<double>(ns + ".min_cost_improvement");
        } catch (...)
        {
        }
        try
        {
            maxDefectSum = pt.get<double>(ns + ".maxDefectSum");
        } catch (...)
        {
        }
        try
        {
            meritFunctionRho = pt.get<double>(ns + ".meritFunctionRho");
        } catch (...)
        {
        }
        try
        {
            meritFunctionRhoConstraints = pt.get<double>(ns + ".meritFunctionRhoConstraints");
        } catch (...)
        {
        }
        try
        {
            max_iterations = pt.get<int>(ns + ".max_iterations");
        } catch (...)
        {
        }
        try
        {
            nThreads = pt.get<int>(ns + ".nThreads");
        } catch (...)
        {
        }
        try
        {
            loggingPrefix = pt.get<std::string>(ns + ".loggingPrefix");
        } catch (...)
        {
        }
        try
        {
            debugPrint = pt.get<bool>(ns + ".debugPrint");
        } catch (...)
        {
        }
        try
        {
            printSummary = pt.get<bool>(ns + ".printSummary");
        } catch (...)
        {
        }
        try
        {
            useSensitivityIntegrator = pt.get<bool>(ns + ".useSensitivityIntegrator");
        } catch (...)
        {
        }
        try
        {
            logToMatlab = pt.get<bool>(ns + ".logToMatlab");
        } catch (...)
        {
        }
        try
        {
            dt = pt.get<double>(ns + ".dt");
        } catch (...)
        {
        }
        try
        {
            K_sim = pt.get<int>(ns + ".K_sim");
        } catch (...)
        {
        }
        try
        {
            K_shot = pt.get<int>(ns + ".K_shot");
        } catch (...)
        {
        }
        try
        {
            nThreadsEigen = pt.get<size_t>(ns + ".nThreadsEigen");
        } catch (...)
        {
        }
        try
        {
            recordSmallestEigenvalue = pt.get<bool>(ns + ".recordSmallestEigenvalue");
        } catch (...)
        {
        }
        try
        {
            fixedHessianCorrection = pt.get<bool>(ns + ".fixedHessianCorrection");
        } catch (...)
        {
        }

        try
        {
            lineSearchSettings.load(filename, verbose, ns + ".line_search");
        } catch (...)
        {
        }
        try
        {
            lqoc_solver_settings.load(filename, verbose, ns + ".lqoc_solver_settings");
        } catch (...)
        {
        }

        try
        {
            std::string integratorStr = pt.get<std::string>(ns + ".integrator");
            if (stringToIntegrator.find(integratorStr) != stringToIntegrator.end())
            {
                integrator = stringToIntegrator[integratorStr];
            }
            else
            {
                std::cout << "Invalid integrator specified in config, should be one of the following:" << std::endl;

                for (auto it = stringToIntegrator.begin(); it != stringToIntegrator.end(); it++)
                {
                    std::cout << it->first << std::endl;
                }

                exit(-1);
            }

            std::string discretizationStr = pt.get<std::string>(ns + ".discretization");
            if (stringToDiscretization.find(discretizationStr) != stringToDiscretization.end())
            {
                discretization = stringToDiscretization[discretizationStr];
            }
            else
            {
                std::cout << "Invalid discretization specified in config, should be one of the following:" << std::endl;

                for (auto it = stringToDiscretization.begin(); it != stringToDiscretization.end(); it++)
                {
                    std::cout << it->first << std::endl;
                }

                exit(-1);
            }

            std::string nlocp_algorithmStr = pt.get<std::string>(ns + ".nlocp_algorithm");
            if (stringToNlocAlgorithm.find(nlocp_algorithmStr) != stringToNlocAlgorithm.end())
            {
                nlocp_algorithm = stringToNlocAlgorithm[nlocp_algorithmStr];
            }
            else
            {
                std::cout << "Invalid nlocp_algorithm specified in config, should be one of the following:"
                          << std::endl;

                for (auto it = stringToNlocAlgorithm.begin(); it != stringToNlocAlgorithm.end(); it++)
                {
                    std::cout << it->first << std::endl;
                }

                exit(-1);
            }

            std::string locp_solverStr = pt.get<std::string>(ns + ".locp_solver");
            if (stringToLqocSolver.find(locp_solverStr) != stringToLqocSolver.end())
            {
                lqocp_solver = stringToLqocSolver[locp_solverStr];
            }
            else
            {
                std::cout << "Invalid locp_solver specified in config, should be one of the following:" << std::endl;

                for (auto it = stringToLqocSolver.begin(); it != stringToLqocSolver.end(); it++)
                {
                    std::cout << it->first << std::endl;
                }

                exit(-1);
            }

        } catch (...)
        {
        }

        if (verbose)
        {
            std::cout << "Loaded NLOptCon config from " << filename << ": " << std::endl;
            print();
        }
    }

    //! load settings from config file and return as settings struct
    /*!
     * @param filename the path to the settings file
     * @param verbose print settings
     * @param ns (optional) settings namespace
     * @return the newly generated settings struct
     */
    static NLOptConSettings fromConfigFile(const std::string& filename,
        bool verbose = true,
        const std::string& ns = "alg")
    {
        NLOptConSettings settings;
        settings.load(filename, true, ns);
        return settings;
    }

private:
    //! mappings for integrator types
    std::map<ct::core::IntegrationType, std::string> integratorToString = {{ct::core::IntegrationType::EULER, "Euler"},
        {ct::core::IntegrationType::RK4, "Runge-Kutta 4th Order"},
        {ct::core::IntegrationType::MODIFIED_MIDPOINT, "Modified midpoint"},
        {ct::core::IntegrationType::ODE45, "ode45"}, {ct::core::IntegrationType::RK5VARIABLE, "RK5 variable step"},
        {ct::core::IntegrationType::RK78, "RK78"}, {ct::core::IntegrationType::BULIRSCHSTOER, "Bulirsch-Stoer"},
        {ct::core::IntegrationType::EULERCT, "Euler (CT)"},
        {ct::core::IntegrationType::RK4CT, "Runge-Kutta 4th Order (CT"},
        {ct::core::IntegrationType::EULER_SYM, "Symplectic Euler"},
        {ct::core::IntegrationType::RK_SYM, "Symplectic Runge Kutta"}};

    std::map<std::string, ct::core::IntegrationType> stringToIntegrator = {{"Euler", ct::core::IntegrationType::EULER},
        {"RK4", ct::core::IntegrationType::RK4}, {"MODIFIED_MIDPOINT", ct::core::IntegrationType::MODIFIED_MIDPOINT},
        {"ODE45", ct::core::IntegrationType::ODE45}, {"RK5VARIABLE", ct::core::IntegrationType::RK5VARIABLE},
        {"RK78", ct::core::IntegrationType::RK78}, {"BULIRSCHSTOER", ct::core::IntegrationType::BULIRSCHSTOER},
        {"EulerCT", ct::core::IntegrationType::EULERCT}, {"RK4CT", ct::core::IntegrationType::RK4CT},
        {"Euler_Sym", ct::core::IntegrationType::EULER_SYM}, {"Rk_Sym", ct::core::IntegrationType::RK_SYM}};

    //! mappings for discretization types
    std::map<APPROXIMATION, std::string> discretizationToString = {{APPROXIMATION::FORWARD_EULER, "Forward_euler"},
        {APPROXIMATION::BACKWARD_EULER, "Backward_euler"}, {APPROXIMATION::SYMPLECTIC_EULER, "Symplectic_euler"},
        {APPROXIMATION::TUSTIN, "Tustin"}, {APPROXIMATION::MATRIX_EXPONENTIAL, "Matrix_exponential"}};

    std::map<std::string, APPROXIMATION> stringToDiscretization = {{"Forward_euler", APPROXIMATION::FORWARD_EULER},
        {"Backward_euler", APPROXIMATION::BACKWARD_EULER}, {"Symplectic_euler", APPROXIMATION::SYMPLECTIC_EULER},
        {"Tustin", APPROXIMATION::TUSTIN}, {"Matrix_exponential", APPROXIMATION::MATRIX_EXPONENTIAL}};

    //! mappings for algorithm types
    std::map<NLOCP_ALGORITHM, std::string> nlocAlgorithmToString = {{GNMS, "GNMS (Gauss-Newton Multiple Shooting)"},
        {ILQR, "ILQR (iterative linear-quadratic optimal control)"}, {MS_ILQR, "MS_ILQR (multiple-shooting iLQR)"},
        {SS_OL, "SS_OL (open-loop Single Shooting)"}, {SS_CL, "SS_CL (closed-loop Single Shooting)"},
        {GNMS_M_OL, "GNMS_M_OL (GNMS(M) with open-loop shooting)"},
        {GNMS_M_CL, "GNMS_M_CL (GNMS(M) with closed-loop shooting)"}};

    std::map<std::string, NLOCP_ALGORITHM> stringToNlocAlgorithm = {{"GNMS", GNMS}, {"ILQR", ILQR},
        {"MS_ILQR", MS_ILQR}, {"SS_OL", SS_OL}, {"SS_CL", SS_CL}, {"GNMS_M_OL", GNMS_M_OL}, {"GNMS_M_CL", GNMS_M_CL}};

    std::map<NLOCP_ALGORITHM, bool> nlocAlgorithmToClosedLoopShooting = {{GNMS, false}, {ILQR, true}, {MS_ILQR, true},
        {SS_OL, false}, {SS_CL, true}, {GNMS_M_OL, false}, {GNMS_M_CL, true}};

    std::map<NLOCP_ALGORITHM, bool> nlocAlgorithmToSingleShooting = {{GNMS, false}, {ILQR, true}, {MS_ILQR, false},
        {SS_OL, true}, {SS_CL, true}, {GNMS_M_OL, false}, {GNMS_M_CL, false}};

    //! mappings for linear-quadratic solver types
    std::map<LQOCP_SOLVER, std::string> lqocSolverToString = {
        {GNRICCATI_SOLVER, "GNRICCATI_SOLVER"}, {HPIPM_SOLVER, "HPIPM_SOLVER"}};

    std::map<std::string, LQOCP_SOLVER> stringToLqocSolver = {
        {"GNRICCATI_SOLVER", GNRICCATI_SOLVER}, {"HPIPM_SOLVER", HPIPM_SOLVER}};
};
}  // namespace optcon
}  // namespace ct


#include <boost/hana/group.hpp>


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

/**
\file

\brief test_units_1.cpp

\details
Test unit class.

Output:
@verbatim
@endverbatim
**/

#include "test_header.hpp"

#include <boost/units/pow.hpp>

namespace bu = boost::units;

int test_main(int,char *[])
{
    BOOST_CONSTEXPR_OR_CONST bu::dimensionless D;

    BOOST_CONSTEXPR_OR_CONST bu::length        L;
    BOOST_CONSTEXPR_OR_CONST bu::mass          M;
    BOOST_CONSTEXPR_OR_CONST bu::time          T;

    BOOST_CHECK(+L == L);
    BOOST_CHECK(-L == L);
    BOOST_CHECK(L+L == L);
    BOOST_CHECK(L-L == L);

    BOOST_CHECK(+M == M);
    BOOST_CHECK(-M == M);
    BOOST_CHECK(M+M == M);
    BOOST_CHECK(M-M == M);

    BOOST_CONSTEXPR_OR_CONST bu::area          A;
    BOOST_CONSTEXPR_OR_CONST bu::energy        E;
    BOOST_CONSTEXPR_OR_CONST bu::velocity      V;

    BOOST_CHECK(L*L == A);
    BOOST_CHECK(A == L*L);

    BOOST_CHECK(L/L == D);
    BOOST_CHECK(D == L/L);

    BOOST_CHECK(L/T == V);
    BOOST_CHECK(V == L/T);

    BOOST_CHECK(M*L*L/T/T == E);
    BOOST_CHECK(M*(L/T)*(L/T) == E);
    BOOST_CHECK(M*bu::pow<2>(L/T) == E);
    BOOST_CHECK(bu::root<2>(E/M) == V);

    return 0;
}


//          Copyright Rein Halbersma 2014-2021.
// 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)

#define BOOST_MPL_CFG_NO_PREPROCESSED_HEADERS
#define BOOST_MPL_LIMIT_VECTOR_SIZE 50

#include <set/compatible/flat_set.hpp>  // op<=>
#include <set/compatible/set.hpp>       // op<=>
#include <set/exhaustive.hpp>           // all_doubleton_set_pairs
#include <set/primitives.hpp>           // op_less
#include <xstd/bit_set.hpp>             // bit_set
#include <boost/container/flat_set.hpp> // flat_set
#include <boost/mpl/vector.hpp>         // vector
#include <boost/test/unit_test.hpp>     // BOOST_AUTO_TEST_SUITE, BOOST_AUTO_TEST_SUITE_END, BOOST_AUTO_TEST_CASE_TEMPLATE
#include <cstdint>                      // uint8_t, uint16_t, uint32_t
#include <set>                          // set

BOOST_AUTO_TEST_SUITE(Quartic)

using namespace xstd;

using int_set_types = boost::mpl::vector
<       std::set<int>
,       boost::container::flat_set<int>
,       bit_set< 0, uint8_t>
,       bit_set< 1, uint8_t>
,       bit_set< 7, uint8_t>
,       bit_set< 8, uint8_t>
,       bit_set< 9, uint8_t>
,       bit_set<15, uint8_t>
,       bit_set<16, uint8_t>
,       bit_set<17, uint8_t>
,       bit_set<24, uint8_t>
,       bit_set< 0, uint16_t>
,       bit_set< 1, uint16_t>
,       bit_set<15, uint16_t>
,       bit_set<16, uint16_t>
,       bit_set<17, uint16_t>
,       bit_set<31, uint16_t>
,       bit_set<32, uint16_t>
,       bit_set<33, uint16_t>
,       bit_set<48, uint16_t>
,       bit_set< 0, uint32_t>
,       bit_set< 1, uint32_t>
,       bit_set<31, uint32_t>
,       bit_set<32, uint32_t>
,       bit_set<33, uint32_t>
,       bit_set<63, uint32_t>
,       bit_set<64, uint32_t>
,       bit_set<65, uint32_t>
>;

BOOST_AUTO_TEST_CASE_TEMPLATE(CompareThreeWay, T, int_set_types)
{
        all_doubleton_set_pairs<T>(op_compare_three_way());
}

BOOST_AUTO_TEST_SUITE_END()


/* ------------------------------------------------------------------------------------------
 *                                      DOCUMENTATION
 * ------------------------------------------------------------------------------------------
 *
 * Author: Robert Dudley (N0686602)
 *
 * The following 13 Boost test cases are designed to test the functionality of the
 * maxGradient() functions.
 *
 * The first few tests are designed to test normal, limited data
 * where either only one routepoint is present, or where certain values remain consistent
 * across routepoints.
 *
 * The next few tests analyse GPX log files where the elevation either
 * increases or decreases, and include a test where the elevation increases considerably.
 *
 * The final Boost tests are designed to check the format of the GPX log file being fed to
 * the maxGradient() function - these tests check whether particular tags are present, and
 * also help determine the application's respond when either an invalid file or non-existant
 * file is presented to the function.
 */

#include <boost/test/unit_test.hpp>

#include "logs.h"
#include "route.h"
#include "track.h"

using namespace GPS;

BOOST_AUTO_TEST_SUITE( Route_MaxGradient_N0686602 )

const bool isFileName = true;

/* Test number:         1
 * Test name:           Check maxGradient() where only one value for elevation is specified
 * Generated by:        GPX file generator
 * Test description:    This test checks for the value of maxGradient() where only one value
 *                      for elevation is provided.
 * Expected result:     The value for maxGradient() should be 0.
 */
BOOST_AUTO_TEST_CASE ( CheckWhereOneElevationValue )
{
   Route route = Route(LogFiles::GPXRoutesDir + "SingleValueTest-N0686602.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.maxGradient(), 0, 0 );
}

/* Test number:         2
 * Test name:           Check maxGradient() where all elevation values are equal to zero
 * Generated by:        GPX file generator
 * Test description:    This test analyses a route where the latitude and longitude change,
 *                      but elevation doesn't.
 * Expected result:     The value for maxGradient() should be 0.
 */
BOOST_AUTO_TEST_CASE ( CheckWhereAllElevationsEqualToZero )
{
   Route route = Route(LogFiles::GPXRoutesDir + "ZeroElevationTest-N0686602.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.maxGradient(), 0, 0 );
}

/* Test number:         3
 * Test name:           Check maxGradient() when the values for latitude and longitude don't
 *                      change
 * Generated by:        GPX file generator
 * Test description:    This test analyses a route where the value for elevation increases, but
 *                      the values for latitude  and longitude don't.
 * Expected result:     The value for maxGradient() should be 0.
 */
BOOST_AUTO_TEST_CASE ( CheckMaxGradientWhereNoChangeInLatOrLon )
{
   Route route = Route(LogFiles::GPXRoutesDir + "SameLatAndLongTest.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.maxGradient(), 0 );
}

/* Test number:         4
 * Test name:           Check maxGradient() where a wide variety of elevation values are specified
 * Generated by:        Pre-compiled GPX file
 * Test description:    This test analyses a route where the value for elevation differs considerably
 *                      between routepoints.
 * Expected result:     The value for maxGradient() should be 1.152, within a 1% degree of accuracy.
 */
BOOST_AUTO_TEST_CASE ( VaryingPositiveElevationNorthYorkMoors )
{
   Route route = Route(LogFiles::GPXRoutesDir + "NorthYorkMoors.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.maxGradient(), 1.152, 1 );
}

/* Test number:         5
 * Test name:           Check maxGradient() where elevation values increase as the route progresses
 * Generated by:        GPX file generator
 * Test description:    This test analyses a route where the elevation values generally increase.
 * Expected result:     The value for maxGradient() should be 1.0641, within a 1% degree of accuracy.
 */
BOOST_AUTO_TEST_CASE ( IncreasingElevationValues )
{
    Route route = Route(LogFiles::GPXRoutesDir + "PositiveElevationTest-N0686602.gpx", isFileName);
    BOOST_CHECK_CLOSE( route.maxGradient(), 1.0641, 1 );
}

/* Test number:         6
 * Test name:           Check maxGradient() where elevation values reduce as the route progresses
 * Generated by:        GPX file generator
 * Test description:    This test analyses a route where the elevation values generally decrease.
 * Expected result:     The value for maxGradient() should be 1.0683, within a 1% degree of accuracy.
 */
BOOST_AUTO_TEST_CASE ( ReducingElevationValues )
{
    Route route = Route(LogFiles::GPXRoutesDir + "NegativeElevationTest-N0686602.gpx", isFileName);
    BOOST_CHECK_CLOSE( route.maxGradient(), 1.0683, 1 );
}

/* Test number:         7
 * Test name:           Check maxGradient() where elevation values change considerably
 * Generated by:        GPX file generator
 * Test description:    This test analyses a route where the elevation values generally decrease.
 * Expected result:     The value for maxGradient() should be 0.241, within a 1% degree of accuracy.
 */
BOOST_AUTO_TEST_CASE ( LargeElevationValues )
{
    Route route = Route(LogFiles::GPXRoutesDir + "LargeElevationTest-N0686602.gpx", isFileName);
    BOOST_CHECK_CLOSE( route.maxGradient(), 0.241, 1 );
}

/* Test number:         8
 * Test name:           Check maxGradient() where no value for latitude is specified
 * Generated by:        Manually
 * Test description:    This test analyses a GPX file where no value for latitude is provided.
 * Expected result:     An std::domain_error exception should be thrown.
 */
BOOST_AUTO_TEST_CASE ( NoLatitudeSpecified )
{
    Route route = Route(LogFiles::GPXRoutesDir + "NoLatTest-N0686602.gpx", isFileName);
    BOOST_CHECK_THROW( route.maxGradient(), std::domain_error );
}

/* Test number:         9
 * Test name:           Check maxGradient() where only no value for longitude is specified
 * Generated by:        Manually
 * Test description:    This test analyses a GPX file where no value for longitude is provided.
 * Expected result:     An std::domain_error exception should be thrown.
 */
BOOST_AUTO_TEST_CASE ( NoLongitudeSpecified )
{
    Route route = Route(LogFiles::GPXRoutesDir + "NoLongTest-N0686602.gpx", isFileName);
    BOOST_CHECK_THROW( route.maxGradient(), std::domain_error );
}

/* Test number:         10
 * Test name:           Check maxGradient() where only no values for latitude or longitude are specified
 * Generated by:        Manually
 * Test description:    This test analyses a GPX file where no value for either latitude or longitude
 *                      are provided.
 * Expected result:     An std::domain_error exception should be thrown.
 */
BOOST_AUTO_TEST_CASE ( NoLatitudeOrLongitudeSpecified )
{
    Route route = Route(LogFiles::GPXRoutesDir + "NoLatOrLongTest-N0686602.gpx", isFileName);
    BOOST_CHECK_THROW( route.maxGradient(), std::domain_error );
}

/* Test number:         11
 * Test name:           Check maxGradient() where elevation values are given as a string
 * Generated by:        Manually
 * Test description:    This test analyses as GPX file  where the value for elevation is given as a string
 *                      as opposed to a numeric value.
 * Expected result:     An std::invalid_argument exception should be thrown.
 */
BOOST_AUTO_TEST_CASE ( InvalidValuesForElevation )
{
    Route route = Route(LogFiles::GPXRoutesDir + "InvalidInputTest-N0686602.gpx", isFileName);
    BOOST_CHECK_THROW ( route.maxGradient(), std::invalid_argument );
}

/* Test number:         12
 * Test name:           Check maxGradient() on a file incorrectly generated by the GPX file generator
 * Generated by:        GPX file generator
 * Test description:    This test analyses a GPX file incorrectly generated by the GPX file generator.
 * Expected result:     Since the file is blank, and no data can be found, an std::domain_error exception
 *                      should be thrown.
 */
BOOST_AUTO_TEST_CASE ( BlankFileSpecified )
{
    Route route = Route(LogFiles::GPXRoutesDir + "BlankTest-N0686602.gpx", isFileName);
    BOOST_CHECK_THROW( route.maxGradient(), std::domain_error );
}

/* Test number:         13
 * Test name:           Check maxGradient() on a non-existant GPX file
 * Generated by:        N/A
 * Test description:    This test attempts to analyse a non-existant GPX file.
 * Expected result:     An std::invalid_argument exception should be thrown.
 */
BOOST_AUTO_TEST_CASE( NonExistantFileSpecified )
{
    Route route = Route(LogFiles::GPXRoutesDir + "NotAFile.gpx", isFileName);
    BOOST_CHECK_THROW( route.maxGradient(), std::invalid_argument );
}

BOOST_AUTO_TEST_SUITE_END()