<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    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.
 */

#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>

#include <Eigen/Core>

#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Astrodynamics/Ephemerides/approximatePlanetPositions.h"
#include "Tudat/Astrodynamics/Ephemerides/tabulatedEphemeris.h"
#include "Tudat/Basics/testMacros.h"

#include "Tudat/External/SpiceInterface/spiceEphemeris.h"
#include "Tudat/External/SpiceInterface/spiceRotationalEphemeris.h"

#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/Mathematics/Interpolators/linearInterpolator.h"
#include "Tudat/SimulationSetup/EstimationSetup/createNumericalSimulator.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h"
#include "Tudat/Astrodynamics/Aerodynamics/UnitTests/testApolloCapsuleCoefficients.h"
#include "Tudat/Astrodynamics/Ephemerides/constantRotationalEphemeris.h"
<<<<<<< HEAD
=======
#include "Tudat/Astrodynamics/ElectroMagnetism/radiationPressureInterface.h"
>>>>>>> origin/master

namespace tudat
{
namespace unit_tests
{

using namespace simulation_setup;
using namespace basic_astrodynamics;
using namespace input_output;
using namespace reference_frames;

BOOST_AUTO_TEST_SUITE( test_acceleration_model_setup )

//! Test set up of point mass gravitational accelerations, both direct and third-body.
BOOST_AUTO_TEST_CASE( test_centralGravityModelSetup )
{
    // Load Spice kernel with gravitational parameters.
    spice_interface::loadStandardSpiceKernels( );

    // Create bodies with gravitational parameters from Spice and JPL approximane positions
    // as ephemerides
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
    bodySettings[ "Mars" ] = std::make_shared< BodySettings >( );
    bodySettings[ "Jupiter" ] = std::make_shared< BodySettings >( );
    bodySettings[ "Sun" ] = std::make_shared< BodySettings >( );
    bodySettings[ "Mars" ]->ephemerisSettings = std::make_shared< ApproximatePlanetPositionSettings >(
                ephemerides::ApproximatePlanetPositionsBase::mars, 0 );
    bodySettings[ "Jupiter" ]->ephemerisSettings = std::make_shared< ApproximatePlanetPositionSettings >(
                ephemerides::ApproximatePlanetPositionsBase::jupiter, 0 );
    bodySettings[ "Mars" ]->gravityFieldSettings =
            std::make_shared< GravityFieldSettings >( central_spice );
    bodySettings[ "Jupiter" ]->gravityFieldSettings =
            std::make_shared< GravityFieldSettings >( central_spice );
    bodySettings[ "Sun" ]->gravityFieldSettings =
            std::make_shared< GravityFieldSettings >( central_spice );
    NamedBodyMap bodyMap = createBodies( bodySettings );
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    // Defins state of Sun to be all zero.
    std::map< double, Eigen::Vector6d > sunStateHistory;
    sunStateHistory[ -1.0E9 ] = Eigen::Vector6d::Zero( );
    sunStateHistory[ 0.0 ] = Eigen::Vector6d::Zero( );
    sunStateHistory[ -1.0E9 ] = Eigen::Vector6d::Zero( );
    std::shared_ptr< interpolators::LinearInterpolator< double, Eigen::Vector6d > >
            sunStateInterpolaotor = std::make_shared<
            interpolators::LinearInterpolator< double, Eigen::Vector6d > >(
                sunStateHistory );
    bodyMap[ "Sun" ] ->setEphemeris( std::make_shared< ephemerides::TabulatedCartesianEphemeris< > >(
                                         sunStateInterpolaotor ) );

    // Update bodies to current state (normally done by numerical integrator).
    for( NamedBodyMap::const_iterator bodyIterator = bodyMap.begin( ); bodyIterator !=
         bodyMap.end( ); bodyIterator++ )
    {
        bodyIterator->second->setStateFromEphemeris( 1.0E7 );
    }

    // Define settings for accelerations: point  mass atraction by Jupiter and Sun on Mars
    SelectedAccelerationMap accelerationSettingsMap;
    accelerationSettingsMap[ "Mars" ][ "Sun" ].push_back(
                std::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationSettingsMap[ "Mars" ][ "Jupiter" ].push_back(
                std::make_shared< AccelerationSettings >( central_gravity ) );

    // Define origin of integration to be barycenter.
    std::map< std::string, std::string > centralBodies;
    centralBodies[ "Mars" ] = "SSB";

    // Create accelerations
    AccelerationMap accelerationsMap = createAccelerationModelsMap(
                bodyMap, accelerationSettingsMap, centralBodies );

    // Retrieve created accelerations.
    std::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > >
            sunAcceleration = accelerationsMap[ "Mars" ][ "Sun" ][ 0 ];
    std::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > >
            jupiterAcceleration = accelerationsMap[ "Mars" ][ "Jupiter" ][ 0 ];

    // Create accelerations manually (point mass inertial).
    std::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > >
            manualSunAcceleration =
            std::make_shared< gravitation::CentralGravitationalAccelerationModel< > >(
                std::bind( &Body::getPosition, bodyMap[ "Mars" ] ),
            spice_interface::getBodyGravitationalParameter( "Sun" ),
            std::bind( &Body::getPosition, bodyMap[ "Sun" ] ) );
    std::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > >
            manualJupiterAcceleration =
            std::make_shared< gravitation::CentralGravitationalAccelerationModel< > >(
                std::bind( &Body::getPosition, bodyMap[ "Mars" ] ),
            spice_interface::getBodyGravitationalParameter( "Jupiter" ),
            std::bind( &Body::getPosition, bodyMap[ "Jupiter" ] ) );

    // Test equivalence of two acceleration models.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( basic_astrodynamics::updateAndGetAcceleration( sunAcceleration ) ),
                ( basic_astrodynamics::updateAndGetAcceleration( manualSunAcceleration ) ),
                std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( basic_astrodynamics::updateAndGetAcceleration( jupiterAcceleration ) ),
                ( basic_astrodynamics::updateAndGetAcceleration( manualJupiterAcceleration ) ),
                std::numeric_limits< double >::epsilon( ) );

    // Change central body to Sun, which will result in modified accelerations.
    centralBodies[ "Mars" ] = "Sun";

    // Recreate and retrieve accelerations.
    accelerationsMap = createAccelerationModelsMap(
                bodyMap, accelerationSettingsMap, centralBodies );
    sunAcceleration = accelerationsMap[ "Mars" ][ "Sun" ][ 0 ];
    jupiterAcceleration = accelerationsMap[ "Mars" ][ "Jupiter" ][ 0 ];

    // Manually create Sun's acceleration on Mars, which now include's Mars'gravitational parameter,
    // since the integration is done w.r.t. the Sun, not the barycenter.
    manualSunAcceleration =
            std::make_shared< gravitation::CentralGravitationalAccelerationModel< > >(
                std::bind( &Body::getPosition, bodyMap[ "Mars" ] ),
            spice_interface::getBodyGravitationalParameter( "Sun" ) +
            spice_interface::getBodyGravitationalParameter( "Mars" ),
            std::bind( &Body::getPosition, bodyMap[ "Sun" ] ) );

    // Manually create Jupiter's acceleration on Mars, which now a third body acceleration,
    // with the Sun the central body.
    manualJupiterAcceleration =
            std::make_shared< gravitation::ThirdBodyAcceleration<
            gravitation::CentralGravitationalAccelerationModel< > > >(
                std::make_shared< gravitation::CentralGravitationalAccelerationModel< > >(
                    std::bind( &Body::getPosition, bodyMap[ "Mars" ] ),
                spice_interface::getBodyGravitationalParameter( "Jupiter" ),
                std::bind( &Body::getPosition, bodyMap[ "Jupiter" ] ) ),
            std::make_shared< gravitation::CentralGravitationalAccelerationModel< > >(
                std::bind( &Body::getPosition, bodyMap[ "Sun" ] ),
            spice_interface::getBodyGravitationalParameter( "Jupiter" ),
            std::bind( &Body::getPosition, bodyMap[ "Jupiter" ] ) ), "Jupiter" );

    // Test equivalence of two acceleration models.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( basic_astrodynamics::updateAndGetAcceleration( sunAcceleration ) ),
                ( basic_astrodynamics::updateAndGetAcceleration( manualSunAcceleration ) ),
                std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( basic_astrodynamics::updateAndGetAcceleration( jupiterAcceleration ) ),
                ( basic_astrodynamics::updateAndGetAcceleration( manualJupiterAcceleration ) ),
                std::numeric_limits< double >::epsilon( ) );
}

//! Test set up of spherical harmonic gravitational accelerations.
BOOST_AUTO_TEST_CASE( test_shGravityModelSetup )
{

    // Create body map
    NamedBodyMap bodyMap;
    bodyMap[ "Earth" ] = std::make_shared< Body >( );
    bodyMap[ "Vehicle" ] = std::make_shared< Body >( );

    // Set constant state for Earth and Vehicle
    Eigen::Vector6d dummyEarthState =
            ( Eigen::Vector6d ( ) << 1.1E11, 0.5E11, 0.01E11, 0.0, 0.0, 0.0
              ).finished( );
    bodyMap[ "Earth" ]->setState( dummyEarthState );
    bodyMap[ "Vehicle" ]->setState(
                ( Eigen::Vector6d ( ) << 7.0e6, 8.0e6, 9.0e6, 0.0, 0.0, 0.0
                  ).finished( ) + dummyEarthState );

    // Define Earth gravity field.
    double gravitationalParameter = 3.986004418e14;
    double planetaryRadius = 6378137.0;
    Eigen::MatrixXd cosineCoefficients =
            ( Eigen::MatrixXd( 6, 6 ) <<
              1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              -4.841651437908150e-4, -2.066155090741760e-10, 2.439383573283130e-6, 0.0, 0.0, 0.0,
              9.571612070934730e-7, 2.030462010478640e-6, 9.047878948095281e-7,
              7.213217571215680e-7, 0.0, 0.0, 5.399658666389910e-7, -5.361573893888670e-7,
              3.505016239626490e-7, 9.908567666723210e-7, -1.885196330230330e-7, 0.0,
              6.867029137366810e-8, -6.292119230425290e-8, 6.520780431761640e-7,
              -4.518471523288430e-7, -2.953287611756290e-7, 1.748117954960020e-7
              ).finished( );
    Eigen::MatrixXd sineCoefficients =
            ( Eigen::MatrixXd( 6, 6 ) <<
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 1.384413891379790e-9, -1.400273703859340e-6, 0.0, 0.0, 0.0,
              0.0, 2.482004158568720e-7, -6.190054751776180e-7, 1.414349261929410e-6, 0.0, 0.0,
              0.0, -4.735673465180860e-7, 6.624800262758290e-7, -2.009567235674520e-7,
              3.088038821491940e-7, 0.0, 0.0, -9.436980733957690e-8, -3.233531925405220e-7,
              -2.149554083060460e-7, 4.980705501023510e-8, -6.693799351801650e-7
              ).finished( );
    bodyMap[ "Earth" ]->setGravityFieldModel(
                std::make_shared< gravitation::SphericalHarmonicsGravityField >(
                    gravitationalParameter, planetaryRadius, cosineCoefficients,
                    sineCoefficients, "IAU_Earth" ) );
    bodyMap[ "Earth" ]->setRotationalEphemeris(
                std::make_shared< ephemerides::SpiceRotationalEphemeris >(
                    "ECLIPJ2000", "IAU_Earth" ) );

    // Define settings for acceleration model (spherical harmonic due to Earth up to degree and
    // order 5.
    SelectedAccelerationMap accelerationSettingsMap;
    accelerationSettingsMap[ "Vehicle" ][ "Earth" ].push_back(
                std::make_shared< SphericalHarmonicAccelerationSettings >( 5, 5 ) );

    // Set accelerations to be calculated w.r.t. the Earth.
    std::map< std::string, std::string > centralBodies;
    centralBodies[ "Vehicle" ] = "Earth";

    // Create and retrieve acceleration.
    AccelerationMap accelerationsMap = createAccelerationModelsMap(
                bodyMap, accelerationSettingsMap, centralBodies );
    std::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > >
            directAcceleration = accelerationsMap[ "Vehicle" ][ "Earth" ][ 0 ];

    // Manually create acceleration model.
    std::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > >
            manualAcceleration =
            std::make_shared< gravitation::SphericalHarmonicsGravitationalAccelerationModel >(
                std::bind( &Body::getPosition, bodyMap[ "Vehicle" ] ),
            gravitationalParameter,
            planetaryRadius, cosineCoefficients, sineCoefficients,
            std::bind( &Body::getPosition, bodyMap[ "Earth" ] ) );

    // Test equivalence of two acceleration models.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( basic_astrodynamics::updateAndGetAcceleration( manualAcceleration ) ),
                ( basic_astrodynamics::updateAndGetAcceleration( directAcceleration ) ),
                std::numeric_limits< double >::epsilon( ) );

    // Set (unrealistically) a gravity field model on the Vehicle, to test its
    // influence on acceleration.
    bodyMap[ "Vehicle" ]->setGravityFieldModel( std::make_shared< gravitation::GravityFieldModel >(
                                                    0.1 * gravitationalParameter ) );

    // Recreate and retrieve acceleration.
    accelerationsMap = createAccelerationModelsMap(
                bodyMap, accelerationSettingsMap, centralBodies );
    directAcceleration = accelerationsMap[ "Vehicle" ][ "Earth" ][ 0 ];

    // Manually create acceleration.
    manualAcceleration =
            std::make_shared< gravitation::SphericalHarmonicsGravitationalAccelerationModel >(
                std::bind( &Body::getPosition, bodyMap[ "Vehicle" ] ),
            gravitationalParameter * 1.1,
            planetaryRadius, cosineCoefficients, sineCoefficients,
            std::bind( &Body::getPosition, bodyMap[ "Earth" ] ) );

    // Test equivalence of two acceleration models.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( basic_astrodynamics::updateAndGetAcceleration( manualAcceleration ) ),
                ( basic_astrodynamics::updateAndGetAcceleration( directAcceleration ) ),
                std::numeric_limits< double >::epsilon( ) );

}

//! Test radiation pressure acceleration
BOOST_AUTO_TEST_CASE( test_radiationPressureAcceleration )
{
    using namespace tudat::simulation_setup;
    using namespace tudat;

    // Load Spice kernels
    spice_interface::loadStandardSpiceKernels( );

    // Get settings for celestial bodies
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
    bodySettings[ "Earth" ] = getDefaultSingleBodySettings( "Earth", 0.0, 10.0 * 86400.0 );
    bodySettings[ "Sun" ] = getDefaultSingleBodySettings( "Sun", 0.0,10.0 * 86400.0 );

    // Get settings for vehicle
    double area = 2.34;
    double coefficient = 1.2;
    bodySettings[ "Vehicle" ] = std::make_shared< BodySettings >( );
    bodySettings[ "Vehicle" ]->radiationPressureSettings[ "Sun" ] =
            std::make_shared< CannonBallRadiationPressureInterfaceSettings >( "Sun", area, coefficient );
    bodySettings[ "Vehicle" ]->ephemerisSettings =
            std::make_shared< KeplerEphemerisSettings >(
                ( Eigen::Vector6d( ) << 12000.0E3, 0.13, 0.3, 0.0, 0.0, 0.0 ).finished( ),
                0.0, spice_interface::getBodyGravitationalParameter( "Earth" ), "Earth", "ECLIPJ2000" );

    // Create bodies
    NamedBodyMap bodyMap = createBodies( bodySettings );
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    // Define settings for accelerations
    SelectedAccelerationMap accelerationSettingsMap;
    accelerationSettingsMap[ "Vehicle" ][ "Sun" ].push_back(
                std::make_shared< AccelerationSettings >( cannon_ball_radiation_pressure ) );

    // Define origin of integration
    std::map< std::string, std::string > centralBodies;
    centralBodies[ "Vehicle" ] = "Earth";

    // Create accelerations
    AccelerationMap accelerationsMap = createAccelerationModelsMap(
                bodyMap, accelerationSettingsMap, centralBodies );
    std::shared_ptr< AccelerationModel3d > radiationPressureAcceleration = accelerationsMap[ "Vehicle" ][ "Sun" ][ 0 ];

    // Set (arbitrary) test time.
    double testTime = 5.0 * 86400.0;

    // Set vehicle mass
    double bodyMass = 500.0;
    bodyMap[ "Vehicle" ]->setBodyMassFunction( [ & ]( const double ){ return bodyMass; } );
    bodyMap[ "Vehicle" ]->updateMass( testTime );

    // Update environment to current time.
    bodyMap[ "Sun" ]->setStateFromEphemeris< double, double >( testTime );
    bodyMap[ "Earth" ]->setStateFromEphemeris< double, double >( testTime );
    bodyMap[ "Vehicle" ]->setStateFromEphemeris< double, double >( testTime );
    bodyMap[ "Vehicle" ]->getRadiationPressureInterfaces( ).at( "Sun" )->updateInterface( testTime );

    // Get acceleration
    Eigen::Vector3d calculatedAcceleration = updateAndGetAcceleration(
                radiationPressureAcceleration );

    // Manually calculate acceleration
    Eigen::Vector3d expectedForceDirection =
            ( bodyMap[ "Vehicle" ]->getState( ) -  bodyMap[ "Sun" ]->getState( ) ).segment( 0, 3 );
    double sourceDistance = expectedForceDirection.norm( );
    double expectedForceMagnitude = electro_magnetism::calculateRadiationPressure(
                defaultRadiatedPowerValues.at( "Sun" ), sourceDistance ) * area * coefficient;
    Eigen::Vector3d expectedAcceleration = expectedForceDirection.normalized( ) * expectedForceMagnitude / bodyMass;

    // Compare results
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                expectedAcceleration, calculatedAcceleration, ( 2.0 * std::numeric_limits< double >::epsilon( ) ) );

}

//! Test setup of aerodynamic accelerations (constant coefficients)
BOOST_AUTO_TEST_CASE( test_aerodynamicAccelerationModelSetup )
{
    using namespace tudat::simulation_setup;
    using namespace tudat;

    // Load Spice kernels
    spice_interface::loadStandardSpiceKernels( );

    // Test creation with coefficients positive/negative in body/aerodynamic frame (4 cases).
    for( unsigned int testCase = 0; testCase < 4; testCase++ )
    {
        // Get settings for Earth.
        std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
        bodySettings[ "Earth" ] = getDefaultSingleBodySettings( "Earth", 0.0, 86400.0 );
        bodySettings[ "Vehicle" ] = std::make_shared< BodySettings >( );

        // Define (arbitrary) aerodynamic coefficient settings.
        Eigen::Vector3d aerodynamicCoefficients =
                ( Eigen::Vector3d( ) << 1.0, 3.0, -2.0 ).finished( );
        double referenceArea = 4.7;

        // Get coefficient settings for current test case
        bool areCoefficientsInAerodynamicFrame = 0;
        bool areCoefficientsInNegativeAxisDirection = 1;
        if( testCase < 2 )
        {
            areCoefficientsInAerodynamicFrame = 1;
        }
        else
        {
            areCoefficientsInAerodynamicFrame = 0;
        }
        if( testCase % 2 == 0 )
        {
            areCoefficientsInNegativeAxisDirection = 1;
        }
        else
        {
            areCoefficientsInNegativeAxisDirection = 0;
        }

        bodySettings[ "Vehicle" ]->aerodynamicCoefficientSettings =
                std::make_shared< ConstantAerodynamicCoefficientSettings >(
                    1.0, referenceArea, 1.0, Eigen::Vector3d::Zero( ), aerodynamicCoefficients, Eigen::Vector3d::Zero( ),
                    areCoefficientsInAerodynamicFrame, areCoefficientsInNegativeAxisDirection );

        // Create body objects.
        NamedBodyMap bodyMap = createBodies( bodySettings );
        setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

        // Define settings for accelerations
        SelectedAccelerationMap accelerationSettingsMap;
        accelerationSettingsMap[ "Vehicle" ][ "Earth" ].push_back(
                    std::make_shared< AccelerationSettings >( aerodynamic ) );

        // Define origin of integration
        std::map< std::string, std::string > centralBodies;
        centralBodies[ "Vehicle" ] = "Earth";

        // Create accelerations
        AccelerationMap accelerationsMap = createAccelerationModelsMap(
                    bodyMap, accelerationSettingsMap, centralBodies );
        std::shared_ptr< AccelerationModel3d > aerodynamicAcceleration = accelerationsMap[ "Vehicle" ][ "Earth" ][ 0 ];

        // Define expected aerodynamic angles (see testAerodynamicAngleCalculator)
        double testHeadingAngle = 1.229357188236127;
        double testFlightPathAngle = -0.024894033070522;
        double testLatitude = -0.385027359562548;
        double testLongitude = -1.849449608688977;

        double angleOfAttack = 1.232;
        double angleOfSideslip = -0.00322;
        double bankAngle = 2.323432;

        // Retrieve flight conditions and define orientation angles.
        std::shared_ptr< aerodynamics::FlightConditions > vehicleFlightConditions =
                bodyMap[ "Vehicle" ]->getFlightConditions( );
        vehicleFlightConditions->getAerodynamicAngleCalculator( )->setOrientationAngleFunctions(
                    [ & ]( ){ return angleOfAttack; },
                    [ & ]( ){ return angleOfSideslip; },
                    [ & ]( ){ return bankAngle; } );

        // Set vehicle body-fixed state (see testAerodynamicAngleCalculator)
        Eigen::Vector6d vehicleBodyFixedState =
                ( Eigen::Vector6d( ) << -1656517.23153109, -5790058.28764025, -2440584.88186829,
                  6526.30784888051, -2661.34558272018, 2377.09572383163 ).finished( );

        double testTime = 0.5E7;

        // Convert vehicle state to inertial frame.
        Eigen::Vector6d vehicleInertialState =
                ephemerides::transformStateToFrameFromRotations(
                    vehicleBodyFixedState,
                    bodyMap[ "Earth" ]->getRotationalEphemeris( )->getRotationToBaseFrame( testTime ),
                bodyMap[ "Earth" ]->getRotationalEphemeris( )->getDerivativeOfRotationToBaseFrame( testTime ) );

        // Set states in environment.
        bodyMap[ "Earth" ]->setState( Eigen::Vector6d::Zero( ) );
        bodyMap[ "Earth" ]->setCurrentRotationalStateToLocalFrameFromEphemeris( testTime );
        bodyMap[ "Vehicle" ]->setState( vehicleInertialState );

        // Set vehicle mass
        double bodyMass = 500.0;
        bodyMap[ "Vehicle" ]->setBodyMassFunction( [ & ]( const double ){ return bodyMass; } );
        bodyMap[ "Vehicle" ]->updateMass( testTime );

        // Update flight conditions.
        vehicleFlightConditions->updateConditions( testTime );

        // Check whether flight conditions object has been correctly automatically created
        // (see testAerodynamicAngleCalculator)
        {
            BOOST_CHECK_SMALL(
                        std::fabs( vehicleFlightConditions->getAerodynamicAngleCalculator( )->
                                   getAerodynamicAngle( latitude_angle ) -
                                   testLatitude), 10.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL(
                        std::fabs( vehicleFlightConditions->getAerodynamicAngleCalculator( )->
                                   getAerodynamicAngle( longitude_angle ) -
                                   testLongitude), 10.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL(
                        std::fabs( vehicleFlightConditions->getAerodynamicAngleCalculator( )->
                                   getAerodynamicAngle( heading_angle ) -
                                   testHeadingAngle), 10.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL(
                        std::fabs( vehicleFlightConditions->getAerodynamicAngleCalculator( )->
                                   getAerodynamicAngle( flight_path_angle ) -
                                   testFlightPathAngle), 10.0 * std::numeric_limits< double >::epsilon( ) );

            BOOST_CHECK_SMALL(
                        std::fabs( vehicleFlightConditions->getAerodynamicAngleCalculator( )->
                                   getAerodynamicAngle( angle_of_attack ) -
                                   angleOfAttack ), 10.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL(
                        std::fabs( vehicleFlightConditions->getAerodynamicAngleCalculator( )->
                                   getAerodynamicAngle( angle_of_sideslip ) -
                                   angleOfSideslip), 10.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL(
                        std::fabs( vehicleFlightConditions->getAerodynamicAngleCalculator( )->
                                   getAerodynamicAngle( bank_angle ) -
                                   bankAngle), 10.0 * std::numeric_limits< double >::epsilon( ) );

            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        vehicleFlightConditions->getCurrentBodyCenteredBodyFixedState( ), vehicleBodyFixedState,
                                               ( 2.0 * std::numeric_limits< double >::epsilon( ) ) );
        }

        // Define current frame for aerodynamic coefficients.
        AerodynamicsReferenceFrames coefficientFrame = aerodynamic_frame;
        if( testCase < 2 )
        {
            coefficientFrame = aerodynamic_frame;
        }
        else
        {
            coefficientFrame = body_frame;
        }

        // Get rotation from coefficient to propagation frame.
        Eigen::Quaterniond rotationToPropagationFrame =
                bodyMap[ "Earth" ]->getCurrentRotationToGlobalFrame( ) *
                vehicleFlightConditions->getAerodynamicAngleCalculator( )->getRotationQuaternionBetweenFrames(
                    coefficientFrame, corotating_frame );

        // Calculate aerodynamic force manually.
        double dynamicPressure = 0.5 * bodyMap[ "Earth" ]->getAtmosphereModel( )->getDensity(
                    vehicleFlightConditions->getCurrentAltitude( ), testLongitude, testLatitude, testTime ) *
                std::pow( vehicleBodyFixedState.segment( 3, 3 ).norm( ), 2.0 );
        Eigen::Vector3d expectedAerodynamicForce = dynamicPressure * referenceArea  *
                ( rotationToPropagationFrame * aerodynamicCoefficients );
        if( testCase % 2 == 0 )
        {
            expectedAerodynamicForce *= -1.0;
        }

        // Get automatic aerodynamic force.
        Eigen::Vector3d computedAerodynamicForce =
                bodyMass * updateAndGetAcceleration< Eigen::Vector3d >( aerodynamicAcceleration );

        // Compare results
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    expectedAerodynamicForce, computedAerodynamicForce, ( 10.0 *  std::numeric_limits< double >::epsilon( ) ) );
    }
}

//! Test setup of aerodynamic accelerations (non-constant coefficients)
BOOST_AUTO_TEST_CASE( test_aerodynamicAccelerationModelSetupWithCoefficientIndependentVariables )
{
    using namespace tudat::simulation_setup;
    using namespace tudat;

    // Load Spice kernels
    spice_interface::loadStandardSpiceKernels( );
    // Get settings for Earth.
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
    bodySettings[ "Earth" ] = getDefaultSingleBodySettings( "Earth", 0.0, 86400.0 );
    bodySettings[ "Vehicle" ] = std::make_shared< BodySettings >( );

    // Create body objects.
    NamedBodyMap bodyMap = createBodies( bodySettings );
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    // Create vehicle aerodynamic coefficients
    bodyMap[ "Vehicle" ]->setAerodynamicCoefficientInterface(
                getApolloCoefficientInterface( ) );

    // Define settings for accelerations: point
    SelectedAccelerationMap accelerationSettingsMap;
    accelerationSettingsMap[ "Vehicle" ][ "Earth" ].push_back(
                std::make_shared< AccelerationSettings >( aerodynamic ) );

    // Define origin of integration
    std::map< std::string, std::string > centralBodies;
    centralBodies[ "Vehicle" ] = "Earth";

    // Create accelerations
    AccelerationMap accelerationsMap = createAccelerationModelsMap(
                bodyMap, accelerationSettingsMap, centralBodies );
    std::shared_ptr< AccelerationModel3d > aerodynamicAcceleration = accelerationsMap[ "Vehicle" ][ "Earth" ][ 0 ];

    // Retrieve flight conditions and orientation angles
    std::shared_ptr< aerodynamics::FlightConditions > vehicleFlightConditions =
            bodyMap[ "Vehicle" ]->getFlightConditions( );
    std::shared_ptr< aerodynamics::AerodynamicCoefficientInterface > coefficientInterface =
            bodyMap[ "Vehicle" ]->getAerodynamicCoefficientInterface( );

    // Define orientation angles.
    double angleOfAttack = 1.232;
    double angleOfSideslip = -0.00322;
    double bankAngle = 2.323432;
    vehicleFlightConditions->getAerodynamicAngleCalculator( )->setOrientationAngleFunctions(
                [ & ]( ){ return angleOfAttack; },
                [ & ]( ){ return angleOfSideslip; },
                [ & ]( ){ return bankAngle; } );

    // Update environment to current time.
    double testTime = 0.5E7;
    bodyMap[ "Earth" ]->setCurrentRotationalStateToLocalFrameFromEphemeris( testTime );
    bodyMap[ "Earth" ]->setState( Eigen::Vector6d::Zero( ) );
    double bodyMass = 500.0;

    // Set vehicle mass
    bodyMap[ "Vehicle" ]->setBodyMassFunction( [ & ]( const double ){ return bodyMass; } );
    bodyMap[ "Vehicle" ]->updateMass( testTime );

    // Test aerodynamic coefficients for various cases of independent variables.
    for( unsigned int i = 0; i < 4; i++ )
    {
        // Define body-fixed vehicle state.
        Eigen::Vector6d vehicleBodyFixedState =
                ( Eigen::Vector6d( ) << -1656517.23153109, -5790058.28764025, -2440584.88186829,
                  6526.30784888051, -2661.34558272018, 2377.09572383163 ).finished( );
        if( i > 0 )
        {
            vehicleBodyFixedState.segment( 3, 3 ) += ( Eigen::Vector3d( ) << -3234.2, 2456.2, 33.245 ).finished( );
        }

        // Define vehicle inertial state.
        Eigen::Vector6d vehicleInertialState =
                ephemerides::transformStateToFrameFromRotations(
                    vehicleBodyFixedState,
                    bodyMap[ "Earth" ]->getRotationalEphemeris( )->getRotationToBaseFrame( testTime ),
                bodyMap[ "Earth" ]->getRotationalEphemeris( )->getDerivativeOfRotationToBaseFrame( testTime ) );
        bodyMap[ "Vehicle" ]->setState( vehicleInertialState );

        // Define orientation angles.
        if( i > 1 )
        {
            angleOfAttack -= 0.6465;
        }
        if( i > 2  )
        {
            angleOfSideslip += 0.00123;
        }
        vehicleFlightConditions->getAerodynamicAngleCalculator( )->setOrientationAngleFunctions(
                    [ & ]( ){ return angleOfAttack; },
                    [ & ]( ){ return angleOfSideslip; },
                    [ & ]( ){ return bankAngle; } );

        // Update flight conditions
        vehicleFlightConditions->resetCurrentTime( TUDAT_NAN );
        vehicleFlightConditions->updateConditions( testTime );

        // Calculate Mach number
        double velocity = vehicleBodyFixedState.segment( 3, 3 ).norm( );
        double speedOfSound = bodyMap[ "Earth" ]->getAtmosphereModel( )->getSpeedOfSound(
                    vehicleFlightConditions->getCurrentAltitude( ), 0.0, 0.0, 0.0 );
        double machNumber = velocity / speedOfSound;

        // Get manual and automatic coefficients and compare.
        Eigen::Vector3d automaticCoefficients = coefficientInterface->getCurrentForceCoefficients( );
        coefficientInterface->updateFullCurrentCoefficients(
                   { machNumber, angleOfAttack, angleOfSideslip } );
        Eigen::Vector3d manualCoefficients = coefficientInterface->getCurrentForceCoefficients( );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    automaticCoefficients, manualCoefficients, ( 5.0 *  std::numeric_limits< double >::epsilon( ) ) );
    }
}

//! Test panelled radiation pressure acceleration
BOOST_AUTO_TEST_CASE( test_panelledRadiationPressureAcceleration )
{
    using namespace tudat::simulation_setup;
    using namespace tudat;

    // Load Spice kernels
    spice_interface::loadStandardSpiceKernels( );

    // Get settings for celestial bodies
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
    bodySettings[ "Earth" ] = getDefaultSingleBodySettings( "Earth", 0.0, 10.0 * 86400.0 );
    bodySettings[ "Sun" ] = getDefaultSingleBodySettings( "Sun", 0.0,10.0 * 86400.0 );

    // Create panelled radiation pressure settings
    std::vector< double > areas;
    areas.push_back( 4.0 );
    areas.push_back( 6.0 );
    areas.push_back( 2.3 );
    areas.push_back( 2.3 );
    areas.push_back( 5.3 );
    areas.push_back( 4.1 );

    std::vector< double > emissivities;
    emissivities.push_back( 0.1 );
    emissivities.push_back( 0.0 );
    emissivities.push_back( 0.1 );
    emissivities.push_back( 0.1 );
    emissivities.push_back( 0.94 );
    emissivities.push_back( 0.94 );

    std::vector< double > diffuseReflectionCoefficients;
    diffuseReflectionCoefficients.push_back( 0.46 );
    diffuseReflectionCoefficients.push_back( 0.06 );
    diffuseReflectionCoefficients.push_back( 0.46 );
    diffuseReflectionCoefficients.push_back( 0.46 );
    diffuseReflectionCoefficients.push_back( 0.06 );
    diffuseReflectionCoefficients.push_back( 0.06 );

    std::vector< Eigen::Vector3d > panelSurfaceNormals;
    panelSurfaceNormals.push_back( Eigen::Vector3d::UnitZ( ) );
    panelSurfaceNormals.push_back( - Eigen::Vector3d::UnitZ( ) );
    panelSurfaceNormals.push_back( Eigen::Vector3d::UnitX( ) );
    panelSurfaceNormals.push_back( - Eigen::Vector3d::UnitX( ) );
    panelSurfaceNormals.push_back( Eigen::Vector3d::UnitY( ) );
    panelSurfaceNormals.push_back( - Eigen::Vector3d::UnitY( ) );

    bodySettings[ "Vehicle" ] = std::make_shared< BodySettings >( );
    bodySettings[ "Vehicle" ]->radiationPressureSettings[ "Sun" ] =
            std::make_shared< PanelledRadiationPressureInterfaceSettings >( "Sun", emissivities, areas, diffuseReflectionCoefficients,
                                                                            panelSurfaceNormals);
    bodySettings[ "Vehicle" ]->ephemerisSettings = std::make_shared< KeplerEphemerisSettings >(
                ( Eigen::Vector6d( ) << 12000.0E3, 0.13, 0.3, 0.0, 0.0, 0.0 ).finished( ),
                0.0, spice_interface::getBodyGravitationalParameter( "Earth" ), "Earth", "ECLIPJ2000" );

    // Create bodies
    NamedBodyMap bodyMap = createBodies( bodySettings );
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    Eigen::Vector7d rotationalStateVehicle;
    rotationalStateVehicle.segment( 0, 4 ) = linear_algebra::convertQuaternionToVectorFormat( Eigen::Quaterniond( Eigen::Matrix3d::Identity() ));
    rotationalStateVehicle.segment( 4, 3 ) = Eigen::Vector3d::Zero();
    bodyMap[ "Vehicle" ]->setRotationalEphemeris( std::make_shared< ephemerides::ConstantRotationalEphemeris >(
                    rotationalStateVehicle, "ECLIPJ2000", "VehicleFixed" ) );

    // Define settings for accelerations
    SelectedAccelerationMap accelerationSettingsMap;
    accelerationSettingsMap[ "Vehicle" ][ "Sun" ].push_back(
                std::make_shared< AccelerationSettings >( panelled_radiation_pressure_acceleration ) );

    // Define origin of integration
    std::map< std::string, std::string > centralBodies;
    centralBodies[ "Vehicle" ] = "Earth";

    // Create accelerations
    AccelerationMap accelerationsMap = createAccelerationModelsMap(
                bodyMap, accelerationSettingsMap, centralBodies );
    std::shared_ptr< AccelerationModel3d > radiationPressureAcceleration = accelerationsMap[ "Vehicle" ][ "Sun" ][ 0 ];

    // Set (arbitrary) test time.
    double testTime = 5.0 * 86400.0;

    // Set vehicle mass
    double bodyMass = 500.0;
    bodyMap[ "Vehicle" ]->setBodyMassFunction( [ & ]( const double ){ return bodyMass; } );
    bodyMap[ "Vehicle" ]->updateMass( testTime );

    // Update environment to current time.
    bodyMap[ "Sun" ]->setStateFromEphemeris< double, double >( testTime );
    bodyMap[ "Earth" ]->setStateFromEphemeris< double, double >( testTime );
    bodyMap[ "Vehicle" ]->setStateFromEphemeris< double, double >( testTime );
    bodyMap[ "Vehicle" ]->setCurrentRotationToLocalFrameFromEphemeris( testTime );
    bodyMap[ "Vehicle" ]->getRadiationPressureInterfaces( ).at( "Sun" )->updateInterface( testTime );

    double currentRadiationPressure = bodyMap[ "Vehicle" ]->getRadiationPressureInterfaces().at( "Sun" )->getCurrentRadiationPressure();
    double currentBodyMass = bodyMap[ "Vehicle" ]->getBodyMass();

    // Get acceleration
    Eigen::Vector3d calculatedAcceleration = updateAndGetAcceleration( radiationPressureAcceleration );
    std::vector< Eigen::Vector3d > expectedAccelerationPerPanel;
    Eigen::Vector3d expectedAcceleration = Eigen::Vector3d::Zero();

    Eigen::Vector3d expectedVehicleToSunNormalisedVector =
            ( bodyMap[ "Sun" ]->getState( ) - bodyMap[ "Vehicle" ]->getState( ) ).segment( 0, 3 ).normalized();

    // Manually calculate acceleration
<<<<<<< HEAD
    for (int currentPanel = 0 ; currentPanel < areas.size() ; currentPanel++){
=======
    for ( unsigned int currentPanel = 0 ; currentPanel < areas.size() ; currentPanel++){
>>>>>>> origin/master

        double cosinusCurrentPanelInclination = expectedVehicleToSunNormalisedVector.dot( panelSurfaceNormals[ currentPanel ] );

        // Initialize force to zero
        Eigen::Vector3d radiationPressureAccelerationCurrentPanel = Eigen::Vector3d::Zero( );

        // If cosinusOfPanelInclination is larger than zero (i.e inclination is smaller than 90 degrees), calculated acceleration force (zero otherwise)
        if( cosinusCurrentPanelInclination > 0.0 )
        {

            radiationPressureAccelerationCurrentPanel = - currentRadiationPressure / currentBodyMass * cosinusCurrentPanelInclination
                    * areas[ currentPanel ] * ( ( 1.0 - emissivities[ currentPanel ] ) * expectedVehicleToSunNormalisedVector
                                                + 2.0 * emissivities[ currentPanel ] * cosinusCurrentPanelInclination
                                                                                * panelSurfaceNormals[ currentPanel ]
                                                + 2.0 / 3.0 * diffuseReflectionCoefficients[ currentPanel ] * panelSurfaceNormals[ currentPanel ]);

       }

        expectedAccelerationPerPanel.push_back( radiationPressureAccelerationCurrentPanel );
        expectedAcceleration += radiationPressureAccelerationCurrentPanel;

    }

<<<<<<< HEAD

    // Compare results
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                expectedAcceleration, calculatedAcceleration, ( 2.0 * std::numeric_limits< double >::epsilon( ) ) );

=======
    // Compare results
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                expectedAcceleration, calculatedAcceleration, ( 2.0 * std::numeric_limits< double >::epsilon( ) ) );

}

//! Test solar sailing radiation pressure acceleration
BOOST_AUTO_TEST_CASE( test_solarSailingRadiationPressureAcceleration )
{
    using namespace tudat::simulation_setup;
    using namespace tudat;

    // Load Spice kernels
    spice_interface::loadStandardSpiceKernels( );

    // Get settings for celestial bodies
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
    bodySettings[ "Earth" ] = getDefaultSingleBodySettings( "Earth", 0.0, 10.0 * 86400.0 );
    bodySettings[ "Sun" ] = getDefaultSingleBodySettings( "Sun", 0.0,10.0 * 86400.0 );

    // Create solar sailing radiation pressure settings.

    // Define area subject to radiation pressure [m^2].
    double area = 2.0;

    // Set cone angle of the solar sail [rad].
    double coneAngle = 0.25;

    // Get cone angle of the solar sail [rad].
    std::function< double( const double ) > coneAngleFunction
            = [ = ]( const double ){ return coneAngle; };

    // Define clock angle of the solar sail [rad].
    double clockAngle = 0.2;

    // Get clock angle of the solar sail [rad].
    std::function< double( const double ) > clockAngleFunction
            = [ = ]( const double ){ return clockAngle; };

    // Define front emissivity coefficient of the solar sail [-].
    double frontEmissivityCoefficient = 0.4;

    // Define back emissivity coefficient of the solar sail [-].
    double backEmissivityCoefficient = 0.4;

    // Define front Lambertian coefficient of the solar sail [-].
    double frontLambertianCoefficient = 0.4;

    // Define back Lambertian coefficient of the solar sail [-].
    double backLambertianCoefficient = 0.4;

    // Define reflectivity coefficient of the solar sail [-].
    double reflectivityCoefficient = 0.3;

    // Define specular reflection coefficient of the solar sail [-].
    double specularReflectionCoefficient = 1.0;

    std::string centralBody = "Earth";

    bodySettings[ "Vehicle" ] = std::make_shared< BodySettings >( );
    bodySettings[ "Vehicle" ]->radiationPressureSettings[ "Sun" ] =
            std::make_shared< SolarSailRadiationInterfaceSettings >( "Sun", area, coneAngleFunction, clockAngleFunction, frontEmissivityCoefficient,
                                                                     backEmissivityCoefficient, frontLambertianCoefficient, backLambertianCoefficient,
                                                                     reflectivityCoefficient, specularReflectionCoefficient, std::vector< std::string >( ),
                                                                     centralBody );

    bodySettings[ "Vehicle" ]->ephemerisSettings = std::make_shared< KeplerEphemerisSettings >(
                ( Eigen::Vector6d( ) << 12000.0E3, 0.13, 0.3, 0.0, 0.0, 0.0 ).finished( ),
                0.0, spice_interface::getBodyGravitationalParameter( "Earth" ), "Earth", "ECLIPJ2000" );

    // Create bodies
    NamedBodyMap bodyMap = createBodies( bodySettings );
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    // Define rotational ephemeris of the vehicle.
    Eigen::Vector7d rotationalStateVehicle;
    rotationalStateVehicle.segment( 0, 4 ) = linear_algebra::convertQuaternionToVectorFormat( Eigen::Quaterniond( Eigen::Matrix3d::Identity() ));
    rotationalStateVehicle.segment( 4, 3 ) = Eigen::Vector3d::Zero();
    bodyMap[ "Vehicle" ]->setRotationalEphemeris( std::make_shared< ephemerides::ConstantRotationalEphemeris >(
                    rotationalStateVehicle, "ECLIPJ2000", "VehicleFixed" ) );

    // Define settings for accelerations
    SelectedAccelerationMap accelerationSettingsMap;
    accelerationSettingsMap[ "Vehicle" ][ "Sun" ].push_back(
                std::make_shared< AccelerationSettings >( solar_sail_acceleration ) );

    // Define origin of integration
    std::map< std::string, std::string > centralBodiesMap;
    centralBodiesMap[ "Vehicle" ] = "Earth";

    // Create accelerations
    AccelerationMap accelerationsMap = createAccelerationModelsMap( bodyMap, accelerationSettingsMap, centralBodiesMap );
    std::shared_ptr< AccelerationModel3d > radiationPressureAcceleration = accelerationsMap[ "Vehicle" ][ "Sun" ][ 0 ];

    // Set (arbitrary) test time.
    double testTime = 5.0 * 86400.0;

    // Set vehicle mass
    double bodyMass = 500.0;
    bodyMap[ "Vehicle" ]->setBodyMassFunction( [ & ]( const double ){ return bodyMass; } );
    bodyMap[ "Vehicle" ]->updateMass( testTime );

    // Update environment to current time.
    bodyMap[ "Sun" ]->setStateFromEphemeris< double, double >( testTime );
    bodyMap[ "Earth" ]->setStateFromEphemeris< double, double >( testTime );
    bodyMap[ "Vehicle" ]->setStateFromEphemeris< double, double >( testTime );
    bodyMap[ "Vehicle" ]->setCurrentRotationToLocalFrameFromEphemeris( testTime );
    bodyMap[ "Vehicle" ]->getRadiationPressureInterfaces( ).at( "Sun" )->updateInterface( testTime );

    // Get acceleration
    Eigen::Vector3d calculatedAcceleration = updateAndGetAcceleration( radiationPressureAcceleration );

   // Retrieve solar sailing radiation pressure interface.
   std::shared_ptr< electro_magnetism::SolarSailingRadiationPressureInterface > radiationPressureInterface =
           std::dynamic_pointer_cast< electro_magnetism::SolarSailingRadiationPressureInterface >(
               bodyMap[ "Vehicle" ]->getRadiationPressureInterfaces().at( "Sun" ) );

    // Manually calculate acceleration.
   std::shared_ptr< AccelerationModel3d > manualAccelerationModel =
           std::make_shared< electro_magnetism::SolarSailAcceleration >(
               std::bind( &Body::getPosition, bodyMap[ "Sun" ] ),
               std::bind( &Body::getPosition, bodyMap[ "Vehicle" ] ),
               std::bind( &Body::getVelocity, bodyMap[ "Vehicle" ] ),
               std::bind( &Body::getVelocity, bodyMap[ centralBodiesMap[ "Vehicle" ] ] ),
               std::bind( &electro_magnetism::SolarSailingRadiationPressureInterface::getCurrentRadiationPressure,
                           radiationPressureInterface ),
               std::bind( &electro_magnetism::SolarSailingRadiationPressureInterface::getCurrentConeAngle,
                           radiationPressureInterface ),
               std::bind( &electro_magnetism::SolarSailingRadiationPressureInterface::getCurrentClockAngle,
                           radiationPressureInterface ),
               frontEmissivityCoefficient, backEmissivityCoefficient, frontLambertianCoefficient, backLambertianCoefficient,
               reflectivityCoefficient, specularReflectionCoefficient, area, bodyMass );

   Eigen::Vector3d manualAcceleration = updateAndGetAcceleration( manualAccelerationModel );

    // Compare results.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( manualAcceleration, calculatedAcceleration, ( 2.0 * std::numeric_limits< double >::epsilon( ) ) );
>>>>>>> origin/master
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include "../src/robust_sampler.h"
#include "../src/point.h"
#include "../src/utils.h"

#define BOOST_TEST_MODULE ClassTest
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>

#include <iostream>
#include <set>
#include <vector>
#include <map>
#include <random>

std::vector<Point> make_data() {
  const int n_centers = 30;
  std::vector<Point> points;
  for (int i = 1; i <= n_centers; i++) {
    Point p{i * i + .1, i + .1, i * i * i + .1};
    points.push_back(p);
  }
  for (int i = 0; i < n_centers; i++) {
    for (int j = 0; j <= 10; ++j) {
      points.push_back(points[i] + rand_point(3, 0, 0.1));
    }
  }
  return points;
}

BOOST_AUTO_TEST_CASE(RobustSampler2_test) {
  PointHash phash(1);
  double side_len = 3;
  double alpha = 0.9;
  RobustSampler2 sampler(3, alpha);

  auto&& dataset = make_data();
  std::random_shuffle(dataset.begin(), dataset.end());
  // for (auto p : dataset) {
  //   std::cerr << p[0] << " " << to_cell(p, side_len)[0] << std::endl;
  // }

  std::map<int, int> ct;
  for (int i = 0; i < 10000; i++) {
    RobustSampler sampler(side_len, alpha);
    for (const auto& p : dataset) {
      sampler.process(p);
    }
    auto p = sampler.get_sample();
    auto h = phash(to_cell(p, side_len));
    ct[h] = ct[h] + 1;
  }
  std::cerr << "|ct| = " << ct.size() << std::endl;
  for (auto& p : ct) {
    std::cout << p.first << " " << p.second << std::endl;
  }
}

// BOOST_AUTO_TEST_CASE(RobustSampler_Test) {
//   PointHash phash(1);
//   double side_len = 3;
//   double alpha = 0.9;

//   auto&& dataset = make_data();
//   std::random_shuffle(dataset.begin(), dataset.end());
//   // for (auto p : dataset) {
//   //   std::cerr << p[0] << " " << to_cell(p, side_len)[0] << std::endl;
//   // }

//   std::map<int, int> ct;
//   for (int i = 0; i < 500; i++) {
//     RobustSampler sampler(side_len, alpha);
//     for (const auto& p : dataset) {
//       sampler.process(p);
//     }
//     auto p = sampler.get_sample();
//     auto h = phash(to_cell(p, side_len));
//     ct[h] = ct[h] + 1;
//   }
//   std::cerr << "|ct| = " << ct.size() << std::endl;
//   for (auto& p : ct) {
//     std::cout << p.first << " " << p.second << std::endl;
//   }
// }

// BOOST_AUTO_TEST_CASE(RobustCounter_Test) {
//   double side_len = 3;
//   double alpha = 0.9;
//   RobustCounter rcounter(3, alpha, 0.3);

//   auto&& dataset = make_data();
//   std::random_shuffle(dataset.begin(), dataset.end());

//   for (const auto& p : dataset) {
//     rcounter.process(p);
//   }
//   std::cout << rcounter.to_string()
//             << "\nf0 = " << rcounter.est_f0() << std::endl;
// }


#define BOOST_TEXT_TESTING
#include <boost/text/unencoded_rope.hpp>

#include <gtest/gtest.h>

using boost::text::unencoded_rope;
using boost::text::string;
using boost::text::string_view;
using boost::text::repeated_string_view;
using namespace boost::text::detail;

inline node_ptr<rope_tag>
make_interior_with_leaves(char const * leaf_name, int leaves)
{
    interior_node_t<rope_tag> * int_node = nullptr;
    node_ptr<rope_tag> node(int_node = new_interior_node<rope_tag>());
    int_node->children_.push_back(make_node(leaf_name));
    int_node->keys_.push_back(size(int_node->children_[0].get()));
    for (int i = 1; i < leaves; ++i) {
        int_node->children_.push_back(make_node(leaf_name));
        int_node->keys_.push_back(
            int_node->keys_.back() + size(int_node->children_[i].get()));
    }
    return node;
}

template<int SizeLeft, int SizeCenter, int SizeRight>
node_ptr<rope_tag> make_tree_left_center_right()
{
    interior_node_t<rope_tag> * int_root = nullptr;
    node_ptr<rope_tag> root(int_root = new_interior_node<rope_tag>());

    node_ptr<rope_tag> left = make_interior_with_leaves("left", SizeLeft);

    int_root->children_.push_back(left);
    int_root->keys_.push_back(size(left.get()));

    if (SizeCenter != -1) {
        node_ptr<rope_tag> center =
            make_interior_with_leaves("center", SizeCenter);

        int_root->children_.push_back(center);
        int_root->keys_.push_back(int_root->keys_.back() + size(center.get()));
    }

    node_ptr<rope_tag> right = make_interior_with_leaves("right", SizeRight);

    int_root->children_.push_back(right);
    int_root->keys_.push_back(int_root->keys_.back() + size(right.get()));

    return root;
}

template<int SizeLeft, int SizeRight>
node_ptr<rope_tag> make_tree_left_right()
{
    return make_tree_left_center_right<SizeLeft, -1, SizeRight>();
}

inline node_ptr<rope_tag> make_tree_left_max()
{
    return make_tree_left_right<max_children, max_children - 1>();
}

inline node_ptr<rope_tag> make_tree_left_min()
{
    return make_tree_left_right<min_children, max_children - 1>();
}

TEST(rope_btree, test_btree_split_child)
{
    node_ptr<rope_tag> root = make_tree_left_max();
    node_ptr<rope_tag> root_2 = btree_split_child(root, 0);

    EXPECT_EQ(root->refs_, 1);
    EXPECT_EQ(root_2->refs_, 1);

    EXPECT_EQ(children(root_2).size(), 3u);
    EXPECT_EQ(keys(root_2)[0], max_children * 2);
    EXPECT_EQ(keys(root_2)[1], max_children * 2 * 2);
    EXPECT_EQ(keys(root_2)[2], max_children * 2 * 2 + (max_children - 1) * 5);

    EXPECT_EQ(num_children(children(root_2)[0]), min_children);
    EXPECT_EQ((std::ptrdiff_t)keys(children(root_2)[0]).size(), min_children);
    EXPECT_EQ(keys(children(root_2)[0])[0], 4);
    EXPECT_EQ(keys(children(root_2)[0])[1], 8);
    EXPECT_EQ(keys(children(root_2)[0])[2], 12);
    EXPECT_EQ(keys(children(root_2)[0])[3], 16);

    EXPECT_EQ(num_children(children(root_2)[1]), min_children);
    EXPECT_EQ((std::ptrdiff_t)keys(children(root_2)[1]).size(), min_children);
    EXPECT_EQ(keys(children(root_2)[1])[0], 4);
    EXPECT_EQ(keys(children(root_2)[1])[1], 8);
    EXPECT_EQ(keys(children(root_2)[1])[2], 12);
    EXPECT_EQ(keys(children(root_2)[1])[3], 16);
}

TEST(rope_btree, test_btree_split_child_extra_ref)
{
    node_ptr<rope_tag> root = make_tree_left_max();
    node_ptr<rope_tag> extra_ref = root;
    node_ptr<rope_tag> root_2 = btree_split_child(root, 0);

    EXPECT_EQ(extra_ref->refs_, 2);
    EXPECT_EQ(root_2->refs_, 1);

    EXPECT_EQ(children(root_2).size(), 3u);
    EXPECT_EQ(keys(root_2)[0], max_children * 2);
    EXPECT_EQ(keys(root_2)[1], max_children * 2 * 2);
    EXPECT_EQ(keys(root_2)[2], max_children * 2 * 2 + (max_children - 1) * 5);

    EXPECT_EQ(num_children(children(root_2)[0]), min_children);
    EXPECT_EQ((std::ptrdiff_t)keys(children(root_2)[0]).size(), min_children);
    EXPECT_EQ(keys(children(root_2)[0])[0], 4);
    EXPECT_EQ(keys(children(root_2)[0])[1], 8);
    EXPECT_EQ(keys(children(root_2)[0])[2], 12);
    EXPECT_EQ(keys(children(root_2)[0])[3], 16);

    EXPECT_EQ(num_children(children(root_2)[1]), min_children);
    EXPECT_EQ((std::ptrdiff_t)keys(children(root_2)[1]).size(), min_children);
    EXPECT_EQ(keys(children(root_2)[1])[0], 4);
    EXPECT_EQ(keys(children(root_2)[1])[1], 8);
    EXPECT_EQ(keys(children(root_2)[1])[2], 12);
    EXPECT_EQ(keys(children(root_2)[1])[3], 16);
}

TEST(rope_btree, test_btree_split_leaf)
{
    {
        node_ptr<rope_tag> root = make_tree_left_min();
        node_ptr<rope_tag> left = children(root)[0];
        btree_split_leaf(left, 1, 4);
        EXPECT_EQ(size(children(left)[1].get()), 4);
    }

    {
        node_ptr<rope_tag> root = make_tree_left_min();
        node_ptr<rope_tag> left = children(root)[0];
        left = btree_split_leaf(left, 1, 5);

        EXPECT_EQ(num_children(left), min_children + 1);

        EXPECT_EQ(keys(left)[0], 4);
        EXPECT_EQ(size(children(left)[1].get()), 1);
        EXPECT_EQ(keys(left)[1], 5);
        EXPECT_EQ(size(children(left)[2].get()), 3);
        EXPECT_EQ(keys(left)[2], 8);
        EXPECT_EQ(keys(left)[3], 12);
        EXPECT_EQ(keys(left)[4], 16);
    }

    {
        node_ptr<rope_tag> root = make_tree_left_min();
        node_ptr<rope_tag> left = children(root)[0];

        // Take an extra reference to the child begin split.
        node_ptr<rope_tag> left_1 = children(left)[1];

        left = btree_split_leaf(left, 1, 5);

        EXPECT_EQ(num_children(left), min_children + 1);

        EXPECT_EQ(keys(left)[0], 4);
        EXPECT_EQ(size(children(left)[1].get()), 1);
        EXPECT_EQ(keys(left)[1], 5);
        EXPECT_EQ(size(children(left)[2].get()), 3);
        EXPECT_EQ(keys(left)[2], 8);
        EXPECT_EQ(keys(left)[3], 12);
        EXPECT_EQ(keys(left)[4], 16);

        EXPECT_EQ(size(left_1.get()), 4);
    }
}

TEST(rope_btree, test_btree_split_leaf_extra_ref)
{
    {
        node_ptr<rope_tag> root = make_tree_left_min();
        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> extra_ref = left;
        left = btree_split_leaf(left, 1, 4);
        EXPECT_EQ(left->refs_, 3);
        EXPECT_EQ(extra_ref->refs_, 3);
        EXPECT_EQ(size(children(left)[1].get()), 4);
    }

    {
        node_ptr<rope_tag> root = make_tree_left_min();
        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> extra_ref = left;
        left = btree_split_leaf(left, 1, 5);

        EXPECT_EQ(left->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 2);

        EXPECT_EQ(num_children(left), min_children + 1);

        EXPECT_EQ(keys(left)[0], 4);
        EXPECT_EQ(size(children(left)[1].get()), 1);
        EXPECT_EQ(keys(left)[1], 5);
        EXPECT_EQ(size(children(left)[2].get()), 3);
        EXPECT_EQ(keys(left)[2], 8);
        EXPECT_EQ(keys(left)[3], 12);
        EXPECT_EQ(keys(left)[4], 16);
    }

    {
        node_ptr<rope_tag> root = make_tree_left_min();
        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> extra_ref = left;

        // Take an extra reference to the child begin split.
        node_ptr<rope_tag> left_1 = children(left)[1];

        left = btree_split_leaf(left, 1, 5);

        EXPECT_EQ(left->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 2);

        EXPECT_EQ(num_children(left), min_children + 1);

        EXPECT_EQ(keys(left)[0], 4);
        EXPECT_EQ(size(children(left)[1].get()), 1);
        EXPECT_EQ(keys(left)[1], 5);
        EXPECT_EQ(size(children(left)[2].get()), 3);
        EXPECT_EQ(keys(left)[2], 8);
        EXPECT_EQ(keys(left)[3], 12);
        EXPECT_EQ(keys(left)[4], 16);

        EXPECT_EQ(size(left_1.get()), 4);
    }
}

TEST(rope_btree, test_btree_insert_nonfull)
{
    // Insert into half-full interior child, then between existing leaves.
    {
        node_ptr<rope_tag> root = make_tree_left_min();

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), min_children);
        EXPECT_EQ(num_children(right), max_children - 1);

        node_ptr<rope_tag> new_root =
            btree_insert_nonfull(root, 4, make_node("new node"));

        left = children(new_root)[0];
        right = children(new_root)[1];

        EXPECT_EQ(num_children(new_root), 2);
        EXPECT_EQ(num_children(left), min_children + 1);

        EXPECT_EQ(keys(left)[0], 4);
        EXPECT_EQ(size(children(left)[1].get()), 8);
        EXPECT_EQ(keys(left)[1], 12);
        EXPECT_EQ(size(children(left)[2].get()), 4);
        EXPECT_EQ(keys(left)[2], 16);
        EXPECT_EQ(size(children(left)[3].get()), 4);
        EXPECT_EQ(keys(left)[3], 20);
        EXPECT_EQ(keys(left)[4], 24);
    }

    // Insert into half-full interior child, then into the middle of an
    // existing leaf.
    {
        node_ptr<rope_tag> root = make_tree_left_min();
        node_ptr<rope_tag> new_node = make_node("new node");

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), min_children);
        EXPECT_EQ(num_children(right), max_children - 1);

        node_ptr<rope_tag> new_root =
            btree_insert_nonfull(root, 5, make_node("new node"));

        left = children(new_root)[0];
        right = children(new_root)[1];

        EXPECT_EQ(num_children(new_root), 2);
        EXPECT_EQ(num_children(left), min_children + 2);

        EXPECT_EQ(keys(left)[0], 4);
        EXPECT_EQ(size(children(left)[1].get()), 1);
        EXPECT_EQ(keys(left)[1], 5);
        EXPECT_EQ(size(children(left)[2].get()), 8);
        EXPECT_EQ(keys(left)[2], 13);
        EXPECT_EQ(size(children(left)[3].get()), 3);
        EXPECT_EQ(keys(left)[3], 16);
        EXPECT_EQ(keys(left)[4], 20);
        EXPECT_EQ(keys(left)[5], 24);
    }

    // Insert into full interior child, then between existing leaves.
    {
        node_ptr<rope_tag> root = make_tree_left_max();

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), max_children);
        EXPECT_EQ(num_children(right), max_children - 1);

        node_ptr<rope_tag> new_root =
            btree_insert_nonfull(root, 4, make_node("new node"));

        left = children(new_root)[0];
        right = children(new_root)[1];

        EXPECT_EQ(num_children(new_root), 3);
        EXPECT_EQ(num_children(left), min_children + 1);

        EXPECT_EQ(keys(left)[0], 4);
        EXPECT_EQ(size(children(left)[1].get()), 8);
        EXPECT_EQ(keys(left)[1], 12);
        EXPECT_EQ(size(children(left)[2].get()), 4);
        EXPECT_EQ(keys(left)[2], 16);
        EXPECT_EQ(size(children(left)[3].get()), 4);
        EXPECT_EQ(keys(left)[3], 20);
        EXPECT_EQ(keys(left)[4], 24);
    }

    // Insert into almost-full interior child, then between existing leaves.
    {
        node_ptr<rope_tag> root = make_tree_left_max();

        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(right), max_children - 1);

        node_ptr<rope_tag> new_root = btree_insert_nonfull(
            root, size(root.get()) - 5, make_node("new node"));

        EXPECT_EQ(num_children(new_root), 3);

        node_ptr<rope_tag> new_right = children(new_root)[2];
        EXPECT_EQ(num_children(new_right), min_children);

        EXPECT_EQ(size(children(new_right)[min_children - 2].get()), 8);
        EXPECT_EQ(
            keys(new_right)[min_children - 2], (min_children - 2) * 5 + 8);
        EXPECT_EQ(size(children(new_right)[min_children - 1].get()), 5);
        EXPECT_EQ(
            keys(new_right)[min_children - 1], (min_children - 2) * 5 + 8 + 5);
    }

    // Insert into almost-full interior child, then into the middle of an
    // existing leaf.
    {
        node_ptr<rope_tag> root = make_tree_left_max();

        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(right), max_children - 1);

        node_ptr<rope_tag> new_root = btree_insert_nonfull(
            root, size(root.get()) - 2, make_node("new node"));

        EXPECT_EQ(num_children(new_root), 3);

        node_ptr<rope_tag> new_right = children(new_root)[2];
        EXPECT_EQ(num_children(new_right), min_children + 1);

        EXPECT_EQ(size(children(new_right)[min_children - 2].get()), 3);
        EXPECT_EQ(
            keys(new_right)[min_children - 2], (min_children - 2) * 5 + 3);
        EXPECT_EQ(size(children(new_right)[min_children - 1].get()), 8);
        EXPECT_EQ(
            keys(new_right)[min_children - 1], (min_children - 2) * 5 + 3 + 8);
        EXPECT_EQ(size(children(new_right)[min_children].get()), 2);
        EXPECT_EQ(
            keys(new_right)[min_children], (min_children - 2) * 5 + 3 + 8 + 2);
    }

    // Insert into almost-full interior child, then after the last leaf.
    {
        node_ptr<rope_tag> root = make_tree_left_max();

        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(right), max_children - 1);

        node_ptr<rope_tag> new_root =
            btree_insert_nonfull(root, size(root.get()), make_node("new node"));

        EXPECT_EQ(num_children(new_root), 3);

        node_ptr<rope_tag> new_right = children(new_root)[2];
        EXPECT_EQ(num_children(new_right), min_children);

        EXPECT_EQ(size(children(new_right)[min_children - 1].get()), 8);
        EXPECT_EQ(
            keys(new_right)[min_children - 1], (min_children - 1) * 5 + 8);
    }

    // Copy vs. mutation coverage.

    // No nodes copied.
    {
        node_ptr<rope_tag> root = make_tree_left_min();

        node_ptr<rope_tag> left = children(root)[0];

        EXPECT_EQ(num_children(left), min_children);

        node_ptr<rope_tag> new_root =
            btree_insert_nonfull(root, 4, make_node("new node"));

        left = children(new_root)[0];

        EXPECT_EQ(num_children(new_root), 2);
        EXPECT_EQ(num_children(left), min_children + 1);
        EXPECT_NE(root.as_interior(), new_root.as_interior());
    }

    // Root copied.
    {
        node_ptr<rope_tag> root = make_tree_left_min();
        node_ptr<rope_tag> root_2 = root;

        node_ptr<rope_tag> left = children(root)[0];

        EXPECT_EQ(num_children(left), min_children);

        node_ptr<rope_tag> new_root =
            btree_insert_nonfull(root, 4, make_node("new node"));

        left = children(new_root)[0];

        EXPECT_EQ(num_children(new_root), 2);
        EXPECT_EQ(num_children(left), min_children + 1);
        EXPECT_NE(root.as_interior(), new_root.as_interior());
        EXPECT_EQ(root.as_interior(), root_2.as_interior());
    }

    // Interior node copied.
    {
        node_ptr<rope_tag> root = make_tree_left_min();

        node_ptr<rope_tag> left = children(root)[0];

        EXPECT_EQ(num_children(left), min_children);

        node_ptr<rope_tag> new_root =
            btree_insert_nonfull(root, 4, make_node("new node"));

        node_ptr<rope_tag> new_left = children(new_root)[0];

        EXPECT_EQ(num_children(new_root), 2);
        EXPECT_EQ(num_children(left), min_children);
        EXPECT_EQ(num_children(new_left), min_children + 1);
    }
}

TEST(rope_btree, test_btree_insert_nonfull_extra_ref)
{
    // Insert into half-full interior child, then between existing leaves.
    {
        node_ptr<rope_tag> root = make_tree_left_min();
        node_ptr<rope_tag> extra_ref = root;

        {
            node_ptr<rope_tag> left = children(root)[0];
            node_ptr<rope_tag> right = children(root)[1];

            EXPECT_EQ(num_children(left), min_children);
            EXPECT_EQ(num_children(right), max_children - 1);
        }

        node_ptr<rope_tag> new_root =
            btree_insert_nonfull(root, 4, make_node("new node"));

        EXPECT_EQ(new_root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 2);

        node_ptr<rope_tag> left = children(new_root)[0];

        EXPECT_EQ(num_children(new_root), 2);
        EXPECT_EQ(num_children(left), min_children + 1);

        EXPECT_EQ(keys(left)[0], 4);
        EXPECT_EQ(size(children(left)[1].get()), 8);
        EXPECT_EQ(keys(left)[1], 12);
        EXPECT_EQ(size(children(left)[2].get()), 4);
        EXPECT_EQ(keys(left)[2], 16);
        EXPECT_EQ(size(children(left)[3].get()), 4);
        EXPECT_EQ(keys(left)[3], 20);
        EXPECT_EQ(keys(left)[4], 24);
    }

    // Insert into half-full interior child, then into the middle of an
    // existing leaf.
    {
        node_ptr<rope_tag> root = make_tree_left_min();
        node_ptr<rope_tag> new_node = make_node("new node");
        node_ptr<rope_tag> extra_ref = root;

        {
            node_ptr<rope_tag> left = children(root)[0];
            node_ptr<rope_tag> right = children(root)[1];

            EXPECT_EQ(num_children(left), min_children);
            EXPECT_EQ(num_children(right), max_children - 1);
        }

        node_ptr<rope_tag> new_root =
            btree_insert_nonfull(root, 5, make_node("new node"));

        EXPECT_EQ(new_root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 2);

        node_ptr<rope_tag> left = children(new_root)[0];

        EXPECT_EQ(num_children(new_root), 2);
        EXPECT_EQ(num_children(left), min_children + 2);

        EXPECT_EQ(keys(left)[0], 4);
        EXPECT_EQ(size(children(left)[1].get()), 1);
        EXPECT_EQ(keys(left)[1], 5);
        EXPECT_EQ(size(children(left)[2].get()), 8);
        EXPECT_EQ(keys(left)[2], 13);
        EXPECT_EQ(size(children(left)[3].get()), 3);
        EXPECT_EQ(keys(left)[3], 16);
        EXPECT_EQ(keys(left)[4], 20);
        EXPECT_EQ(keys(left)[5], 24);
    }

    // Insert into full interior child, then between existing leaves.
    {
        node_ptr<rope_tag> root = make_tree_left_max();
        node_ptr<rope_tag> extra_ref = root;

        {
            node_ptr<rope_tag> left = children(root)[0];
            node_ptr<rope_tag> right = children(root)[1];

            EXPECT_EQ(num_children(left), max_children);
            EXPECT_EQ(num_children(right), max_children - 1);
        }

        node_ptr<rope_tag> new_root =
            btree_insert_nonfull(root, 4, make_node("new node"));

        EXPECT_EQ(new_root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 2);

        node_ptr<rope_tag> left = children(new_root)[0];

        EXPECT_EQ(num_children(new_root), 3);
        EXPECT_EQ(num_children(left), min_children + 1);

        EXPECT_EQ(keys(left)[0], 4);
        EXPECT_EQ(size(children(left)[1].get()), 8);
        EXPECT_EQ(keys(left)[1], 12);
        EXPECT_EQ(size(children(left)[2].get()), 4);
        EXPECT_EQ(keys(left)[2], 16);
        EXPECT_EQ(size(children(left)[3].get()), 4);
        EXPECT_EQ(keys(left)[3], 20);
        EXPECT_EQ(keys(left)[4], 24);
    }

    // Insert into almost-full interior child, then between existing leaves.
    {
        node_ptr<rope_tag> root = make_tree_left_max();
        node_ptr<rope_tag> extra_ref = root;

        {
            node_ptr<rope_tag> right = children(root)[1];

            EXPECT_EQ(num_children(right), max_children - 1);
        }

        node_ptr<rope_tag> new_root = btree_insert_nonfull(
            root, size(root.get()) - 5, make_node("new node"));

        EXPECT_EQ(new_root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 2);

        EXPECT_EQ(num_children(new_root), 3);

        node_ptr<rope_tag> new_right = children(new_root)[2];
        EXPECT_EQ(num_children(new_right), min_children);

        EXPECT_EQ(size(children(new_right)[min_children - 2].get()), 8);
        EXPECT_EQ(
            keys(new_right)[min_children - 2], (min_children - 2) * 5 + 8);
        EXPECT_EQ(size(children(new_right)[min_children - 1].get()), 5);
        EXPECT_EQ(
            keys(new_right)[min_children - 1], (min_children - 2) * 5 + 8 + 5);
    }

    // Insert into almost-full interior child, then into the middle of an
    // existing leaf.
    {
        node_ptr<rope_tag> root = make_tree_left_max();
        node_ptr<rope_tag> extra_ref = root;

        {
            node_ptr<rope_tag> right = children(root)[1];

            EXPECT_EQ(num_children(right), max_children - 1);
        }

        node_ptr<rope_tag> new_root = btree_insert_nonfull(
            root, size(root.get()) - 2, make_node("new node"));

        EXPECT_EQ(new_root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 2);

        EXPECT_EQ(num_children(new_root), 3);

        node_ptr<rope_tag> new_right = children(new_root)[2];
        EXPECT_EQ(num_children(new_right), min_children + 1);

        EXPECT_EQ(size(children(new_right)[min_children - 2].get()), 3);
        EXPECT_EQ(
            keys(new_right)[min_children - 2], (min_children - 2) * 5 + 3);
        EXPECT_EQ(size(children(new_right)[min_children - 1].get()), 8);
        EXPECT_EQ(
            keys(new_right)[min_children - 1], (min_children - 2) * 5 + 3 + 8);
        EXPECT_EQ(size(children(new_right)[min_children].get()), 2);
        EXPECT_EQ(
            keys(new_right)[min_children], (min_children - 2) * 5 + 3 + 8 + 2);
    }

    // Insert into almost-full interior child, then after the last leaf.
    {
        node_ptr<rope_tag> root = make_tree_left_max();
        node_ptr<rope_tag> extra_ref = root;

        {
            node_ptr<rope_tag> right = children(root)[1];

            EXPECT_EQ(num_children(right), max_children - 1);
        }

        node_ptr<rope_tag> new_root =
            btree_insert_nonfull(root, size(root.get()), make_node("new node"));

        EXPECT_EQ(new_root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 2);

        EXPECT_EQ(num_children(new_root), 3);

        node_ptr<rope_tag> new_right = children(new_root)[2];
        EXPECT_EQ(num_children(new_right), min_children);

        EXPECT_EQ(size(children(new_right)[min_children - 1].get()), 8);
        EXPECT_EQ(
            keys(new_right)[min_children - 1], (min_children - 1) * 5 + 8);
    }
}

int height_at(node_ptr<rope_tag> const & node, std::ptrdiff_t at)
{
    found_leaf<rope_tag> found;
    find_leaf(node, at, found);
    return (int)found.path_.size();
}

void check_leaf_heights(node_ptr<rope_tag> const & node)
{
    found_leaf<rope_tag> found;
    find_leaf(node, 0, found);
    int const first_leaf_height = (int)found.path_.size();
    std::ptrdiff_t offset = 0;
    while (offset != size(node.get())) {
        EXPECT_EQ(height_at(node, offset), first_leaf_height);
        found_leaf<rope_tag> found_next;
        find_leaf(node, offset, found_next);
        offset += found_next.leaf_->as_leaf()->size();
    }
}

TEST(rope_btree, test_btree_insert)
{
    {
        node_ptr<rope_tag> root = make_node("root");
        root = btree_insert(root, 0, make_node("new"));

        EXPECT_FALSE(root->leaf_);
        EXPECT_EQ(num_children(root), 2);

        check_leaf_heights(root);
    }

    {
        node_ptr<rope_tag> root = make_node("root");
        root = btree_insert(root, 4, make_node("new"));

        EXPECT_FALSE(root->leaf_);
        EXPECT_EQ(num_children(root), 2);

        check_leaf_heights(root);
    }

    {
        node_ptr<rope_tag> root = make_node("root");
        root = btree_insert(root, 2, make_node("new"));

        EXPECT_FALSE(root->leaf_);
        EXPECT_EQ(num_children(root), 3);

        check_leaf_heights(root);
    }

    {
        node_ptr<rope_tag> root =
            make_interior_with_leaves("child", max_children - 1);
        root = btree_insert(root, 2, make_node("new 1"));

        EXPECT_EQ(num_children(root), 2);

        check_leaf_heights(root);
    }

    {
        node_ptr<rope_tag> root =
            make_interior_with_leaves("child", max_children);
        root = btree_insert(root, 2, make_node("new 1"));

        EXPECT_EQ(num_children(root), 2);

        check_leaf_heights(root);
    }

    // Check that many inserts maintains balance.
    {
        node_ptr<rope_tag> root = make_node("node");

        int const N = 100000;
        for (int i = 0; i < N; ++i) {
            root = btree_insert(root, 2, make_node("new node"));
        }

        check_leaf_heights(root);

        std::cout << "N=" << N << " leaves gives a tree of height "
                  << height_at(root, 0) << "\n";
    }
}

TEST(rope_btree, test_btree_insert_extra_ref)
{
    {
        node_ptr<rope_tag> root = make_node("root");
        node_ptr<rope_tag> extra_ref = root;
        node_ptr<rope_tag> extra_ref_2 = root;
        root = btree_insert(root, 0, make_node("new"));

        EXPECT_EQ(root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 3);

        EXPECT_FALSE(root->leaf_);
        EXPECT_EQ(num_children(root), 2);

        check_leaf_heights(root);
    }

    {
        node_ptr<rope_tag> root = make_node("root");
        node_ptr<rope_tag> extra_ref = root;
        node_ptr<rope_tag> extra_ref_2 = root;
        root = btree_insert(root, 4, make_node("new"));

        EXPECT_EQ(root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 3);

        EXPECT_FALSE(root->leaf_);
        EXPECT_EQ(num_children(root), 2);

        check_leaf_heights(root);
    }

    {
        node_ptr<rope_tag> root = make_node("root");
        node_ptr<rope_tag> extra_ref = root;
        node_ptr<rope_tag> extra_ref_2 = root;
        root = btree_insert(root, 2, make_node("new"));

        EXPECT_EQ(root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 4);

        EXPECT_FALSE(root->leaf_);
        EXPECT_EQ(num_children(root), 3);

        check_leaf_heights(root);
    }

    {
        node_ptr<rope_tag> root =
            make_interior_with_leaves("child", max_children - 1);
        node_ptr<rope_tag> extra_ref = root;
        node_ptr<rope_tag> extra_ref_2 = root;
        root = btree_insert(root, 2, make_node("new 1"));

        EXPECT_EQ(root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 2);

        EXPECT_EQ(num_children(root), 2);

        check_leaf_heights(root);
    }

    {
        node_ptr<rope_tag> root =
            make_interior_with_leaves("child", max_children);
        node_ptr<rope_tag> extra_ref = root;
        node_ptr<rope_tag> extra_ref_2 = root;
        root = btree_insert(root, 2, make_node("new 1"));

        EXPECT_EQ(root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 2);

        EXPECT_EQ(num_children(root), 2);

        check_leaf_heights(root);
    }

    // Check that many inserts maintains balance.
    {
        node_ptr<rope_tag> root = make_node("node");
        node_ptr<rope_tag> extra_ref = root;
        node_ptr<rope_tag> extra_ref_2 = root;

        int const N = 100000;
        for (int i = 0; i < N; ++i) {
            root = btree_insert(root, 2, make_node("new node"));
        }

        EXPECT_EQ(root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 4);

        check_leaf_heights(root);

        std::cout << "N=" << N << " leaves gives a tree of height "
                  << height_at(root, 0) << "\n";
    }
}

TEST(rope_btree, test_btree_erase_entire_node_leaf_children)
{
    {
        node_ptr<rope_tag> root = make_interior_with_leaves("leaf", 3);

        EXPECT_EQ(num_children(root), 3);

        root = btree_erase(root, 0, children(root)[0].as_leaf());

        EXPECT_EQ(keys(root)[0], 4);
        EXPECT_EQ(size(children(root)[0].get()), 4);
        EXPECT_EQ(keys(root)[1], 8);
        EXPECT_EQ(size(children(root)[1].get()), 4);
    }

    {
        node_ptr<rope_tag> root = make_interior_with_leaves("leaf", 3);

        EXPECT_EQ(num_children(root), 3);

        root = btree_erase(root, 4, children(root)[1].as_leaf());

        EXPECT_EQ(keys(root)[0], 4);
        EXPECT_EQ(size(children(root)[0].get()), 4);
        EXPECT_EQ(keys(root)[1], 8);
        EXPECT_EQ(size(children(root)[1].get()), 4);
    }

    {
        node_ptr<rope_tag> root = make_interior_with_leaves("leaf", 3);

        EXPECT_EQ(num_children(root), 3);

        root = btree_erase(root, 8, children(root)[2].as_leaf());

        EXPECT_EQ(keys(root)[0], 4);
        EXPECT_EQ(size(children(root)[0].get()), 4);
        EXPECT_EQ(keys(root)[1], 8);
        EXPECT_EQ(size(children(root)[1].get()), 4);
    }

    {
        node_ptr<rope_tag> root = make_interior_with_leaves("leaf", 3);

        EXPECT_EQ(num_children(root), 3);

        root = btree_erase(root, 12, children(root)[2].as_leaf());

        EXPECT_EQ(keys(root)[0], 4);
        EXPECT_EQ(size(children(root)[0].get()), 4);
        EXPECT_EQ(keys(root)[1], 8);
        EXPECT_EQ(size(children(root)[1].get()), 4);
    }

    {
        node_ptr<rope_tag> root;
        {
            interior_node_t<rope_tag> * int_root = nullptr;
            root = node_ptr<rope_tag>(int_root = new_interior_node<rope_tag>());
            int_root->children_.push_back(make_node("left"));
            int_root->keys_.push_back(size(int_root->children_[0].get()));
            int_root->children_.push_back(make_node("right"));
            int_root->keys_.push_back(
                int_root->keys_.back() + size(int_root->children_[1].get()));
        }

        EXPECT_EQ(num_children(root), 2);

        root = btree_erase(root, 0, children(root)[0].as_leaf());

        EXPECT_TRUE(root->leaf_);
        EXPECT_EQ(size(root.get()), 5);
    }

    {
        node_ptr<rope_tag> root;
        {
            interior_node_t<rope_tag> * int_root = nullptr;
            root = node_ptr<rope_tag>(int_root = new_interior_node<rope_tag>());
            int_root->children_.push_back(make_node("left"));
            int_root->keys_.push_back(size(int_root->children_[0].get()));
            int_root->children_.push_back(make_node("right"));
            int_root->keys_.push_back(
                int_root->keys_.back() + size(int_root->children_[1].get()));
        }

        EXPECT_EQ(num_children(root), 2);

        root = btree_erase(root, 4, children(root)[1].as_leaf());

        EXPECT_TRUE(root->leaf_);
        EXPECT_EQ(size(root.get()), 4);
    }

    {
        node_ptr<rope_tag> root;
        {
            interior_node_t<rope_tag> * int_root = nullptr;
            root = node_ptr<rope_tag>(int_root = new_interior_node<rope_tag>());
            int_root->children_.push_back(make_node("left"));
            int_root->keys_.push_back(size(int_root->children_[0].get()));
            int_root->children_.push_back(make_node("right"));
            int_root->keys_.push_back(
                int_root->keys_.back() + size(int_root->children_[1].get()));
        }

        EXPECT_EQ(num_children(root), 2);

        root = btree_erase(root, 9, children(root)[1].as_leaf());

        EXPECT_TRUE(root->leaf_);
        EXPECT_EQ(size(root.get()), 4);
    }
}

TEST(rope_btree, test_btree_erase_entire_node_leaf_children_extra_ref)
{
    {
        node_ptr<rope_tag> root = make_interior_with_leaves("leaf", 3);
        node_ptr<rope_tag> extra_ref = root;
        node_ptr<rope_tag> extra_ref_2 = root;

        EXPECT_EQ(num_children(root), 3);

        root = btree_erase(root, 0, children(root)[0].as_leaf());

        EXPECT_EQ(root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 2);

        EXPECT_EQ(keys(root)[0], 4);
        EXPECT_EQ(size(children(root)[0].get()), 4);
        EXPECT_EQ(keys(root)[1], 8);
        EXPECT_EQ(size(children(root)[1].get()), 4);
    }

    {
        node_ptr<rope_tag> root = make_interior_with_leaves("leaf", 3);
        node_ptr<rope_tag> extra_ref = root;
        node_ptr<rope_tag> extra_ref_2 = root;

        EXPECT_EQ(num_children(root), 3);

        root = btree_erase(root, 4, children(root)[1].as_leaf());

        EXPECT_EQ(root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 2);

        EXPECT_EQ(keys(root)[0], 4);
        EXPECT_EQ(size(children(root)[0].get()), 4);
        EXPECT_EQ(keys(root)[1], 8);
        EXPECT_EQ(size(children(root)[1].get()), 4);
    }

    {
        node_ptr<rope_tag> root = make_interior_with_leaves("leaf", 3);
        node_ptr<rope_tag> extra_ref = root;
        node_ptr<rope_tag> extra_ref_2 = root;

        EXPECT_EQ(num_children(root), 3);

        root = btree_erase(root, 8, children(root)[2].as_leaf());

        EXPECT_EQ(root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 2);

        EXPECT_EQ(keys(root)[0], 4);
        EXPECT_EQ(size(children(root)[0].get()), 4);
        EXPECT_EQ(keys(root)[1], 8);
        EXPECT_EQ(size(children(root)[1].get()), 4);
    }

    {
        node_ptr<rope_tag> root = make_interior_with_leaves("leaf", 3);
        node_ptr<rope_tag> extra_ref = root;
        node_ptr<rope_tag> extra_ref_2 = root;

        EXPECT_EQ(num_children(root), 3);

        root = btree_erase(root, 12, children(root)[2].as_leaf());

        EXPECT_EQ(root->refs_, 1);
        EXPECT_EQ(extra_ref->refs_, 2);

        EXPECT_EQ(keys(root)[0], 4);
        EXPECT_EQ(size(children(root)[0].get()), 4);
        EXPECT_EQ(keys(root)[1], 8);
        EXPECT_EQ(size(children(root)[1].get()), 4);
    }

    {
        node_ptr<rope_tag> root;
        {
            interior_node_t<rope_tag> * int_root = nullptr;
            root = node_ptr<rope_tag>(int_root = new_interior_node<rope_tag>());
            int_root->children_.push_back(make_node("left"));
            int_root->keys_.push_back(size(int_root->children_[0].get()));
            int_root->children_.push_back(make_node("right"));
            int_root->keys_.push_back(
                int_root->keys_.back() + size(int_root->children_[1].get()));
        }
        node_ptr<rope_tag> extra_ref = root;

        EXPECT_EQ(num_children(root), 2);

        root = btree_erase(root, 0, children(root)[0].as_leaf());

        EXPECT_NE(root.get(), extra_ref.get());

        EXPECT_TRUE(root->leaf_);
        EXPECT_EQ(size(root.get()), 5);
    }

    {
        node_ptr<rope_tag> root;
        {
            interior_node_t<rope_tag> * int_root = nullptr;
            root = node_ptr<rope_tag>(int_root = new_interior_node<rope_tag>());
            int_root->children_.push_back(make_node("left"));
            int_root->keys_.push_back(size(int_root->children_[0].get()));
            int_root->children_.push_back(make_node("right"));
            int_root->keys_.push_back(
                int_root->keys_.back() + size(int_root->children_[1].get()));
        }
        node_ptr<rope_tag> extra_ref = root;

        EXPECT_EQ(num_children(root), 2);

        root = btree_erase(root, 4, children(root)[1].as_leaf());

        EXPECT_NE(root.get(), extra_ref.get());

        EXPECT_TRUE(root->leaf_);
        EXPECT_EQ(size(root.get()), 4);
    }

    {
        node_ptr<rope_tag> root;
        {
            interior_node_t<rope_tag> * int_root = nullptr;
            root = node_ptr<rope_tag>(int_root = new_interior_node<rope_tag>());
            int_root->children_.push_back(make_node("left"));
            int_root->keys_.push_back(size(int_root->children_[0].get()));
            int_root->children_.push_back(make_node("right"));
            int_root->keys_.push_back(
                int_root->keys_.back() + size(int_root->children_[1].get()));
        }
        node_ptr<rope_tag> extra_ref = root;

        EXPECT_EQ(num_children(root), 2);

        root = btree_erase(root, 9, children(root)[1].as_leaf());

        EXPECT_NE(root.get(), extra_ref.get());

        EXPECT_TRUE(root->leaf_);
        EXPECT_EQ(size(root.get()), 4);
    }
}

// The rest of these don't have _extra_ref variants, because the
// rope::operator() tests exercise the shared node cases well enough.

TEST(rope_btree, test_btree_erase_entire_node_interior_children)
{
    // Last interior node has more than min children.
    {
        node_ptr<rope_tag> root = make_tree_left_min();

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), min_children);
        EXPECT_EQ(num_children(right), max_children - 1);

        root =
            btree_erase(root, min_children * 4, children(right)[0].as_leaf());

        left = children(root)[0];
        right = children(root)[1];

        EXPECT_EQ(num_children(root), 2);
        EXPECT_EQ(num_children(left), min_children);
        EXPECT_EQ(num_children(right), max_children - 2);

        EXPECT_EQ(keys(left).back(), min_children * 4);
        EXPECT_EQ(keys(right).back(), (max_children - 2) * 5);
    }

    {
        node_ptr<rope_tag> root = make_tree_left_min();

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), min_children);
        EXPECT_EQ(num_children(right), max_children - 1);

        root = btree_erase(
            root,
            min_children * 4 + (max_children - 1) * 5,
            children(right).back().as_leaf());

        left = children(root)[0];
        right = children(root)[1];

        EXPECT_EQ(num_children(root), 2);
        EXPECT_EQ(num_children(left), min_children);
        EXPECT_EQ(num_children(right), max_children - 2);

        EXPECT_EQ(keys(left).back(), min_children * 4);
        EXPECT_EQ(keys(right).back(), (max_children - 2) * 5);
    }

    // Last interior node min children, left has min children.
    {
        node_ptr<rope_tag> root =
            make_tree_left_right<min_children, max_children>();

        auto const root_initial_size = size(root.get());

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), min_children);
        EXPECT_EQ(num_children(right), max_children);

        root = btree_erase(root, 0, children(left).front().as_leaf());

        left = children(root)[0];
        right = children(root)[1];

        EXPECT_EQ(num_children(root), 2);
        EXPECT_EQ(num_children(left), min_children);
        EXPECT_EQ(num_children(right), max_children - 1);

        EXPECT_EQ(keys(root)[0], (min_children - 1) * 4 + 5);
        EXPECT_EQ(keys(root)[1], root_initial_size - 4);
        EXPECT_EQ(keys(left).back(), (min_children - 1) * 4 + 5);
        EXPECT_EQ(keys(right).back(), (max_children - 1) * 5);
    }

    {
        node_ptr<rope_tag> root =
            make_tree_left_right<min_children, max_children>();

        auto const root_initial_size = size(root.get());

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), min_children);
        EXPECT_EQ(num_children(right), max_children);

        root = btree_erase(
            root, min_children * 4 - 1, children(left).back().as_leaf());

        left = children(root)[0];
        right = children(root)[1];

        EXPECT_EQ(num_children(root), 2);
        EXPECT_EQ(num_children(left), min_children);
        EXPECT_EQ(num_children(right), max_children - 1);

        EXPECT_EQ(keys(root)[0], (min_children - 1) * 4 + 5);
        EXPECT_EQ(keys(root)[1], root_initial_size - 4);
        EXPECT_EQ(keys(left).back(), (min_children - 1) * 4 + 5);
        EXPECT_EQ(keys(right).back(), (max_children - 1) * 5);
    }

    // Last interior node min children, right has min children.
    {
        node_ptr<rope_tag> root =
            make_tree_left_right<max_children, min_children>();

        auto const root_initial_size = size(root.get());

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), max_children);
        EXPECT_EQ(num_children(right), min_children);

        root = btree_erase(
            root, size(root.get()), children(right).back().as_leaf());

        left = children(root)[0];
        right = children(root)[1];

        EXPECT_EQ(num_children(root), 2);
        EXPECT_EQ(num_children(left), max_children - 1);
        EXPECT_EQ(num_children(right), min_children);

        EXPECT_EQ(keys(root)[0], (max_children - 1) * 4);
        EXPECT_EQ(keys(root)[1], root_initial_size - 5);
        EXPECT_EQ(keys(left).back(), (max_children - 1) * 4);
        EXPECT_EQ(keys(right).back(), 4 + (min_children - 1) * 5);
    }

    {
        node_ptr<rope_tag> root =
            make_tree_left_right<max_children, min_children>();

        auto const root_initial_size = size(root.get());

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), max_children);
        EXPECT_EQ(num_children(right), min_children);

        root = btree_erase(
            root, max_children * 4, children(right).front().as_leaf());

        left = children(root)[0];
        right = children(root)[1];

        EXPECT_EQ(num_children(root), 2);
        EXPECT_EQ(num_children(left), max_children - 1);
        EXPECT_EQ(num_children(right), min_children);

        EXPECT_EQ(keys(root)[0], (max_children - 1) * 4);
        EXPECT_EQ(keys(root)[1], root_initial_size - 5);
        EXPECT_EQ(keys(left).back(), (max_children - 1) * 4);
        EXPECT_EQ(keys(right).back(), 4 + (min_children - 1) * 5);
    }

    // Last interior node min children, both sides have min children.
    {
        node_ptr<rope_tag> root =
            make_tree_left_right<min_children, min_children>();

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), min_children);
        EXPECT_EQ(num_children(right), min_children);

        root = btree_erase(
            root, size(root.get()), children(right).back().as_leaf());

        EXPECT_EQ(num_children(root), max_children - 1);

        int i = 0;
        std::ptrdiff_t sz = 0;
        for (; i < min_children - 1; ++i) {
            sz += size(children(root)[i].get());
            EXPECT_EQ(keys(root)[i], sz) << "i=" << i;
        }
        for (; i < max_children - 1; ++i) {
            sz += size(children(root)[i].get());
            EXPECT_EQ(keys(root)[i], sz) << "i=" << i;
        }
    }

    {
        node_ptr<rope_tag> root =
            make_tree_left_right<min_children, min_children>();

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), min_children);
        EXPECT_EQ(num_children(right), min_children);

        root = btree_erase(root, 0, children(left).front().as_leaf());

        EXPECT_EQ(num_children(root), max_children - 1);

        int i = 0;
        std::ptrdiff_t sz = 0;
        for (; i < min_children; ++i) {
            sz += size(children(root)[i].get());
            EXPECT_EQ(keys(root)[i], sz) << "i=" << i;
        }
        for (; i < max_children - 1; ++i) {
            sz += size(children(root)[i].get());
            EXPECT_EQ(keys(root)[i], sz) << "i=" << i;
        }
    }

    // Last interior node min children, all three children have min children.
    {
        node_ptr<rope_tag> root = make_tree_left_center_right<
            min_children,
            min_children,
            min_children>();

        auto const root_initial_size = size(root.get());

        {
            node_ptr<rope_tag> left = children(root)[0];
            node_ptr<rope_tag> center = children(root)[1];
            node_ptr<rope_tag> right = children(root)[2];

            EXPECT_EQ(num_children(left), min_children);
            EXPECT_EQ(num_children(center), min_children);
            EXPECT_EQ(num_children(right), min_children);

            root = btree_erase(root, 0, children(left).front().as_leaf());
        }

        EXPECT_EQ(num_children(root), 2);

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), max_children - 1);
        EXPECT_EQ(num_children(right), min_children);
        EXPECT_EQ(keys(root)[0], (min_children - 1) * 4 + min_children * 6);
        EXPECT_EQ(keys(root)[1], root_initial_size - 4);
        EXPECT_EQ(keys(left).back(), (min_children - 1) * 4 + min_children * 6);
        EXPECT_EQ(keys(right).back(), min_children * 5);
    }

    {
        node_ptr<rope_tag> root = make_tree_left_center_right<
            min_children,
            min_children,
            min_children>();

        auto const root_initial_size = size(root.get());

        {
            node_ptr<rope_tag> left = children(root)[0];
            node_ptr<rope_tag> center = children(root)[1];
            node_ptr<rope_tag> right = children(root)[2];

            EXPECT_EQ(num_children(left), min_children);
            EXPECT_EQ(num_children(center), min_children);
            EXPECT_EQ(num_children(right), min_children);

            root = btree_erase(
                root, min_children * 4, children(center).front().as_leaf());
        }

        EXPECT_EQ(num_children(root), 2);

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), max_children - 1);
        EXPECT_EQ(num_children(right), min_children);
        EXPECT_EQ(keys(root)[0], min_children * 4 + (min_children - 1) * 6);
        EXPECT_EQ(keys(root)[1], root_initial_size - 6);
        EXPECT_EQ(keys(left).back(), min_children * 4 + (min_children - 1) * 6);
        EXPECT_EQ(keys(right).back(), min_children * 5);
    }

    {
        node_ptr<rope_tag> root = make_tree_left_center_right<
            min_children,
            min_children,
            min_children>();

        auto const root_initial_size = size(root.get());

        {
            node_ptr<rope_tag> left = children(root)[0];
            node_ptr<rope_tag> center = children(root)[1];
            node_ptr<rope_tag> right = children(root)[2];

            EXPECT_EQ(num_children(left), min_children);
            EXPECT_EQ(num_children(center), min_children);
            EXPECT_EQ(num_children(right), min_children);

            root = btree_erase(
                root, size(root.get()), children(right).back().as_leaf());
        }

        EXPECT_EQ(num_children(root), 2);

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), min_children);
        EXPECT_EQ(num_children(right), max_children - 1);
        EXPECT_EQ(keys(root)[0], min_children * 4);
        EXPECT_EQ(keys(root)[1], root_initial_size - 5);
        EXPECT_EQ(keys(left).back(), min_children * 4);
        EXPECT_EQ(
            keys(right).back(), min_children * 6 + (min_children - 1) * 5);
    }
}

TEST(rope_btree, test_btree_erase)
{
    // Erasure from a leaf node
    {
        node_ptr<rope_tag> root = make_node("sliceable");

        root = btree_erase(root, 0, 9);

        EXPECT_EQ(root.get(), nullptr);
    }

    {
        node_ptr<rope_tag> root = make_node("sliceable");

        root = btree_erase(root, 0, 8);

        EXPECT_TRUE(root->leaf_);
        EXPECT_EQ(size(root.get()), 1);
    }

    {
        node_ptr<rope_tag> root = make_node("sliceable");

        root = btree_erase(root, 1, 8);

        EXPECT_FALSE(root->leaf_);
        EXPECT_EQ(size(root.get()), 2);
    }

    // Erasure from non-leaf nodes, entire segments only

    {
        node_ptr<rope_tag> root =
            make_tree_left_right<max_children, max_children>();

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), max_children);
        EXPECT_EQ(num_children(right), max_children);

        root = btree_erase(root, (max_children - 1) * 4, max_children * 4);

        left = children(root)[0];
        right = children(root)[1];

        EXPECT_EQ(num_children(root), 2);
        EXPECT_EQ(num_children(left), max_children - 1);
        EXPECT_EQ(num_children(right), max_children);

        EXPECT_EQ(keys(root)[0], (max_children - 1) * 4);
        EXPECT_EQ(keys(root)[1], (max_children - 1) * 4 + max_children * 5);
        EXPECT_EQ(keys(left).back(), (max_children - 1) * 4);
        EXPECT_EQ(keys(right).back(), max_children * 5);
    }

    {
        node_ptr<rope_tag> root =
            make_tree_left_right<max_children, max_children>();

        node_ptr<rope_tag> left = children(root)[0];
        node_ptr<rope_tag> right = children(root)[1];

        EXPECT_EQ(num_children(left), max_children);
        EXPECT_EQ(num_children(right), max_children);

        root = btree_erase(root, max_children * 4, max_children * 4 + 5);

        left = children(root)[0];
        right = children(root)[1];

        EXPECT_EQ(num_children(root), 2);
        EXPECT_EQ(num_children(left), max_children);
        EXPECT_EQ(num_children(right), max_children - 1);

        EXPECT_EQ(keys(root)[0], max_children * 4);
        EXPECT_EQ(keys(root)[1], max_children * 4 + (max_children - 1) * 5);
        EXPECT_EQ(keys(left).back(), max_children * 4);
        EXPECT_EQ(keys(right).back(), (max_children - 1) * 5);
    }

    // Erasure from non-leaf nodes, including partial segments

    node_ptr<rope_tag> const tree =
        make_tree_left_right<max_children, max_children>();
    unencoded_rope const tree_rope(tree);
    auto const tree_size = size(tree.get());
    std::string const tree_string(tree_rope.begin(), tree_rope.end());

    std::cout << "Initial size of tree used in the erase tests: " << tree_size
              << "\n";

    for (std::ptrdiff_t i = 0; i < tree_size; ++i) {
        for (std::ptrdiff_t j = i + 1; j < tree_size; ++j) {
            node_ptr<rope_tag> root =
                make_tree_left_right<max_children, max_children>();
            std::string str(tree_string);

            root = btree_erase(root, i, j);
            str.erase(str.begin() + i, str.begin() + j);

            unencoded_rope const r(root);

            EXPECT_EQ(size(root.get()), (std::ptrdiff_t)str.size())
                << "i=" << i << " j=" << j;
            EXPECT_TRUE(std::equal(str.begin(), str.end(), r.begin()))
                << "i=" << i << " j=" << j;
            check_leaf_heights(root);
        }
    }
}