/*!
 *
 * \file interpolation.hpp
 *
 * \brief File containing n-dimensional interpolation functions
 *        for linear and cubic order.
 *
 *
 */
#pragma once
#include <array>
#include <cmath>
#include <utility>

#include <Eigen/Dense>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>

/*! \brief Namespace containing interpolation functionality for
 *         interpolating n-dimensional data using linear or cubic
 *         interpolation
 */
namespace interpolation
{
/*! \brief Struct for interpolation data
 *
 *  The Data struct is a simple wrapper of the std::array that allows
 *  you to have arrays of arrays. The idea is that for example for cubic
 *  interpolation, you need in 1D 4 data points, in 2D 4x4 data points etc.
 *  In 1D this array contains the necessary points for a given x. In 2D
 *  This array contains arrays for a given constant y whose interpolation
 *  results can then again be used for constant x, etc.
 *
 *  @tparam Func   The interpolation function for which to use the data.
 *                 This determines how many data points are needed. E.g.
 *                 cubic interpolation requires 4 points. The function must
 *                 provide the value NUMBER_OF_VALUES.
 *  @tparam Dim    The dimensionality of the data to interpolate. E.g. for
 *                 2 dimensional data, the result would be an array of arrays.
 */
template <typename Func, int Dim>
struct Data
{
    /*! \brief array of Dim-1 dimensional nested arrays.
     */
    std::array<Data<Func, Dim - 1>, Func::NUMBER_OF_VALUES> data;

    /*! \brief function to access this n-dimensional data
     *
     *  @param[in] index_0 the first index of the n-dimensional position
     *                     for which to look up the value
     *  @param[in] indices further indices depending on dimensionality,
     *                     i.e. Dim-1 indices for Dim-dimensional data.
     *
     *  @returns The value in the Dim-dimensional data structure at the
     *           given position
     *
     */
    template <typename... Indices>
    typename Func::VALUE_TYPE operator()(size_t index_0,
                                         Indices... indices) const
    {
        return data[index_0](indices...);
    }

    /*! \brief function to access this n-dimensional data
     *
     *  @param[in] index_0 the first index of the n-dimensional position
     *                     for which to look up the value
     *  @param[in] indices further indices depending on dimensionality,
     *                     i.e. Dim-1 indices for Dim-dimensional data.
     *
     *  @returns A reference to the value in the Dim-dimensional data structure
     *           at the given position
     *
     */
    template <typename... Indices>
    typename Func::VALUE_TYPE& operator()(size_t index_0, Indices... indices)
    {
        return data[index_0](indices...);
    }
};

/*! \brief 1D specialization of data
 *
 *  While all other Data<Func, Dim> structs contain arrays of Data<func,
 *  Dim-1>, the struct for 1D only contains an array of values of the
 *  value type the interpolation function works with. As such it works as
 *  a recursion stop.
 *
 *  @tparam Func   The interpolation function for which to use the data.
 *                 This determines how many data points are needed. E.g.
 *                 cubic interpolation requires 4 points.
 */
template <typename Func>
struct Data<Func, 1>
{
    /*! \brief Look-up type for nested arrays for the case of 1D.
     *
     *  In this special case of 1D the type is simply an array with the
     *  number of values and type defined by the interpolation function Func.
     *  E.g. for cubic<double> it would be std::array<double, 4>.
     */
    std::array<typename Func::VALUE_TYPE, Func::NUMBER_OF_VALUES> data;

    /*! \brief function to access this 1-dimensional data
     *
     *  @param[in] index the index of the 1-dimensional position for which to
     *                   look up the value
     *
     *  @returns The value in the 1-dimensional data structure at the
     *           given position
     *
     */
    typename Func::VALUE_TYPE operator()(size_t index) const
    {
        return data[index];
    }

    /*! \brief function to access this 1-dimensional data
     *
     *  @param[in] index the index of the 1-dimensional position for which to
     *                   look up the value
     *
     *  @returns A reference to the value in the 1-dimensional data structure at
     *           the given position
     *
     */
    typename Func::VALUE_TYPE& operator()(size_t index) { return data[index]; }
};

/*! \brief Cause a compile error for the illegal zero dimensional case
 */
template <typename Func>
struct Data<Func, 0>;

/*! \brief 1D linear interpolation
 *
 *  @tparam T   The datatype to use for the interpolation. E.g.
 *              float/double/etc.
 */
template <typename T>
struct linear
{
    /*! \brief The data type this function is operating with, e.g. for float
     *         images, it is float.
     */
    typedef T VALUE_TYPE;
    /*! \brief The number of datapoints required to perform the interpolation.
     *         Linear interpolation needs two datapoints.
     */
    static constexpr int NUMBER_OF_VALUES = 2;

    /*! \brief Compute the linear interpolation of the given points and
     * position.
     *
     *  Given 2 values, and a position in the interval [0, 1], this function
     *  returns the interpolated value using simple linear interpolation.
     *
     *  \note This function operates in double mode. The result is cast to the
     *        set template type T. NO rounding to nearest etc. is performed for
     *        e.g. integers.
     *
     *  @param[in] p   The 2 values to interpolate
     *  @param[in] x   The position in the interval [0, 1] to interpolate
     *
     *  @returns   The result of the interpolation
     */
    T operator()(const Data<linear<T>, 1>& p, double x)
    {
        return static_cast<T>(p(0) * (1 - x) + p(1) * x);
    }
};

/*! \brief 1D cubic interpolation
 *
 *  @tparam T   The datatype to use for the interpolation. E.g.
 *              float/double/etc.
 */
template <typename T>
struct cubic
{
    /*! \brief The data type this function is operating with, e.g. for float
     *         images, it is float.
     */
    typedef T VALUE_TYPE;
    /*! \brief The number of datapoints required to perform the interpolation.
     *         Cubic interpolation needs four datapoints.
     */
    static constexpr int NUMBER_OF_VALUES = 4;

    /*! \brief Compute the cubic interpolation of the given points and position.
     *
     *  Given 4 values, and a position in the interval [0, 1], this function
     *  returns the interpolated value using a uniform Catmull-Rom spline.
     *
     *  \note This function operates in double mode. The result is cast to the
     *        set template type T. NO rounding to nearest etc. is performed for
     *        e.g. integers.
     *
     *  @param[in] p   The 4 values to interpolate
     *  @param[in] x   The position in the interval [0, 1] to interpolate
     *
     *  @returns   The result of the interpolation
     */
    T operator()(const Data<cubic<T>, 1>& p, double x)
    {
        return static_cast<T>(
            p(1) + 0.5 * x *
                       (p(2) - p(0) +
                        x * (2.0 * p(0) - 5.0 * p(1) + 4.0 * p(2) - p(3) +
                             x * (3.0 * (p(1) - p(2)) + p(3) - p(0)))));
    }
};

/*! \brief Namespace containing implementation details for the interpolation
 *         functionality.
 */
namespace detail
{
/*! \brief Function to extract data from a pybind11 array using
 *         a std::array for indexing
 *
 *  Additional function required to realize the variadic indexing using
 *  the std::array.
 *
 *  @param[in] image          The image from which to extract a value
 *  @param[in] array_indices  The location of the data point to extract
 *
 *  @returns                  The value at the given position
 *
 *  @tparam T                 The data type of the image
 *  @tparam Dim               The dimensions of the image
 *  @tparam I                 Template parameter for unpacking the std::array
 */
template <typename T, int Dim, std::size_t... I>
T get_image_value_impl(
    const pybind11::detail::unchecked_reference<T, Dim>& image,
    const std::array<int, Dim>& array_indices, std::index_sequence<I...>)
{
    return image(array_indices[I]...);
}

/*! \brief Function to extract data from a pybind11 array using
 *         a std::array for indexing
 *  @param[in] image          The image from which to extract a value
 *  @param[in] array_indices  The location of the data point to extract
 *
 *  @returns                  The value at the given position
 *
 *  @tparam T                 The data type of the image
 *  @tparam Dim               The dimensions of the image
 */
template <typename T, int Dim>
T get_image_value(const pybind11::detail::unchecked_reference<T, Dim>& image,
                  const std::array<int, Dim>& array_indices)
{
    return get_image_value_impl<T, Dim>(image, array_indices,
                                std::make_index_sequence<Dim>{});
}

/*! \brief Apply Func to the given data and position.
 *
 * This function works by realising that the n-dimensional interpolation can
 * be broken down into a list of (n-1)-dimensional interpolations that are
 * interpolated via a 1D interpolation. This recursive call to
 * lower-dimensional interpolation is realized with recursive template
 * programming.
 *
 *  @param[in] indices   Indices used to unpack the array p to call the
 *                       lower dimensional interpolation on each sub-array
 *                       (for 2D and higher dimensional data).
 *  @param[in] chunk     N-dimensional array data for the interpolation.
 *                       Needs the right number of points in each dimension,
 *                       e.g. 4 for cubic interpolation. Should be nested
 *                       arrays in C order.
 *  @param[in] x         The first coordinate value of the N-dimensional
 *                       position at which to interpolate
 *  @param[in] xs        The last N-1 coordinate values of the N-dimensional
 *                       position. One argument for each dimension.
 *
 *  @returns             The interpolation value
 *
 *  @tparam Func         The interpolation order func
 *  @tparam Ts           The type of the position coordinate values. This is
 *                       enforced to be double
 *  @tparam I            Needed only to unpack array into separate
 *                       function calls.
 */
template <typename Func, typename... Ts, std::size_t... I>
static typename Func::VALUE_TYPE
apply_func_impl(std::index_sequence<I...> indices,
                const Data<Func, sizeof...(Ts) + 1>& chunk, double x, Ts... xs)
{
    if constexpr (sizeof...(Ts) > 0)
    {
        return apply_func_impl<Func>(
            indices,
            {{apply_func_impl(indices, std::get<I>(chunk.data), xs...)...}}, x);
    }
    else
    {
        return Func()(chunk, x);
    }
}

/*! \brief Extract interpolation patch from given data around given position.
 *
 *  This function uses variadic templates to iterate over n-dimensional
 *  data using the given boundary function.
 *
 *  @param[in,out] chunk           N-dimensional array data for the
 *                                 interpolation. Needs the right number of
 *                                 points in each dimension, e.g. 4 for cubic
 *                                 interpolation.
 *  @param[in] image               The image from which to extract the data.
 *  @param[in] lower_corner        Lower corner of the sub-cube to extract from
 *                                 the image into the given chunk.
 *  @param[in] background_value    The background value to use in case the value
 *                                 is outside the image domain (might be ignored
 *                                 depending on the boundary function used)
 *  @param[in] loop_indices        The loop indices of all the for loops to
 *                                 fill the n-dimensional chunk
 *
 *  @tparam Func                   The interpolation order func
 *  @tparam BoundaryFunc           The boundary type to use. E.g.
 *                                 ConstantBoundary
 *  @tparam Dim                    The dimensionality of the given image
 */
template <typename Func, typename BoundaryFunc, int Dim, typename... Ts>
static void extract_impl(
    Data<Func, Dim>& chunk,
    const pybind11::detail::unchecked_reference<typename Func::VALUE_TYPE, Dim>&
        image,
    const std::array<int, Dim>& lower_corner,
    typename Func::VALUE_TYPE background_value, Ts... loop_indices)
{
    for (int i = 0; i < Func::NUMBER_OF_VALUES; ++i)
    {
        if constexpr (Dim == sizeof...(Ts) + 1)
        {
            std::array<int, Dim> voxel_position{{loop_indices..., i}};

            for (int l = 0; l < Dim; ++l)
            {
                voxel_position[l] += lower_corner[l];
            }
            chunk(loop_indices..., i) =
                BoundaryFunc::template apply<typename Func::VALUE_TYPE, Dim>(image, voxel_position, background_value);
        }
        else
        {
            extract_impl<Func, BoundaryFunc, Dim>(chunk, image, lower_corner,
                                                  background_value,
                                                  loop_indices..., i);
        }
    }
}
}; // namespace detail

/*! \brief Struct for dealing with n-dimensional image boundaries by
 *         returning a constant values for coordinates out of bounds.
 */
struct ConstantBoundary
{
    /*! \brief Apply the constant boundary to the given image and position.
     *
     *  @param[in] image            The image to sample
     *  @param[in] voxel_position   The position at which to sample
     *  @param[in] background_value The background value to use if the given
     *                              position is out of bounds
     *
     *  @returns                    The value at the given position using the
     *                              rules defined by ConstantBoundary for
     *                              positions outside of the given image.
     *
     *  @tparam T                   The data type of the given image
     *  @tparam Dim                 The dimensions of the given image
     */
    template <typename T, int Dim>
    static T apply(const pybind11::detail::unchecked_reference<T, Dim>& image,
                   const std::array<int, Dim>& voxel_position,
                   T background_value)
    {
        for (int l = 0; l < Dim; ++l)
        {
            if (voxel_position[l] < 0 || voxel_position[l] >= image.shape(l))
            {
                return background_value;
            }
        }
        return detail::get_image_value<T, Dim>(image, voxel_position);
    }
};

/*! \brief Apply Func to the given data and position.
 *
 *  @param[in] chunk  N-dimensional array data for the interpolation. Needs
 *                    the right number of points in each dimension, e.g. 4
 *                    for cubic interpolation. Should be nested arrays in C
 *                    order.
 *  @param[in] x      The first coordinate value of the N-dimensional
 *                    position at which to interpolate
 *  @param[in] xs     The last N-1 coordinate values of the N-dimensional
 *                    position. One argument for each dimension.
 *
 *  @returns          The interpolation value
 *
 *  @tparam Func       The interpolation order func
 *  @tparam Ts        The type of the position coordinate values. This is
 *                    enforced to be double
 */
template <typename Func, typename... Ts>
static typename Func::VALUE_TYPE
apply_func(const Data<Func, sizeof...(Ts) + 1>& chunk, double x, Ts... xs)
{
    return detail::apply_func_impl(
        std::make_index_sequence<Func::NUMBER_OF_VALUES>{}, chunk, x, xs...);
}

/*! \brief Extract interpolation patch from given data around given position.
 *
 *  Different interpolation schemes require a different number of points
 *  to operate. This function takes the appropriate chunk and fills it with
 *  data based on the information of the function given.
 *
 *  @param[in,out] chunk           N-dimensional array data for the
 *                                 interpolation. Needs the right number of
 *                                 points in each dimension, e.g. 4 for cubic
 *                                 interpolation.
 *  @param[in] image               The image from which to extract the data.
 *  @param[in, out] point_floored  The point in the image where to interpolate
 *                                 floored. Will point to the lower corner of
 *                                 the data grid used for the interpolation at
 *                                 the end.
 *  @param[in] background_value    The background value to use in case the value
 *                                 is outside the image domain (might be ignored
 *                                 depending on the boundary function used)
 *  @tparam Func                    The interpolation order func
 *  @tparam BoundaryFunc           The boundary type to use. E.g.
 *                                 ConstantBoundary
 *  @tparam Dim                    The dimensionality of the given image
 */
template <typename Func, typename BoundaryFunc, int Dim>
static void
extract(Data<Func, Dim>& chunk,
        const pybind11::detail::unchecked_reference<typename Func::VALUE_TYPE,
                                                    Dim>& image,
        std::array<int, Dim>& point_floored,
        typename Func::VALUE_TYPE background_value)
{
    for (size_t l = 0; l < Dim; ++l)
    {
        point_floored[l] -= (Func::NUMBER_OF_VALUES - 2) / 2;
    }

    detail::extract_impl<Func, BoundaryFunc, Dim>(chunk, image, point_floored,
                                                  background_value);
}

}; // namespace interpolation


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 Delft University of Technology 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 THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      110117    E. Iorfida        File created.
 *      110128    E. Iorfida        Added boolean variable that sets the necessity of
 *                                  Newton-Raphson method, particularly in unit test.
 *      110202    J. Melman         Renamed certain parameters and added comments to clarify the
 *                                  code more.
 *      110203    E. Iorfida        Changed some variables names and modified punctuation.
 *      110205    J. Melman         Removed the trailing underscores in some public variables. Some
 *                                  comment rephrasing. Changed and added some notes.
 *      110212    J. Melman         Added a reference to my own thesis.
 *      110214    E. Iorfida        Deleted temporary centralBodyRadius, replaced by an element of
 *                                  GeometricShapes.
 *      120326    D. Dirkx          Changed raw pointers to shared pointers.
 *      120508    P. Musegaas       The gravitational parameter is now passed as double.
 *      120530    P. Musegaas       Complete revision. Removed class structure, made it a free
 *                                  function. Added two functions for propagating a gravity assist
 *                                  (powered and unpowered).
 *      120625    P. Musegaas       Minor changes.
 *      120703    T. Secretin       Minor layout changes.
 *      120704    P. Musegaas       Minor change. Reduced negligence of velocity effect from 1 cm/s
 *                                  to 1 micrometer/second.
 *      120713    P. Musegaas       Fixed various bugs (case with both bending + velocity effect
 *                                  was wrong, limit cases failed, rootfinder not ideal, all for
 *                                  original deltaV calculation function). Added iteration on
 *                                  pericenter radius instead of eccentricity. Improved efficiency.
 *      120813    P. Musegaas       Changed code to new root finding structure.
 *
 *    References
 *      References for deltaV computation function:
 *          Melman J. Trajectory optimization for a mission to Neptune and Triton, MSc thesis
 *              report, Delft University of Technology, 2007.
 *          Musegaas, P., Optimization of Space Trajectories Including Multiple Gravity Assists and
 *              Deep Space Maneuvers, MSc thesis report, Delft University of Technology, 2012.
 *              [unpublished so far].
 *      Reference for unpowered gravity assist propagation function:
 *          Conway, B.A., Spacecraft Trajectory Optimization, Chapter 7, Cambridge University
 *              Press, 2010.
 *      Reference for powered gravity assist propagation function:
 *          Musegaas, P., Optimization of Space Trajectories Including Multiple Gravity Assists and
 *              Deep Space Maneuvers, MSc thesis report, Delft University of Technology, 2012.
 *              [unpublished so far].
 *
 *    Notes
 *      Gravity assist and swing-by are two different words for the same thing. The delta-V that is
 *      computed for a powered swing-by has not been proven to be the optimum (lowest) to achieve
 *      the desired geometry of incoming and outgoing hyperbolic legs. Some literature research will
 *      have to be done to look at the alternatives.
 *
 *      Note that the exact implementation of Newton Raphson as root finder should be updated if
 *      someone would want to use a different root finding technique.
 *
 *      Note that by default a velocity effect deltaV of less than 1 micrometer/second is deemed
 *      negligable in this code. This value can be set though.
 *
 */

#include <cmath>

#include <boost/bind.hpp>

#include <Eigen/Dense>

#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"

#include "Tudat/Astrodynamics/MissionSegments/gravityAssist.h"
#include "Tudat/Mathematics/BasicMathematics/functionProxy.h"

namespace tudat
{
namespace mission_segments
{

using namespace root_finders;

//! Calculate deltaV of a gravity assist.
double gravityAssist( const double centralBodyGravitationalParameter,
                      const Eigen::Vector3d& centralBodyVelocity,
                      const Eigen::Vector3d& incomingVelocity,
                      const Eigen::Vector3d& outgoingVelocity,
                      const double smallestPeriapsisDistance,
                      const bool useEccentricityInsteadOfPericenter,
                      const double speedTolerance,
                      RootFinderPointer rootFinder )
{
    using basic_mathematics::UnivariateProxyPointer;
    using basic_mathematics::UnivariateProxy;

    // Compute incoming and outgoing hyperbolic excess velocity.
    const Eigen::Vector3d incomingHyperbolicExcessVelocity
            = incomingVelocity - centralBodyVelocity;
    const Eigen::Vector3d outgoingHyperbolicExcessVelocity
            = outgoingVelocity - centralBodyVelocity;

    // Compute absolute values of the hyperbolic excess velocities.
    const double absoluteIncomingExcessVelocity = incomingHyperbolicExcessVelocity.norm( );
    const double absoluteOutgoingExcessVelocity = outgoingHyperbolicExcessVelocity.norm( );

    // Compute bending angle.
    double bendingAngle = linear_algebra::computeAngleBetweenVectors(
                            incomingHyperbolicExcessVelocity, outgoingHyperbolicExcessVelocity );

    // Compute maximum achievable bending angle.
    const double maximumBendingAngle =
            std::asin( 1.0 / ( 1.0 + ( smallestPeriapsisDistance *
                                       absoluteIncomingExcessVelocity *
                                       absoluteIncomingExcessVelocity /
                                       centralBodyGravitationalParameter ) ) ) +
            std::asin( 1.0 / ( 1.0 + ( smallestPeriapsisDistance *
                                       absoluteOutgoingExcessVelocity *
                                       absoluteOutgoingExcessVelocity /
                                       centralBodyGravitationalParameter ) ) );

    // Initialize bending effect deltaV, which is zero, unless extra bending angle is required.
    double bendingEffectDeltaV = 0.0;

    // Initialize velocity effect delta V parameter.
    double velocityEffectDeltaV = 0.0;

    // Check if an additional bending angle is required. If so, the additional bending angle
    // maneuver has to be performed. Also the pericenter radius will be the minimum pericenter
    // radius to obtain the largest possible bending angle 'for free'. Hence no root finding is
    // required for this case. As noted above, this may not be ideal for all cases. (for cases
    // in which the excess velocities are relatively small)
    if ( bendingAngle > maximumBendingAngle )
    {
        // Compute required extra bending angle that cannot be delivered by an unpowered swing-by.
        const double extraBendingAngle = bendingAngle - maximumBendingAngle;

        // Compute necessary delta-V due to bending-effect.
        bendingEffectDeltaV = 2.0 * std::min( absoluteIncomingExcessVelocity,
                                              absoluteOutgoingExcessVelocity ) *
                                    std::sin( extraBendingAngle / 2.0 );

        // This means the pericenter radius is now equal to the smallest pericenter radius, to
        // ensure the largest possible bending angle.
        const double pericenterRadius = smallestPeriapsisDistance;

        // Compute semi-major axis of hyperbolic legs.
        const double incomingSemiMajorAxis = -1.0 * centralBodyGravitationalParameter /
                                             absoluteIncomingExcessVelocity /
                                             absoluteIncomingExcessVelocity;
        const double outgoingSemiMajorAxis = -1.0 * centralBodyGravitationalParameter /
                                             absoluteOutgoingExcessVelocity /
                                             absoluteOutgoingExcessVelocity;

        // Compute incoming hyperbolic leg eccentricity.
        const double incomingEccentricity = 1 - pericenterRadius / incomingSemiMajorAxis;

        // Compute outgoing hyperbolic leg eccentricity.
        const double outgoingEccentricity = 1 - pericenterRadius / outgoingSemiMajorAxis;

        // Compute incoming and outgoing velocities at periapsis.
        const double incomingVelocityAtPeriapsis = absoluteIncomingExcessVelocity *
                std::sqrt( ( incomingEccentricity + 1.0 ) / ( incomingEccentricity - 1.0 ) );
        const double outgoingVelocityAtPeriapsis = absoluteOutgoingExcessVelocity *
                std::sqrt( ( outgoingEccentricity + 1.0 ) / ( outgoingEccentricity - 1.0 ) );

        // Compute necessary delta-V due to velocity-effect.
        velocityEffectDeltaV = std::fabs( incomingVelocityAtPeriapsis -
                                          outgoingVelocityAtPeriapsis );
    }

    else if ( ( std::fabs( absoluteIncomingExcessVelocity - absoluteOutgoingExcessVelocity )
                <= speedTolerance ) )
    {
        // In this case no maneuver has to be performed. Hence no iteration is performed, and the
        // delta V is simply kept at 0.0.
    }

    // Here the required maneuver to patch the incoming and outgoing excess velocities is
    // calculated. In this implementation, the eccentricity will be used as iteration parameter.
    else if ( useEccentricityInsteadOfPericenter )
    {
        // Compute semi-major axis of hyperbolic legs.
        const double incomingSemiMajorAxis = -1.0 * centralBodyGravitationalParameter /
                                             absoluteIncomingExcessVelocity /
                                             absoluteIncomingExcessVelocity;
        const double outgoingSemiMajorAxis = -1.0 * centralBodyGravitationalParameter /
                                             absoluteOutgoingExcessVelocity /
                                             absoluteOutgoingExcessVelocity;

        // Set the gravity assist function with the variables to perform root finder calculations.
        EccentricityFindingFunctions eccentricityFindingFunctions( incomingSemiMajorAxis,
                                                                   outgoingSemiMajorAxis,
                                                                   bendingAngle );

        // Create an object containing the function of which we whish to obtain the root from.
        UnivariateProxyPointer rootFunction = boost::make_shared< UnivariateProxy >(
                    boost::bind( &EccentricityFindingFunctions::
                                 computeIncomingEccentricityFunction,
                                 eccentricityFindingFunctions, _1 ) );

        // Add the first derivative of the root function.
        rootFunction->addBinding( -1, boost::bind(
                                      &EccentricityFindingFunctions::
                                      computeFirstDerivativeIncomingEccentricityFunction,
                                      eccentricityFindingFunctions, _1 ) );

        // Initialize incoming eccentricity.
        double incomingEccentricity = TUDAT_NAN;

        // Set initial guess of the variable computed in Newton-Rapshon method.
        if ( ( absoluteOutgoingExcessVelocity / absoluteIncomingExcessVelocity ) < 100.0 )
        {
            // In these cases the very low estimate (which is given under else) may in some cases
            // result in no convergence. Hence a higher value of 1.01 is necessary. This will not
            // result in 'going through' 1.0 as mentioned below, because the eccentricity in these
            // cases is always high!
            incomingEccentricity = rootFinder->execute( rootFunction, 1.0 + 1.0e-2 );
        }

        else
        {
            // This is set to a value that is close to 1.0. This is more robust than higher values,
            // because for those higher values Newton Raphson sometimes 'goes through' 1.0. This
            // results in NaN values for the derivative of the eccentricity finding function.
            incomingEccentricity = rootFinder->execute( rootFunction, 1.0 + 1.0e-10 );
        }

        // Compute outgoing hyperbolic leg eccentricity.
        const double outgoingEccentricity = 1.0 - ( incomingSemiMajorAxis /
                                                    outgoingSemiMajorAxis ) *
                                            ( 1.0 - incomingEccentricity );

        // Compute incoming and outgoing velocities at periapsis.
        const double incomingVelocityAtPeriapsis = absoluteIncomingExcessVelocity *
                std::sqrt( ( incomingEccentricity + 1.0 ) / ( incomingEccentricity - 1.0 ) );
        const double outgoingVelocityAtPeriapsis = absoluteOutgoingExcessVelocity *
                std::sqrt( ( outgoingEccentricity + 1.0 ) / ( outgoingEccentricity - 1.0 ) );

        // Compute necessary delta-V due to velocity-effect.
        velocityEffectDeltaV = std::fabs( incomingVelocityAtPeriapsis -
                                          outgoingVelocityAtPeriapsis );
    }

    // Here the required maneuver to patch the incoming and outgoing excess velocities is
    // calculated. In this implementation, the pericenter radius will be used as iteration
    // parameter.
    else
    {
        // Compute semi-major axis of hyperbolic legs. This is the absolute semi major axis, because
        // it will otherwisely result in the root of a negative function for various cases during
        // the rootfinding process.
        const double absoluteIncomingSemiMajorAxis = 1.0 * centralBodyGravitationalParameter /
                                                     absoluteIncomingExcessVelocity /
                                                     absoluteIncomingExcessVelocity;
        const double absoluteOutgoingSemiMajorAxis = 1.0 * centralBodyGravitationalParameter /
                                                     absoluteOutgoingExcessVelocity /
                                                     absoluteOutgoingExcessVelocity;

        // Set the gravity assist function with the variables to perform root finder calculations.
        PericenterFindingFunctions pericenterFindingFunctions( absoluteIncomingSemiMajorAxis,
                                                               absoluteOutgoingSemiMajorAxis,
                                                               bendingAngle);

        // Create an object containing the function of which we whish to obtain the root from.
        UnivariateProxyPointer rootFunction = boost::make_shared< UnivariateProxy >(
                    boost::bind( &PericenterFindingFunctions::computePericenterRadiusFunction,
                                 pericenterFindingFunctions, _1 ) );

        // Add the first derivative of the root function.
        rootFunction->addBinding( -1, boost::bind( &PericenterFindingFunctions::
                                                   computeFirstDerivativePericenterRadiusFunction,
                                                   pericenterFindingFunctions, _1 ) );

        // Set pericenter radius based on result of Newton-Raphson root-finding algorithm.
        const double pericenterRadius = rootFinder->execute( rootFunction,
                                                             smallestPeriapsisDistance );

        // Compute incoming hyperbolic leg eccentricity.
        const double incomingEccentricity = 1.0 + pericenterRadius / absoluteIncomingSemiMajorAxis;

        // Compute outgoing hyperbolic leg eccentricity.
        const double outgoingEccentricity = 1.0 + pericenterRadius / absoluteOutgoingSemiMajorAxis;

        // Compute incoming and outgoing velocities at periapsis.
        const double incomingVelocityAtPeriapsis = absoluteIncomingExcessVelocity *
                std::sqrt( ( incomingEccentricity + 1.0 ) / ( incomingEccentricity - 1.0 ) );
        const double outgoingVelocityAtPeriapsis = absoluteOutgoingExcessVelocity *
                std::sqrt( ( outgoingEccentricity + 1.0 ) / ( outgoingEccentricity - 1.0 ) );

        // Compute necessary delta-V due to velocity-effect.
        velocityEffectDeltaV = std::fabs( incomingVelocityAtPeriapsis -
                                          outgoingVelocityAtPeriapsis );
    }

    // Compute and return the total delta-V.
    return bendingEffectDeltaV + velocityEffectDeltaV;
}

//! Propagate an unpowered gravity assist.
Eigen::Vector3d gravityAssist( const double centralBodyGravitationalParameter,
                               const Eigen::Vector3d& centralBodyVelocity,
                               const Eigen::Vector3d& incomingVelocity,
                               const double rotationAngle,
                               const double pericenterRadius )
{
    // Calculate the incoming velocity.
    const Eigen::Vector3d relativeIncomingVelocity = incomingVelocity - centralBodyVelocity;
    const double absoluteRelativeIncomingVelocity = relativeIncomingVelocity.norm( );

    // Calculate the eccentricity and bending angle.
    const double eccentricity = 1.0 + pericenterRadius / centralBodyGravitationalParameter *
                            absoluteRelativeIncomingVelocity * absoluteRelativeIncomingVelocity;
    const double bendingAngle = 2.0 * std::asin ( 1.0 / eccentricity );

    // Calculate the unit vectors.
    const Eigen::Vector3d unitVector1 = relativeIncomingVelocity /
                                        absoluteRelativeIncomingVelocity;
    const Eigen::Vector3d unitVector2 = unitVector1.cross( centralBodyVelocity ).normalized( );
    const Eigen::Vector3d unitVector3 = unitVector1.cross( unitVector2 );

    // Calculate the relative outgoing velocity.
    const Eigen::Vector3d relativeOutgoingVelocity = absoluteRelativeIncomingVelocity *
            ( std::cos( bendingAngle ) * unitVector1 + std::sin( bendingAngle ) *
              std::cos( rotationAngle ) * unitVector2 + std::sin( bendingAngle ) *
              std::sin( rotationAngle ) * unitVector3 );

    // Add the relative outgoing velocity to the swing-by body velocity and return it.
    return centralBodyVelocity + relativeOutgoingVelocity;
}

//! Propagate a powered gravity assist.
Eigen::Vector3d gravityAssist( const double centralBodyGravitationalParameter,
                               const Eigen::Vector3d& centralBodyVelocity,
                               const Eigen::Vector3d& incomingVelocity,
                               const double rotationAngle,
                               const double pericenterRadius,
                               const double deltaV )
{
    // Calculate the incoming velocity.
    const Eigen::Vector3d relativeIncomingVelocity = incomingVelocity - centralBodyVelocity;
    const double absoluteRelativeIncomingVelocity = relativeIncomingVelocity.norm( );

    // Calculate the incoming eccentricity and bending angle.
    const double incomingEccentricity = 1.0 + pericenterRadius /
                                        centralBodyGravitationalParameter *
                                        absoluteRelativeIncomingVelocity *
                                        absoluteRelativeIncomingVelocity;
    const double incomingBendingAngle = std::asin ( 1.0 / incomingEccentricity );

    // Calculate the pericenter velocities.
    const double incomingPericenterVelocity = std::sqrt( absoluteRelativeIncomingVelocity *
                                                         absoluteRelativeIncomingVelocity *
                                                         ( incomingEccentricity + 1.0 ) /
                                                         ( incomingEccentricity - 1.0 ) );
    const double outgoingPericenterVelocity = incomingPericenterVelocity + deltaV;

    // Calculate magnitude of the absolute relative outgoing velocity.
    const double absoluteRelativeOutgoingVelocity =
            std::sqrt( outgoingPericenterVelocity * outgoingPericenterVelocity -
                       2.0 * centralBodyGravitationalParameter / pericenterRadius );

    // Calculate the remaining bending angles.
    const double outgoingBendingAngle =
            std::asin ( 1.0 / ( 1.0 + absoluteRelativeOutgoingVelocity *
                                absoluteRelativeOutgoingVelocity * pericenterRadius /
                                centralBodyGravitationalParameter ) );
    const double bendingAngle = incomingBendingAngle + outgoingBendingAngle;

    // Calculate the unit vectors.
    const Eigen::Vector3d unitVector1 = relativeIncomingVelocity /
                                        absoluteRelativeIncomingVelocity;
    const Eigen::Vector3d unitVector2 = unitVector1.cross( centralBodyVelocity ).normalized( );
    const Eigen::Vector3d unitVector3 = unitVector1.cross( unitVector2 );

    // Calculate the relative outgoing velocity.
    const Eigen::Vector3d relativeOutgoingVelocity = absoluteRelativeOutgoingVelocity *
            ( std::cos( bendingAngle ) * unitVector1 + std::sin( bendingAngle ) *
              std::cos( rotationAngle ) * unitVector2 + std::sin( bendingAngle ) *
              std::sin( rotationAngle ) * unitVector3 );

    // Add the relative outgoing velocity to the swing-by body velocity and return it.
    return centralBodyVelocity + relativeOutgoingVelocity;
}

//! Compute pericenter radius function.
double PericenterFindingFunctions::computePericenterRadiusFunction( const double pericenterRadius )
{
    return std::asin( absoluteIncomingSemiMajorAxis_ / ( absoluteIncomingSemiMajorAxis_ +
                                                         pericenterRadius ) ) +
           std::asin( absoluteOutgoingSemiMajorAxis_ / ( absoluteOutgoingSemiMajorAxis_ +
                                                         pericenterRadius ) ) - bendingAngle_;
}

//! Compute first-derivative of the pericenter radius function.
double PericenterFindingFunctions::computeFirstDerivativePericenterRadiusFunction(
        const double pericenterRadius )
{
    return -absoluteIncomingSemiMajorAxis_ / ( absoluteIncomingSemiMajorAxis_ + pericenterRadius )
            / std::sqrt( ( pericenterRadius + 2.0 * absoluteIncomingSemiMajorAxis_ )
                         * pericenterRadius ) -
           absoluteOutgoingSemiMajorAxis_ / ( absoluteOutgoingSemiMajorAxis_ + pericenterRadius )
            / std::sqrt( ( pericenterRadius + 2.0 * absoluteOutgoingSemiMajorAxis_ ) *
                         pericenterRadius );
}

//! Compute incoming eccentricity function.
double EccentricityFindingFunctions::computeIncomingEccentricityFunction(
        const double incomingEccentricity )
{
    return std::asin( 1.0 / incomingEccentricity )
            + std::asin( 1.0 / ( 1.0 - incomingSemiMajorAxis_ / outgoingSemiMajorAxis_ *
                                 ( 1.0 - incomingEccentricity ) ) ) - bendingAngle_;
}

//! Compute first-derivative of the incoming eccentricity function.
double EccentricityFindingFunctions::computeFirstDerivativeIncomingEccentricityFunction(
        const double incomingEccentricity )
{
    const double eccentricitySquareMinusOne_ = incomingEccentricity * incomingEccentricity - 1.0;
    const double semiMajorAxisRatio_ = incomingSemiMajorAxis_ / outgoingSemiMajorAxis_ ;
    const double bParameter_ = 1.0 - semiMajorAxisRatio_ * ( 1.0 - incomingEccentricity );

    return -1.0 / ( incomingEccentricity * std::sqrt( eccentricitySquareMinusOne_ ) ) -
            semiMajorAxisRatio_ / ( bParameter_ * std::sqrt( bParameter_ * bParameter_ - 1.0 ) );
}

} // namespace mission_segments
} // namespace tudat


/*
 * Copyright (c) 2013-2015 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ALLSOL_AFFINE_HPP
#define ALLSOL_AFFINE_HPP

#include <iostream>
#include <list>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/matrix-inversion.hpp>
#include <kv/affine.hpp>
#include <kv/lp.hpp>

#ifndef USE_TRIM
#define USE_TRIM 1
#endif

#ifndef USE_SUMABS
#define USE_SUMABS 0
#endif

#ifndef USE_LP
#define USE_LP 0
#endif

#ifndef USE_FI
#define USE_FI 1
#endif

namespace kv {

namespace ub = boost::numeric::ublas;

namespace allsol_affine_sub {

// return index of I_i which has maximum width

template <class T> int search_maxwidth (const ub::vector< interval<T> >& I) {
	int s = I.size();
	int i, mi;
	T m, tmp;

	m = 0.;
	for (i=0; i<s; i++) {
		tmp = width(I(i));
		if (tmp > m) {
			m = tmp; mi = i;
		}
	}

	return mi;
}

// return max width(I_i) / width(J_i)

template <class T> T widthratio_max (const ub::vector< interval<T> >& I, const ub::vector< interval<T> >& J) {
	int s = I.size();
	int i;
	T tmp, r;

	r = 0.;

	for (i=0; i<s; i++) {
		tmp = width(I(i)) / width(J(i));
		if (tmp > r) r = tmp;
	}

	return r;
}

// return min width(I_i) / width(J_i)

template <class T> T widthratio_min (const ub::vector< interval<T> >& I, const ub::vector< interval<T> >& J) {
	int s = I.size();
	int i;
	T tmp, r;

	r = std::numeric_limits<T>::max();

	for (i=0; i<s; i++) {
		tmp = width(I(i)) / width(J(i));
		if (tmp < r) r = tmp;
	}

	return r;
}

} // namespace allsol_affine_sub

// find all solution of f in I

template <class T, class F> std::list< ub::vector< interval<T> > >
allsol_affine(F f, const ub::vector< interval<T> >& I, int verbose=1)
{
	std::list< ub::vector < interval<T> > > targets;
	targets.push_back(I);
	return allsol_list_affine(f, targets, verbose);
}

// find all solution of f in targets (list of intervals)

template <class T, class F> std::list< ub::vector< interval<T> > >
allsol_list_affine(F f, std::list< ub::vector< interval<T> > > targets, int verbose=1)
{
	int s = (targets.front()).size();
	ub::vector< interval<T> > I, fc, fi, C, CK, K, mvf, I1, I2;
	ub::matrix< interval<T> > fdi, M;
	ub::matrix<T> L, R, E;
	std::list< ub::vector< interval<T> > > solutions, solutions_big;
	typename std::list< ub::vector< interval<T> > >::iterator p, p2;
	int i, j, k, mi;
	T tmp;
	bool r, M_calculated, flag, flag2;
	int count_ne_test = 0;
	int count_ex_test = 0;
	int count_unknown = targets.size();
	int count_ne = 0;
	int count_ex = 0;
	ub::vector< affine<T> > ax, ay, ak;
	ub::vector< interval<T> > IR;
	T trim_tmp;
	interval<T> trim_I;
	affine<T> sum_abs;
	ub::vector< ub::vector<T> > ay2;
	interval<T> I_tmp;
	T lp_tmp;
	ub::vector< interval<T> > objfunc_I;
	ub::vector<T> objfunc;
	std::list< ub::vector<T> > constraints;
	ub::vector<T> c_tmp;
	int ep_size, lp_size;
	bool lp_flag;
	T lp_return;

	E = ub::identity_matrix<T>(s);
	L.resize(s,s);

	while (!targets.empty()) {
		if (verbose >= 2) {
			std::cout << "ne_test: " << count_ne_test << ", ex_test: " << count_ex_test << ", unknown: " << count_unknown << ", ne: " << count_ne << ", ex: " << count_ex << "    \r" << std::flush;
		}

		I = targets.front();
		targets.pop_front();
		count_unknown--;

		// non-existence test

		count_ne_test++;

#if USE_FI == 1
		try {
			fi = f(I);
		}
		catch (std::domain_error& e) {
			goto label;
		}

		if (!zero_in(fi)) {
			count_ne++;
			continue;
		}
#endif

		affine<T>::maxnum() = 0;
		ax = I;
		try {
			ay = f(ax);
		}
		catch (std::domain_error& e) {
			goto label;
		}

		fi = to_interval(ay);
		if (!zero_in(fi)) {
			count_ne++;
			continue;
		}

#if USE_SUMABS == 1
		sum_abs = 0.;
		for (i=0; i<s; i++) {
			sum_abs += abs(ay(i));
		}
		if (to_interval(sum_abs).lower() > 0.) {
			count_ne++;
			continue;
		}
#endif

#if USE_LP == 1
#if USE_LP_FAST == 1
		ep_size = 2 * s;
		lp_size = 1 + ep_size * 2;
		ay2.resize(s + ep_size);
		for (i=0; i<s; i++) {
			ay2(i).resize(lp_size);
			for (j=s + 1; j < lp_size; j++) {
				ay2(i)(j) = 0.;
			}
			I_tmp = ay(i).get_mid();
			for (j=1; j <= s; j++) {
				tmp = ay(i).get_coef(j);
				ay2(i)(j) = tmp;
				I_tmp -= tmp;
			}
			lp_tmp = ay(i).get_err();
			rop<T>::begin();
			for (j=s+1; j <= affine<T>::maxnum(); j++) {
				tmp = ay(i).get_coef(j);
				if (tmp < 0.) tmp = -tmp;
				lp_tmp = rop<T>::add_up(lp_tmp, tmp);
			}
			rop<T>::end();
			I_tmp -= lp_tmp;
			rop<T>::begin();
			lp_tmp = rop<T>::add_up(lp_tmp, rad(I_tmp));
			rop<T>::end();
			ay2(i)(s + 1 + i) = lp_tmp;
			ay2(i)(0) = I_tmp.lower();
			if (ay2(i)(0) > 0.) {
				for (j=0; j<lp_size; j++) {
					ay2(i)(j) = -ay2(i)(j);
				}
			}
		}
#else
		ep_size = affine<T>::maxnum() + s;
		lp_size = 1 + ep_size * 2;
		ay2.resize(s + ep_size);
		for (i=0; i<s; i++) {
			ay2(i).resize(lp_size);
			for (j=affine<T>::maxnum() + 1; j < lp_size; j++) {
				ay2(i)(j) = 0.;
			}
			I_tmp = ay(i).get_mid();
			for (j=1; j <= affine<T>::maxnum(); j++) {
				tmp = ay(i).get_coef(j);
				ay2(i)(j) = tmp;
				I_tmp -= tmp;
			}
			lp_tmp = ay(i).get_err();
			I_tmp -= lp_tmp;
			rop<T>::begin();
			lp_tmp = rop<T>::add_up(lp_tmp, rad(I_tmp));
			rop<T>::end();
			ay2(i)(affine<T>::maxnum() + 1 + i) = lp_tmp;
			ay2(i)(0) = I_tmp.lower();
			if (ay2(i)(0) > 0.) {
				for (j=0; j<lp_size; j++) {
					ay2(i)(j) = -ay2(i)(j);
				}
			}
		}
#endif
		for (i=0; i<ep_size; i++) {
			ay2(s + i).resize(lp_size);
			ay2(s + i)(0) = -2.;
			for (j=1; j<lp_size; j++) {
				ay2(s + i)(j) = 0.;
			}
			ay2(s + i)(1 + i) = 1.;
			ay2(s + i)(ep_size + 1 + i) = 1.;
		}
		objfunc_I.resize(lp_size);
		for (j=0; j<lp_size; j++) objfunc_I(j) = 0.;
		for (i=0; i<s + ep_size; i++) {
			for (j=0; j<lp_size; j++) {
				objfunc_I(j) -= ay2(i)(j);
			}
		}
		objfunc.resize(lp_size);
		for (j=0; j<lp_size; j++) {
			objfunc(j) = objfunc_I(j).lower();
		}

		lp_flag = true;
		constraints.clear();
		for (i=0; i<s + ep_size; i++) {
			c_tmp.resize(lp_size);
			for (j=0; j<lp_size; j++) {
				c_tmp(j) = ay2(i)(j);
			}
			/*
			if (c_tmp(0) >= 0.) {
				lp_flag = false;
			}
			*/
			constraints.push_back(c_tmp);
		}

		if (lp_flag) {
			lp_return = lp_minimize_verified(objfunc, constraints, -1);
			if (lp_return > 0.) {
				count_ne++;
				continue;
			}
		}

#endif

#if USE_TRIM == 1
		IR = I;
		flag = false;
		flag2 = false;
		for (i=0; i<s; i++) {
			trim_tmp = rad(ay(i));
			for (j=0; j<s; j++) {
				tmp = ay(i).get_coef(j+1);
				if (tmp == 0.) continue;
				if (tmp < 0.) tmp = -tmp;
				rop<T>::begin();
				tmp = rop<T>::sub_up(trim_tmp, tmp);
				rop<T>::end();
				trim_I = ax(j).get_mid() - (ay(i).get_mid() + tmp * interval<T>(-1., 1.)) * ax(j).get_coef(j+1) / ay(i).get_coef(j+1);
				if (overlap(IR(j), trim_I)) {
					if (!subset(IR(j), trim_I)) {
						IR(j) = intersect(IR(j), trim_I);
						flag2 = true;
					}
				} else {
					flag = true;
					break;
				}
			}
			if (flag == true) break;
		}

		if (flag == true) {
			count_ne++;
			continue;
		}
#endif

		// existence test

		for (i=0; i<s; i++) {
			for (j=0; j<s; j++) {
				L(i, j) = ay(i).get_coef(j+1) / ax(j).get_coef(j+1);
			}
		}

		count_ex_test++;

		r = invert(L, R);
		if (!r) goto label;

		ak = ax - prod(R, ay);
		K = to_interval(ak);

		if (!overlap(K, I)) {
			count_ne++;
			continue;
		}

		if (proper_subset(K, I)) {
			// check whether the solution is already found or not
			flag = true;
			p = solutions.begin();
			p2 = solutions_big.begin();
			while (p != solutions.end()) {
				if (overlap(K, *p)) {
					if (subset(K, *p2)||subset(*p, I)) {
						flag = false;
						break;
					}
					while (true) {
						affine<T>::maxnum() = 0;
						ax = K;
						ay = f(ax);
						#if 0
						for (i=0; i<s; i++) {
							for (j=0; j<s; j++) {
								L(i, j) = ay(i).get_coef(j+1) / ax(j).get_coef(j+1);
							}
						}
						invert(L, R);
						#endif
						ak = ax - prod(R, ay);
						I1 = to_interval(ak);
						K = intersect(K, I1);
						if (subset(K, *p2)) {
							flag2 = true;
							break;
						}
						if (!overlap(K, *p)) {
							flag2 = false;
							break;
						}
					}
					if (flag2 == true) {
						flag = false;
						break;
					} else {
						/* never reach? */
						std::cout << "two overlap intervals includes different solutions";
					}
				}
				p++;
				p2++;
			}
			if (flag) { // new solution found
				if (verbose >= 1) std::cout << I << "(ex)\n";
				solutions_big.push_back(I);
				// iterative refinement
				while (1) {
					affine<T>::maxnum() = 0;
					ax = K;
					ay = f(ax);
					#if 0
					for (i=0; i<s; i++) {
						for (j=0; j<s; j++) {
							L(i, j) = ay(i).get_coef(j+1) / ax(j).get_coef(j+1);
						}
					}
					r = invert(L, R);
					#endif
					ak = ax - prod(R, ay);
					I1 = to_interval(ak);
					I1 = intersect(K, I1);
					tmp = allsol_affine_sub::widthratio_min(I1, K);
					K = I1;
					if (tmp > 0.9) break;
				}
				solutions.push_back(K);
				count_ex++;
				if (verbose >= 1) std::cout << K << "(ex:improved)\n";
			}
			continue;
		}

		// check the case that solution may exist near boundary.
		// If so, use K as next interval
		if (allsol_affine_sub::widthratio_max(K, I) < 0.9) {
			targets.push_back(K);
			count_unknown++;
			continue;
		}

#if USE_TRIM == 1
		if (!overlap(IR, K)) {
			count_ne++;
			continue;
		} else {
			I = intersect(IR, K);
		}
#else
		I = intersect(I, K);
#endif

		label:

		// divide interval

		mi = allsol_affine_sub::search_maxwidth(I);

		tmp = mid(I(mi));
		if (tmp == I(mi).lower() || tmp == I(mi).upper()) {
			std::cout << "too small interval (may be multiple root?):\n" << I << "\n";
			continue;
		}

		I1 = I; I2 = I;
		I1(mi).assign(I1(mi).lower(), tmp);
		I2(mi).assign(tmp, I2(mi).upper());
		targets.push_back(I1);
		targets.push_back(I2);
		count_unknown += 2;
	}

	if (verbose >= 1) {
			std::cout << "ne_test: " << count_ne_test << ", ex_test: " << count_ex_test << ", ne: " << count_ne << ", ex: " << count_ex << "    \n";
	}

	return solutions;
}

} // namespace kv

#endif // ALLSOL_AFFINE_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

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

/*! \file mcgaussian1dnonstandardswaptionengine.hpp
    \brief Monte Carlo engine for bermudan (non standard) swaptions
           in a gaussian 1d model
*/

#ifndef quantlib_mc_gaussian1d_nonstandardswaption_engine_hpp
#define quantlib_mc_gaussian1d_nonstandardswaption_engine_hpp

#include <ql/pricingengines/genericmodelengine.hpp>
#include <ql/pricingengines/mclongstaffschwartzengine.hpp>
#include <ql/experimental/models/longstaffschwartzproxypathpricer.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/instruments/nonstandardswaption.hpp>

#include <boost/make_shared.hpp>

namespace QuantLib {

/*! All fixed coupons with start date greater or equal to the
    respective option expiry are considered to be part of the
    exercise into right.

    \warning Cash settled swaptions are not supported
*/

template <class RNG = PseudoRandom, class S = Statistics>
class McGaussian1dNonstandardSwaptionEngine
    : public MCLongstaffSchwartzEngine<
          GenericModelEngine<Gaussian1dModel, NonstandardSwaption::arguments,
                             NonstandardSwaption::results>,
          SingleVariate, RNG, S> {

  public:
    typedef MCLongstaffSchwartzEngine<
        GenericModelEngine<Gaussian1dModel, NonstandardSwaption::arguments,
                           NonstandardSwaption::results>,
        SingleVariate, RNG, S> base_class;

    /*! proxy function */
    struct LsFunction : NonstandardSwaption::Proxy::ProxyFunction {
        Real operator()(const Real state) const {
            Real tmp = 0.0;
            if (state > cutoff) {
                for (Size i = 0; i < coeffItm.size(); ++i)
                    tmp += coeffItm[i] * v[i](state);
            } else {
                for (Size i = 0; i < coeffOtm.size(); ++i)
                    tmp += coeffOtm[i] * v[i](state);
            }
            return std::max(tmp, 0.0); // continuation value is always positive
        }
        Array coeffItm, coeffOtm;
        Real cutoff;
        std::vector<boost::function1<Real, Real> > v;
    };

    McGaussian1dNonstandardSwaptionEngine(
        const boost::shared_ptr<Gaussian1dModel> &model,
        const Handle<Quote>
            &oas, // continuously compounded w.r.t. yts daycounter
        const Handle<YieldTermStructure> &discountCurve,
        Size timeSteps, Size timeStepsPerYear, bool brownianBridge,
        bool antitheticVariate, Size requiredSamples, Real requiredTolerance,
        Size maxSamples, BigNatural seed, Size nCalibrationSamples,
        bool generateProxy);

    void calculate() const;
    void reset() const;

  protected:
    boost::shared_ptr<LongstaffSchwartzPathPricer<Path> > lsmPathPricer() const;

  private:
    boost::shared_ptr<Gaussian1dModel> model_;
    Handle<Quote> oas_;
    Handle<YieldTermStructure> discountCurve_;
    bool generateProxy_;
    mutable boost::shared_ptr<NonstandardSwaption::Proxy> proxy_;
};

//! factory

template <class RNG = PseudoRandom, class S = Statistics>
class MakeMcGaussian1dNonstandardSwaptionEngine {
  public:
    MakeMcGaussian1dNonstandardSwaptionEngine(
        const boost::shared_ptr<Gaussian1dModel> &model);
    MakeMcGaussian1dNonstandardSwaptionEngine &withSteps(Size steps);
    MakeMcGaussian1dNonstandardSwaptionEngine &withStepsPerYear(Size steps);
    MakeMcGaussian1dNonstandardSwaptionEngine &
    withBrownianBridge(bool b = true);
    MakeMcGaussian1dNonstandardSwaptionEngine &
    withAntitheticVariate(bool b = true);
    MakeMcGaussian1dNonstandardSwaptionEngine &withSamples(Size samples);
    MakeMcGaussian1dNonstandardSwaptionEngine &
    withAbsoluteTolerance(Real tolerance);
    MakeMcGaussian1dNonstandardSwaptionEngine &withMaxSamples(Size samples);
    MakeMcGaussian1dNonstandardSwaptionEngine &withSeed(BigNatural seed);
    MakeMcGaussian1dNonstandardSwaptionEngine &
    withCalibrationSamples(Size calibrationSamples);
    MakeMcGaussian1dNonstandardSwaptionEngine &
    withOas(const Handle<Quote> &oas);
    MakeMcGaussian1dNonstandardSwaptionEngine &
    withDiscount(const Handle<YieldTermStructure> &discount);
    MakeMcGaussian1dNonstandardSwaptionEngine &withProxy(bool b = true);
    // conversion to pricing engine
    operator boost::shared_ptr<PricingEngine>() const;

  private:
    boost::shared_ptr<Gaussian1dModel> model_;
    Handle<Quote> oas_;
    Handle<YieldTermStructure> discount_;
    bool brownianBridge_, antithetic_;
    Size steps_, stepsPerYear_, samples_, calibrationSamples_, maxSamples_;
    Real tolerance_;
    BigNatural seed_;
    bool generateProxy_;
};

//! Path Pricer

class Gaussian1dNonstandardSwaptionPathPricer
    : public EarlyExercisePathPricer<Path> {
  public:
    Gaussian1dNonstandardSwaptionPathPricer(
        const boost::shared_ptr<Gaussian1dModel> &model,
        const NonstandardSwaption::arguments *arguments,
        const Handle<YieldTermStructure> &discount, const Handle<Quote> &oas);
    Real operator()(const Path &path, Size t) const;
    Real state(const Path &path, Size t) const;
    std::vector<boost::function1<Real, Real> > basisSystem() const;
    std::vector<boost::function1<Real, Real> > basisSystem2() const;

  private:
    void initExerciseIndices(const Path &path) const;
    const boost::shared_ptr<Gaussian1dModel> model_;
    const NonstandardSwaption::arguments *arguments_;
    const Handle<YieldTermStructure> discount_;
    const Handle<Quote> oas_;
    std::vector<boost::function1<Real, Real> > basis_, basis2_;
    mutable std::vector<Size> exerciseIdx_;
    Size minIdxAlive_;
};

// implementation

template <class RNG, class S>
McGaussian1dNonstandardSwaptionEngine<RNG, S>::
    McGaussian1dNonstandardSwaptionEngine(
        const boost::shared_ptr<Gaussian1dModel> &model,
        const Handle<Quote>
            &oas, // continuously compounded w.r.t. yts daycounter
        const Handle<YieldTermStructure> &discountCurve,
        Size timeSteps, Size timeStepsPerYear, bool brownianBridge,
        bool antitheticVariate, Size requiredSamples, Real requiredTolerance,
        Size maxSamples, BigNatural seed, Size nCalibrationSamples,
        bool generateProxy)
    : MCLongstaffSchwartzEngine<
          GenericModelEngine<Gaussian1dModel, NonstandardSwaption::arguments,
                             NonstandardSwaption::results>,
          SingleVariate, RNG, S>(
          model->stateProcess(), timeSteps, timeStepsPerYear, brownianBridge,
          antitheticVariate, false, requiredSamples, requiredTolerance,
          maxSamples, seed, nCalibrationSamples),
      model_(model), oas_(oas), discountCurve_(discountCurve),
      generateProxy_(generateProxy) {

    if (!oas_.empty())
        this->registerWith(oas_);
    if (!discountCurve_.empty())
        this->registerWith(discountCurve_);
}

template <class RNG, class S>
void McGaussian1dNonstandardSwaptionEngine<RNG, S>::calculate() const {
// a lazy object is not thread safe, neither is the caching
// in gsrprocess. therefore we trigger computations here such
// that neither lazy object recalculation nor write access
// during caching occurs in the parallized loop in MonteCarloModel
#ifdef _OPENMP
    boost::shared_ptr<LongstaffSchwartzPathPricer<Path> > tmp =
        this->lsmPathPricer();
    tmp->calibrate();
    model_->numeraire(1.0);
    tmp->operator()(this->pathGenerator()->next().value);
#endif
    // continue with usual calculations
    base_class::calculate();
    // transform the deflated values into plain ones
    this->results_.value *= model_->numeraire(0.0, 0.0, discountCurve_);
    this->results_.errorEstimate *= model_->numeraire(0.0, 0.0, discountCurve_);
    if (generateProxy_) {
        // generate proxy
        this->proxy_ = boost::make_shared<NonstandardSwaption::Proxy>();
        Date today = Settings::instance().evaluationDate();
        // original evaluation date
        // open expiry dates
        std::vector<Date>::const_iterator start =
            std::upper_bound(this->arguments_.exercise->dates().begin(),
                             this->arguments_.exercise->dates().end(), today);
        for (std::vector<Date>::const_iterator i = start;
             i != this->arguments_.exercise->dates().end(); ++i) {
            this->proxy_->expiryDates.push_back(*i);
        }
        // model
        this->proxy_->model = model_;
        // discount curve (may be empty)
        this->proxy_->discount = discountCurve_;
        // oas (may be empty)
        this->proxy_->oas = oas_;
        // regression functions for values
        for (int i = 0; i < static_cast<int>(this->proxy_->expiryDates.size());
             ++i) {
            boost::shared_ptr<LsFunction> lsTmp =
                boost::make_shared<LsFunction>();
            boost::shared_ptr<LongstaffSchwartzProxyPathPricer> pathPricer =
                boost::dynamic_pointer_cast<LongstaffSchwartzProxyPathPricer>(
                    this->pathPricer_);
            lsTmp->coeffItm = pathPricer->coefficientsItm()[i];
            lsTmp->coeffOtm = pathPricer->coefficientsOtm()[i];
            lsTmp->cutoff = pathPricer->cutoff();
            lsTmp->v = pathPricer->basisSystem();
            this->proxy_->regression.push_back(lsTmp);
        }
        this->results_.proxy = this->proxy_;
    }
}

template <class RNG, class S>
void McGaussian1dNonstandardSwaptionEngine<RNG, S>::reset() const {
    base_class::reset();
    this->proxy_ = boost::shared_ptr<NonstandardSwaption::Proxy>();
}

template <class RNG, class S>
boost::shared_ptr<LongstaffSchwartzPathPricer<Path> >
McGaussian1dNonstandardSwaptionEngine<RNG, S>::lsmPathPricer() const {

    // reduced grid which only contains the exercise times and 0
    std::vector<Real> exerciseTimes;
    exerciseTimes.push_back(0.0);
    for (Size i = 0; i < this->arguments_.exercise->dates().size(); ++i) {
        Time tmp =
            model_->stateProcess()->time(this->arguments_.exercise->date(i));
        if (tmp > 0.0) {
            exerciseTimes.push_back(tmp);
        }
    }
    TimeGrid exerciseGrid =
        TimeGrid(exerciseTimes.begin(), exerciseTimes.end());
    boost::shared_ptr<Gaussian1dNonstandardSwaptionPathPricer>
        earlyExercisePricer =
            boost::make_shared<Gaussian1dNonstandardSwaptionPathPricer>(
                model_, &this->arguments_, discountCurve_, oas_);
    // we work with deflated npvs produced in the early exercise pricer
    // so we pass a dummyCurve, which produces discount factors of 1.0 always
    boost::shared_ptr<YieldTermStructure> dummyCurve =
        boost::make_shared<FlatForward>(0, NullCalendar(), 0.0,
                                        Actual365Fixed());
    if (generateProxy_) {
        return boost::make_shared<LongstaffSchwartzProxyPathPricer>(
            exerciseGrid, earlyExercisePricer, dummyCurve);
    } else {
        return boost::make_shared<LongstaffSchwartzPathPricer<Path> >(
            exerciseGrid, earlyExercisePricer, dummyCurve);
    }
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::
    MakeMcGaussian1dNonstandardSwaptionEngine(
        const boost::shared_ptr<Gaussian1dModel> &model)
    : model_(model), oas_(Handle<Quote>()),
      discount_(Handle<YieldTermStructure>()), brownianBridge_(false),
      antithetic_(false), steps_(Null<Size>()), stepsPerYear_(Null<Size>()),
      samples_(Null<Size>()), calibrationSamples_(Null<Size>()),
      maxSamples_(Null<Size>()), tolerance_(Null<Real>()), seed_(0),
      generateProxy_(false) {}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withSteps(Size steps) {
    steps_ = steps;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withStepsPerYear(
    Size steps) {
    stepsPerYear_ = steps;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withBrownianBridge(
    bool brownianBridge) {
    brownianBridge_ = brownianBridge;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withAntitheticVariate(
    bool b) {
    antithetic_ = b;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withSamples(Size samples) {
    QL_REQUIRE(tolerance_ == Null<Real>(), "tolerance already set");
    samples_ = samples;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withCalibrationSamples(
    Size samples) {
    calibrationSamples_ = samples;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withAbsoluteTolerance(
    Real tolerance) {
    QL_REQUIRE(samples_ == Null<Size>(), "number of samples already set");
    QL_REQUIRE(RNG::allowsErrorEstimate, "chosen random generator policy "
                                         "does not allow an error estimate");
    tolerance_ = tolerance;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withMaxSamples(
    Size samples) {
    maxSamples_ = samples;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withSeed(BigNatural seed) {
    seed_ = seed;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withOas(
    const Handle<Quote> &oas) {
    oas_ = oas;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withDiscount(
    const Handle<YieldTermStructure> &discount) {
    discount_ = discount;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withProxy(bool b) {
    generateProxy_ = b;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::
operator boost::shared_ptr<PricingEngine>() const {
    QL_REQUIRE(steps_ != Null<Size>() || stepsPerYear_ != Null<Size>(),
               "number of steps not given");
    QL_REQUIRE(steps_ == Null<Size>() || stepsPerYear_ == Null<Size>(),
               "number of steps overspecified");
    return boost::shared_ptr<PricingEngine>(
        new McGaussian1dNonstandardSwaptionEngine<RNG, S>(
            model_, oas_, discount_, steps_, stepsPerYear_, brownianBridge_,
            antithetic_, samples_, tolerance_, maxSamples_, seed_,
            calibrationSamples_, generateProxy_));
}

} // namespace QuantLib

#endif


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <ql/exercise.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/time/calendars/nullcalendar.hpp>
#include <qle/models/fxeqoptionhelper.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {

FxEqOptionHelper::FxEqOptionHelper(const Period& maturity, const Calendar& calendar, const Real strike,
                                   const Handle<Quote> spot, const Handle<Quote> volatility,
                                   const Handle<YieldTermStructure>& domesticYield,
                                   const Handle<YieldTermStructure>& foreignYield,
                                   BlackCalibrationHelper::CalibrationErrorType errorType)
    : BlackCalibrationHelper(volatility, errorType), termStructure_(domesticYield), hasMaturity_(true),
      maturity_(maturity), calendar_(calendar), strike_(strike), spot_(spot), foreignYield_(foreignYield) {
    registerWith(spot_);
    registerWith(foreignYield_);
}

FxEqOptionHelper::FxEqOptionHelper(const Date& exerciseDate, const Real strike, const Handle<Quote> spot,
                                   const Handle<Quote> volatility, const Handle<YieldTermStructure>& domesticYield,
                                   const Handle<YieldTermStructure>& foreignYield,
                                   BlackCalibrationHelper::CalibrationErrorType errorType)
    : BlackCalibrationHelper(volatility, errorType), termStructure_(domesticYield), hasMaturity_(false),
      exerciseDate_(exerciseDate), strike_(strike), spot_(spot), foreignYield_(foreignYield) {
    registerWith(spot_);
    registerWith(foreignYield_);
}

void FxEqOptionHelper::performCalculations() const {
    if (hasMaturity_)
        exerciseDate_ = calendar_.advance(termStructure_->referenceDate(), maturity_);
    tau_ = termStructure_->timeFromReference(exerciseDate_);
    atm_ = spot_->value() * foreignYield_->discount(tau_) / termStructure_->discount(tau_);
    effStrike_ = strike_;
    if (effStrike_ == Null<Real>())
        effStrike_ = atm_;
    type_ = effStrike_ >= atm_ ? Option::Call : Option::Put;
    boost::shared_ptr<StrikedTypePayoff> payoff(new PlainVanillaPayoff(type_, effStrike_));
    boost::shared_ptr<Exercise> exercise = boost::make_shared<EuropeanExercise>(exerciseDate_);
    option_ = boost::shared_ptr<VanillaOption>(new VanillaOption(payoff, exercise));
    BlackCalibrationHelper::performCalculations();
}

Real FxEqOptionHelper::modelValue() const {
    calculate();
    option_->setPricingEngine(engine_);
    return option_->NPV();
}

Real FxEqOptionHelper::blackPrice(Real volatility) const {
    calculate();
    const Real stdDev = volatility * std::sqrt(tau_);
    return blackFormula(type_, effStrike_, atm_, stdDev, termStructure_->discount(tau_));
}

} // namespace QuantExt


#include <mvp/Core/ConvexPolygon.h>

#include <boost/foreach.hpp>

namespace mvp {
namespace core {

ConvexPolygon::ConvexPolygon(VertexList pts) {
  // Algorithm is the 'giftwrap' algorithm http://www.cse.unsw.edu.au/~lambert/java/3d/giftwrap.html
  // Probably not the most efficient implementation...
  VW_ASSERT(pts.size() >= 3, vw::ArgumentErr() << "Need at least 3 points to construct a polygon!");

  vw::Vector2 end = pts[0];
  BOOST_FOREACH(vw::Vector2 const& v, pts) {
    if (end.y() < v.y()) {
      end = v;
    }
  }

  bool done = false;
  vw::Vector2 curr = end;

  while (!done) {
    bool found_next = false;
    BOOST_FOREACH(vw::Vector2 const& next, pts) {
      if (curr != next) {
        found_next = true;
        BOOST_FOREACH(vw::Vector2 const& pt, pts) {
          if (circulation_direction(curr, next, pt) > 0 && pt != next) {
            found_next = false;
            break;
          }
        }
        if (found_next) {
          curr = next;
          break;
        }
      }
    }

    VW_ASSERT(found_next, vw::LogicErr() << "Unable to construct convex hull");

    m_vertices.push_back(curr);
    if (curr == end) {
      done = true;
    }
  }
}

ConvexPolygon::ConvexPolygon(vw::BBox2 const& bbox) : m_vertices(4) {
  m_vertices[0] = bbox.max();
  m_vertices[1] = vw::Vector2(bbox.max().x(), bbox.min().y());
  m_vertices[2] = bbox.min();
  m_vertices[3] = vw::Vector2(bbox.min().x(), bbox.max().y());
}

vw::BBox2 ConvexPolygon::bounding_box() const {
  vw::BBox2 bbox;

  BOOST_FOREACH(vw::Vector2 const& v, m_vertices) {
    bbox.grow(v);
  }

  return bbox;
}

bool ConvexPolygon::contains(vw::Vector2 const& pt) const {
  // Use solution 3 from http://paulbourke.net/geometry/insidepoly/
  for (VertexList::const_iterator curr = m_vertices.begin(); curr != m_vertices.end(); curr++) {
    VertexList::const_iterator next = curr;
    if (++next == m_vertices.end()) {
      next = m_vertices.begin();
    }

    double circulation = circulation_direction(*curr, *next, pt);

    // Round to zero if close... (assume colinear)
    circulation = circulation * circulation < 1e-6 ? 0 : circulation;

    if (circulation > 0) {
      return false;
    }
  }

  return true;
}

bool ConvexPolygon::intersects(ConvexPolygon const& other) const {
  // Algorithm from http://www.gpwiki.org/index.php/Polygon_Collision
  // Straight up dumb check, no optimizations attempted...

  for (VertexList::const_iterator cursor = m_vertices.begin(); cursor != m_vertices.end(); cursor++) {
    VertexList::const_iterator next = cursor;
    if (++next == m_vertices.end()) {
      next = m_vertices.begin();
    }

    vw::Vector2 dir = *next - *cursor;
    vw::Vector2 perp_dir(-dir[1], dir[0]);

    double poly1_min = std::numeric_limits<double>::max();
    double poly1_max = std::numeric_limits<double>::min();
    BOOST_FOREACH(vw::Vector2 const& v, m_vertices) {
      double res = dot_prod(perp_dir, v);
      poly1_min = std::min(res, poly1_min);
      poly1_max = std::max(res, poly1_max);
    }

    double poly2_min = std::numeric_limits<double>::max();
    double poly2_max = std::numeric_limits<double>::min();
    BOOST_FOREACH(vw::Vector2 const& v, other.m_vertices) {
      double res = dot_prod(perp_dir, v);
      poly2_min = std::min(res, poly2_min);
      poly2_max = std::max(res, poly2_max);
    }

    if (poly1_min > poly2_max || poly1_max < poly2_min) {
      return false;
    }
  }

  return true;
}

}} // namespace core,mvp


#ifndef MANUALPHASE_HPP
#define MANUALPHASE_HPP

#include <set>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>

namespace raisim
{
    constexpr int n_gait = 9;

    template<typename T>
    class ManualPhase
    {
    public:
        ManualPhase()
        {
            for (int i = 0; i < n_gait; ++i)
            {
                TIME[i] = UNIT[i] * unit_time_;
                STANCE_TIME[i] = DUTY_UNIT[i] * unit_time_ / (UNIT[i] / 2.0);
            }

            reset();
        }

        Eigen::Matrix<T, Eigen::Dynamic, 1> get_raw_phase()
        {
            phase_raw_.setZero();
            for (int i = 0; i < 4; ++i)
            {
                phase_raw_[i] = std::atan2(q_[2 * i + 1], q_[2 * i]);
            }
            return phase_raw_;
        }

        Eigen::Matrix<T, 4, 1> get_omega()
        {
            return omega_;
        }

        int get_hold_leg()
        {
            // not implemented in manual phase
            return -1;
        }

        bool get_transition_status()
        {
            // not implemented in manual phase
            return false;
        }

        inline void three_leg_mode(int leg)
        {
            // not implemented in manual phase
            ;
        }

        inline void change_gait(int gait_idx)
        {
            target_gait_idx_ = gait_idx;
        }

        inline void change_gait_()
        {
            if (target_gait_idx_ != gait_idx_ && std::fabs(phase_scalar_) <= dt_ / 2.0)
            {
                gait_idx_ = target_gait_idx_;
            }
        }

        inline int get_gait_index()
        {
            return gait_idx_;
        }

        inline T get_stance_time()
        {
            return STANCE_TIME[gait_idx_];
        }

        inline void reset()
        {
            unit_ = UNIT[0];
            time_ = TIME[0];
            duty_unity_ = DUTY_UNIT[0];
            stance_time_ = STANCE_TIME[0];

            q_ << 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0;
            q_dot_.setZero();

            phase_raw_ << - M_PI / 2.0, - M_PI / 2.0, - M_PI / 2.0, - M_PI / 2.0;
            phase_ = {0.0, 0.0, 0.0, 0.0};
            omega_ << - M_PI / unit_time_, - M_PI / unit_time_, - M_PI / unit_time_, - M_PI / unit_time_;

            phase_scalar_ = 0.0;
        }

        inline void step()
        {
            if (phase_scalar_ > TIME[gait_idx_])
            {
                phase_scalar_ = 0.0;
            }
            change_gait_();

            get_q_and_q_dot();

            phase_scalar_ += dt_;
        }

        inline void get_q_and_q_dot()
        {
            if (gait_idx_ == 0)
            {
                T theta = - phase_scalar_ / unit_time_ * M_PI + M_PI;
                for (int i = 0; i < 4; ++i)
                {
                    q_[2 * i] = std::cos(theta);
                    q_[2 * i + 1] = std::sin(theta);
                }
                omega_.setConstant(- M_PI / unit_time_);
            }

            if (gait_idx_ == 1)
            {
                T theta_fast = - phase_scalar_ / unit_time_ * M_PI + M_PI;
                T theta_slow = - phase_scalar_ / unit_time_ * M_PI / 3;

                if (phase_scalar_ / unit_time_ < 3)
                {
                    // 0, 3
                    q_[0] = std::cos(theta_slow);
                    q_[1] = std::sin(theta_slow);
                    q_[6] = std::cos(theta_slow);
                    q_[7] = std::sin(theta_slow);

                    omega_[0] = - M_PI / unit_time_ / 3;
                    omega_[3] = - M_PI / unit_time_ / 3;

                    // 1, 2
                    q_[2] = std::cos(theta_fast);
                    q_[3] = std::sin(theta_fast);
                    q_[4] = std::cos(theta_fast);
                    q_[5] = std::sin(theta_fast);

                    omega_[1] = - M_PI / unit_time_;
                    omega_[2] = - M_PI / unit_time_;
                }
                else
                {
                    theta_fast += M_PI;
                    theta_slow += M_PI;

                    // 0, 3
                    q_[0] = std::cos(theta_fast);
                    q_[1] = std::sin(theta_fast);
                    q_[6] = std::cos(theta_fast);
                    q_[7] = std::sin(theta_fast);

                    omega_[0] = - M_PI / unit_time_;
                    omega_[3] = - M_PI / unit_time_;

                    // 1, 2
                    q_[2] = std::cos(theta_slow);
                    q_[3] = std::sin(theta_slow);
                    q_[4] = std::cos(theta_slow);
                    q_[5] = std::sin(theta_slow);

                    omega_[1] = - M_PI / unit_time_ / 3;
                    omega_[2] = - M_PI / unit_time_ / 3;
                }
            }

            if (gait_idx_ == 2)
            {
                T theta_fast = - phase_scalar_ / unit_time_ * M_PI + M_PI;
                T theta_slow = - phase_scalar_ / unit_time_ * M_PI / 2 - M_PI / 2;

                // 0, 3
                q_[0] = std::cos(theta_slow);
                q_[1] = std::sin(theta_slow);
                q_[6] = std::cos(theta_slow);
                q_[7] = std::sin(theta_slow);

                omega_[0] = - M_PI / unit_time_ / 2;
                omega_[3] = - M_PI / unit_time_ / 2;

                // 1, 2
                q_[2] = std::cos(theta_fast);
                q_[3] = std::sin(theta_fast);
                q_[4] = std::cos(theta_fast);
                q_[5] = std::sin(theta_fast);

                omega_[1] = - M_PI / unit_time_;
                omega_[2] = - M_PI / unit_time_;
            }

            if (gait_idx_ == 3)
            {
                T theta_fast = - phase_scalar_ / unit_time_ * M_PI + M_PI;
                T theta_slow = - phase_scalar_ / unit_time_ * M_PI / 2 - M_PI;

                // 0, 3
                q_[0] = std::cos(theta_fast);
                q_[1] = std::sin(theta_fast);
                q_[6] = std::cos(theta_fast);
                q_[7] = std::sin(theta_fast);

                omega_[0] = - M_PI / unit_time_;
                omega_[3] = - M_PI / unit_time_;

                // 1, 2
                q_[2] = std::cos(theta_slow);
                q_[3] = std::sin(theta_slow);
                q_[4] = std::cos(theta_slow);
                q_[5] = std::sin(theta_slow);

                omega_[0] = - M_PI / unit_time_ / 2;
                omega_[3] = - M_PI / unit_time_ / 2;
            }

            if (gait_idx_ == 4)
            {
                T theta_fast = - phase_scalar_ / unit_time_ * M_PI + M_PI;
                T theta_slow = - phase_scalar_ / unit_time_ * M_PI / 3.0 + M_PI;

                // 0, 3
                q_[0] = std::cos(theta_fast);
                q_[1] = std::sin(theta_fast);
                q_[6] = std::cos(theta_fast);
                q_[7] = std::sin(theta_fast);

                omega_[0] = - M_PI / unit_time_;
                omega_[3] = - M_PI / unit_time_;

                // 1, 2
                q_[2] = std::cos(theta_slow);
                q_[3] = std::sin(theta_slow);
                q_[4] = std::cos(theta_slow);
                q_[5] = std::sin(theta_slow);

                omega_[0] = - M_PI / unit_time_ / 3;
                omega_[3] = - M_PI / unit_time_ / 3;
            }

            if (gait_idx_ == 5)
            {
                T theta_fast = - phase_scalar_ / unit_time_ * M_PI + M_PI;
                T theta_slow = - phase_scalar_ / unit_time_ * M_PI / 3.0 + M_PI * 2.0 / 3.0;

                // 0, 3
                q_[0] = std::cos(theta_slow);
                q_[1] = std::sin(theta_slow);
                q_[6] = std::cos(theta_slow);
                q_[7] = std::sin(theta_slow);

                omega_[0] = - M_PI / unit_time_ / 3;
                omega_[3] = - M_PI / unit_time_ / 3;

                // 1, 2
                q_[2] = std::cos(theta_fast);
                q_[3] = std::sin(theta_fast);
                q_[4] = std::cos(theta_fast);
                q_[5] = std::sin(theta_fast);

                omega_[1] = - M_PI / unit_time_;
                omega_[2] = - M_PI / unit_time_;
            }

            if (gait_idx_ == 6)
            {
                T theta_1 = - phase_scalar_ / unit_time_ * M_PI / 2.0 + M_PI;
                T theta_2 = - phase_scalar_ / unit_time_ * M_PI / 2.0 + M_PI / 2.0;

                // 0, 3
                q_[0] = std::cos(theta_1);
                q_[1] = std::sin(theta_1);
                q_[6] = std::cos(theta_1);
                q_[7] = std::sin(theta_1);

                omega_[0] = - M_PI / unit_time_ / 2;
                omega_[3] = - M_PI / unit_time_ / 2;

                // 1, 2
                q_[2] = std::cos(theta_2);
                q_[3] = std::sin(theta_2);
                q_[4] = std::cos(theta_2);
                q_[5] = std::sin(theta_2);

                omega_[0] = - M_PI / unit_time_ / 2;
                omega_[3] = - M_PI / unit_time_ / 2;
            }

            if (gait_idx_ == 7)
            {
                if (phase_scalar_ / unit_time_ < 2.0)
                {
                    T theta_1 = - phase_scalar_ / unit_time_ * M_PI + M_PI;

                    // 0, 3
                    q_[0] = std::cos(theta_1);
                    q_[1] = std::sin(theta_1);
                    q_[6] = std::cos(theta_1);
                    q_[7] = std::sin(theta_1);

                    omega_[0] = - M_PI / unit_time_;
                    omega_[3] = - M_PI / unit_time_;
                }
                else
                {
                    T theta_1 = - (phase_scalar_ / unit_time_ - 2.0) * M_PI / 2.0 + M_PI;

                    // 0, 3
                    q_[0] = std::cos(theta_1);
                    q_[1] = std::sin(theta_1);
                    q_[6] = std::cos(theta_1);
                    q_[7] = std::sin(theta_1);

                    omega_[0] = - M_PI / unit_time_ / 2.0;
                    omega_[3] = - M_PI / unit_time_ / 2.0;
                }

                if (phase_scalar_ / unit_time_ < 3.0 || phase_scalar_ / unit_time_ >= 5.0)
                {
                    T theta_2 = phase_scalar_ / unit_time_ < 3.0 ? - (phase_scalar_ / unit_time_ + 1.0) * M_PI / 2.0 + M_PI :  - (phase_scalar_ / unit_time_ - 5.0) * M_PI / 2.0 + M_PI;

                    // 1, 2
                    q_[2] = std::cos(theta_2);
                    q_[3] = std::sin(theta_2);
                    q_[4] = std::cos(theta_2);
                    q_[5] = std::sin(theta_2);

                    omega_[1] = - M_PI / unit_time_ / 2.0;
                    omega_[2] = - M_PI / unit_time_ / 2.0;
                }
                if (phase_scalar_ / unit_time_ >= 3.0 && phase_scalar_ / unit_time_ < 5.0)
                {
                    T theta_2 = - (phase_scalar_ / unit_time_ - 3.0) * M_PI + M_PI;

                    // 1, 2
                    q_[2] = std::cos(theta_2);
                    q_[3] = std::sin(theta_2);
                    q_[4] = std::cos(theta_2);
                    q_[5] = std::sin(theta_2);

                    omega_[1] = - M_PI / unit_time_;
                    omega_[2] = - M_PI / unit_time_;
                }
            }

            if (gait_idx_ == 8)
            {
                T theta_1 = phase_scalar_ / unit_time_ < 2.0 ?  - (phase_scalar_ / unit_time_) * M_PI + M_PI : - (phase_scalar_ / unit_time_ - 2.0) * M_PI / 2.0 + M_PI;
                T theta_2 = phase_scalar_ / unit_time_ < 4.0 ?  - (phase_scalar_ / unit_time_) * M_PI / 2.0 + M_PI : - (phase_scalar_ / unit_time_ - 4.0) * M_PI + M_PI;

                T omega_1 = phase_scalar_ / unit_time_ < 2.0 ? - M_PI / unit_time_ : - M_PI / unit_time_ / 2.0;
                T omega_2 = phase_scalar_ / unit_time_ < 4.0 ? - M_PI / unit_time_ / 2.0 : - M_PI / unit_time_;

                // 0, 3
                q_[0] = std::cos(theta_1);
                q_[1] = std::sin(theta_1);
                q_[6] = std::cos(theta_1);
                q_[7] = std::sin(theta_1);

                omega_[0] = omega_1;
                omega_[3] = omega_1;

                // 1, 2
                q_[2] = std::cos(theta_2);
                q_[3] = std::sin(theta_2);
                q_[4] = std::cos(theta_2);
                q_[5] = std::sin(theta_2);

                omega_[0] = omega_2;
                omega_[3] = omega_2;
            }
        }

        Eigen::Matrix<T, 8, 1> get_status()
        {
            return q_;
        }

        Eigen::Matrix<T, 8, 1> get_velocity()
        {
            // not implemented in manual phase
            return q_dot_;
        }

    private:
        int gait_idx_ = 0;
        int prev_gait_idx_ = 0;
        int target_gait_idx_ = 0;

        T phase_scalar_ = 0.0;
        T dt_ = 0.01;
        T unit_time_ = 0.15;

        std::array<T, n_gait> UNIT = {2.0, 6.0, 4.0, 4.0, 6.0, 6.0, 4.0, 6.0, 6.0};
        std::array<T, n_gait> DUTY_UNIT = {1.0, 2.0, 2.0, 2.0, 3.0, 3.0, 2.0, 3.0, 3.0};
        std::array<T, n_gait> TIME;
        std::array<T, n_gait> STANCE_TIME;

        T unit_, duty_unity_, time_, stance_time_;

        Eigen::Matrix<T, 8, 1> q_;
        Eigen::Matrix<T, 8, 1> q_dot_;

        Eigen::Matrix<T, 4, 1> phase_raw_; // -pi to +pi
        std::array<T, 4> phase_; // 0 to 2 * pi
        Eigen::Matrix<T, 4, 1> omega_;
    };
}

#endif