/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Press W.H., et al. Numerical Recipes in C++: The Art of Scientific Computing. Cambridge
 *          University Press, February 2002.
 *      Torok, J.S. Analytical Mechanics: with an Introduction to Dynamical Systems, John Wiley and
 *          Sons, Inc., 2000.
 *      Vallado, D.A. Fundamentals of astro and Applications. Microcosm Press, 2001.
 *
 */

#include <cmath>
#include <iostream>
#include <limits>

#include <boost/math/special_functions/sign.hpp>

#include "tudat/math/basic/basicMathematicsFunctions.h"
#include "tudat/math/basic/mathematicalConstants.h"
#include "tudat/math/basic/coordinateConversions.h"

namespace tudat
{

namespace coordinate_conversions
{

//! Convert cylindrical to Cartesian coordinates.
Eigen::Vector3d convertCylindricalToCartesian( const double radius,
                                               const double azimuthAngle, const double z )
{
    // Create Cartesian coordinates vector.
    Eigen::Vector3d cartesianCoordinates;

    // If radius < 0, then give warning.
    if ( radius < 0.0 )
    {
        std::cerr << "Warning: cylindrical radial coordinate is negative!, This could give incorrect results!" << std::endl;
    }

    // Compute and set Cartesian coordinates.
    cartesianCoordinates << radius * std::cos( azimuthAngle ),   // x-coordinate
                            radius * std::sin( azimuthAngle ),   // y-coordinate
                            z;                                   // z-coordinate

    return cartesianCoordinates;
}

//! Convert cylindrical to cartesian coordinates.
Eigen::Vector3d convertCylindricalToCartesian( const Eigen::Vector3d& cylindricalCoordinates )
{
    // Create Cartesian coordinates vector.
    Eigen::Vector3d cartesianCoordinates;

    // If radius < 0, then give warning.
    if ( cylindricalCoordinates( 0 ) < 0.0 )
    {
        std::cerr << "Warning: cylindrical radial coordinate is negative!, This could give incorrect results!" << std::endl;
    }

    // Compute and set Cartesian coordinates.
    cartesianCoordinates
            << cylindricalCoordinates( 0 )
               * std::cos( cylindricalCoordinates( 1 ) ),    // x-coordinate
               cylindricalCoordinates( 0 )
               * std::sin( cylindricalCoordinates( 1 ) ),    // y-coordinate
               cylindricalCoordinates( 2 );                  // z-coordinate

    return cartesianCoordinates;
}

//! Convert cylindrical to Cartesian state.
Eigen::Vector6d convertCylindricalToCartesianState(
        const Eigen::Vector6d& cylindricalState )
{
    // Create Cartesian state vector, initialized with zero entries.
    Eigen::Vector6d cartesianState = Eigen::Vector6d::Zero( );

    // Get azimuth angle, theta.
    double azimuthAngle = cylindricalState( 1 );

    // Compute and set Cartesian coordinates.
    cartesianState.head( 3 ) = convertCylindricalToCartesian(
                Eigen::Vector3d( cylindricalState.head( 3 ) ) );

    // If r = 0 AND Vtheta > 0, then give warning and assume Vtheta=0.
    if ( std::fabs(cylindricalState( 0 )) <= std::numeric_limits< double >::epsilon( )
         && std::fabs(cylindricalState( 4 )) > std::numeric_limits< double >::epsilon( ) )
    {
        std::cerr << "Warning: cylindrical velocity Vtheta (r*thetadot) does not equal zero while the radius (r) is zero! Vtheta is taken equal to zero!" << std::endl;

        // Compute and set Cartesian velocities.
        cartesianState.tail( 3 )
                << cylindricalState( 3 ) * std::cos( azimuthAngle ),   // xdot
                   cylindricalState( 3 ) * std::sin( azimuthAngle ),   // ydot
                   cylindricalState( 5 );                              // zdot
    }

    else
    {
        // Compute and set Cartesian velocities.
        cartesianState.tail( 3 )
                << cylindricalState( 3 ) * std::cos( azimuthAngle )
                   - cylindricalState( 4 ) * std::sin( azimuthAngle ),   // xdot
                   cylindricalState( 3 ) * std::sin( azimuthAngle )
                   + cylindricalState( 4 ) * std::cos( azimuthAngle ),   // ydot
                   cylindricalState( 5 );                                // zdot
    }

    return cartesianState;
}

//! Convert Cartesian to cylindrical coordinates.
Eigen::Vector3d convertCartesianToCylindrical( const Eigen::Vector3d& cartesianCoordinates )
{
    // Create cylindrical coordinates vector.
    Eigen::Vector3d cylindricalCoordinates;

    // Declare new variable, the azimuth angle.
    double azimuthAngle;

    // Compute azimuth angle, theta.
    /* If x = 0, then azimuthAngle = pi/2 (y>0) or 3*pi/2 (y<0) or 0 (y=0),
       else azimuthAngle = arctan(y/x).
    */
    using mathematical_constants::PI;
    if ( std::fabs(cartesianCoordinates( 0 ) ) <= std::numeric_limits< double >::epsilon( ) )
    {
        azimuthAngle = basic_mathematics::computeModulo(
                    static_cast< double >( boost::math::sign( cartesianCoordinates( 1 ) ) )
                    * 0.5 * PI, 2.0 * PI );
    }

    else
    {
        azimuthAngle = basic_mathematics::computeModulo(
                    std::atan2( cartesianCoordinates( 1 ),
                                cartesianCoordinates( 0 ) ), 2.0 * PI );
    }

    // Compute and set cylindrical coordinates.
    cylindricalCoordinates <<
        std::sqrt( pow( cartesianCoordinates( 0 ), 2 )
                   + pow( cartesianCoordinates( 1 ), 2 ) ), // Radius
        azimuthAngle,                                       // Azimuth angle, theta
        cartesianCoordinates( 2 );                          // z-coordinate

    return cylindricalCoordinates;
}

//! Convert Cartesian to cylindrical state.
Eigen::Vector6d convertCartesianToCylindricalState(
        const Eigen::Vector6d& cartesianState )
{
    // Create cylindrical state vector, initialized with zero entries.
    Eigen::Vector6d cylindricalState = Eigen::Vector6d::Zero( );

    // Compute and set cylindrical coordinates.
    cylindricalState.head( 3 ) = convertCartesianToCylindrical(
                Eigen::Vector3d( cartesianState.head( 3 ) ) );

    // Compute and set cylindrical velocities.
    /* If radius = 0, then Vr = sqrt(xdot^2+ydot^2) and Vtheta = 0,
       else Vr = (x*xdot+y*ydot)/radius and Vtheta = (x*ydot-y*xdot)/radius.
    */
    if ( cylindricalState( 0 ) <= std::numeric_limits< double >::epsilon( ) )
    {
        cylindricalState.tail( 3 ) <<
            std::sqrt( pow( cartesianState( 3 ), 2 ) + pow( cartesianState( 4 ), 2 ) ), // Vr
            0.0,                                                                        // Vtheta
            cartesianState( 5 );                                                        // Vz
    }

    else
    {
        cylindricalState.tail( 3 ) <<
            ( cartesianState( 0 ) * cartesianState( 3 )
              + cartesianState( 1 ) * cartesianState( 4 ) ) / cylindricalState( 0 ),    // Vr
            ( cartesianState( 0 ) * cartesianState( 4 )
              - cartesianState( 1 ) * cartesianState( 3 ) ) / cylindricalState( 0 ),    // Vtheta
                cartesianState( 5 );                                                    // Vz
    }

    return cylindricalState;
}

//! Compute matrix by which to precompute a spherical gradient vector to obtain the Cartesian gradient
Eigen::Matrix3d getSphericalToCartesianGradientMatrix( const Eigen::Vector3d& cartesianCoordinates )
{
    // Compute radius.
    const double radius = std::sqrt( cartesianCoordinates( 0 ) * cartesianCoordinates( 0 )
                                     + cartesianCoordinates( 1 ) * cartesianCoordinates( 1 )
                                     + cartesianCoordinates( 2 ) * cartesianCoordinates( 2 ) );

    // Compute square of distance within xy-plane.
    const double xyDistanceSquared = cartesianCoordinates( 0 ) * cartesianCoordinates( 0 )
            + cartesianCoordinates( 1 ) * cartesianCoordinates( 1 );

    // Compute distance within xy-plane.
    const double xyDistance = std::sqrt( xyDistanceSquared );

    // Compute transformation matrix.
    const Eigen::Matrix3d transformationMatrix = (
                Eigen::Matrix3d( 3, 3 ) <<
                cartesianCoordinates( 0 ) / radius,
                - cartesianCoordinates( 0 ) * cartesianCoordinates( 2 ) / ( radius * radius * xyDistance ),
                - cartesianCoordinates( 1 ) / xyDistanceSquared,
                cartesianCoordinates( 1 ) / radius,
                - cartesianCoordinates( 1 ) * cartesianCoordinates( 2 ) / ( radius * radius * xyDistance ),
                + cartesianCoordinates( 0 ) / xyDistanceSquared,
                cartesianCoordinates( 2 ) / radius,
                xyDistance / ( radius * radius ),   0.0
                ).finished( );
    return transformationMatrix;
}

//! Convert spherical to Cartesian gradient.
Eigen::Vector3d convertSphericalToCartesianGradient( const Eigen::Vector3d& sphericalGradient,
                                                     const Eigen::Vector3d& cartesianCoordinates )
{

    // Return Cartesian gradient.
    return getSphericalToCartesianGradientMatrix( cartesianCoordinates ) * sphericalGradient;
}

Eigen::Matrix3d getDerivativeOfSphericalToCartesianGradient( const Eigen::Vector3d& sphericalGradient,
                                                             const Eigen::Vector3d& cartesianCoordinates,
                                                             std::vector< Eigen::Matrix3d >& subMatrices )
{
    Eigen::Matrix3d totalPartialMatrix;
    totalPartialMatrix.setZero( );

    Eigen::Matrix3d currentPartialMatrix;

    // Precomputed quantities
    double radius = cartesianCoordinates.norm( );
    const double xyDistanceSquared = cartesianCoordinates( 0 ) * cartesianCoordinates( 0 )
            + cartesianCoordinates( 1 ) * cartesianCoordinates( 1 );
    const double xyDistance = std::sqrt( xyDistanceSquared );
    const double radiusSquaredXyDistance = xyDistance * radius * radius;

    // Precompute partials
    Eigen::Vector3d oneOverRPartial = -cartesianCoordinates / ( radius * radius * radius );
    Eigen::Vector3d oneOverRSquaredPartial = -2.0 * cartesianCoordinates / ( radius * radius * radius * radius );
    Eigen::Vector3d oneOverXyDistancePartial =
            -( Eigen::Vector3d( ) << cartesianCoordinates( 0 ), cartesianCoordinates( 1 ), 0.0 ).finished( )/
            ( xyDistanceSquared * xyDistance );
    Eigen::Vector3d oneOverXyDistanceSquaredPartial =
            -2.0 * ( Eigen::Vector3d( ) << cartesianCoordinates( 0 ), cartesianCoordinates( 1 ), 0.0 ).finished( )/
            ( xyDistanceSquared * xyDistanceSquared );
    Eigen::Vector3d oneOverRSquaredXyDistancePartial =
            oneOverRSquaredPartial / xyDistance + oneOverXyDistancePartial / ( radius * radius );

    Eigen::Vector3d xyDistancePartial =
            ( Eigen::Vector3d( ) << cartesianCoordinates( 0 ), cartesianCoordinates( 1 ), 0.0 ).finished( ) / xyDistance;

    // Compute partials w.r.t x, y and z components.
    for( unsigned int i = 0; i < 3; i++ )
    {
        currentPartialMatrix.setZero( );
        switch( i )
        {
        case 0:
        {
            currentPartialMatrix << 1.0 / radius + cartesianCoordinates( 0 ) * oneOverRPartial ( 0 ),
                    - cartesianCoordinates( 2 ) / radiusSquaredXyDistance -
                    cartesianCoordinates( 0 ) * cartesianCoordinates( 2 ) * oneOverRSquaredXyDistancePartial( 0 ),
                    - cartesianCoordinates( 1 ) * oneOverXyDistanceSquaredPartial( 0 ),
                    cartesianCoordinates( 1 ) * oneOverRPartial( 0 ),
                     - cartesianCoordinates( 1 ) * cartesianCoordinates( 2 ) * oneOverRSquaredXyDistancePartial( 0 ),
                    1.0 / ( xyDistanceSquared ) + cartesianCoordinates( 0 ) * oneOverXyDistanceSquaredPartial( 0 ),
                     cartesianCoordinates( 2 ) * oneOverRPartial( 0 ),
                    xyDistance * oneOverRSquaredPartial( 0 ) + 1.0 / ( radius * radius ) * xyDistancePartial( 0 ),
                    0.0 ;
            break;
        }
        case 1:
        {
            currentPartialMatrix << cartesianCoordinates( 0 ) * oneOverRPartial ( 1 ),
                    - cartesianCoordinates( 0 ) * cartesianCoordinates( 2 ) * oneOverRSquaredXyDistancePartial( 1 ),
                    -1.0 / ( xyDistanceSquared ) - cartesianCoordinates( 1 ) * oneOverXyDistanceSquaredPartial( 1 ),
                    1.0 / radius + cartesianCoordinates( 1 ) * oneOverRPartial ( 1 ),
                    - cartesianCoordinates( 2 ) / radiusSquaredXyDistance -
                    cartesianCoordinates( 1 ) * cartesianCoordinates( 2 ) * oneOverRSquaredXyDistancePartial( 1 ),
                    cartesianCoordinates( 0 ) * oneOverXyDistanceSquaredPartial( 1 ),
                     cartesianCoordinates( 2 ) * oneOverRPartial( 1 ),
                    xyDistance * oneOverRSquaredPartial( 1 ) + 1.0 / ( radius * radius ) * xyDistancePartial( 1 ),
                    0.0 ;
            break;
        }
        case 2:
        {
            currentPartialMatrix <<  cartesianCoordinates( 0 ) * oneOverRPartial ( 2 ),
                    - cartesianCoordinates( 0 ) / radiusSquaredXyDistance -
                    cartesianCoordinates( 0 ) * cartesianCoordinates( 2 ) * oneOverRSquaredXyDistancePartial( 2 ),
                    - cartesianCoordinates( 1 ) * oneOverXyDistanceSquaredPartial( 2 ),
                    cartesianCoordinates( 1 ) * oneOverRPartial( 2 ),
                    - cartesianCoordinates( 1 ) / radiusSquaredXyDistance -
                    cartesianCoordinates( 1 ) * cartesianCoordinates( 2 ) * oneOverRSquaredXyDistancePartial( 2 ),
                    cartesianCoordinates( 0 ) * oneOverXyDistanceSquaredPartial( 2 ),
                     1.0 / radius + cartesianCoordinates( 2 ) * oneOverRPartial( 2 ),
                    xyDistance * oneOverRSquaredPartial( 2 ) +  + 1.0 / ( radius * radius ) * xyDistancePartial( 2 ),
                    0.0 ;
            break;
        }
        }

        // Save computed matrix
        if( subMatrices.size( ) == 3 )
        {
            subMatrices[ i ] = currentPartialMatrix;
        }

        // Add current entry to results.
        totalPartialMatrix.block( 0, i, 3, 1 ) = currentPartialMatrix * sphericalGradient;
    }

    return totalPartialMatrix;
}

Eigen::Matrix3d getDerivativeOfSphericalToCartesianGradient( const Eigen::Vector3d& sphericalGradient,
                                                             const Eigen::Vector3d& cartesianCoordinates )
{
    static std::vector< Eigen::Matrix3d > subMatrices( 3 );
    return getDerivativeOfSphericalToCartesianGradient(
                sphericalGradient, cartesianCoordinates, subMatrices );
}

//! Convert spherical to Cartesian state.
Eigen::Vector6d convertSphericalToCartesianState(
        const Eigen::Vector6d& sphericalState )
{
    // Create Cartesian state vector, initialized with zero entries.
    Eigen::Vector6d convertedCartesianState = Eigen::Vector6d::Zero( );

    // Create local variables.
    const double radius = sphericalState( 0 );
    const double azimuthAngle = sphericalState( 1 );
    const double elevationAngle = sphericalState( 2 );

    // Precompute sine/cosine of angles, which has multiple usages, to save computation time.
    const double cosineOfElevationAngle = std::cos( elevationAngle );
    const double sineOfElevationAngle = std::sin( elevationAngle );
    const double cosineOfAzimuthAngle = std::cos( azimuthAngle );
    const double sineOfAzimuthAngle = std::sin( azimuthAngle );

    // Set up transformation matrix for spherical to cylindrical conversion.
    Eigen::Matrix3d transformationMatrixSphericalToCylindrical = Eigen::Matrix3d::Zero( );
    transformationMatrixSphericalToCylindrical( 0, 0 ) = cosineOfElevationAngle;
    transformationMatrixSphericalToCylindrical( 0, 2 ) = -sineOfElevationAngle;
    transformationMatrixSphericalToCylindrical( 1, 1 ) = 1.0;
    transformationMatrixSphericalToCylindrical( 2, 0 ) = sineOfElevationAngle;
    transformationMatrixSphericalToCylindrical( 2, 2 ) = cosineOfElevationAngle;

    // Set up transformation matrix for cylindrical to Cartesian conversion.
    Eigen::Matrix3d transformationMatrixCylindricalToCartesian = Eigen::Matrix3d::Zero( );
    transformationMatrixCylindricalToCartesian( 0, 0 ) = cosineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 0, 1 ) = -sineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 1, 0 ) = sineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 1, 1 ) = cosineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 2, 2 ) = 1.0;

    // Compute transformation matrix for spherical to Cartesian conversion.
    const Eigen::Matrix3d transformationMatrixSphericalToCartesian
            = transformationMatrixCylindricalToCartesian
            * transformationMatrixSphericalToCylindrical;

    // Perform transformation of position coordinates.
    convertedCartesianState( 0 ) = radius * cosineOfAzimuthAngle * cosineOfElevationAngle;
    convertedCartesianState( 1 ) = radius * sineOfAzimuthAngle * cosineOfElevationAngle;
    convertedCartesianState( 2 ) = radius * sineOfElevationAngle;

    // Perform transformation of velocity vector.
    convertedCartesianState.segment( 3, 3 ) =
        transformationMatrixSphericalToCartesian * sphericalState.segment( 3, 3 );

    // Return Cartesian state vector.
    return convertedCartesianState;
}

//! Convert Cartesian to spherical state.
Eigen::Vector6d convertCartesianToSphericalState(
        const Eigen::Vector6d& cartesianState )
{
    // Create spherical state vector, initialized with zero entries.
    Eigen::Vector6d convertedSphericalState = Eigen::Vector6d::Zero( );

    // Compute radius.
    convertedSphericalState( 0 ) = cartesianState.segment( 0, 3 ).norm( );

    // Check if radius is nonzero.
    /*
     * If r > 0, the elevation and azimuth angles are computed using trigonometric relationships.
     * If r = 0, the coordinates are at the origin, the elevation and azimuth angles equal to zero.
     * Since the state vector was initialized with zeroes, this is already the case.
     */
    if ( convertedSphericalState( 0 ) > std::numeric_limits< double >::epsilon( ) )
    {
        // Compute elevation and azimuth angles using trigonometric relationships.
        // Azimuth angle.
        convertedSphericalState( 1 ) = std::atan2( cartesianState( 1 ), cartesianState( 0 ) );
        // Elevation angle.
        convertedSphericalState( 2 ) = std::asin( cartesianState( 2 )
                                                   / convertedSphericalState( 0 ) );
    }

    // Precompute sine/cosine of angles, which has multiple usages, to save computation time.
    const double cosineOfElevationAngle = std::cos( convertedSphericalState( 2 ) );
    const double sineOfElevationAngle = std::sin( convertedSphericalState( 2 ) );
    const double cosineOfAzimuthAngle = std::cos( convertedSphericalState( 1 ) );
    const double sineOfAzimuthAngle = std::sin( convertedSphericalState( 1 ) );

    // Set up transformation matrix for cylindrical to spherical conversion.
    Eigen::Matrix3d transformationMatrixCylindricalToSpherical = Eigen::Matrix3d::Zero( );
    transformationMatrixCylindricalToSpherical( 0, 0 ) = cosineOfElevationAngle;
    transformationMatrixCylindricalToSpherical( 0, 2 ) = sineOfElevationAngle;
    transformationMatrixCylindricalToSpherical( 1, 1 ) = 1.0;
    transformationMatrixCylindricalToSpherical( 2, 0 ) = -sineOfElevationAngle;
    transformationMatrixCylindricalToSpherical( 2, 2 ) = cosineOfElevationAngle;

    // Set up transformation matrix for Cartesian to cylindrical conversion.
    Eigen::Matrix3d transformationMatrixCartesianToCylindrical = Eigen::Matrix3d::Zero( );
    transformationMatrixCartesianToCylindrical( 0, 0 ) = cosineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 0, 1 ) = sineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 1, 0 ) = -sineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 1, 1 ) = cosineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 2, 2 ) = 1.0;

    // Compute transformation matrix for Cartesian to spherical conversion.
    const Eigen::Matrix3d transformationMatrixCartesianToSpherical
            = transformationMatrixCylindricalToSpherical
            * transformationMatrixCartesianToCylindrical;

    // Perform transformation of velocity vector.
    convertedSphericalState.segment( 3, 3 )
            = transformationMatrixCartesianToSpherical * cartesianState.segment( 3, 3 );

    // Return spherical state vector.
    return convertedSphericalState;
}

} // namespace coordinate_conversions

} // namespace tudat


#include <ql/quantlib.hpp>

#include <boost/make_shared.hpp>

#include <iostream>
#include <fstream>

using namespace QuantLib;

void spreads() {

    // set up market data

    Date refDate = Date(14, October, 2013);
    Date settlDate = TARGET().advance(refDate, 2, Days);
    Settings::instance().evaluationDate() = refDate;

    boost::shared_ptr<SimpleQuote> rateLevel0(new SimpleQuote(0.025));
    boost::shared_ptr<SimpleQuote> rateLevel(new SimpleQuote(0.03));
    Handle<Quote> forward0(rateLevel0);
    Handle<Quote> forward(rateLevel);
    Handle<YieldTermStructure> yts0(
        boost::make_shared<FlatForward>(refDate, forward0, Actual365Fixed()));
    Handle<YieldTermStructure> yts(
        boost::make_shared<FlatForward>(refDate, forward, Actual365Fixed()));

    boost::shared_ptr<IborIndex> euribor6m(new Euribor(6 * Months, yts));

    // swap and bond

    boost::shared_ptr<VanillaSwap> swap =
        MakeVanillaSwap(20 * Years, euribor6m, 0.04).receiveFixed(false);
    Leg fix = swap->leg(0);
    fix.push_back(
        boost::shared_ptr<CashFlow>(new Redemption(1.0, fix.back()->date())));

    for (Size i = 0; i < fix.size(); i++) {
        std::cout << fix[i]->date() << "  \t" << fix[i]->amount() << std::endl;
    }

    boost::shared_ptr<PricingEngine> discountingEngine(
        new DiscountingSwapEngine(yts0, boost::none, settlDate, settlDate));
    swap->setPricingEngine(discountingEngine);

    std::cout << "swap npv = " << swap->NPV() << std::endl;
    std::cout << "swap bps = " << swap->floatingLegBPS() << std::endl;

    // tabulate zSpread against asset swap spread

    std::ofstream out1;
    out1.open("spreads1c.dat");

    Real zSpread = 0.0;
    while (zSpread <= 0.10) {

        Real bondNpv = CashFlows::npv(
            fix, *yts, zSpread, Actual365Fixed(), Continuous,
            NoFrequency, false, settlDate, settlDate);

        Real swapNpv = swap->NPV();
        Real swapBps = swap->floatingLegBPS();

        Real aswSpread = (1.0 - (bondNpv + swapNpv)) / swapBps / 10000.0;

        out1 << zSpread * 10000.0 << " " << aswSpread * 10000.0 << std::endl;

        zSpread += 0.0001;
    }

    out1.close();

    // tabulate da/dz against zSpread level

    out1.open("spreads2c.dat");

    zSpread = 0.0;
    while (zSpread <= 0.10) {

        Real bondNpv = CashFlows::npv(
            fix, *yts, zSpread, Actual365Fixed(), Continuous,
            NoFrequency, false, settlDate, settlDate);

        Real bondNpvP =
            CashFlows::npv(fix, *yts, zSpread + 0.0001, Actual365Fixed(),
                           Continuous, NoFrequency,
                           false, settlDate, settlDate);

        // Real bondNpv = CashFlows::npv(
        //     fix, *yts, zSpread, Actual360(), Compounding::Compounded,
        //     Frequency::Annual, false, settlDate, settlDate);

        // Real bondNpvP =
        //     CashFlows::npv(fix, *yts, zSpread + 0.0001, Actual360(),
        //                    Compounding::Compounded, Frequency::Annual,
        //                    false, settlDate, settlDate);

        Real swapNpv = swap->NPV();
        Real swapBps = swap->floatingLegBPS();

        Real aswSpread = (1.0 - (bondNpv + swapNpv)) / swapBps / 10000.0;
        Real aswSpreadP = (1.0 - (bondNpvP + swapNpv)) / swapBps / 10000.0;

        out1 << zSpread * 10000.0 << " " << (aswSpreadP - aswSpread) * 10000.0
             << std::endl;

        zSpread += 0.0001;
    }

    out1.close();

    // tabulate da/dy against zSpread level

    out1.open("spreads3c.dat");

    zSpread = 0.0;
    while (zSpread <= 0.10) {

        Real swapNpv = swap->NPV();
        Real swapBps = swap->floatingLegBPS();
        Real bondNpv = CashFlows::npv(
            fix, *yts, zSpread, Actual365Fixed(), Continuous,
            NoFrequency, false, settlDate, settlDate);

        rateLevel0->setValue(rateLevel0->value() + 0.0001);
        rateLevel->setValue(rateLevel->value() + 0.0001);

        Real bondNpvP = CashFlows::npv(
            fix, *yts, zSpread, Actual365Fixed(), Continuous,
             NoFrequency, false, settlDate, settlDate);

        Real swapNpvP = swap->NPV();
        Real swapBpsP = swap->floatingLegBPS();

        rateLevel0->setValue(rateLevel0->value() - 0.0001);
        rateLevel->setValue(rateLevel->value() - 0.0001);

        Real aswSpread = (1.0 - (bondNpv + swapNpv)) / swapBps / 10000.0;
        Real aswSpreadP = (1.0 - (bondNpvP + swapNpvP)) / swapBpsP / 10000.0;

        out1 << zSpread * 10000.0 << " " << (aswSpreadP - aswSpread) * 10000.0
             << std::endl;

        zSpread += 0.0001;
    }

    out1.close();
}

int main(int, char * []) { spreads(); }


#include <omp.h>

#include <Eigen/Dense>
#include <iostream>
#include <set>
#include <tuple>

#include "qpp.h"

using namespace qpp;
using uint = unsigned int;

struct SimStateDmat {
  cmat state = cmat::Zero(1, 1);
  int num_qubits = 0;
  SimStateDmat() { state << 1; }
};

typedef SimStateDmat* state_t;

extern "C" state_t empty_dmat() { return new SimStateDmat; }

extern "C" void discard_dmat(state_t s) { delete s; }

extern "C" int qinit_dmat(state_t s) {
  s->state = kron(s->state, prj(0_ket));
  return s->num_qubits++;
}

enum Gate : int {
  X = 0,
  Y = 1,
  Z = 2,
  H = 3,
  CNOT = 4,
  CZ = 5,
  TOF = 6,
  FRED = 7,
  PHASE = 8,
  CPHASE = 9
};

extern "C" void unitary1_dmat(state_t s, Gate g, uint q) {
  cmat u;
  switch (g) {
    case X:
      u = gt.X;
      break;
    case Y:
      u = gt.Y;
      break;
    case Z:
      u = gt.Z;
      break;
    case H:
      u = gt.H;
      break;
    default:
      abort();
  }
  s->state = apply(s->state, u, {q});
}

extern "C" void unitary2_dmat(state_t s, Gate g, uint q1, uint q2) {
  cmat u;
  switch (g) {
    case CNOT:
      u = gt.CNOT;
      break;
    case CZ:
      u = gt.CZ;
      break;
    default:
      abort();
  }
  s->state = apply(s->state, u, {q1, q2});
}

extern "C" void unitary3_dmat(state_t s, Gate g, uint q1, uint q2, uint q3) {
  cmat u;
  switch (g) {
    case TOF:
      u = gt.TOF;
      break;
    case FRED:
      u = gt.FRED;
      break;
    default:
      abort();
  }
  s->state = apply(s->state, u, {q1, q2, q3});
}

extern "C" void punitary1_dmat(state_t s, Gate g, uint q, double p) {
  cmat u = std::polar(1.0, M_PI * p) * gt.RZ(2 * M_PI * p);
  switch (g) {
    case PHASE:
      break;
    default:
      abort();
  }
  s->state = apply(s->state, u, {q});
}

extern "C" void punitary2_dmat(state_t s, Gate g, uint q1, uint q2, double p) {
  cmat u = std::polar(1.0, M_PI * p) * gt.RZ(2 * M_PI * p);
  switch (g) {
    case CPHASE:
      break;
    default:
      abort();
  }
  s->state = applyCTRL(s->state, u, {q1}, {q2});
}

extern "C" void measure_dmat(state_t s, uint q, bool outcome) {
  cmat p = cmat::Zero(2, 2);
  if (outcome) {
    p << 1, 0, 0, 0;
  } else {
    p << 0, 0, 0, 1;
  }
  s->state = apply(s->state, p, {q});
}

inline bool is_pure(const cmat& t) {
  return std::norm((t * t).trace() - std::complex<double>(1.0f)) < 1e-5;
}

extern "C" bool separable_dmat(state_t s, const uint* const qs, uint n) {
  if (n == s->num_qubits) {
    return is_pure(s->state);
  }
  std::set<idx> target;
  for (idx i = 0; i < s->num_qubits; ++i) {
    target.insert(i);
  }
  for (int i = 0; i < n; ++i) {
    target.erase(qs[i]);
  }
  return is_pure(ptrace(s->state, std::vector<idx>(target.begin(), target.end())));
}

extern "C" state_t clone_dmat(state_t s) {
  state_t t = new SimStateDmat;
  t->state = s->state;
  t->num_qubits = s->num_qubits;
  return t;
}

inline uint swap_bits(uint x, uint p1, uint p2) {
  const uint y = ((x >> p1) & 1) ^ ((x >> p2) & 1);
  return x ^ ((y << p1) | (y << p2));
}

inline void swap(cmat& state, const idx numdims, const uint* const q1, const uint* const q2, uint n) {
  using namespace Eigen;
  PermutationMatrix<Dynamic, Dynamic> perm(1UL << numdims);

#ifdef HAS_OPENMP
#pragma omp parallel for
#endif
  for (idx i = 0; i < 1UL << numdims; ++i) {
    idx j = i;
    for (uint k = 0; k < n; ++k) {
      j = swap_bits(j, q2[k], q1[k]);
    }
    perm.indices()[i] = j;
  }

  state = perm * state * perm.transpose();
}

extern "C" void sum_dmat(state_t s1, state_t s2, const uint* const q1, const uint* const q2, uint nqs) {
  while (s1->num_qubits > s2->num_qubits) {
    s2->state = kron(s2->state, prj(0_ket));
    s2->num_qubits++;
  }
  while (s2->num_qubits > s1->num_qubits) {
    s1->state = kron(s1->state, prj(0_ket));
    s1->num_qubits++;
  }
  swap(s2->state, s2->num_qubits, q1, q2, nqs);
  s1->state += s2->state;
  delete s2;
}

extern "C" void print_dmat(state_t s) {
  if (s->state.size() <= 1) {
    std::cout << "(empty)" << std::endl;
  } else {
    std::cout << disp(s->state) << std::endl;
  }
}


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

/*
 Copyright (C) 2010 Sebastian Schlenkrich

*/

#ifndef quantlib_templateauxilliaries_solver1d_hpp
#define quantlib_templateauxilliaries_solver1d_hpp

#include <ql/types.hpp>
#include <boost/function.hpp>

namespace TemplateAuxilliaries {

    //! Template for 1-D solution f(x) = 0, s.t. x \in [a,b] via secant method
    template <class Type>
    Type solve1d( const boost::function<Type (Type)>& f, Type xTol, Type a, Type b, size_t nTrials = 10 ) {
        Type fa = f(a);
        Type fb = f(b);
        if (fa*fb>0) {  // we need new arguments enclosing the solution
            b = (a + b);
            a = b / 4.0;
            for (size_t k=0; k<nTrials; ++k) {
                fa = f(a);
                fb = f(b);
                if (fa*fb<=0) break;
                a = a/2.0;
                b = b*2.0;
            }
        }
        QL_REQUIRE(fa*fb<=0,"Solve1d: Can't find intervall enclosing a solution");
        if (a>b) { // swap a <-> b
            Type tmp = a; a = b; b = tmp;
            tmp = fa; fa = fb; fb = tmp;
        }
        Type m = (fb - fa)/(b-a);
        Type x1 = a;
        Type y1 = fa;
        Type s = - y1/m;
        while (fabs(s)>xTol) {
            Type x0=x1, y0=y1;
            // find a new solution
            x1 = x0 + s;
            if ((x1<a)||(x1>b)) x1 = (a + b)/2.0;
            y1 = f(x1);
            m  = (y1-y0)/(x1-x0);
            s  = -y1/m;
            // update intervalls
            if (fa*y1>=0) {
                a  = x1;
                fa = y1;
            } else {
                b  = x1;
                fb = y1;
            }
        }
        return x1;
    }

}

#endif  /* ifndef quantlib_solve1d_hpp */


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

#include "get_fitzgibbon_cvpr_2001.hpp"
#include <float.h>  // For DBL_MAX
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include "posedata.hpp"
#include "radial.hpp"

namespace HomLib {
namespace FitzgibbonCVPR2001 {
    inline Eigen::Matrix3d vec2asym(const Eigen::Vector3d& t);

    HomLib::PoseData get(const Eigen::MatrixXd& x1n, const Eigen::MatrixXd& x2n) {
        // This is a five point method
        int n_points = 5;

        // Make homogenous
        Eigen::MatrixXd x1 = x1n.colwise().homogeneous();
        Eigen::MatrixXd x2 = x2n.colwise().homogeneous();

        // Compute the distance to center point
        Eigen::MatrixXd z1(3, n_points);
        z1 << Eigen::MatrixXd::Zero(2, n_points), x1n.colwise().squaredNorm();
        Eigen::MatrixXd z2(3, n_points);
        z2 << Eigen::MatrixXd::Zero(2, n_points), x2n.colwise().squaredNorm();

        // Initialize D0, D1 and D2
        Eigen::MatrixXd D0(9, 9);
        D0.setZero();
        Eigen::MatrixXd D1(9, 9);
        D1.setZero();
        Eigen::MatrixXd D2(9, 9);
        D2.setZero();

        Eigen::Matrix3d Bx2, Bz2;
        Eigen::Matrix3d e1, e2;
        Eigen::Vector3d tmp;

        for (int k = 0; k < n_points; k++) {
            tmp = x2.col(k);
            Bx2 = HomLib::FitzgibbonCVPR2001::vec2asym(tmp);
            tmp = z2.col(k);
            Bz2 = HomLib::FitzgibbonCVPR2001::vec2asym(tmp);

            // D0
            e1 = Bx2.row(0).transpose() * x1.col(k).transpose();
            e2 = Bx2.row(1).transpose() * x1.col(k).transpose();
            D0.row(2*k) = Eigen::Map<Eigen::VectorXd>(e1.data(), 9);
            if (k < 4) {  // Assure it is 9x9
                D0.row(2*k + 1) = Eigen::Map<Eigen::VectorXd>(e2.data(), 9);
            }
            // D1
            e1 = Bx2.row(0).transpose() * z1.col(k).transpose() + Bz2.row(0).transpose() * x1.col(k).transpose();
            e2 = Bx2.row(1).transpose() * z1.col(k).transpose() + Bz2.row(1).transpose() * x1.col(k).transpose();
            D1.row(2*k) = Eigen::Map<Eigen::VectorXd>(e1.data(), 9);
            if (k < 4) {
                D1.row(2*k + 1) = Eigen::Map<Eigen::VectorXd>(e2.data(), 9);
            }

            // D2
            e1 = Bz2.row(0).transpose() * z1.col(k).transpose();
            e2 = Bz2.row(1).transpose() * z1.col(k).transpose();
            D2.row(2*k) = Eigen::Map<Eigen::VectorXd>(e1.data(), 9);
            if (k < 4) {
                D2.row(2*k + 1) = Eigen::Map<Eigen::VectorXd>(e2.data(), 9);
            }
        }

        // Create generalized eigenvalue problem
        Eigen::MatrixXd A(18, 18);
        Eigen::MatrixXd B(18, 18);
        A.setZero();
        B.setZero();

        A.topLeftCorner(9, 9) = -D0;
        A.bottomRightCorner(9, 9) = Eigen::MatrixXd::Identity(9, 9);
        B.topLeftCorner(9, 9) = D1;
        B.topRightCorner(9, 9) = D2;
        B.bottomLeftCorner(9, 9) = Eigen::MatrixXd::Identity(9, 9);

        Eigen::GeneralizedEigenSolver<Eigen::MatrixXd> ges;
        ges.compute(A, B, true);
        Eigen::VectorXcd l;
        Eigen::MatrixXd X;
        l = ges.eigenvalues();
        Eigen::MatrixXcd eigvecs;
        eigvecs = ges.eigenvectors();
        X = eigvecs.real().topRows(9);

        // Extract correct solution
        Eigen::Matrix3d Htmp;
        Eigen::MatrixXd z(3, 5);
        double res;
        double minres = DBL_MAX;
        HomLib::PoseData posedata;
        double ltmp;
        Eigen::Array<bool, 1, 18> is_ok;
        is_ok = l.array().isFinite() && l.array().imag() == 0;

        for (int k = 0; k < 18; k++) {
            if (is_ok(k)) {
                ltmp = l(k).real();
                Htmp = Eigen::Map<Eigen::Matrix3d>(X.col(k).data(), 3, 3);
                z = Htmp * radialundistort(x1.colwise().hnormalized(), ltmp).colwise().homogeneous();
                res = (x2.colwise().hnormalized() - radialdistort(z.colwise().hnormalized(), ltmp)).squaredNorm();
                if (res < minres) {
                    minres = res;
                    posedata.homography = Htmp;
                    posedata.distortion_parameter = ltmp;
                }
            }
        }

        return posedata;
    }

    // TODO(marcusvaltonen): Refactor -> helpers when necessary
    inline Eigen::Matrix3d vec2asym(const Eigen::Vector3d& t) {
        Eigen::Matrix3d t_hat;
        t_hat << 0, -t(2), t(1),
                 t(2), 0, -t(0),
                -t(1), t(0), 0;
        return t_hat;
    }
}  // namespace FitzgibbonCVPR2001
}  // namespace HomLib


#include <Engine/MeshEdit/Simulate.h>
#include <windows.h>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/Dense>

using namespace Ubpa;
using namespace std;
using namespace Eigen;

void Simulate::SetFast(){
	isfast = true;
}

void Simulate::Clear() {
	this->positions.clear();
}

bool Simulate::Init() {
	isfast = true;
	m = positions.size(); // number of vertices
	s = edgelist.size() / 2;  // number of springs
	g = 9.8;
	iteration = 10;
	stiff = 1e5;

	this->velocity.resize(positions.size());
	for (int i = 0; i < positions.size(); i++)
		for (int j = 0; j < 3; j++)
			this->velocity[i][j] = 0;

	// init l
	for (int i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];
		pointf3 v1 = positions[index1];
		pointf3 v2 = positions[index2];
		l.push_back((v1 - v2).norm());
	}

	mass.resize(3 * positions.size());
	f_int.resize(3 * positions.size());
	for (int i = 0; i < 3 * positions.size(); i++) {
		f_int[i] = 0.0;
		mass[i] = 1;
	}

	x.resize(3 * m); // vector x, initialized to be y
	y.resize(3 * m); // vector y = 2q_n - q_n-1
	x_pre.resize(3 * m);
	for (int i = 0; i < m; i++) {
		x.segment(3 * i, 3) << positions[i][0], positions[i][1], positions[i][2];
		y.segment(3 * i, 3) = x.segment(3 * i, 3);
		x_pre.segment(3 * i, 3) = x.segment(3 * i, 3);
	}

	// init Mass
	M = MatrixXd::Identity(m * 3, m * 3);

	// init f_ext, i.e. gravity
	f_ext.resize(3 * m);
	for (int i = 0; i < m; i++)
		f_ext.segment(3 * i, 3) = Vector3d(0, -mass[i] * g, 0);

	L = MatrixXd::Zero(m * 3, m * 3);
	buildL();
	J = MatrixXd::Zero(m * 3, s * 3);
	buildJ();

	FixPoint();
	buildK();
	getb();

	// prefactorization
	MatrixXd A_;
	A_.resize(K.rows(), K.rows());
	A_ = K * (M + h * h * L) * K.transpose();

	A = A_.sparseView();
	LLT_.compute(A);

	return true;
}

void Simulate::SetLeftFix() {
	// 固定网格x坐标最小点
	fixed_id.clear();
	double x = 100000;
	for (int i = 0; i < positions.size(); i++)
	{
		if (positions[i][0] < x)
		{
			x = positions[i][0];
		}
	}

	for (int i = 0; i < positions.size(); i++)
	{
		if (abs(positions[i][0] - x) < 1e-5)
		{
			fixed_id.push_back(i);
		}
	}

	Init();
}

void Simulate::FixPoint() {
	fixed_id.push_back(10);
 	fixed_id.push_back(120);
}

void Simulate::buildK() {
	K = MatrixXd::Zero(m * 3 - 3 * fixed_id.size(), m * 3);
	set<int> fix(fixed_id.begin(), fixed_id.end());
	for (int i = 0, j = 0; i < m * 3; i++) {
		if (fix.find(i / 3) == fix.end()) {
			K(j++, i) = 1;
		}
	}
}

void Simulate::buildL() {
	// L is 3m * 3m matrix
	MatrixXd temp = MatrixXd::Zero(m, m);

	for (size_t i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];
		VectorXd Ai = VectorXd::Zero(m);
		Ai(index1) += 1;
		Ai(index2) -= 1;
		temp += stiff * Ai * Ai.transpose();
	}

	// kronecker product, L = kronecker(temp, I3)
	Matrix3d I3 = Matrix3d::Identity();
	for (int i = 0; i < m; i++)
		for (int j = 0; j < m; j++)
			L.block(i * 3, j * 3, 3, 3) = temp(i, j) * I3;
}

void Simulate::buildJ() {
	// J is 3m * 3s matrix
	MatrixXd temp = MatrixXd::Zero(m, s);

	for (size_t i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];
		VectorXd Ai = VectorXd::Zero(m);
		VectorXd Si = VectorXd::Zero(s);
		Ai(index1) += 1;
		Ai(index2) -= 1;
		Si(i) += 1;
		temp += stiff * Ai * Si.transpose();
	}

	// kronecker product, J = kronecker(temp, I3)
	Matrix3d I3 = Matrix3d::Identity();
	for (int i = 0; i < m; i++)
		for (int j = 0; j < s; j++)
			J.block(i * 3, j * 3, 3, 3) = temp(i, j) * I3;
}

void Simulate::local() {
	d = VectorXd::Ones(s * 3);

	for (size_t i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];
		Vector3d p1 = x.segment(3 * index1, 3);
		Vector3d p2 = x.segment(3 * index2, 3);

		d.segment(3 * i, 3) = l[i] * (p1 - p2) / (p1 - p2).norm();
	}

	cout << "Local Step" << endl;
}

void Simulate::global() {
	VectorXd RHS = K * (h * h * J * d + M * y + h * h * f_ext - (M + h * h * L) * b);
	VectorXd xf = LLT_.solve(RHS);
	x = K.transpose() * xf + b;
	cout << "Global Step" << endl;
}

void Simulate::getb() {
	b = x - K.transpose() * K * x;
}

void Simulate::UpdatePos() {
	for (int i = 0; i < m * 3; i++)
		positions[i / 3][i % 3] = x(i);
}

void Simulate::SimulateOnce() {
	if (!isfast) {
		buildX();
		buildV();
	} else {
		//update y, y = 2q_n - q_n-1
		y = 2 * x - x_pre;
		x_pre = x;
		size_t step = 0;
		while (step++ < iteration) {
			local();
			global();
		}
	}
	UpdatePos();
	cout << "Simulate Once" << endl;
}

bool Simulate::Run() {
	SimulateOnce();
	return true;
}

void Simulate::buildX() {
	std::vector<double> y(m * 3);
	for (int i = 0; i < x.size(); i++)
		y[i] = x(i) + h * velocity[i / 3][i % 3] + h * h / mass[i] * f_ext[i];
	xk = y;
	int i = 0;
	do {
		CalForce();
		GetGX();
		CalDiff();
		CalGxM();
		Eigen::MatrixXd t = G_inverse * gx_m;
		xk_1.clear();
		xk_1.resize(gx.size());
		for (int i = 0; i < gx.size(); i++)
			xk_1[i] = xk[i] - t(i, 0);
		i++;
		UpdateX();
		xk.resize(3 * m);
		for (int i = 0; i < m * 3; i++)
			xk[i] = x(i);
	} while (!isConv() && i <= 10);
}

void Simulate::UpdateX() {
	Eigen::MatrixXd xt;
	xt.resize((xk_1.size()), 1);
	for (int i = 0; i < xk_1.size(); i++)
		xt(i, 0) = xk_1[i];

	Eigen::MatrixXd t = K.transpose() * xt;
	for (int i = 0; i < x.size(); i++)
		x(i) = t(i, 0) + b[i];
}

void Simulate::CalGxM() {
	gx_m.resize(gx.size(), 1);
	for (int i = 0; i < gx.size(); i++)
		gx_m(i, 0) = gx[i];
}

bool Simulate::isConv() {
	double delta = 0.01;
	std::vector<double> zero(gx.size(), delta);
	for (int i = 0; i < gx.size(); i++)
		if (abs(gx[i]) > zero[i])
			return false;
	return true;
}

void Simulate::GetGX() {
	std::vector<double> y(xk.size());
	gx.resize(xk.size());

	for (int i = 0; i < xk.size(); i++)
		y[i] = xk[i] + h * velocity[i / 3][i % 3] + h * h / mass[i] * f_ext[i];

	for (int i = 0; i < xk.size(); i++)
		gx[i] = mass[i] * (xk[i] - y[i]) - h * h * f_int[i];

	Eigen::MatrixXd t(gx.size(), 1);
	for (int i = 0; i < gx.size(); i++)
		t(i, 0) = gx[i];

	t = K * t;

	gx.clear();
	gx.resize(t.rows());
	for (int i = 0; i < gx.size(); i++)
		gx[i] = t(i, 0);

	Eigen::MatrixXd xt;
	xt.resize((xk.size()), 1);

	for (int i = 0; i < xk.size(); i++)
		xt(i, 0) = xk[i];

	t = K * xt;
	xk.clear();
	xk.resize(t.rows());
	for (int i = 0; i < xk.size(); i++)
		xk[i] = t(i, 0);
}

void Simulate::CalDiff() {
	std::vector<Eigen::Triplet<double> > triple;
	Eigen::MatrixXd I = MatrixXd::Identity(3, 3);
	for (int i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];
		pointf3 v1 = pointf3(x(3 * index1 + 0), x(3 * index1 + 1), x(3 * index1 + 2));
		pointf3 v2 = pointf3(x(3 * index2 + 0), x(3 * index2 + 1), x(3 * index2 + 2));
		vecf3 r = v1 - v2;
		Vector3d t;
		t << r[0], r[1], r[2];

		Eigen::MatrixXd dif;
		dif.resize(3, 3);
		dif = stiff * (l[i] / r.norm() - 1) * I - stiff * l[i]
			/ ((r.norm()) * (r.norm()) * (r.norm())) * t * t.transpose();
		for (int j = 0; j < 3; j++)
			for (int k = 0; k < 3; k++) {
				triple.push_back(Eigen::Triplet<double>(3 * index1 + j, 3 * index1 + k, -h * h * dif(j, k)));
				triple.push_back(Eigen::Triplet<double>(3 * index2 + k, 3 * index2 + j, -h * h * dif(j, k)));
			}
	}

	for (int i = 0; i < m * 3; i++)
		triple.push_back(Eigen::Triplet<double>(i, i, mass[i]));

	Eigen::MatrixXd diff_;
	Eigen::SparseLU<Eigen::SparseMatrix<double>> LU_;

	diff.setZero();
	diff.resize(m * 3, m * 3);
	diff.setFromTriplets(triple.begin(), triple.end());

	diff_ = K * diff * K.transpose();
	diff = diff_.sparseView();

	I = MatrixXd::Identity(gx.size(), gx.size());
	LU_.analyzePattern(diff);
	LU_.factorize(diff);
	G_inverse = LU_.solve(I);
}

void Simulate::buildV() {
	for (int i = 0; i < x.size(); i++)
		velocity[i / 3][i % 3] = (x(i) - positions[i / 3][i % 3]) / h;
}

void Simulate::CalForce() {
	f_int.clear();
	f_int.resize(3 * positions.size());
	for (int i = 0; i < edgelist.size() / 2; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];
		pointf3 v1 = pointf3(x(3 * index1 + 0), x(3 * index1 + 1), x(3 * index1 + 2));
		pointf3 v2 = pointf3(x(3 * index2 + 0), x(3 * index2 + 1), x(3 * index2 + 2));
		vecf3 r = v1 - v2;
		for (int j = 0; j < 3; j++) {
			f_int[3 * index1 + j] += -stiff * (r.norm() - l[i]) * r[j] / r.norm();
			f_int[3 * index2 + j] += stiff * (r.norm() - l[i]) * r[j] / r.norm();
		}
	}
}


// Copyright 2004-5 The Trustees of Indiana University.
// Copyright 2002 Brad King and Douglas Gregor

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine

#ifndef BOOST_GRAPH_PAGE_RANK_HPP
#define BOOST_GRAPH_PAGE_RANK_HPP

#include <boost/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <vector>

namespace boost { namespace graph {

struct n_iterations
{
  explicit n_iterations(std::size_t n) : n(n) { }

  template<typename RankMap, typename Graph>
  bool
  operator()(const RankMap&, const Graph&)
  {
    return n-- == 0;
  }

 private:
  std::size_t n;
};

namespace detail {
  template<typename Graph, typename RankMap, typename RankMap2>
  void page_rank_step(const Graph& g, RankMap from_rank, RankMap2 to_rank,
                      typename property_traits<RankMap>::value_type damping,
                      incidence_graph_tag)
  {
    typedef typename property_traits<RankMap>::value_type rank_type;

    // Set new rank maps
    BGL_FORALL_VERTICES_T(v, g, Graph) put(to_rank, v, rank_type(1 - damping));

    BGL_FORALL_VERTICES_T(u, g, Graph) {
      rank_type u_rank_out = damping * get(from_rank, u) / out_degree(u, g);
      BGL_FORALL_ADJ_T(u, v, g, Graph)
        put(to_rank, v, get(to_rank, v) + u_rank_out);
    }
  }

  template<typename Graph, typename RankMap, typename RankMap2>
  void page_rank_step(const Graph& g, RankMap from_rank, RankMap2 to_rank,
                      typename property_traits<RankMap>::value_type damping,
                      bidirectional_graph_tag)
  {
    typedef typename property_traits<RankMap>::value_type damping_type;
    BGL_FORALL_VERTICES_T(v, g, Graph) {
      typename property_traits<RankMap>::value_type rank(0);
      BGL_FORALL_INEDGES_T(v, e, g, Graph)
        rank += get(from_rank, source(e, g)) / out_degree(source(e, g), g);
      put(to_rank, v, (damping_type(1) - damping) + damping * rank);
    }
  }
} // end namespace detail

template<typename Graph, typename RankMap, typename Done, typename RankMap2>
void
page_rank(const Graph& g, RankMap rank_map, Done done,
          typename property_traits<RankMap>::value_type damping,
          typename graph_traits<Graph>::vertices_size_type n,
          RankMap2 rank_map2)
{
  typedef typename property_traits<RankMap>::value_type rank_type;

  rank_type initial_rank = rank_type(rank_type(1) / n);
  BGL_FORALL_VERTICES_T(v, g, Graph) put(rank_map, v, initial_rank);

  bool to_map_2 = true;
  while ((to_map_2 && !done(rank_map, g)) ||
         (!to_map_2 && !done(rank_map2, g))) {
    typedef typename graph_traits<Graph>::traversal_category category;

    if (to_map_2) {
      detail::page_rank_step(g, rank_map, rank_map2, damping, category());
    } else {
      detail::page_rank_step(g, rank_map2, rank_map, damping, category());
    }
    to_map_2 = !to_map_2;
  }

  if (!to_map_2) {
    BGL_FORALL_VERTICES_T(v, g, Graph) put(rank_map, v, get(rank_map2, v));
  }
}

template<typename Graph, typename RankMap, typename Done>
void
page_rank(const Graph& g, RankMap rank_map, Done done,
          typename property_traits<RankMap>::value_type damping,
          typename graph_traits<Graph>::vertices_size_type n)
{
  typedef typename property_traits<RankMap>::value_type rank_type;

  std::vector<rank_type> ranks2(num_vertices(g));
  page_rank(g, rank_map, done, damping, n,
            make_iterator_property_map(ranks2.begin(), get(vertex_index, g)));
}

template<typename Graph, typename RankMap, typename Done>
inline void
page_rank(const Graph& g, RankMap rank_map, Done done,
          typename property_traits<RankMap>::value_type damping = 0.85)
{
  page_rank(g, rank_map, done, damping, num_vertices(g));
}

template<typename Graph, typename RankMap>
inline void
page_rank(const Graph& g, RankMap rank_map)
{
  page_rank(g, rank_map, n_iterations(20));
}

// TBD: this could be _much_ more efficient, using a queue to store
// the vertices that should be reprocessed and keeping track of which
// vertices are in the queue with a property map. Baah, this only
// applies when we have a bidirectional graph.
template<typename MutableGraph>
void
remove_dangling_links(MutableGraph& g)
{
  typename graph_traits<MutableGraph>::vertices_size_type old_n;
  do {
    old_n = num_vertices(g);

    typename graph_traits<MutableGraph>::vertex_iterator vi, vi_end;
    for (tie(vi, vi_end) = vertices(g); vi != vi_end; /* in loop */) {
      typename graph_traits<MutableGraph>::vertex_descriptor v = *vi++;
      if (out_degree(v, g) == 0) {
        clear_vertex(v, g);
        remove_vertex(v, g);
      }
    }
  } while (num_vertices(g) < old_n);
}

} } // end namespace boost::graph

#endif // BOOST_GRAPH_PAGE_RANK_HPP


#ifndef TVMTL_MANIFOLD_EUC_HPP
#define TVMTL_MANIFOLD_EUC_HPP

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

#include "enumerators.hpp"

namespace tvmtl {

// Specialization EUCLIDIAN
template < int N >
struct Manifold< EUCLIDIAN, N > {

    public:
	static const MANIFOLD_TYPE MyType;
	static const int manifold_dim ;
	static const int value_dim; // TODO: maybe rename to embedding_dim

	static const bool non_isometric_embedding;

	// Scalar type of manifold
	typedef double scalar_type;
	typedef double dist_type;
	typedef std::vector<double>						weight_list;

	// Value Typedef
	typedef Eigen::Matrix< scalar_type, N, 1>   value_type;
	typedef value_type&			    ref_type;
	typedef const value_type&		    cref_type;
	typedef std::vector<value_type, Eigen::aligned_allocator<value_type> >	value_list;

	// Tangent space typedefs
	typedef Eigen::Matrix < scalar_type, N, N> tm_base_type;
	typedef tm_base_type& tm_base_ref_type;

	// Derivative Typedefs
	typedef value_type			     deriv1_type;
	typedef deriv1_type&			     deriv1_ref_type;

	typedef Eigen::Matrix<scalar_type, N, N>     deriv2_type;
	typedef deriv2_type&			     deriv2_ref_type;
	typedef	Eigen::Matrix<scalar_type, N, N>     restricted_deriv2_type;

	// Manifold distance functions (for IRLS)
	inline static dist_type dist_squared(cref_type x, cref_type y);
	inline static void deriv1x_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);
	inline static void deriv1y_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);

	inline static void deriv2xx_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2xy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2yy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);

	// Manifold exponentials und logarithms ( for Proximal point)
	template <typename DerivedX, typename DerivedY, typename DerivedZ>
	inline static void exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedZ>& result);
	inline static void log(cref_type x, cref_type y, ref_type result);

	inline static void convex_combination(cref_type x, cref_type y, double t, ref_type result);

	// Implementations of the Karcher mean
	// Slow list version
	inline static void karcher_mean(ref_type x, const value_list& v, double tol=1e-10, int maxit=15);
	inline static void weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol=1e-10, int maxit=15);
	// Variadic templated version
	template <typename V, class... Args>
	inline static void karcher_mean(V& x, const Args&... args);
	template <typename V>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y);
	template <typename V, class... Args>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y1, const Args&... args);

	// Basis transformation for restriction to tangent space
	inline static void tangent_plane_base(cref_type x, tm_base_ref_type result);

	// Projection to manifold
	inline static void projector(ref_type x);

	// Interpolation pre- and postprocessing
	inline static void interpolation_preprocessing(ref_type x) {};
	inline static void interpolation_postprocessing(ref_type x) {};

};

/*-----IMPLEMENTATION EUCLIDIAN----------*/

// Static constants, Outside definition to avoid linker error

template <int N>
const MANIFOLD_TYPE Manifold < EUCLIDIAN, N>::MyType = EUCLIDIAN;

template <int N>
const int Manifold < EUCLIDIAN, N>::manifold_dim = N;

template <int N>
const int Manifold < EUCLIDIAN, N>::value_dim = N;

template <int N>
const bool Manifold < EUCLIDIAN, N>::non_isometric_embedding = false;

// Squared Euclidian distance function
template <int N>
inline typename Manifold < EUCLIDIAN, N>::dist_type Manifold < EUCLIDIAN, N>::dist_squared( cref_type x, cref_type y ){
    //value_type v = x-y;
    return (x-y).squaredNorm();
}

// Derivative of Squared Euclidian distance w.r.t. first argument
template <int N>
inline void Manifold < EUCLIDIAN, N>::deriv1x_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    result =  2 * (x-y);
}
// Derivative of Squared Euclidian distance w.r.t. second argument
template <int N>
inline void Manifold < EUCLIDIAN, N>::deriv1y_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    result = 2 * (y-x);
}

// Second Derivative of Squared Euclidian distance w.r.t first argument
template <int N>
inline void Manifold < EUCLIDIAN, N>::deriv2xx_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    result = 2 * deriv2_type::Identity();
}
// Second Derivative of Squared Euclidian distance w.r.t first and second argument
template <int N>
inline void Manifold < EUCLIDIAN, N>::deriv2xy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    result = -2 * deriv2_type::Identity();
}
// Second Derivative of Squared Euclidian distance w.r.t second argument
template <int N>
inline void Manifold < EUCLIDIAN, N>::deriv2yy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    result = 2 * deriv2_type::Identity();
}

// Exponential and Logarithm Map
template <int N>
template <typename DerivedX, typename DerivedY, typename DerivedZ>
inline void Manifold <EUCLIDIAN, N>::exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedZ>& result){
    result=x+y;
}

template <int N>
inline void Manifold <EUCLIDIAN, N>::log(cref_type x, cref_type y, ref_type result){
    result = y-x;
}

// Tangent Plane restriction
template <int N>
inline void Manifold <EUCLIDIAN, N>::tangent_plane_base(cref_type x, tm_base_ref_type result){
    result = tm_base_type::Identity();
}

// Projector, cut off values outside [0,1] (if noise is added)
template <int N>
inline void Manifold <EUCLIDIAN, N>::projector(ref_type x){
    for(int i=0; i<N; ++i){
	if(x[i] > 1.0) x[i] = 1.0;
	if(x[i] < 0) x[i] = 0;
    }
}

// Convex combination along geodesic
template <int N>
inline void Manifold <EUCLIDIAN, N>::convex_combination(cref_type x, cref_type y, double t, ref_type result){
    result = x + t * (y-x);
}

// Karcher mean implementations
template <int N>
inline void Manifold <EUCLIDIAN, N>::karcher_mean(ref_type x, const value_list& v, double tol, int maxit){
    value_type L = value_type::Zero();
    for(int i = 0; i < v.size(); ++i)
	L += v[i];
    x = L / v.size();
}

template <int N>
inline void Manifold <EUCLIDIAN, N>::weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol, int maxit){
    value_type L = value_type::Zero();
    for(int i = 0; i < v.size(); ++i)
	L += w[i]*v[i];
    x = L / v.size();
}

template <int N>
template <typename V, class... Args>
inline void Manifold<EUCLIDIAN, N>::karcher_mean(V& x, const Args&... args){
    int numArgs = sizeof...(args);
    variadic_karcher_mean_gradient(x, args...);
    x /= numArgs;
}

template <int N>
template <typename V>
inline void Manifold<EUCLIDIAN, N>::variadic_karcher_mean_gradient(V& x, const V& y){
    x = y;
}

template <int N>
template <typename V, class... Args>
inline void Manifold<EUCLIDIAN, N>::variadic_karcher_mean_gradient(V& x, const V& y1, const Args& ... args){
    V temp = x;
    variadic_karcher_mean_gradient(temp, args...);
    x = y1 + temp;
}

} // end namespace tvmtl

#endif


#include <fstream>
#include <iostream>
#include <unordered_set>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/graphviz.hpp>

class ShortestPathHeuristic {
 public:
  ShortestPathHeuristic(size_t dimx, size_t dimy,
                        const std::unordered_set<Location>& obstacles)
      : m_shortestDistance(nullptr), m_dimx(dimx), m_dimy(dimy) {
    searchGraph_t searchGraph;

    // add vertices
    for (size_t x = 0; x < dimx; ++x) {
      for (size_t y = 0; y < dimy; ++y) {
        boost::add_vertex(searchGraph);
      }
    }

    // add edges
    for (size_t x = 0; x < dimx; ++x) {
      for (size_t y = 0; y < dimy; ++y) {
        Location l(x, y);
        if (obstacles.find(l) == obstacles.end()) {
          Location right(x + 1, y);
          if (x < dimx - 1 && obstacles.find(right) == obstacles.end()) {
            auto e =
                boost::add_edge(locToVert(l), locToVert(right), searchGraph);
            searchGraph[e.first].weight = 1;
          }
          Location below(x, y + 1);
          if (y < dimy - 1 && obstacles.find(below) == obstacles.end()) {
            auto e =
                boost::add_edge(locToVert(l), locToVert(below), searchGraph);
            searchGraph[e.first].weight = 1;
          }
        }
      }
    }

    writeDotFile(searchGraph, "searchGraph.dot");

    m_shortestDistance = new distanceMatrix_t(boost::num_vertices(searchGraph));
    distanceMatrixMap_t distanceMap(*m_shortestDistance, searchGraph);
    // The following generates a clang-tidy error, see
    // https://svn.boost.org/trac10/ticket/10830
    boost::floyd_warshall_all_pairs_shortest_paths(
        searchGraph, distanceMap,
        boost::weight_map(boost::get(&Edge::weight, searchGraph)));
  }

  ~ShortestPathHeuristic() { delete m_shortestDistance; }

  int getValue(const Location& a, const Location& b) {
    vertex_t idx1 = locToVert(a);
    vertex_t idx2 = locToVert(b);
    return (*m_shortestDistance)[idx1][idx2];
  }

 private:
  size_t locToVert(const Location& l) const { return l.x + m_dimx * l.y; }

  Location idxToLoc(size_t idx) {
    int x = idx % m_dimx;
    int y = idx / m_dimx;
    return Location(x, y);
  }

 private:
  typedef boost::adjacency_list_traits<boost::vecS, boost::vecS,
                                       boost::undirectedS>
      searchGraphTraits_t;
  typedef searchGraphTraits_t::vertex_descriptor vertex_t;
  typedef searchGraphTraits_t::edge_descriptor edge_t;

  struct Vertex {};

  struct Edge {
    int weight;
  };

  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
                                Vertex, Edge>
      searchGraph_t;
  typedef boost::exterior_vertex_property<searchGraph_t, int>
      distanceProperty_t;
  typedef distanceProperty_t::matrix_type distanceMatrix_t;
  typedef distanceProperty_t::matrix_map_type distanceMatrixMap_t;

  class VertexDotWriter {
   public:
    explicit VertexDotWriter(const searchGraph_t& graph, size_t dimx)
        : m_graph(graph), m_dimx(dimx) {}

    void operator()(std::ostream& out, const vertex_t& v) const {
      static const float DX = 100;
      static const float DY = 100;
      out << "[label=\"";
      int x = v % m_dimx;
      int y = v / m_dimx;
      out << "\" pos=\"" << x * DX << "," << y * DY << "!\"]";
    }

   private:
    const searchGraph_t& m_graph;
    size_t m_dimx;
  };

  class EdgeDotWriter {
   public:
    explicit EdgeDotWriter(const searchGraph_t& graph) : m_graph(graph) {}

    void operator()(std::ostream& out, const edge_t& e) const {
      out << "[label=\"" << m_graph[e].weight << "\"]";
    }

   private:
    const searchGraph_t& m_graph;
  };

 private:
  void writeDotFile(const searchGraph_t& graph, const std::string& fileName) {
    VertexDotWriter vw(graph, m_dimx);
    EdgeDotWriter ew(graph);
    std::ofstream dotFile(fileName);
    boost::write_graphviz(dotFile, graph, vw, ew);
  }

 private:
  distanceMatrix_t* m_shortestDistance;
  size_t m_dimx;
  size_t m_dimy;
};


/// @file
/// @copyright The code is licensed under the BSD License
///            <http://opensource.org/licenses/BSD-2-Clause>,
///            Copyright (c) 2013-2015 Alexandre Hamez.
/// @author Alexandre Hamez

#pragma once

#include <string>
#include <set>
#include <unordered_map>
#include <vector>

#include <boost/variant.hpp>

#include "support/pn/types.hh"
#include "support/properties/formulae.hh"

namespace pnmc { namespace mc { namespace classic {

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

struct integer_constant
{
  int value;
};
struct integer_sum;
struct integer_product;
struct integer_difference;
struct integer_division;
struct integer_tokens
{
  std::size_t pos;
};

using integer_ast = boost::variant< integer_constant
                                  , boost::recursive_wrapper<integer_sum>
                                  , boost::recursive_wrapper<integer_product>
                                  , boost::recursive_wrapper<integer_difference>
                                  , boost::recursive_wrapper<integer_division>
                                  , integer_tokens>;

struct integer_sum
{
  std::vector<integer_ast> expressions;
};

struct integer_product
{
  std::vector<integer_ast> expressions;
};

struct integer_difference
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

struct integer_division
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

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

struct invariant;
struct impossibility;
struct possibility;

struct true_ {};
struct false_ {};
struct negation;
struct conjunction;
struct disjunction;
struct exclusive_disjonction;
struct implication;
struct equivalence;

struct integer_eq;
struct integer_ne;
struct integer_lt;
struct integer_le;
struct integer_gt;
struct integer_ge;

using boolean_ast = boost::variant< boost::recursive_wrapper<invariant>
                                  , boost::recursive_wrapper<impossibility>
                                  , boost::recursive_wrapper<possibility>
                                  , true_
                                  , false_
                                  , boost::recursive_wrapper<negation>
                                  , boost::recursive_wrapper<conjunction>
                                  , boost::recursive_wrapper<disjunction>
                                  , boost::recursive_wrapper<exclusive_disjonction>
                                  , boost::recursive_wrapper<implication>
                                  , boost::recursive_wrapper<equivalence>
                                  , boost::recursive_wrapper<integer_eq>
                                  , boost::recursive_wrapper<integer_ne>
                                  , boost::recursive_wrapper<integer_lt>
                                  , boost::recursive_wrapper<integer_le>
                                  , boost::recursive_wrapper<integer_gt>
                                  , boost::recursive_wrapper<integer_ge>>;

struct invariant
{
  boolean_ast expression;
};

struct impossibility
{
  boolean_ast expression;
};

struct possibility
{
  boolean_ast expression;
};

struct negation
{
  boolean_ast expression;
};

struct conjunction
{
  std::vector<boolean_ast> expressions;
};

struct disjunction
{
  std::vector<boolean_ast> expressions;
};

struct exclusive_disjonction
{
  std::vector<boolean_ast> expressions;
};

struct implication
{
  boolean_ast lhs_expression;
  boolean_ast rhs_expression;
};

struct equivalence
{
  boolean_ast lhs_expression;
  boolean_ast rhs_expression;
};

struct integer_eq
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

struct integer_ne
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

struct integer_lt
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

struct integer_le
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

struct integer_gt
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

struct integer_ge
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

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

integer_ast
make_ast( const properties::integer_expression&
        , const std::unordered_map<std::string, pn::valuation_type>& bounds);

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

boolean_ast
make_ast( const properties::boolean_expression&, bool has_deadlock
        , const std::set<std::string>& dead_transitions
        , const std::unordered_map<std::string, pn::valuation_type>& bounds);

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

}}} // namespace pnmc::mc::classic


#ifndef KALMAN_FILTER_MULTIARM_BANDIT_HPP
#define KALMAN_FILTER_MULTIARM_BANDIT_HPP

#include <assert.h>
#include <vector>
#include <random>
#include <utility>

#include <Eigen/Eigenvalues>

#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/pretty_print.hpp>
#include <arc_utilities/eigen_helpers.hpp>

namespace smmap
{
    template <typename Generator = std::mt19937_64>
    class KalmanFilterMANB
    {
        public:
            KalmanFilterMANB(
                    const Eigen::VectorXd& prior_mean = Eigen::VectorXd::Zero(1),
                    const Eigen::VectorXd& prior_var = Eigen::VectorXd::Ones(1))
                : num_bandits_(prior_mean.rows())
                , arm_mean_(prior_mean)
                , arm_var_(prior_var)
            {
                assert(arm_mean_.cols() == arm_var_.cols());
            }

            /**
             * @brief selectArmToPull Perform Thompson sampling on the bandits,
             *                        and select the bandit with the largest sample.
             * @param generator
             * @return
             */
            ssize_t selectArmToPull(Generator& generator)
            {
                // Sample from the current distribuition
                std::normal_distribution<double> normal_dist(0.0, 1.0);

                ssize_t best_arm = -1;
                double best_sample = -std::numeric_limits<double>::infinity();
                for (ssize_t arm_ind = 0; arm_ind < num_bandits_; arm_ind++)
                {
                    const double sample = std::sqrt(arm_var_(arm_ind)) * normal_dist(generator) + arm_mean_(arm_ind);

                    if (sample > best_sample)
                    {
                        best_arm = arm_ind;
                        best_sample = sample;
                    }
                }

                assert(best_arm >= 0);
                return best_arm;
            }

            bool generateAllModelActions() const
            {
                return false;
            }

            /**
             * @brief updateArms
             * @param transition_variance
             * @param arm_pulled
             * @param observed_reward
             * @param observation_variance
             */
            void updateArms(
                    const Eigen::VectorXd& transition_variance,
                    const ssize_t arm_pulled,
                    const double observed_reward,
                    const double observation_variance)
            {
                for (ssize_t arm_ind = 0; arm_ind < num_bandits_; arm_ind++)
                {
                    if (arm_ind != arm_pulled)
                    {
                        arm_var_(arm_ind) += transition_variance(arm_ind);
                    }
                    else
                    {
                        arm_mean_(arm_ind) = ((arm_var_(arm_ind) + transition_variance(arm_ind)) * observed_reward + observation_variance * arm_mean_(arm_ind))
                                            / (arm_var_(arm_ind) + transition_variance(arm_ind) + observation_variance);

                        arm_var_(arm_ind) = (arm_var_(arm_ind) + transition_variance(arm_ind)) * observation_variance
                                           / (arm_var_(arm_ind) + transition_variance(arm_ind) + observation_variance);
                    }
                }
            }

            const Eigen::VectorXd& getMean() const
            {
                return arm_mean_;
            }

            Eigen::VectorXd getMean()
            {
                return arm_mean_;
            }

            const Eigen::VectorXd& getVariance() const
            {
                return arm_var_;
            }

            Eigen::VectorXd getVariance()
            {
                return arm_var_;
            }

        private:
            ssize_t num_bandits_;

            Eigen::VectorXd arm_mean_;
            Eigen::VectorXd arm_var_;
    };

    template<typename Generator = std::mt19937_64>
    class KalmanFilterMANDB
    {
        public:
            KalmanFilterMANDB(
                    const Eigen::VectorXd& prior_mean = Eigen::VectorXd::Ones(1),
                    const Eigen::MatrixXd& prior_covar = Eigen::MatrixXd::Identity(1, 1))
                : arm_mean_(prior_mean)
                , arm_covar_(prior_covar)
            {
                assert(arm_covar_.rows() == arm_covar_.cols());
                assert(arm_covar_.rows() == arm_mean_.rows());
            }

            /**
             * @brief selectArmToPull Perform Thompson sampling on the bandits,
             *                        and select the bandit with the largest sample.
             * @param generator
             * @return
             */
            ssize_t selectArmToPull(Generator& generator)
            {
                // Sample from the current distribuition
                arc_helpers::MultivariteGaussianDistribution distribution(arm_mean_, arm_covar_);
                const Eigen::VectorXd sample = distribution(generator);

                // Find the arm with the highest sample
                ssize_t best_arm = -1;
                sample.maxCoeff(&best_arm);

                return best_arm;
            }

            bool generateAllModelActions() const
            {
                return true;
            }

            /**
             * @brief updateArms
             * @param transition_covariance
             * @param arm_pulled
             * @param obs_reward
             * @param obs_var
             */
            void updateArms(
                    const Eigen::MatrixXd& transition_covariance,
                    const Eigen::MatrixXd& observation_matrix,
                    const Eigen::VectorXd& observed_reward,
                    const Eigen::MatrixXd& observation_covariance)
            {
                #pragma GCC diagnostic push
                #pragma GCC diagnostic ignored "-Wconversion"
                const Eigen::MatrixXd& C = observation_matrix;

                // Kalman predict
                const Eigen::VectorXd& predicted_mean = arm_mean_;                      // No change to mean
                const auto predicted_covariance = arm_covar_ + transition_covariance;   // Add process noise

                // Kalman update - symbols from wikipedia article
                const auto innovation = observed_reward - C * predicted_mean;                                            // tilde y_k
                const auto innovation_covariance = C * predicted_covariance.selfadjointView<Eigen::Lower>() * C.transpose() + observation_covariance;    // S_k
                const auto kalman_gain = predicted_covariance.selfadjointView<Eigen::Lower>() * C.transpose() * innovation_covariance.inverse();         // K_k

                arm_mean_ = predicted_mean + kalman_gain * innovation;                                                              // hat x_k|k
                arm_covar_ = predicted_covariance - kalman_gain * C * predicted_covariance.selfadjointView<Eigen::Lower>();         // P_k|k
                #pragma GCC diagnostic pop

                // Numerical problems fixing
                arm_covar_ = ((arm_covar_ + arm_covar_.transpose()) * 0.5).selfadjointView<Eigen::Lower>();

                assert(!(arm_mean_.unaryExpr([] (const double &val) { return std::isnan(val); })).any() && "NaN Found in arm_mean_ in kalman banidt!");
                assert(!(arm_mean_.unaryExpr([] (const double &val) { return std::isinf(val); })).any() && "Inf Found in arm_mean_ in kalman banidt!");
                assert(!(arm_covar_.unaryExpr([] (const double &val) { return std::isinf(val); })).any() && "NaN Found in arm_covar_ in kalman bandit!");
                assert(!(arm_covar_.unaryExpr([] (const double &val) { return std::isinf(val); })).any() && "Inf Found in arm_covar_ in kalman bandit!");
            }

            const Eigen::VectorXd& getMean() const
            {
                return arm_mean_;
            }

            Eigen::VectorXd getMean()
            {
                return arm_mean_;
            }

            const Eigen::MatrixXd& getCovariance() const
            {
                return arm_covar_;
            }

            Eigen::MatrixXd getCovariance()
            {
                return arm_covar_;
            }

        private:
            Eigen::VectorXd arm_mean_;
            Eigen::MatrixXd arm_covar_;
    };
}

#endif // KALMAN_FILTER_MULTIARM_BANDIT_HPP


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_TENPOWER_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_TENPOWER_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/ten.hpp>
#include <boost/simd/function/simd/abs.hpp>
#include <boost/simd/function/simd/abs.hpp>
#include <boost/simd/function/simd/any.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/is_ltz.hpp>
#include <boost/simd/function/simd/is_odd.hpp>
#include <boost/simd/function/simd/multiplies.hpp>
#include <boost/simd/function/simd/rec.hpp>
#include <boost/simd/function/simd/shift_right.hpp>
#include <boost/simd/function/simd/sqr.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/mpl/equal_to.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(tenpower_
                             , (typename A0, typename X)
                             , bd::cpu_
                             , bs::pack_<bd::int_<A0>, X>
                             )
   {
      using result = bd::as_floating_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        result res = One<result>();
        result base = Ten<result>();
        A0 exp = bs::abs(a0);
        while(any(exp))
        {
          //       res *= if_else(is_odd(exp), base, One<result>()); TO DO
          res =  res * if_else(is_odd(exp), base, One<result>());
          //  exp >>= 1; TODO
          exp =  shift_right(exp, 1);
          base = sqr(base);
        }
        return if_else(is_ltz(a0), bs::rec(res), res);
      }
   };

   BOOST_DISPATCH_OVERLOAD(tenpower_
                             , (typename A0, typename X)
                             , bd::cpu_
                             , bs::pack_<bd::uint_<A0>, X>
                             )
   {
      using result = bd::as_floating_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        result res = One<result>();
        result base = Ten<result>();
        A0 exp = a0;
        while(any(exp))
        {
          res = res*if_else(is_odd(exp), base, One<result>()); // TODO
//          res *= if_else(is_odd(exp), base, One<result>());
          //  exp >>= 1; TODO
          exp =  shift_right(exp, 1);
          base = sqr(base);
        }
        return res;
      }
   };

} } }

#endif


#ifndef MATH_DUAL_HPP
#define MATH_DUAL_HPP

#ifndef _E_of
#define _E_of(X) X "E"
//#define _E_of(X) X "\u0190"
#endif

#include <boost/operators.hpp>

namespace Math {
	template<typename>
		struct quat;

	template<typename R = float>
	struct dual: public boost::operators<dual<R>> {
		quat<R> u, v;

		/** Additive inverse */
		dual<R> operator-(void) const;
		/** Multiplicative inverse */
		dual<R> operator!(void) const;
		/** Distributes conjugation to members */
		dual<R> operator~(void) const;
		/** Cast operator, as Euclidean norm */
		explicit operator R(void) const;
		/** Squared Euclidean norm */
		R operator()(void) const;
		/** Distributes equality test */
		bool operator==(dual<R> const &rhs) const;
		/** Apply (lhs * rhs * ~lhs) */
		dual<R> operator()(quat<R> const &rhs) const;
		dual<R> operator()(dual<R> const &rhs) const;

		dual<R>& operator=(quat<R> const &rhs) {
			u = rhs; v = 0;
			return *this;
		}
		dual<R>& operator=(dual<R> const &rhs) = default;
		dual<R>& operator+=(R const &rhs) {
			u.w += rhs;
			return *this;
		}
		dual<R>& operator+=(dual<R> const &rhs) {
			u += rhs.u;
			v += rhs.v;
			return *this;
		}
		dual<R>& operator-=(dual<R> const &rhs) {
			u -= rhs.u;
			v -= rhs.v;
			return *this;
		}
		dual<R>& operator*=(dual<R> const &rhs) {
			auto const& p = rhs.u, q = rhs.v;
 			auto const r = u, s = v;
			return *this = {
				r.w*p.w - r.x*p.x - r.y*p.y - r.z*p.z,
				r.w*p.x + r.x*p.w + r.y*p.z - r.z*p.y,
				r.w*p.y - r.x*p.z + r.y*p.w + r.z*p.x,
				r.w*p.z + r.x*p.y - r.y*p.x + r.z*p.w,
				s.w*p.w - s.x*p.x - s.y*p.y - s.z*p.z
					+ r.w*q.w - r.x*q.x - r.y*q.y - r.z*q.z,
				s.x*p.w + s.w*p.x - s.z*p.y + s.y*p.z
					+ r.x*q.w + r.w*q.x - r.z*q.y + r.y*q.z,
				s.y*p.w + s.z*p.x + s.w*p.y - s.x*p.z
					+ r.y*q.w + r.z*q.x + r.w*q.y - r.x*q.z,
				s.z*p.w - s.y*p.x + s.x*p.y + s.w*p.z
					+ r.z*q.w - r.y*q.x + r.x*q.y + r.w*q.z
			};
		}
		dual<R>& operator*=(quat<R> const &rhs) {
			u *= rhs;
			v *= rhs;
			return *this;
		}
		dual<R>& operator*=(R const &rhs) {
			u *= rhs;
			v *= rhs;
			return *this;
		}
		dual<R>& operator/=(R const &rhs) {
			u /= rhs;
			v /= rhs;
			return *this;
		}

		dual(void) = default;
		dual(dual<R> const&) = default;
		dual(dual<R> &&) = default;
		dual(R uw, R ux = 0, R uy = 0, R uz = 0,
				R vw = 0, R vx = 0, R vy = 0, R vz = 0):
			u(uw, ux, uy, uz), v(vw, vx, vy, vz) {}
		dual(const quat<R> &u, const quat<R> v = {0}):
			u(u), v(v) {}
	};

	template<typename R>
	dual<R> dual<R>::operator-(void) const {
		return {-u, -v};
	}
	template<typename R>
	dual<R> dual<R>::operator!(void) const {
		dual<R> conj = ~*this;
		return conj/conj();
	}
	template<typename R>
	dual<R> dual<R>::operator~(void) const {
		return {{u.w,-u.x,-u.y,-u.z},
			{-v.w, v.x, v.y, v.z}};
	}
	template<typename R>
	dual<R>::operator R(void) const {
		return sqrt((*this)());
	}
	template<typename R>
	R dual<R>::operator()(void) const {
		return u()+v();
	}

	template<typename R>
	bool dual<R>::operator==(dual<R> const& rhs) const {
		return u == rhs.u && v == rhs.v;
	}
	template<typename R>
	dual<R> dual<R>::operator()(quat<R> const& rhs) const {
		return *this * rhs * ~*this;
	}
	template<typename R>
	dual<R> dual<R>::operator()(dual<R> const& rhs) const {
		return *this * rhs * ~*this;
	}
}

#endif


#include <CGAL/Cartesian.h>
#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_2.h>
#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_traits_2.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel_with_sqrt.h>
#include <CGAL/Hyperbolic_octagon_translation.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Periodic_2_Delaunay_triangulation_traits_2.h>
#include <CGAL/Periodic_2_Delaunay_triangulation_2.h>
#include <CGAL/determinant.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Timer.h>

#include <boost/tuple/tuple.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/variate_generator.hpp>

typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_traits_2<>           Traits;
typedef Traits::FT                                                              NT;
typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_2<Traits>            Triangulation;
typedef CGAL::Hyperbolic_octagon_translation_matrix<NT>                         Octagon_matrix;
typedef Triangulation::Point                                                    Point;
typedef Triangulation::Vertex_handle                                            Vertex_handle;
typedef Traits::Side_of_original_octagon                                        Side_of_original_octagon;

typedef CGAL::Cartesian<double>::Point_2                                        Point_double;
typedef CGAL::Creator_uniform_2<double, Point_double >                          Creator;

typedef CGAL::Exact_predicates_exact_constructions_kernel_with_sqrt             EKernel;
typedef CGAL::Delaunay_triangulation_2<EKernel>                                 Euclidean_triangulation;
typedef CGAL::Periodic_2_Delaunay_triangulation_traits_2<EKernel>               Ptraits;
typedef CGAL::Periodic_2_Delaunay_triangulation_2<Ptraits>                      PEuclidean_triangulation;

typedef double                                                                  dNT;
typedef CGAL::Cartesian<dNT>                                                    dKernel;
typedef CGAL::Delaunay_triangulation_2<dKernel>                                 dTriangulation;

int main(int argc, char** argv)
{
  int N, iters;
  iters = 1;
  if(argc < 2)
  {
    std::cout << "usage: " << argv[0] << " [number_of_points_to_insert] [optional: number_of_iterations]" << std::endl;
    std::cout << "Defaulting to values: 10000000, 10..." << std::endl;
    N = 1000000;
    iters = 10;
  } else {
    N = atoi(argv[1]);
    if (argc < 3)
      iters = 1;
    else
      iters = atoi(argv[2]);
  }

  Side_of_original_octagon pred;

  std::cout << "---- for best results, make sure that you have compiled me in Release mode ----" << std::endl;

  double extime1 = 0.0;
  double extime2 = 0.0;
  double extime3 = 0.0;

  for(int exec = 1; exec <= iters; ++exec)
  {
    std::vector<Point> pts;
    std::vector<Point_double> dpts;
    CGAL::Random_points_in_disc_2<Point_double, Creator> g(0.85);

    int cnt = 0;
    std::cout << "================ iteration " << exec << " : generating points ================" << std::endl;
    do
    {
      Point_double pd = *(++g);
      Point pt = Point(pd.x(), pd.y());
      if(pred(pt) != CGAL::ON_UNBOUNDED_SIDE)
      {
        pts.push_back(pt);
        dpts.push_back(pd);
        ++cnt;
      }
    }
    while(cnt < N);

    if(cnt < N)
    {
      std::cout << "Failed to generate all the random points! Exiting..." << std::endl;
      return -1;
    }

    std::cout << "Inserting into hyperbolic periodic  CORE  triangulation...    "; std::cout.flush();
    Triangulation tr;
    CGAL::Timer t1;
    t1.start();
    tr.insert(pts.begin(), pts.end());
    t1.stop();
    extime1 += t1.time();
    std::cout << "DONE! (# of vertices = " << tr.number_of_vertices() << ", time = " << t1.time() << " secs)" << std::endl;

    std::cout << "inserting into Euclidean non-periodic  CORE  triangulation... "; std::cout.flush();
    Euclidean_triangulation etr;
    CGAL::Timer t2;
    t2.start();
    etr.insert(pts.begin(), pts.end());
    t2.stop();
    extime2 += t2.time();
    std::cout << "DONE! (# of vertices = " << etr.number_of_vertices() << ", time = " << t2.time() << " secs)" << std::endl;

    std::cout << "Inserting into Euclidean non-periodic DOUBLE triangulation... "; std::cout.flush();
    dTriangulation dtr;
    CGAL::Timer t3;
    t3.start();
    dtr.insert(dpts.begin(), dpts.end());
    t3.stop();
    extime3 += t3.time();
    std::cout << "DONE! (# of vertices = " << dtr.number_of_vertices() << ", time = " << t3.time() << " secs)" << std::endl;
  }

  double diters(iters);
  extime1 /= diters;
  extime2 /= diters;
  extime3 /= diters;

  std::cout << "Hyperbolic periodic      CORE  triangulation: average time = " << extime1 << std::endl;
  std::cout << "Euclidean  non-periodic  CORE  triangulation: average time = " << extime2 << std::endl;
  std::cout << "Euclidean  non-periodic DOUBLE triangulation: average time = " << extime3 << std::endl;

  return EXIT_SUCCESS;
}


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

/*
 Copyright (C) 2014 Jose Aparicio

 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.
*/

#include <ql/experimental/math/convolvedstudentt.hpp>
#include <ql/errors.hpp>
#include <ql/math/factorial.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/math/functional.hpp>

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/math/distributions/students_t.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

namespace QuantLib {

    CumulativeBehrensFisher::CumulativeBehrensFisher(const std::vector<Integer>& degreesFreedom,
                                                     const std::vector<Real>& factors)
    : degreesFreedom_(degreesFreedom), factors_(factors), polyConvolved_(std::vector<Real>(1, 1.))

    {
        QL_REQUIRE(degreesFreedom.size() == factors.size(),
            "Incompatible sizes in convolution.");
        for (int i : degreesFreedom) {
            QL_REQUIRE(i % 2 != 0, "Even degree of freedom not allowed");
            QL_REQUIRE(i >= 0, "Negative degree of freedom not allowed");
        }
        for(Size i=0; i<degreesFreedom_.size(); i++)
            polynCharFnc_.push_back(polynCharactT((degreesFreedom[i]-1)/2));
        // adjust the polynomial coefficients by the factors in the linear
        //   combination:
        for(Size i=0; i<degreesFreedom_.size(); i++) {
            Real multiplier = 1.;
            for(Size k=1; k<polynCharFnc_[i].size(); k++) {
                multiplier *= std::abs(factors_[i]);
                polynCharFnc_[i][k] *= multiplier;
            }
        }
        //convolution, here it is a product of polynomials and exponentials
        for (auto& i : polynCharFnc_)
            polyConvolved_ = convolveVectorPolynomials(polyConvolved_, i);
        // trim possible zeros that might have arised:
        auto it = polyConvolved_.rbegin();
        while (it != polyConvolved_.rend()) {
            if (*it == 0.) {
                polyConvolved_.pop_back();
                it = polyConvolved_.rbegin();
              }else{
                  break;
              }
          }
          // cache 'a' value (the exponent)
          for(Size i=0; i<degreesFreedom_.size(); i++)
              a_ += std::sqrt(static_cast<Real>(degreesFreedom_[i]))
                * std::abs(factors_[i]);
          a2_ = a_ * a_;
    }

    std::vector<Real> CumulativeBehrensFisher::polynCharactT(Natural n) const {
        Natural nu = 2 * n +1;
        std::vector<Real> low(1,1.), high(1,1.);
        high.push_back(std::sqrt(static_cast<Real>(nu)));
        if(n==0) return low;
        if(n==1) return high;

        for(Size k=1; k<n; k++) {
            std::vector<Real> recursionFactor(1,0.); // 0 coef
            recursionFactor.push_back(0.); // 1 coef
            recursionFactor.push_back(nu/((2.*k+1.)*(2.*k-1.))); // 2 coef
            std::vector<Real> lowUp =
                convolveVectorPolynomials(recursionFactor, low);
            //add them up:
            for(Size i=0; i<high.size(); i++)
                lowUp[i] += high[i];
            low = high;
            high = lowUp;
        }
        return high;
    }

    std::vector<Real> CumulativeBehrensFisher::convolveVectorPolynomials(
        const std::vector<Real>& v1,
        const std::vector<Real>& v2) const {
    #if defined(QL_EXTRA_SAFETY_CHECKS)
        QL_REQUIRE(!v1.empty() && !v2.empty(),
            "Incorrect vectors in polynomial.");
    #endif

        const std::vector<Real>& shorter = v1.size() < v2.size() ? v1 : v2;
        const std::vector<Real>& longer = (v1 == shorter) ? v2 : v1;

        Size newDegree = v1.size()+v2.size()-2;
        std::vector<Real> resultB(newDegree+1, 0.);
        for(Size polyOrdr=0; polyOrdr<resultB.size(); polyOrdr++) {
            for(Size i=std::max<Integer>(0, polyOrdr-longer.size()+1);
                i<=std::min(polyOrdr, shorter.size()-1); i++)
                resultB[polyOrdr] += shorter[i]*longer[polyOrdr-i];
        }
        return resultB;
    }

    Probability CumulativeBehrensFisher::operator()(const Real x) const {
        // 1st & 0th terms with the table integration
        Real integral = polyConvolved_[0] * std::atan(x/a_);
        Real squared = a2_ + x*x;
        Real rootsqr = std::sqrt(squared);
        Real atan2xa = std::atan2(-x,a_);
        if(polyConvolved_.size()>1)
            integral += polyConvolved_[1] * x/squared;

        for(Size exponent = 2; exponent <polyConvolved_.size(); exponent++) {
            integral -= polyConvolved_[exponent] *
                Factorial::get(exponent-1) * std::sin((exponent)*atan2xa)
                    /std::pow(rootsqr, static_cast<Real>(exponent));
         }
        return .5 + integral / M_PI;
    }

    Probability
    CumulativeBehrensFisher::density(const Real x) const {
        Real squared = a2_ + x*x;
        Real integral = polyConvolved_[0] * a_ / squared;
        Real rootsqr = std::sqrt(squared);
        Real atan2xa = std::atan2(-x,a_);
        for(Size exponent=1; exponent <polyConvolved_.size(); exponent++) {
            integral += polyConvolved_[exponent] *
                Factorial::get(exponent) * std::cos((exponent+1)*atan2xa)
                    /std::pow(rootsqr, static_cast<Real>(exponent+1) );
        }
        return integral / M_PI;
    }

    InverseCumulativeBehrensFisher::InverseCumulativeBehrensFisher(
        const std::vector<Integer>& degreesFreedom,
        const std::vector<Real>& factors,
        Real accuracy)
    : normSqr_(std::inner_product(factors.begin(), factors.end(),
        factors.begin(), 0.)),
      accuracy_(accuracy), distrib_(degreesFreedom, factors) { }

    Real InverseCumulativeBehrensFisher::operator()(const Probability q) const {
        Probability effectiveq;
        Real sign;
        // since the distrib is symmetric solve only on the right side:
        if(q==0.5) {
            return 0.;
        }else if(q < 0.5) {
            sign = -1.;
            effectiveq = 1.-q;
        }else{
            sign = 1.;
            effectiveq = q;
        }
        Real xMin =
            InverseCumulativeNormal::standard_value(effectiveq) * normSqr_;
        // inversion will fail at the Brent's bounds-check if this is not enough
        // (q is very close to 1.), in a bad combination fails around 1.-1.e-7
        Real xMax = 1.e6;
        return sign *
            Brent().solve([&](Real x){ return distrib_(x) - effectiveq; },
                          accuracy_, (xMin+xMax)/2., xMin, xMax);
    }

}