/*
 This program is free software; you can redistribute it and/or modify it under
 the terms of the European Union Public Licence - EUPL v.1.1 as published by
 the European Commission.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE. See the European Union Public Licence - EUPL v.1.1
 for more details.

 You should have received a copy of the European Union Public Licence - EUPL v.1.1
 along with this program.

 Further information about the European Union Public Licence - EUPL v.1.1 can
 also be found on the world wide web at http://ec.europa.eu/idabc/eupl

*/

/*
 ------ Copyright (C) 2010 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/

/*
 ------------------ Author: Tiziana Sabatini  ------------------------------------------------
 ------------------ E-mail: (tiziana.sabatini@yahoo.it) --------------------------------------
 Patched by Guillermo to correct the behaviour of the + button on the 3rd body selection panel. Nov 09
 */

#include "perturbations.h"
#include "Atmosphere/AtmosphereModel.h"
#include "cartesianTOorbital.h"
#include "cartesianTOspherical.h"
#include "statevector.h"
#include "stabody.h"
#include "stamath.h"
#include "date.h"
#include "math.h"
#include "inertialTOfixed.h"
#include "getGreenwichHourAngle.h"
#include <QFile>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "Entry/capsule.h"
#include <QErrorMessage>
#include <QDebug>

const double PI = 3.141592;

Perturbations::Perturbations()
{
}

Perturbations::~Perturbations()
{
}

// TODO: This method should be abstract
Vector3d
Perturbations::calculateAcceleration(sta::StateVector /* state */, double /* time */, double /* dt */)
{
    return Vector3d::Zero();
}

/////////////////////////////// Gravity Field Perturbation ///////////////////////////////
GravityPerturbations::GravityPerturbations(const StaBody* centralBody,
                                           const ScenarioGravityModel* gravityModel)
{
    m_body = centralBody;
    m_modelName = gravityModel->modelName();
    m_zonalCount = gravityModel->numberOfZonals();
    m_tesseralCount = gravityModel->numberOfTesserals();

    //Assigning dimension to matrices of harmonical coefficients
    J.resize(m_zonalCount + 1);
    JJ.resize(m_zonalCount + 1 , m_tesseralCount + 1);
    gamma.resize(m_zonalCount + 1, m_tesseralCount + 1);

    loadGravityConstants();
}

GravityPerturbations::~GravityPerturbations()
{
}

Vector3d GravityPerturbations::calculateAcceleration(sta::StateVector state, double time, double dt)
{
    double R = body()->equatorialRadius();
    double mu = body()->mu();
    double greenwich = getGreenwichHourAngle (time);

    int n_zonals = zonalCount();
    int m_tesserals = tesseralCount();

    //Operating coordinate conversion from inertial cartesian to earth-fixed spherical
    double longitude, latitude, r, V, g, chi;
    VectorXd state_f(6);

    //TO DO : change the number 0 and put directly the stabody
    inertialTOfixed(0, greenwich,
                state.position.x(), state.position.y(), state.position.z(),
                state.position.x(), state.position.y(), state.position.z(),
                state_f(0), state_f(1), state_f(2), state_f(3), state_f(4), state_f(5));

    cartesianTOspherical(state_f(0), state_f(1), state_f(2), state_f(3), state_f(4), state_f(5),
                         longitude, latitude, r, V, g, chi);

    //Calculating the accelerations
    double derivative_r_zonals(0);
    double derivative_r_tesserals(0);
    double derivative_lat_zonals(0);
    double derivative_lat_tesserals(0);
    double derivative_long_tesserals(0);

    double x = sin(latitude);

    //P Legendre Polynomials, PP associated Legendre functions of the first kind
    VectorXd P(n_zonals + 1);
    MatrixXd PP(n_zonals + 1, m_tesserals + 1);

    //Assign values of P and PP with index below 2
    P(0) = 1;
    PP(0,0) = P(0);
    if (n_zonals != 0)
    {
        P(1) = x;
        PP(1,0) = P(1);
    }
    if (m_tesserals != 0)
        PP(1,1) = - pow((1 - pow(x,2.0)),0.5);

    for (int n = 2; n <= n_zonals ; n++)
    {
        //Legendre polynomials calculation:
        P(n) = ((2*n - 1) * P(n-1) * x - (n-1) * P(n-2)) / n;  //three term recurrence relation
        PP(n,0) = P(n);

        //Spherical coordinates derivative calculation (zonals terms):
        derivative_r_zonals += ((n+1) * J(n) * pow(R,n) * P(n) * pow(r,-(n+2)));
        derivative_lat_zonals += - cos(latitude) * J(n) * pow(r,-(n+2)) * pow(R,n) * (1.0  / (pow(x,2) - 1) * n * (x*P(n) - P(n-1)));

        for (int m = 1; (m <= m_tesserals && m <= n) ; m++)
        {
            //Associated Legendre functions calculation:
            if(n == m)
            {
                PP(n,m) = pow(-1.0,n) * doublefactorial(2*n - 1) * pow((1 - pow(x,2.0)),n/2);
            }
            else if(n == m+1)
            {
                PP(n,m) = x * (2*m + 1) * PP(n-1,m);
            }
            else
            {
                PP(n,m) = 1/(n-m) * ((2*n-1) * x * PP(n-1,m) - (n+m-1) * PP(n-2,m));
            }

            //Spherical coordinates derivative calculation (tesserals terms):
            derivative_r_tesserals += (n+1) * JJ(n,m) * pow(R,n) * PP(n,m) * pow(r,-(n+2)) * cos(m * (longitude - gamma(n,m)));
            derivative_lat_tesserals += - sin(latitude)/pow(r,n+2) * JJ(n,m) * pow(R,n) * (-(n+m) * (n-m+1) * sqrt(1 - (pow(x,2))) / ((pow(x,2)) -1) * PP(n,m-1) - m * x * PP(n,m) / (pow(x,2) - 1)) * cos(m * (longitude - gamma(n,m)));
            derivative_long_tesserals +=  ((1/pow(r,n+2) * JJ(n,m) * pow(R,n) * PP(n,m)) * m * sin(m*(longitude - gamma(n,m))))/cos(latitude) ;

        }
    }

    Vector3d acc_spherical;
    acc_spherical.x() = mu * (derivative_r_zonals + derivative_r_tesserals);
    acc_spherical.y() = -mu * (derivative_lat_zonals + derivative_lat_tesserals);
    acc_spherical.z() = mu * derivative_long_tesserals;

    //TO DO change the following line and use astro-core functions
    //Coordinates transformation from spherical/fixed to cartesian/inertial
    Vector3d acceleration;
    Matrix3d rotation1, rotation2;

    rotation1 << cos(latitude) * cos(longitude), -sin(longitude)*cos(latitude), -sin(latitude),
                sin(longitude), cos(longitude), 0,
                sin(latitude) * cos(longitude), -sin(latitude) * sin(longitude), cos(latitude);
    rotation2 << cos(greenwich), -sin(greenwich), 0,
                sin(greenwich), cos(greenwich), 0,
                0, 0, 1;

    acceleration = rotation1 * rotation2 * acc_spherical;

    return acceleration;
}

void GravityPerturbations::loadGravityConstants()
{
    //Assign a null value to constant harmonics below index 2
    J(0) = 0;
    J(1) = 0;
    JJ(0,0) = 0;    gamma(0,0) = 0;
    JJ(1,0) = 0;    gamma(1,0) = 0;
    if (m_tesseralCount != 0)
    {
        JJ(1,1) = 0;
        gamma(1,1) = 0;
    }

    //Open the file .stad containing the normalized gravity constants.
    //The number of loaded constants is consistent with the accuracy order the user selected.
    QString path = QString("data/bodies/");
    path.append(m_modelName);

    //QTextStream out (stdout); out << "===> bodies path: " << path << endl;

    QFile gravity(path);

    if (!gravity.open(QIODevice::ReadOnly))
    {
        // TODO: It's an error if the gravity model doesn't exist! Need a mechanism
        // for reporting this to the user.
        return;
    }

    QTextStream gravitystream(&gravity);

    MatrixXd C(m_zonalCount + 1, m_zonalCount + 1), S(m_zonalCount + 1, m_zonalCount + 1);
    int n = 0, m = 0;

    while (gravitystream.status() == QTextStream::Ok)
    {
        gravitystream >> n;
        if (n > m_zonalCount) break;
        gravitystream >> m;
        gravitystream >> C(n,m);
        gravitystream >> S(n,m);
    }
    gravity.close();

    //Converting the gravity constants.
    for (int i = 2 ; i <= m_zonalCount ; i++)
    {
        J(i) = - sqrt(2*i + 1.0) * C(i,0);
        if (m_tesseralCount != 0)
        {
            for (int j = 1 ; (j <= m_tesseralCount && j <= i) ; j++)
            {
                gamma(i,j) = atan(S(i,j)/C(i,j)) / j;
                JJ(i,j) = - fabs(sqrt(factorial(i-j) * (2*i + 1) * 2 / factorial(i+j)) * C(i,j) / cos(j * gamma(i,j)));
            }
        }
    }
}

double factorial(int num)
{
    if (num==0 || num==1)
    return 1.0;
    return factorial(num-1)*num;
}

double doublefactorial(int num)
{
    if (num==0 || num==1)
    return 1.0;
    return doublefactorial(num-2)*num;
}

/////////////////////////////// Atmospheric Drag Perturbation ///////////////////////////////
AtmosphericDragPerturbations::AtmosphericDragPerturbations(const QString& atmosphereModel)
{
#if OLDSCENARIO
    m_atmosphericModel = perturbation->atmosphericModel();
    m_body = perturbation->centralBody();
    m_surface = properties->physicalProperties()->physicalCharacteristics()->surfaceArea();
    m_cdCoefficients = properties->aerodynamicProperties()->CDCoefficients();
    m_mass = properties->physicalProperties()->physicalCharacteristics()->mass();
#endif
}

AtmosphericDragPerturbations::~AtmosphericDragPerturbations()
{
}

Vector3d
AtmosphericDragPerturbations::calculateAcceleration(sta::StateVector state, double time, double dt)
{
    //Conversion from kilometers to meters has been made.
    //Calculating the density from the altitude:
    AtmosphereModel atmosphere;
    atmosphere.selectModel(atmosphericModel());
    double h = altitude(body(), state, time);
    double rho = atmosphere.density(h * pow(10,3.0)) * pow(10,9.0);

    //Calculating the CD coefficient from the altitude (calling re-entry module functions)
    capsule_class vehicle;
    vehicle.selectCdCprofile(cdCoefficients());
    double cd = vehicle.cdc(h * pow(10,3.0));

    //Considering the atmosphere rotating with the Earth
    //TO DO put omega as a property of StaBody; only earth rotation has been considered
    double omega = 7.29211585494e-5;
    state.velocity.x() += omega * state.position.y();
    state.velocity.y() -= omega * state.position.x();

    //Calculating the accelerations vector
    Vector3d acceleration = - 0.5 * rho * state.velocity.norm() * state.velocity * (cd * surface()*pow(10,-6.0))/mass();

    return acceleration;
}

/////////////////////////////// Solar Pressure Perturbation ///////////////////////////////
SolarPressurePerturbations::SolarPressurePerturbations(StaBody* centralBody,
                                                       double reflectivity,
                                                       double albedo,
                                                       double ir,
                                                       double mass,
                                                       double surfaceArea) :
    m_body(centralBody),
    m_reflectivity(reflectivity),
    m_albedo(albedo),
    m_ir(ir),
    m_mass(mass),
    m_surface(surfaceArea)
{
}

SolarPressurePerturbations::~SolarPressurePerturbations()
{
}

Vector3d
SolarPressurePerturbations::calculateAcceleration(sta::StateVector state, double time, double dt)
{
    //calculation of the Sun position via the ephemeris
    sta::StateVector sunVector = STA_SOLAR_SYSTEM->sun()->stateVector(time, body(), sta::COORDSYS_EME_J2000);

    double sunAscension = atan(sunVector.position.y()/sunVector.position.x());
    double sunDeclination = atan(sunVector.position.z() / sqrt(pow(sunVector.position.y(),2.0) + pow(sunVector.position.x(),2.0)));

    double distancePlanetSun = sunVector.position.norm();
    double distancePlanetSat = state.position.norm();
    double distanceSunSat = (sunVector.position - state.position).norm();

    //Modeling the solar radiation (using Km)
    double sigma = 5.6704 * pow(10,-8.0); //Boltzmann constant
    double Tsun = 5778; //Sun temperature
    double Rsun = 6.955 * pow(10,5.0); // Sun radius

    double W = sigma * pow(Tsun, 4.0) * pow((Rsun / distanceSunSat), 2.0);

    //Calculation of the acceleration module
    double c = 300000;
    double fp = (1 + reflectivity()) * W * (surface() * pow(10,-6.0)) / c / mass();

    Vector3d accelerationAlbedo(0,0,0);
    Vector3d accelerationIR(0,0,0);
    //Calculation of the albedo and infra-red radiations
    if (albedo())
    {
        double Walbedo = albedoReflectivityCoefficient(body()) * W * pow((body()->meanRadius() / distancePlanetSat), 2.0);
        double fpalbedo = (1 + reflectivity()) * Walbedo * (surface() * pow(10,-6.0)) / c / mass();
        accelerationAlbedo = fpalbedo * state.position/distancePlanetSat;
    }

    if (ir())
    {
        double Wir = irRadiationFlux(body()) * pow((body()->meanRadius() / distancePlanetSat), 2.0);
        double fpir = (1 + reflectivity()) * Wir * (surface() * pow(10,-6.0)) / c / mass();
        accelerationIR = fpir * state.position/distancePlanetSat;
    }

    Vector3d acceleration(-fp * cos(sunAscension) * cos(sunDeclination),
                          -fp * sin(sunAscension) * cos(sunDeclination),
                          -fp * sin(sunDeclination));

    //calculation of the Earth shadow; if the spacecraft is overshadowed by the Earth the solar pressure will be ignored
    double gamma = acos((pow(distanceSunSat,2.0) + pow(distancePlanetSun,2.0) - pow(distancePlanetSat,2.0)) / (2 * distancePlanetSun * distanceSunSat));
    double gamma_c = atan(body()->meanRadius() / distancePlanetSun);
//
//    if (gamma < gamma_c)
//    {
//        if (distanceSunSat > distancePlanetSun)
//            acceleration << 0, 0, 0;
//    }

    acceleration += accelerationAlbedo + accelerationIR;

    return acceleration;
}

double albedoReflectivityCoefficient(const StaBody* body)
{
    switch(body->id())
    {
    case STA_MERCURY:
        return 0.53;
    case STA_VENUS:
        return 0.76;
    case STA_EARTH:
        return 0.35;
    case STA_MARS:
        return 0.16;
    case STA_JUPITER:
        return 0.73;
    case STA_SATURN:
        return 0.76;
    case STA_URANUS:
        return 0.93;
    case STA_NEPTUNE:
        return 0.84;
    case STA_PLUTO:
        return 0.14;
    case STA_SUN:
        return 0;
    case STA_MOON:
        return 0.067;
    default:
        return 0;
    }
}

double irRadiationFlux(const StaBody* body)
{
    switch(body->id())
    {
    case STA_MERCURY:
        return 2139;
    case STA_VENUS:
        return 155;
    case STA_EARTH:
        return 240;
    case STA_MARS:
        return 123;
    case STA_JUPITER:
        return 3.4;
    case STA_SATURN:
        return 0.9;
    case STA_URANUS:
        return 0.063;
    case STA_NEPTUNE:
        return 0.06;
    case STA_PLUTO:
        return 0.191;
    case STA_SUN:
        return 0;
    case STA_MOON:
        return 316;
    default:
        return 0;
    }
}

/////////////////////////////// Third Body Perturbation ///////////////////////////////

ExternalBodyPerturbations::ExternalBodyPerturbations(const StaBody* centralBody,
                                                     const QList<const StaBody*>& bodies) :
    m_body(centralBody),
    m_perturbingBodyList(bodies)
{
}

ExternalBodyPerturbations::~ExternalBodyPerturbations()
{
}

Vector3d
ExternalBodyPerturbations::calculateAcceleration(sta::StateVector state, double time, double dt)
{
    Vector3d acceleration(0.0, 0.0, 0.0);

    foreach(const StaBody* thirdbody,  perturbingBodyList())
    {
        double mu = thirdbody->mu();
        sta::StateVector thirdbodystate = thirdbody->stateVector(time, body(), sta::COORDSYS_EME_J2000);

        acceleration += mu * ((thirdbodystate.position - state.position) / pow((thirdbodystate.position - state.position).norm(),3.0) - thirdbodystate.position / pow(thirdbodystate.position.norm(), 3.0));
    }

    return acceleration;
}

/////////////////////////////// Space Debris Perturbation ///////////////////////////////

DebrisPerturbations::DebrisPerturbations(const StaBody* centralBody,
                                         double mass,
                                         double surfaceArea) :
    m_body(centralBody),
    m_mass(mass),
    m_surface(surfaceArea),
    m_time(0)
{
    m_counterDebris.resize(25);
    m_counterDebris.setZero();
    m_counterMeteoroids.resize(25);
    m_counterMeteoroids.setZero();
}

DebrisPerturbations::~DebrisPerturbations()
{
}

Vector3d
DebrisPerturbations::calculateAcceleration(sta::StateVector state, double time, double dt)
{
    double mu = body()->mu();
    m_time += dt;
    sta::KeplerianElements keplerian = cartesianTOorbital(mu, state);

    //Trajectory analysis:
    double h = keplerian.SemimajorAxis * (1 - pow(keplerian.Eccentricity, 2)) / (1 + keplerian.Eccentricity * cos(keplerian.TrueAnomaly)) - body()->meanRadius();

    int Ndatapoints = 0;
    VectorXd diameter, Ndebris, Nmet, Ntot;

    // TODO: Shouldn't we use the absolute value of inclination here???
    double inclinationDeg = sta::radToDeg(keplerian.Inclination);

    if ( 300 <= h && h <= 500 && 46.6 <= inclinationDeg && inclinationDeg <= 57.6 )
        loadStatistics("debris_impact_low_altitude.stad", diameter, Ndebris, Nmet, Ntot, Ndatapoints);

    else if ( 700 <= h && h <= 900 && 70 <= inclinationDeg && inclinationDeg <= 90 )
        loadStatistics("debris_impact_medium_altitude.stad", diameter, Ndebris, Nmet, Ntot, Ndatapoints);

    else if ( 35600 <= h && h <= 35900 && 0 <= inclinationDeg && inclinationDeg <= 5.0 )
        loadStatistics("debris_impact_geo_altitude.stad", diameter, Ndebris, Nmet, Ntot, Ndatapoints);

    else return Vector3d(0.0, 0.0, 0.0);

    double density_deb = 0.0028; //kg/cm^3
    double density_met = 0.0025; //kg/cm^3
    double deltaT = 0.0001; //10^-4 s
    double accelerationDebris = 0;
    /* double accelerationMeteoroids = 0; */
    VectorXd nDebris(Ndatapoints), diffDebris(Ndatapoints);
    VectorXd nMeteoroids(Ndatapoints), diffMeteoroids(Ndatapoints);
    Vector3d acceleration(0.0, 0.0, 0.0);

    nDebris = Ndebris/365/24/60/60 * m_time * surface();
    nMeteoroids = Nmet/365/24/60/60 * m_time * surface();

    for (int i = 0; i < Ndatapoints; i++)
    {
        if (i != Ndatapoints-1)
        {
            diffDebris(i) = nDebris(i) - nDebris(i + 1);
            diffMeteoroids(i) = nMeteoroids(i) - nMeteoroids(i + 1);
        }
        else
        {
            diffDebris(i) = nDebris(i);
            diffMeteoroids(i) = nMeteoroids(i);
        }

        // Debris collisions calculation
        if (floor(diffDebris(i)) > counterDebris()(i))
        {
            setCounterDebris(i, nDebris(i));

            double accDebrisModule;
            if (i == 0)
                accDebrisModule = 3 * (density_deb * 4/3 * PI * pow(0.5 * randomNumber(0, diameter(i)),3)) / mass() * randomNumber(4.5,5.5) / deltaT;
            else
                accDebrisModule = 3 * (density_deb * 4/3 * PI * pow(0.5 * randomNumber(diameter(i-1),diameter(i)),3)) / mass() * randomNumber(4.5,5.5) / deltaT;

            //generate a random direction
            double alpha = randomNumber(0, 2*PI);
            double beta = randomNumber(-PI, PI);
            acceleration.x() += accDebrisModule * cos(alpha) * cos(beta);
            acceleration.y() += accDebrisModule * sin(alpha) * cos(beta);
            acceleration.z() += accDebrisModule * sin(beta);
        }

        // Meteoroids collisions calculation
        if (floor(diffMeteoroids(i)) > counterMeteoroids()(i))
        {
            setCounterMeteoroids(i, nMeteoroids(i));

            double accMeteoroidsModule;
            if (i == 0)
                accMeteoroidsModule = 3 * (density_met * 4/3 * PI * pow(0.5 * randomNumber(0, diameter(i)),3)) / mass() * randomNumber(4.5,5.5) / deltaT;
            else
                accMeteoroidsModule = 3 * (density_met * 4/3 * PI * pow(0.5 * randomNumber(diameter(i-1),diameter(i)),3)) / mass() * randomNumber(4.5,5.5) / deltaT;

            //generate a random direction
            double alpha = randomNumber(0, 2*PI);
            double beta = randomNumber(-PI, PI);
            acceleration.x() += accMeteoroidsModule * cos(alpha) * cos(beta);
            acceleration.y() += accMeteoroidsModule * sin(alpha) * cos(beta);
            acceleration.z() += accMeteoroidsModule * sin(beta);
        }
    }
    return acceleration;
}

void
DebrisPerturbations::loadStatistics(QString filename, VectorXd& diameter, VectorXd& Ndebris, VectorXd& Nmet, VectorXd& Ntot, int& Ndatapoints)
{
    QString path("data/atmospheres/");
    path.append(filename);
    QFile model("data/atmospheres/debris_impact_low_altitude.stad");

    model.open(QIODevice::ReadOnly);
    QTextStream modelstream(&model);

    while (!modelstream.atEnd())
    {
        modelstream.readLine();
        Ndatapoints ++;
    }
    model.close();
    diameter.resize(Ndatapoints); Ndebris.resize(Ndatapoints); Nmet.resize(Ndatapoints); Ntot.resize(Ndatapoints);

    model.open(QIODevice::ReadOnly);
    for (int i = 0; i < Ndatapoints; i++)
    {
        modelstream >> diameter(i);
        modelstream >> Ndebris(i);
        modelstream >> Nmet(i);
        modelstream >> Ntot(i);
    }
    model.close();
}

double randomNumber(double inf, double sup)
{
    const float scale = rand()/float(RAND_MAX);
    return inf + scale * (sup - inf);
}

/**
 * Function: evaluation of Legendre's associated function using recursive form.
 * Source: Vallado, Fundamentals of Astrodynamics and Applications
 * @param x The argument
 * @param l The degree of polynomial
 * @param m The order of derivative
 * @return The evaluation of Legendre associated function
 * Author: Michele Scotti
 * E-mail: michele.scotti@gmail.com
 */
double legendre (double x, int l, int m)
{
        if (m > l)						// m > l;
                return 0;
        else if ((l == 0) && (m == 0))	// l = 0; m = 0;
                return 1;
        else if ((l == 1) && (m == 0))	// l = 1; m = 0;
                return x;
        else if ((l == 1) && (m == 1))	// l = 1; m = 1;
                return sqrt(1 - x*x);
        else if (l >= 2 && m == 0)
            return ((2*l-1)*x*legendre(x, l-1, 0) - (l-1)*legendre(x, l-2, 0)) / (l);
        else if (m != 0 && m < l)
            return (legendre(x, l-2, m) + (2*l-1)*(sqrt(1- x*x))*legendre(x, l-1, m-1));
        else if(l == m)
            return ((2*l-1)*sqrt(1-x*x)*legendre(x, l-1, l-1));
        else
            return -1;
}


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

#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <boost/math/constants/constants.hpp>
#include <map>
#include <iostream>
#include <iomanip>
#include "mp_t.hpp"

using namespace std;
using namespace boost::math;

//
// This program calculates the coefficients of the polynomials
// used for the regularized incomplete gamma functions gamma_p
// and gamma_q when parameter a is large, and sigma is small
// (where sigma = fabs(1 - x/a) ).
//
// See "The Asymptotic Expansion of the Incomplete Gamma Functions"
// N. M. Temme.
// Siam J. Math Anal. Vol 10 No 4, July 1979, p757.
// Coeffient calculation is described from Eq 3.8 (p762) onwards.
//

//
// Alpha:
//
mp_t alpha(unsigned k)
{
   static map<unsigned, mp_t> data;
   if(data.empty())
   {
      data[1] = 1;
   }

   map<unsigned, mp_t>::const_iterator pos = data.find(k);
   if(pos != data.end())
      return (*pos).second;
   //
   // OK try and calculate the value:
   //
   mp_t result = alpha(k-1);
   for(unsigned j = 2; j <= k-1; ++j)
   {
      result -= j * alpha(j) * alpha(k-j+1);
   }
   result /= (k+1);
   data[k] = result;
   return result;
}

mp_t gamma(unsigned k)
{
   static map<unsigned, mp_t> data;

   map<unsigned, mp_t>::const_iterator pos = data.find(k);
   if(pos != data.end())
      return (*pos).second;

   mp_t result = (k&1) ? -1 : 1;

   for(unsigned i = 1; i <= (2 * k + 1); i += 2)
      result *= i;
   result *= alpha(2 * k + 1);
   data[k] = result;
   return result;
}

mp_t Coeff(unsigned n, unsigned k)
{
   map<unsigned, map<unsigned, mp_t> > data;
   if(data.empty())
      data[0][0] = mp_t(-1) / 3;

   map<unsigned, map<unsigned, mp_t> >::const_iterator p1 = data.find(n);
   if(p1 != data.end())
   {
      map<unsigned, mp_t>::const_iterator p2 = p1->second.find(k);
      if(p2 != p1->second.end())
      {
         return p2->second;
      }
   }

   //
   // If we don't have the value, calculate it:
   //
   if(k == 0)
   {
      // special case:
      mp_t result = (n+2) * alpha(n+2);
      data[n][k] = result;
      return result;
   }
   // general case:
   mp_t result = gamma(k) * Coeff(n, 0) + (n+2) * Coeff(n+2, k-1);
   data[n][k] = result;
   return result;
}

void calculate_terms(double sigma, double a, unsigned bits)
{
   cout << endl << endl;
   cout << "Sigma:        " << sigma << endl;
   cout << "A:            " << a << endl;
   double lambda = 1 - sigma;
   cout << "Lambda:       " << lambda << endl;
   double y = a * (-sigma - log1p(-sigma));
   cout << "Y:            " << y << endl;
   double z = -sqrt(2 * (-sigma - log1p(-sigma)));
   cout << "Z:            " << z << endl;
   double dom = erfc(sqrt(y)) / 2;
   cout << "Erfc term:    " << dom << endl;
   double lead = exp(-y) / sqrt(2 * constants::pi<double>() * a);
   cout << "Remainder factor: " << lead << endl;
   double eps = ldexp(1.0, 1 - static_cast<int>(bits));
   double target = dom * eps / lead;
   cout << "Target smallest term: " << target << endl;

   unsigned max_n = 0;

   for(unsigned n = 0; n < 10000; ++n)
   {
      double term = tools::real_cast<double>(Coeff(n, 0) * pow(z, (double)n));
      if(fabs(term) < target)
      {
         max_n = n-1;
         break;
      }
   }
   cout << "Max n required:  " << max_n << endl;

   unsigned max_k;
   for(unsigned k = 1; k < 10000; ++k)
   {
      double term = tools::real_cast<double>(Coeff(0, k) * pow(a, -((double)k)));
      if(fabs(term) < target)
      {
         max_k = k-1;
         break;
      }
   }
   cout << "Max k required:  " << max_k << endl << endl;

   bool code = false;
   cout << "Print code [0|1]? ";
   cin >> code;

   int prec = 2 + (static_cast<double>(bits) * 3010LL)/10000;
   std::cout << std::scientific << std::setprecision(40);

   if(code)
   {
      cout << "   T workspace[" << max_k+1 << "];\n\n";
      for(unsigned k = 0; k <= max_k; ++k)
      {
         cout <<
            "   static const T C" << k << "[] = {\n";
         for(unsigned n = 0; n < 10000; ++n)
         {
            double term = tools::real_cast<double>(Coeff(n, k) * pow(a, -((double)k)) * pow(z, (double)n));
            if(fabs(term) < target)
            {
               break;
            }
            cout << "      " << Coeff(n, k) << "L,\n";
         }
         cout <<
            "   };\n"
            "   workspace[" << k << "] = tools::evaluate_polynomial(C" << k << ", z);\n\n";
      }
      cout << "   T result = tools::evaluate_polynomial(workspace, 1/a);\n\n";
   }
}

int main()
{
   bool cont;
   do{
      cont  = false;
      double sigma;
      cout << "Enter max value for sigma (sigma = |1 - x/a|): ";
      cin >> sigma;
      double a;
      cout << "Enter min value for a: ";
      cin >> a;
      unsigned precision;
      cout << "Enter number of bits precision required: ";
      cin >> precision;

      calculate_terms(sigma, a, precision);

      cout << "Try again[0|1]: ";
      cin >> cont;

   }while(cont);

   return 0;
}


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

#include <random>
#include <benchmark/benchmark.h>
#include <boost/math/tools/agm.hpp>
#include <boost/multiprecision/float128.hpp>

using boost::math::tools::agm;
template<class Real>
void AGM(benchmark::State& state)
{
    std::random_device rd;
    std::mt19937_64 mt(rd());
    std::uniform_real_distribution<long double> unif(1,100);

    Real x = static_cast<Real>(unif(mt));
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(agm(x,Real(1)));
    }
}

BENCHMARK_TEMPLATE(AGM, float);
BENCHMARK_TEMPLATE(AGM, double);
BENCHMARK_TEMPLATE(AGM, long double);
BENCHMARK_TEMPLATE(AGM, boost::multiprecision::float128);

BENCHMARK_MAIN();


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/nthroot.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/half.hpp>

STF_CASE_TPL (" nthroot",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::nthroot;
  using iT = bd::as_integer_t<T>;
  using r_t = decltype(nthroot(T(), iT()));

  // return type conformity test
 STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(nthroot(bs::Inf<T>(),iT(3)), bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Inf<T>(),iT(4)), bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Minf<T>(),iT(3)), bs::Minf<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Minf<T>(),iT(4)), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Nan<T>(),iT(3)), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Nan<T>(),iT(4)), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Mone<T>(),iT(4)), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(nthroot(bs::Mone<T>(),iT(0)), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::One <T>(),iT(0)), bs::One<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Half<T>(),iT(0)), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Two <T>(),iT(0)), bs::Inf <r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Zero<T>(),iT(0)), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Two <T>(),iT(0)), bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Half<T>(),iT(0)), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Mone<T>(),iT(3)), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::One<T>(),iT(3)), bs::One<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::One<T>(),iT(4)), bs::One<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Zero<T>(),iT(3)), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Zero<T>(),iT(4)), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(T(-8),iT(3)), r_t(-2), 0.5);
  STF_ULP_EQUAL(nthroot(T(256),iT(4)), r_t(4), 0.5);
  STF_ULP_EQUAL(nthroot(T(8),iT(3)), r_t(2), 0.5);
  STF_ULP_EQUAL(nthroot(T(0.5), iT(4)), r_t(0.84089641525371454303112547623321), 0.5);
}


/*******************************************************************************
Copyright (c) 2011, Dr. D. Studios
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or other
materials provided with the distribution.
Neither the name of the Dr. D. Studios nor the names of its contributors may be
used to endorse or promote products derived from this software without specific
prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/

#ifndef _PIMATH_ROOTS__H_
#define _PIMATH_ROOTS__H_

#include <boost/python.hpp>
#include <ImathMath.h>
#include <ImathRoots.h>
#include "util.h"

/**
 * These functions have been changed to return tuples rather than the root count.
 * If there are no roots, an empty tuple will be returned (rather than None).
 * Return arguments have been removed.
 */

namespace pimath
{
	namespace bp = boost::python;

	template<typename T>
	struct RootsBind
	{
		RootsBind()
		{
			bp::def("solveLinear", solveLinear);
			bp::def("solveQuadratic", solveQuadratic);
			bp::def("solveNormalizedCubic", solveNormalizedCubic );
			bp::def("solveCubic", solveCubic );
		}

		static bp::object
		solveLinear( T a, T b )
		{
			T rv;
			return Imath::solveLinear( a, b, rv ) == 1 ?
					bp::make_tuple( rv ) : bp::make_tuple();
		}

		static bp::object
		solveQuadratic( T a, T b, T c )
		{
			T x[2];
			int count = Imath::solveQuadratic( a, b, c, x );
			switch( count )
			{
				case 1:
					return bp::make_tuple(x[0]);
				case 2:
					return bp::make_tuple(x[0], x[1]);
			}
			return bp::make_tuple();
		}

		static bp::object
		solveNormalizedCubic( T r, T s, T t )
		{
			T x[3];
			int count = Imath::solveNormalizedCubic( r, s, t, x );
			switch( count )
			{
				case 1:
					return bp::make_tuple(x[0]);
				case 2:
					return bp::make_tuple(x[0],x[1]);
				case 3:
					return bp::make_tuple(x[0],x[1],x[2]);
			}
			return bp::make_tuple();
		}

		static bp::object
		solveCubic( T a, T b, T c, T d )
		{
			T x[3];
			int count = Imath::solveCubic( a, b, c, d, x );
			switch( count )
			{
				case 1:
					return bp::make_tuple(x[0]);
				case 2:
					return bp::make_tuple(x[0],x[1]);
				case 3:
					return bp::make_tuple(x[0],x[1],x[2]);
			}
			return bp::make_tuple();
		}
	};
}

#endif


#pragma once
#include <Eigen/Dense>
#include <nlohmann/json.hpp>
#include "Optimizers/Optimizer.hpp"

namespace yannq
{
template<typename T>
class SGDMomentum
	: public Optimizer<T>
{
public:
	using typename Optimizer<T>::Vector;
	using typename Optimizer<T>::RealVector;

	static constexpr double DEFAULT_PARAMS[] = {0.05, 0.0, 0.9};

private:
	double alpha_;
	double p_;
	double gamma_;

	Vector m_;
	int t_;

public:

	SGDMomentum(double alpha = DEFAULT_PARAMS[0], double p = DEFAULT_PARAMS[1], double gamma = DEFAULT_PARAMS[2])
		: alpha_{alpha}, p_{p}, gamma_{gamma}, t_{0}
	{
	}

	SGDMomentum(const nlohmann::json& params)
		: alpha_{params.value("alpha", DEFAULT_PARAMS[0])},
			p_{params.value("p", DEFAULT_PARAMS[1])},
			gamma_{params.value("gamma", DEFAULT_PARAMS[2])},
			t_{0}
	{
	}

	nlohmann::json desc() const override
	{
		return nlohmann::json
		{
			{"name", "SGD"},
			{"alhpa", alpha_},
			{"gamma", gamma_},
			{"p", p_}
		};
	}

	Vector getUpdate(const Vector& v) override
	{
		using std::pow;
		if(t_ == 0)
		{
			m_ = Vector::Zero(v.size());
		}

		++t_;
		m_ *= gamma_;
		m_ += (1-gamma_)*v;
		double eta = std::max((alpha_/pow(t_, p_)), 1e-4)/(1.0 - pow(gamma_, t_));
		return -eta*m_;
	}
};
}//namespace yannq
template<typename T>
constexpr double yannq::SGDMomentum<T>::DEFAULT_PARAMS[];


#include <iostream>
#include <string>
#include <tuple>
#include <vector>
#include <fstream>
#include <algorithm>
#include <limits>
#include <iterator>
#include <armadillo>
#include <cmath>
#include <iomanip>

using namespace std;

// Block of global vars
const long double PI = 3.141592653589793238L;
// Block of global vars

// Block of declarations
tuple < vector < tuple < int, int > >, int, int > read_input( string file_name );
vector < string > string_tokenizer( string string_to_tok, char separator );
vector < tuple < int, int > > greedy( tuple < vector < tuple < int, int > >, int, int > input );
vector < tuple < int, int > > divide_n_conquer( tuple < vector < tuple < int, int > >, int, int > input );
vector < tuple < int, int > > divide_n_conquer_( vector < tuple < int, int > > input, vector < tuple < int, int > > fixed_points, int width, int height );
vector < tuple < int, int > > divide_n_conquer2_( vector < tuple < int, int > > input, vector < tuple < int, int > > fixed_points, int width, int height );
vector < tuple < int, int > > divide_n_conquer3_( vector < tuple < int, int > > input, int width, int height );
vector < tuple < int, int > > dynamic( tuple < vector < tuple < int, int > >, int, int > input );
vector < tuple < int, int > > dynamic_( vector < tuple < int, int > > input, vector < tuple < int, int > > fixed_points, int width, int height );
vector < tuple < int, int > > dynamic2_( vector < tuple < int, int > > input, int width, int height );
vector < tuple < int, int > > dynamic3_( vector < tuple < int, int > > input, int width, int height );
vector < int > sort_by_radius( vector < int > input );
bool are_intersected( tuple < int, int > new_point, vector < tuple < int, int > > fixed_points, int  height, int width );
vector < tuple < int, int > > divide_n_conquer_merge_( vector < tuple < int, int > > input_0, vector < tuple < int, int > > input_1, int width, int height );
string get_geogebra_plot_command( vector < tuple < int, int > > points, int  height, int width );
vector < vector < tuple < int, int > > > split_vector( vector < tuple < int, int > > vector_, int split_point );
vector < tuple < int, int > > merge_vectors( vector < tuple < int, int > > vector_0, vector < tuple < int, int > > vector_1 );
vector < tuple < int, int > > max( vector < tuple < int, int > > vector_0, vector < tuple < int, int > > vector_1 );
double circle_area( double radius );
void merge_sort(int arr[], int l, int r);
void merge(int arr[], int l, int m, int r);
double calculate_input_area( vector < tuple < int, int > > input );
// End block of declarations

int main( int argc, const char* argv[] ){

    tuple < vector < tuple < int, int > >, int, int > input = read_input( "input.txt" );

    int width = get< 1 >( input );
    int height = get< 2 >( input );
    vector < tuple < int, int > > points =  get< 0 >( input );

    cout << endl << "Comandos para Geogebra [ Entrada ]: ";
    cout << get_geogebra_plot_command( points, height, width ) << endl;

    vector < tuple < int, int > > greedy_solution = greedy( input );
    cout << endl << "Comandos para Geogebra [ Salida: Voraz, Área: " << calculate_input_area( greedy_solution ) << " ]: ";
    cout << get_geogebra_plot_command( greedy_solution, height, width ) << endl;

    vector < tuple < int, int > > divide_n_conquer_solution = divide_n_conquer( input );
    cout << endl << "Comandos para Geogebra [ Salida: Divide y vencerás, Área: " << calculate_input_area( divide_n_conquer_solution ) << " ]: ";
    cout << get_geogebra_plot_command( divide_n_conquer_solution, height, width ) << endl;

    vector < tuple < int, int > > dynamic_solution = dynamic( input );
    cout << endl << "Comandos para Geogebra [ Salida: Programación dinámica, Área: " << calculate_input_area( dynamic_solution ) << " ]: ";
    cout << get_geogebra_plot_command( dynamic_solution, height, width ) << endl;

    cout << endl;
    return 0;
};

tuple < vector < tuple < int, int > >, int, int > read_input( string file_name ){
    tuple < vector < tuple < int, int > >, int, int > to_return;
    vector < tuple < int, int > > points_info;
    int width, height = 0;
    bool first_line = true;
    ifstream f_input( file_name.c_str() );
    string line = "";
    while( getline( f_input, line ) ){
        if( ( line[0] != '#' ) && ( line[0] != ' ' ) ){
            if( first_line ){
                vector < string > map_info = string_tokenizer( line, ' ' );
                width = stoi( map_info[0] );
                height = stoi( map_info[1] );
                first_line = false;
            }else{
                vector < string > map_info = string_tokenizer( line, ' ' );
                tuple < int, int > point_info = make_tuple( stoi( map_info[0] ), stoi( map_info[1] ) );
                points_info.push_back( point_info );
            };
        };
    };
    f_input.close();
    to_return = make_tuple( points_info, width, height );
    return to_return;
};

vector < string > string_tokenizer( string string_to_tok, char separator ){
    vector < string > to_return;
    string tmp_line = "";
    for( int i = 0; i < string_to_tok.size(); i++ ){
        if( string_to_tok[i] == separator ){
            to_return.push_back( tmp_line );
            tmp_line = "";
        }else{
            tmp_line.push_back( string_to_tok[i] );
        };
    };
    to_return.push_back( tmp_line );
    return to_return;
};

vector < tuple < int, int > > greedy( tuple < vector < tuple < int, int > >, int, int > input ){
    vector < tuple < int, int > > to_return;
    int width = get< 1 >( input );
    int height = get< 2 >( input );
    vector < tuple < int, int > > points =  get< 0 >( input );
    vector < int > radius;
    for( int i = 0; i < points.size(); i++ ){
        radius.push_back( get< 1 >( points[i] ) );
    };
    radius = sort_by_radius( radius );
    vector < tuple < int, int > > sorted_points;
    for( int i = 0; i < radius.size(); i++ ){
        for( int j = 0; j < points.size(); j++ ){
            if( radius[i] == get< 1 >( points[j] ) ){
                sorted_points.push_back( points[j] );
                points.erase( points.begin() + j );
            };
        };
    };
    reverse( sorted_points.begin(), sorted_points.end() );
    for( int i = 0; i < sorted_points.size(); i++ ){
        bool are_intersected_ = are_intersected_ = are_intersected( sorted_points[i], to_return, height, width );
        if( !are_intersected_ ){
            to_return.push_back( sorted_points[i] );
        };
        //cout << "P: " << get<0>( sorted_points[i] ) << " - R:" << get<1>( sorted_points[i] ) << " - TR: " << are_intersected_ << endl;
    };
    return to_return;
};

vector < tuple < int, int > > divide_n_conquer( tuple < vector < tuple < int, int > >, int, int > input ){
    int width = get< 1 >( input );
    int height = get< 2 >( input );
    vector < tuple < int, int > > fixed_points;
    vector < tuple < int, int > > input_x = get<0>( input );
    //Limpiando la entrada
    for( int i = 0; i < input_x.size(); i++ ){
        if( are_intersected( input_x[i], fixed_points, height, width ) ){
            input_x.erase(input_x.begin() + i);
        };
    };
    int input_x_size = input_x.size();
    vector < int > radius;
    vector < tuple < int, int > > sorted_points;
    for( int i = 0; i < input_x.size(); i++ ){
        radius.push_back( get< 1 >( input_x[i] ) );
    };
    int radius_size = radius.size();
    radius = sort_by_radius( radius );
    for( int i = 0; i < radius_size; i++ ){
        for( int j = 0; j < input_x_size; j++ ){
            if( radius[i] == get< 1 >( input_x[j] ) ){
                sorted_points.push_back( input_x[j] );
                input_x.erase( input_x.begin() + j );
            };
        };
    };
    reverse( sorted_points.begin(), sorted_points.end() );

    //return divide_n_conquer_( get<0>( input ), fixed_points, width, height );
    //return divide_n_conquer2_( input_x, fixed_points, width, height );
    vector < tuple < int, int > > to_return = divide_n_conquer3_( sorted_points, width, height );
    int to_return_size = to_return.size();
    for(int i = 0; i < to_return_size; i++){
        if( ( get<0>( to_return[i] ) == 0) && ( get<1>( to_return[i] ) == 0 ) ){
            to_return.erase( to_return.begin() + i );
        };
    };
    return to_return;
};

vector < tuple < int, int > > divide_n_conquer3_( vector < tuple < int, int > > input, int width, int height ){
    int input_size = input.size();
    if( input_size == 1 ){
        return input;
    }else{
        int s_p = input_size/2;
        vector < vector < tuple < int, int > > > vector_splited = split_vector( input, s_p );
        return divide_n_conquer_merge_(
            divide_n_conquer3_( vector_splited[0], width, height ),
            divide_n_conquer3_( vector_splited[1], width, height ),
            width,
            height
        );
    };
};

vector < tuple < int, int > > divide_n_conquer_merge_( vector < tuple < int, int > > input_0, vector < tuple < int, int > > input_1, int width, int height ){
    vector < tuple < int, int > > to_return;
    int size_input_0 = input_0.size();
    int size_input_1 = input_1.size();
    int i = 0;
    int j = 0;
    int k;
    if( size_input_0 >= size_input_1 ){
        k = size_input_0 + ( size_input_0 - size_input_1 );
    }else{
        k = size_input_1 + ( size_input_1 - size_input_0 );
    };
    while( k >= 0 ){
        if( ( size_input_0 > 0 ) && ( size_input_1 > 0 ) ){
            int area_i = circle_area( get<1>( input_0[i] ) );
            int area_j = circle_area( get<1>( input_1[j] ) );
            if( area_i >= area_j ){
                if( !are_intersected( input_0[i], to_return, height, width ) ){
                    to_return.push_back( input_0[i] );
                };
                i++;
            }else{
                if( !are_intersected( input_1[j], to_return, height, width ) ){
                    to_return.push_back( input_1[j] );
                };
                j++;
            };
        }else if( ( size_input_0 == 0 ) && ( size_input_1 > 0 ) ){
            if( !are_intersected( input_1[j], to_return, height, width ) ){
                to_return.push_back( input_1[j] );
            };
            j++;
        }else if( ( size_input_0 > 0 ) && ( size_input_1 == 0 ) ){
            if( !are_intersected( input_0[i], to_return, height, width ) ){
                to_return.push_back( input_0[i] );
            };
            i++;
        };
        k--;
    };
    return to_return;
};

vector < tuple < int, int > > divide_n_conquer2_( vector < tuple < int, int > > input, vector < tuple < int, int > > fixed_points, int width, int height ){

     if( input.size() == 1 ){
         if( !are_intersected( input[0], fixed_points, height, width  ) ){
            fixed_points.push_back( input[0] );
            return fixed_points;
         };
    }else if( input.size() == 2 ){
        vector < tuple < int, int > > points;
        vector < tuple < int, int > > to_return;
        points.push_back( input[1] );
        if( !are_intersected( input[0], points, height, width  ) ){
            points.push_back( points[0] );
            return points;
        }else{
            if( get<1>(input[0]) > get<1>(input[1]) ){
                to_return.push_back( input[0] );
            }else{
                to_return.push_back( input[1] );
            };
            return to_return;
        };
    }else{
        int s_p = input.size()/2;
        vector < vector < tuple < int, int > > > vector_splited = split_vector( input, s_p );
        //vector < tuple < int, int > > max( vector < tuple < int, int > > vector_0, vector < tuple < int, int > > vector_1 );
        return merge_vectors(
            max (
                divide_n_conquer2_( vector_splited[0], fixed_points, width, height ), fixed_points ),
                merge_vectors(
                        max( divide_n_conquer2_( vector_splited[1], fixed_points, width, height ),fixed_points ), fixed_points )
                );
    };

};

vector < tuple < int, int > > divide_n_conquer_( vector < tuple < int, int > > input, vector < tuple < int, int > > fixed_points, int width, int height ){
    if( input.size() == 0 ){
        return fixed_points;
    }else if( input.size() > 0 ){
        int major_radius_index = 0;
        for( int i = 0; i < input.size(); i++ ){
            if( get<1>( input[i] ) >= get<1>( input[major_radius_index] ) ){
                major_radius_index = i;
            };
        };
        bool a_i = are_intersected( input[major_radius_index], fixed_points, height, width );
        if( !a_i ){
            fixed_points.push_back( input[major_radius_index] );
            input.erase(input.begin() + major_radius_index);
            return divide_n_conquer_( input, fixed_points, width, height );
        }else{
            input.erase(input.begin() + major_radius_index);
            return divide_n_conquer_( input, fixed_points, width, height );
        };
    };
};

vector < tuple < int, int > > dynamic( tuple < vector < tuple < int, int > >, int, int > input ){
    vector < tuple < int, int > > points = get<0>( input );
    vector < tuple < int, int > > empty;
    int width = get< 1 >( input );
    int height = get< 2 >( input );
    // int area = width * height;
    //Limpiando la entrada
    for( int i = 0; i < points.size(); i++ ){
        if( are_intersected( points[i], empty, height, width ) ){
            points.erase(points.begin() + i);
        };
    };
    //return dynamic_( points, empty, width, height );
    //return dynamic2_( points, width, height );
    return dynamic3_( points, width, height );
};

vector < tuple < int, int > > dynamic_( vector < tuple < int, int > > input, vector < tuple < int, int > > fixed_points, int width, int height ){
    vector < tuple < int, int > > to_return;
    if( input.size() == 2 ){
        vector < vector < tuple < int, int > > > v_s = split_vector( input, 1 );
        bool a_i = are_intersected( v_s[0][0], v_s[1], height, width );
        if( !a_i ){
            return input;
        }else{
            vector < tuple < int, int > > empty;
            return empty;
        };
    }else{
        int s_p = input.size()/2;
        vector < vector < tuple < int, int > > > vector_splited = split_vector( input, s_p );
        fixed_points = merge_vectors( max( max (dynamic_( vector_splited[0], fixed_points, width, height ), fixed_points), max( dynamic_( vector_splited[1], fixed_points, width, height ), fixed_points ) ), fixed_points );
        return fixed_points;
    };
};

vector < tuple < int, int > > dynamic2_( vector < tuple < int, int > > input, int width, int height ){
    cout << endl << "DEBUG" << endl << endl;
    vector < tuple < int, int > > to_return;
    vector < tuple < int, int > > empty;
    vector < double > total_areas;
    int input_size = input.size();
    //Limpiando la entrada
    for( int i = 0; i < input_size; i++ ){
        if( are_intersected( input[i], empty, height, width ) ){
            input.erase(input.begin() + i);
        };
    };
    input_size = input.size();

    vector < int > radius;
    for( int i = 0; i < input.size(); i++ ){
        radius.push_back( get< 1 >( input[i] ) );
    };
    radius = sort_by_radius( radius );
    vector < tuple < int, int > > sorted_input;
    for( int i = 0; i < radius.size(); i++ ){
        for( int j = 0; j < input.size(); j++ ){
            if( radius[i] == get< 1 >( input[j] ) ){
                sorted_input.push_back( input[j] );
                input.erase( input.begin() + j );
            };
        };
    };
    reverse( sorted_input.begin(), sorted_input.end() );
    input = sorted_input;

    double areas[ input_size ][ input_size ];
    int id_major_area_row = 0;
    for( int i = 0; i < input_size; i++ ){
        double total_area_row = 0;
        for( int j = 0; j < input_size; j++ ){
            if( i == j ){
                if( !are_intersected( input[i], empty, height, width ) ){
                    areas[i][j] =PI * pow( get<1>( input[j] ), 2 );
                };
            }else{
                bool flag_valid = true;
                if( are_intersected( input[i], empty, height, width ) ){
                    flag_valid = false;
                };
                if( are_intersected( input[j], empty, height, width ) ){
                    flag_valid = false;
                };
                if( flag_valid ){
                    vector < tuple < int, int > > fixed_points;
                    fixed_points.push_back( input[j] );
                    flag_valid = !are_intersected( input[i], fixed_points, height, width );
                    cout << i << " " << j << " " << flag_valid << endl;
                };
                if( flag_valid ){
                    areas[i][j] = ( PI * pow( get<1>( input[i] ), 2 ) ) + ( PI * pow( get<1>( input[j] ), 2 ) );
                }else{
                    areas[i][j] = 0;
                };
            };
            total_area_row += areas[i][j];
        };
        total_areas.push_back( total_area_row );
        if( total_area_row > total_areas[id_major_area_row] ){
            id_major_area_row = i;
        };
    };

    for( int x = 0; x < input_size; x++ ){
        int total = 0;
        for( int y = 0; y < input_size; y++ ){
            cout << areas[x][y] << " # ";
            total += areas[x][y];
        };
        cout << "   => TOTAL:" << total;
        cout << endl;
    };

    int pivot_row = id_major_area_row;
    int filter_row[ input_size ][ input_size ];
    cout << endl;
    for( int i = 0; i < input_size; i++ ){
        cout << "Pivot: " << pivot_row << endl;
        for( int j = 0; j < input_size; j++ ){
            if( areas[pivot_row][j] != 0 ){
                filter_row[ i ][ j ] = 1;
            }else{
                filter_row[ i ][ j ] = 0;
            };
        };
        for( int k = i; k < input_size; k++ ){
            if( ( filter_row[ 0 ][k] == 1 ) ){
                pivot_row = k;
                break;
            };
        };
    };
    cout << endl << endl;
    for( int i = 0; i < input_size; i++ ){
        for( int j = 0; j < input_size; j++ ){
            cout << filter_row[i][j] << " ";
        };
        cout << endl;
    };
    cout << endl << "Resultado" << endl;
    int result[ input_size ];
    for( int i = 0; i < input_size; i++ ){
        bool zero_flag = false;
        for( int j = 0; j < input_size; j++ ){
            if( filter_row[j][i] == 0 ){
                zero_flag = true;
                break;
            };
        };
        if( zero_flag ){
            result[i] = 0;
        }else{
            result[i] = 1;
            to_return.push_back( input[i] );
        };
    };

    for( int i = 0; i < input_size; i++ ){
        cout << result[i] << " ";
    };
    cout << endl;

    for( int i = 0; i < input_size; i++ ){
        cout << get<0>(input[i]) << " - " << get<1>(input[i]) << " ||| ";
    };

    cout << endl << "END DEBUG" << endl;
    return to_return;
};

vector < tuple < int, int > > dynamic3_( vector < tuple < int, int > > input, int width, int height ){

    vector < tuple < int, int > > to_return;
    vector < tuple < int, int > > empty;
    int input_size = input.size();
    //Limpiando la entrada
    for( int i = 0; i < input_size; i++ ){
        if( are_intersected( input[i], empty, height, width ) ){
            input.erase(input.begin() + i);
        };
    };
    input_size = input.size();
    vector < tuple < int, int, double > > vec_start_end_n_area;
    int array_end_points[ input_size ];
    for( int i = 0; i < input_size; i++ ){
        int point = get<0>( input[i] );
        int radius = get<1>( input[i] );
        tuple < int, int, double > tmp = make_tuple( (point - radius), (point + radius), circle_area( radius ) );
        vec_start_end_n_area.push_back( tmp );
        array_end_points[ i ] = (point + radius);
    };
    merge_sort( array_end_points, 0, input_size - 1 );
    // std::cout << endl;
    // for(int i = 0; i < input_size; i++){
    //     std::cout << "Start Point: " << get<0>( vec_start_end_n_area[i] ) << " end point " << get<1>( vec_start_end_n_area[i] ) << std::endl;
    //     std::cout << array_end_points[i] << std::endl;
    // };

    vector < tuple < int, int, double > > sorted_vec_start_end_n_area;
    for(int i = 0; i < input_size; i++){
        int vec_start_end_n_area_size = vec_start_end_n_area.size();
        for(int j = 0; j < vec_start_end_n_area_size; j++){
            int end = get<1>( vec_start_end_n_area[j] );
            if( array_end_points[i] == end ){
                sorted_vec_start_end_n_area.push_back( vec_start_end_n_area[j] );
                vec_start_end_n_area.erase( vec_start_end_n_area.begin() + j );
            };
        };
    };

    std::cout << std::endl << "Sorted circles" << std::endl;
    for(int i = 0; i < input_size; i++){
        std::cout << "Start Point: " << get<0>( sorted_vec_start_end_n_area[i] ) << ", " << get<1>( sorted_vec_start_end_n_area[i] ) << ", " << get<2>( sorted_vec_start_end_n_area[i] )  << std::endl;
    };

    double total_areas[ input_size ];
    // Fist case
    total_areas[ 0 ] = get<2>( sorted_vec_start_end_n_area[ 0 ] );

    vector < tuple < vector < tuple < int, int, double > >, int > > sub_solutions;
    int index_of_circles[ input_size ][ input_size ];
    for( int i = 0; i < input_size; i++ ){
        for( int j = 0; j < input_size; j++ ){
            index_of_circles[i][j] = 0;
        };
    };

    index_of_circles[0][0] = 1;

    for( int i = 1; i < input_size; i++ ){

        int start = get<0>( sorted_vec_start_end_n_area[ i ] );
        double acumulated_area = get<2>( sorted_vec_start_end_n_area[i] );
        int new_j = -1;
        //vector < tuple < int, int, double > > sub_solution;
        for( int j = i - 1; j >= 0; j-- ){
            int sub_end = get<1>( sorted_vec_start_end_n_area[ j ] );
            if( sub_end <= start ){
                new_j = j;
                break;
            };
        };
        if( new_j != -1 ){
            acumulated_area += total_areas[ new_j ];
        };

        // CASO 1 AUMENTA Y ENCUENTRA
        // CASO 2 NO AUMENTA PERO ENCUENTRA
        // CASO 3 NO AUMENTA NO ENCUENTRA

        total_areas[i] = std::max( total_areas[ i - 1 ], acumulated_area );
        /*if( ( total_areas[i] == acumulated_area ) && (  new_j != -1 ) ){
            for( int x = 0; x < input_size; x++ ){
                //index_of_circles[i][x] = index_of_circles[i-1][x];
                index_of_circles[i][x] = 0;
             };
            index_of_circles[i][new_j] = 1;
            index_of_circles[i][i] = 1;
        }else if( ( total_areas[i] == total_areas[ i - 1 ] ) && (  new_j != -1 ) ){
            for( int x = 0; x < input_size; x++ ){
                //index_of_circles[i][x] = index_of_circles[i-1][x];
                index_of_circles[i][x] = 0;
            };
            //index_of_circles[i][new_j] = 1;
            index_of_circles[i][new_j] = 1;
            index_of_circles[i][i] = 1;
        }else if( ( total_areas[i] == total_areas[ i - 1 ] ) && (  new_j == -1 ) ){
            for( int x = 0; x < input_size; x++ ){
                index_of_circles[i][x] = index_of_circles[i-1][x];
             };
            //index_of_circles[i][new_j] = 1;
        }else if( ( total_areas[i] == acumulated_area ) && (  new_j == -1 ) ){
            for( int x = 0; x < input_size; x++ ){
                index_of_circles[i][x] = 0;
             };
            index_of_circles[i][i] = 1;
        };*/

        if( new_j != -1 ){
            index_of_circles[i][new_j] = 2;
            index_of_circles[i][i] = 1;
        }else{
            index_of_circles[i][i] = 1;
        };

        //tuple < vector < tuple < int, int, double > >, int > sub_solution_with_area = make_tuple( sub_solution, acumulated_area );
        //sub_solutions.push_back( sub_solution_with_area );
    };

    std::cout << "\n" ;
    double matrix_areas[ input_size ];
    int id_max_matrix_areas = 0;
    for( int i = 0; i < input_size; i++ ){
        matrix_areas[ i ] = 0;
        for( int j = 0; j < input_size; j++ ){
            std::cout << index_of_circles[i][j] << " ";
            if( ( index_of_circles[i][j] == 1 ) || ( index_of_circles[i][j] == 2 ) ){
                matrix_areas[ i ] += get<2>( sorted_vec_start_end_n_area[ j ] );
            };
        };
        if( matrix_areas[ i ] >= matrix_areas[ id_max_matrix_areas ] ){
            id_max_matrix_areas = i;
        };
        std::cout << "  <= BA: " << total_areas[i] << " A: " << matrix_areas[i];
        std::cout << endl;
    };

    std::cout << "\nÁrea máxima, prog dinámica: " << total_areas[ input_size - 1 ] << std::endl;

    // std::cout << std::endl << "Areas" << std::endl;
    // int id_max_subsolution = 0;
    // int total_area_solution = 0;
    // for(int i = 0; i < sub_solutions.size(); i++){
    //     total_area_solution += get<1>(sub_solutions[i]);
    //     std::cout << "Sub_sol Area: " << get<1>( sub_solutions[ i ] ) << " T:" << get<0>( sub_solutions[ i ] ).size() << std::endl;
    //     int i_total_area_sub_solution = get<1>( sub_solutions[ i ] );
    //     int current_total_area_solution = get<1>( sub_solutions[ id_max_subsolution ] );
    //     if( i_total_area_sub_solution >= current_total_area_solution ){
    //         id_max_subsolution = i;
    //     };
    // };

    // vector < tuple < int, int, double > > best_solution = get<0>( sub_solutions[ id_max_subsolution ] );
    // int total_area_best_solution = get<1>( sub_solutions[ id_max_subsolution ] );
    // int size_of_elements_in_solution = best_solution.size();
    // for(int i = 0; i < size_of_elements_in_solution; i++){
    //     std::cout << "Start Point: " << get<0>( best_solution[i] ) << " end point " << get<1>( best_solution[i] ) << std::endl;
    // };

    // Build solution
    vector < int > index_sol_circles;
    int i = id_max_matrix_areas;
    int sorted_vec_start_end_n_area_size = sorted_vec_start_end_n_area.size();
    while( i >= 0 ){
        bool flag_two = false;
        int pivot = -1;
        for( int j = 0; j < sorted_vec_start_end_n_area_size; j++ ){
            if( index_of_circles[ i ][ j ] == 1 ){
                int index_sol_circles_size = index_sol_circles.size();
                bool exist = false;
                for( int k = 0; k < index_sol_circles_size; k++ ){
                    if( index_sol_circles[k] == j ){
                        exist = true;
                        break;
                    };
                };
                if( !exist ){
                    index_sol_circles.push_back( j );
                };
            };
            if( index_of_circles[ i ][ j ] == 2 ){
                int index_sol_circles_size = index_sol_circles.size();
                bool exist = false;
                for( int k = 0; k < index_sol_circles_size; k++ ){
                    if( index_sol_circles[k] == j ){
                        exist = true;
                        break;
                    };
                };
                if( !exist ){
                    index_sol_circles.push_back( j );
                };
                flag_two = true;
                pivot = j;
            };
        };
        if( !flag_two ){
            break;
        }else{
            i = pivot;
        };
    };

    int index_sol_circles_size = index_sol_circles.size();
    for( int i = 0; i < index_sol_circles_size; i++ ){
        int radius = ( get<1>( sorted_vec_start_end_n_area[ index_sol_circles[i] ] ) - get<0>( sorted_vec_start_end_n_area[ index_sol_circles[i] ] ) ) / 2;
        int point = ( ( get<1>( sorted_vec_start_end_n_area[ index_sol_circles[i] ] ) - get<0>( sorted_vec_start_end_n_area[ index_sol_circles[i] ] ) ) / 2 ) + get<0>( sorted_vec_start_end_n_area[ index_sol_circles[i] ] );
        tuple < int, int > circle_r = make_tuple( point, radius );
        to_return.push_back( circle_r );
    };

    return to_return;
};

vector < int > sort_by_radius( vector < int > input ){
    /*int mid = floor( input.size() / 2 );
    if( input.size() == 1 ){
        return input;
    }else if( input.size() == 2 ){
        if( input[0] > input[1] ){
            iter_swap( input[0], input[1] );
        }else{
            return input;
        };
    }else{
    };*/
    vector < int > to_return;
    int input_size = input.size();
    for( int i = 0; i < input_size; i++ ){
        int index_minor = -1;
        int value = std::numeric_limits<int>::max();;
        for( int j = 0; j < input.size(); j++ ){
            if( value > input[j] ){
                value = input[j];
                index_minor = j;
            };
        };
        to_return.push_back( input[index_minor] );
        input.erase( input.begin() + index_minor );
    };
    return to_return;
};

bool are_intersected( tuple < int, int > new_point, vector < tuple < int, int > > fixed_points, int  height, int width ){
    if( fixed_points.size() == 0 ){
        int point_x = get< 0 >( new_point );
        int radius_ = get< 1 >( new_point );
        double y_max = (double) radius_ + (double) ( (double) height / (double) 2 );
        double y_min = (double) ( (double) height / (double) 2 ) - (double) radius_;
        int x_max = point_x + radius_;
        int x_min = point_x - radius_;
        if( (y_min < 0)||(x_min < 0) ){
            //cout << "C1: " << y_min << " " << x_min << " =>";
            return true;
        }else if( (y_max > height)||(x_max > width) ){
            //cout << "C2" << endl;
            return true;
        }else{
            //cout << "CT: " << y_min << " " << x_min << " =>";
            return false;
        };
    }else{
        for( int i = 0; i < fixed_points.size(); i++ ){
            if( get< 0 >( fixed_points[i] ) ==  get< 0 >( new_point ) ){
                return true;
            }else{
                int fixed_point_x_max = get< 0 >( fixed_points[i] ) + get< 1 >( fixed_points[i] );
                int fixed_point_x_min = get< 0 >( fixed_points[i] ) - get< 1 >( fixed_points[i] );
                int point_x = get< 0 >( new_point );
                int radius_ = get< 1 >( new_point );
                double y_max = (double) radius_ + (double) ( (double) height / (double) 2 );
                double y_min =  (double) ( (double) height / (double) 2 ) - (double) radius_;
                int x_max = point_x + radius_;
                int x_min = point_x - radius_;
                if( (y_min < 0)||(x_min < 0) ){
                    return true;
                }else if( (y_max > height)||(x_max > width) ){
                    return true;
                }else if(
                            //Casos 1 sentido
                            ( (x_max > fixed_point_x_min) && (x_max < fixed_point_x_max) ) ||
                            ( (x_min < fixed_point_x_max) && (x_min > fixed_point_x_min) ) ||
                            ( (x_max == fixed_point_x_max) && ( x_min == fixed_point_x_min ) ) ||
                            ( (x_max < fixed_point_x_max) && ( x_min == fixed_point_x_min ) ) ||
                            ( (x_max == fixed_point_x_max) && ( x_min > fixed_point_x_min ) )  ||
                            // Casos 2 sentido
                            ( (x_max > fixed_point_x_min) && (x_min < fixed_point_x_max) ) ||
                            ( (x_max > fixed_point_x_max) && (x_min < fixed_point_x_min) ) ||
                            ( (x_max > fixed_point_x_max) && ( x_min == fixed_point_x_min ) ) ||
                            ( (x_max == fixed_point_x_max) && ( x_min < fixed_point_x_min ) )
                        ){
                    //cout << "C3: " << x_max << " " << fixed_point_x_min << " - " << x_min << " " << fixed_point_x_max << " - " << " =>";
                    return true;
                };
            };
        };
        return false;
    };
};

string get_geogebra_plot_command( vector < tuple < int, int > > points, int  height, int width ){
    string command = "";
    command += "Execute[{\" x = " + to_string(0) + " \",";
    command += " \" x = " + to_string((double) width) + " \",";
    command += " \" y = " + to_string((double) height / (double) 2) + " \",";
    command += " \" y = -" + to_string((double) height / (double) 2) + " \",";
    for( int i = 0; i < points.size(); i++ ){
        command += " \" Circle(( " +  to_string(get<0>( points[i] )) + " ,0), " +  to_string(get<1>( points[i] )) + ")\"";
        if( i < ( points.size() - 1) ){
            command += ",";
        }else{
            command += "}]";
        };
    };
    return command;
};

vector < vector < tuple < int, int > > > split_vector( vector < tuple < int, int > > vector_, int split_point ) {
    vector < vector < tuple < int, int > > > to_return;
    vector < tuple < int, int > > vector_0;
    vector < tuple < int, int > > vector_1;
    bool vector_toggle = false;
    for( int i = 0; i < vector_.size(); i++ ){
        if( !vector_toggle ){
            vector_0.push_back( vector_[i] );
            if( i == ( split_point - 1 ) ){
                vector_toggle = true;
            };
        }else{
            vector_1.push_back( vector_[i] );
        };
    };
    to_return.push_back( vector_0 );
    to_return.push_back( vector_1 );
    return to_return;
};

vector < tuple < int, int > > merge_vectors( vector < tuple < int, int > > vector_0, vector < tuple < int, int > > vector_1 ) {
    vector < tuple < int, int > > to_return;
    for( int i = 0; i < vector_0.size(); i++ ){
        to_return.push_back( vector_0[i] );
    };
    for( int i = 0; i < vector_1.size(); i++ ){
        to_return.push_back( vector_1[i] );
    };
    return to_return;
};

vector < tuple < int, int > > max( vector < tuple < int, int > > vector_0, vector < tuple < int, int > > vector_1 ) {
    int area_0 = 0;
    int area_1 = 0;
    for( int i = 0; i < vector_0.size(); i++ ){
        area_0 += PI * pow( get<1>( vector_0[i] ), 2);
    };
    for( int i = 0; i < vector_1.size(); i++ ){
        area_1 += PI * pow( get<1>( vector_1[i] ), 2);
    };
    if( area_0 >= 1 ){
        return vector_0;
    }else{
        return vector_1;
    };
};

double circle_area( double radius ){
    return PI * pow( radius, 2 );
};

void merge_sort(int arr[], int l, int r)
{
    if (l < r)
    {
        int m = l+(r-l)/2;

        merge_sort(arr, l, m);
        merge_sort(arr, m+1, r);

        merge(arr, l, m, r);
    };
};

void merge(int arr[], int l, int m, int r){

    int i, j, k;
    int n1 = m - l + 1;
    int n2 =  r - m;

    int L[n1], R[n2];

    for (i = 0; i < n1; i++){
        L[i] = arr[l + i];
    };
    for (j = 0; j < n2; j++){
        R[j] = arr[m + 1+ j];
    };

    i = 0;
    j = 0;
    k = l;

    while (i < n1 && j < n2){
        if (L[i] <= R[j])
        {
            arr[k] = L[i];
            i++;
        }
        else
        {
            arr[k] = R[j];
            j++;
        }
        k++;
    }

    while (i < n1){
        arr[k] = L[i];
        i++;
        k++;
    }

    while (j < n2){
        arr[k] = R[j];
        j++;
        k++;
    };
};

double calculate_input_area( vector < tuple < int, int > > input ){
    double to_return = 0;
    int input_size = input.size();
    for( int i = 0; i < input_size; i++ ){
        to_return += circle_area( get<1>( input[ i ] ) );
    };
    return to_return;
};


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2000 - 2019 by the deal.II authors
 *
 * This file is modification of the version in the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Wolfgang Bangerth, University of Heidelberg, 2000
 * Modified version.
 */
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/vector.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/vector_tools.h>
#include <fstream>

#include <bench_base.hpp>

DEFINE_uint32(
    num_refine_cycles, 1,
    "Number of refinement cycles for the adaptive refinement within deal.ii");
DEFINE_uint32(init_refine_level, 4,
              "Initial level for the refinement of the mesh.");
DEFINE_bool(dealii_orig, false, "Solve with dealii iterative CG");
DEFINE_bool(vis_sol, false, "Print the solution for visualization");

#define CHECK_HERE std::cout << "Here " << __LINE__ << std::endl;

using namespace dealii;
template <int dim, typename ValueType = double, typename IndexType = int,
          typename MixedValueType = double>
class BenchDealiiLaplace : public BenchBase<ValueType, IndexType> {
public:
    BenchDealiiLaplace();
    void run();
    void run(MPI_Comm mpi_communicator);

private:
    void setup_system();
    void assemble_system();
    void solve();
    void solve(MPI_Comm mpi_communicator);
    void refine_grid();
    void output_results(const unsigned int cycle) const;

    Triangulation<dim> triangulation;
    FE_Q<dim> fe;
    DoFHandler<dim> dof_handler;
    AffineConstraints<double> constraints;
    SparseMatrix<double> system_matrix;
    SparsityPattern sparsity_pattern;
    Vector<double> solution;
    Vector<double> system_rhs;
};

template <int dim>
double coefficient(const Point<dim> &p)
{
    if (p.square() < 0.5 * 0.5)
        return 20;
    else
        return 1;
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
BenchDealiiLaplace<dim, ValueType, IndexType,
                   MixedValueType>::BenchDealiiLaplace()
    : fe(2), dof_handler(triangulation)
{}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType,
                        MixedValueType>::setup_system()
{
    dof_handler.distribute_dofs(fe);
    solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());
    constraints.clear();
    DoFTools::make_hanging_node_constraints(dof_handler, constraints);
    VectorTools::interpolate_boundary_values(
        dof_handler, 0, Functions::ZeroFunction<dim>(), constraints);
    constraints.close();
    DynamicSparsityPattern dsp(dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler, dsp, constraints,
                                    /*keep_constrained_dofs = */ false);
    sparsity_pattern.copy_from(dsp);
    system_matrix.reinit(sparsity_pattern);
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType,
                        MixedValueType>::assemble_system()
{
    const QGauss<dim> quadrature_formula(fe.degree + 1);
    FEValues<dim> fe_values(fe, quadrature_formula,
                            update_values | update_gradients |
                                update_quadrature_points | update_JxW_values);
    const unsigned int dofs_per_cell = fe.dofs_per_cell;
    const unsigned int n_q_points = quadrature_formula.size();
    FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
    Vector<double> cell_rhs(dofs_per_cell);
    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);
    for (const auto &cell : dof_handler.active_cell_iterators()) {
        cell_matrix = 0;
        cell_rhs = 0;
        fe_values.reinit(cell);
        for (unsigned int q_index = 0; q_index < n_q_points; ++q_index) {
            const double current_coefficient =
                coefficient<dim>(fe_values.quadrature_point(q_index));
            for (unsigned int i = 0; i < dofs_per_cell; ++i) {
                for (unsigned int j = 0; j < dofs_per_cell; ++j)
                    cell_matrix(i, j) +=
                        (current_coefficient *               // a(x_q)
                         fe_values.shape_grad(i, q_index) *  // grad phi_i(x_q)
                         fe_values.shape_grad(j, q_index) *  // grad phi_j(x_q)
                         fe_values.JxW(q_index));            // dx
                cell_rhs(i) +=
                    (1.0 *                                // f(x)
                     fe_values.shape_value(i, q_index) *  // phi_i(x_q)
                     fe_values.JxW(q_index));             // dx
            }
        }
        cell->get_dof_indices(local_dof_indices);
        constraints.distribute_local_to_global(cell_matrix, cell_rhs,
                                               local_dof_indices, system_matrix,
                                               system_rhs);
    }
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType, MixedValueType>::solve()
{
    SolverControl solver_control(1000, 1e-12);
    SolverCG<> solver(solver_control);
    PreconditionSSOR<> preconditioner;
    preconditioner.initialize(system_matrix, 1.2);
    auto start_time = std::chrono::steady_clock::now();
    solver.solve(system_matrix, solution, system_rhs, preconditioner);
    auto elapsed_time = std::chrono::duration<double>(
        std::chrono::steady_clock::now() - start_time);
    std::cout << "Time for solve only: " << elapsed_time.count() << std::endl;
    constraints.distribute(solution);
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType, MixedValueType>::solve(
    MPI_Comm mpi_communicator)
{
    schwz::Metadata<ValueType, IndexType> metadata;
    schwz::Settings settings(FLAGS_executor);

    // Set solver metadata from command line args.
    metadata.mpi_communicator = mpi_communicator;
    MPI_Comm_rank(metadata.mpi_communicator, &metadata.my_rank);
    MPI_Comm_size(metadata.mpi_communicator, &metadata.comm_size);
    metadata.tolerance = FLAGS_set_tol;
    metadata.max_iters = FLAGS_num_iters;
    metadata.num_subdomains = metadata.comm_size;
    metadata.num_threads = FLAGS_num_threads;
    metadata.oned_laplacian_size = FLAGS_set_1d_laplacian_size;

    // Generic settings
    settings.write_debug_out = FLAGS_enable_debug_write;
    settings.write_perm_data = FLAGS_write_perm_data;
    settings.write_iters_and_residuals = FLAGS_write_iters_and_residuals;
    settings.print_matrices = FLAGS_print_matrices;
    settings.shifted_iter = FLAGS_shifted_iter;

    // Set solver settings from command line args.
    // Comm settings
    settings.comm_settings.enable_onesided = FLAGS_enable_onesided;
    if (FLAGS_remote_comm_type == "put") {
        settings.comm_settings.enable_put = true;
        settings.comm_settings.enable_get = false;
    } else if (FLAGS_remote_comm_type == "get") {
        settings.comm_settings.enable_put = false;
        settings.comm_settings.enable_get = true;
    }
    settings.comm_settings.enable_one_by_one = FLAGS_enable_one_by_one;
    settings.comm_settings.stage_through_host = FLAGS_stage_through_host;
    settings.comm_settings.enable_overlap = FLAGS_enable_comm_overlap;
    if (FLAGS_flush_type == "flush-all") {
        settings.comm_settings.enable_flush_all = true;
    } else if (FLAGS_flush_type == "flush-local") {
        settings.comm_settings.enable_flush_all = false;
        settings.comm_settings.enable_flush_local = true;
    }
    if (FLAGS_lock_type == "lock-all") {
        settings.comm_settings.enable_lock_all = true;
    } else if (FLAGS_lock_type == "lock-local") {
        settings.comm_settings.enable_lock_all = false;
        settings.comm_settings.enable_lock_local = true;
    }

    // Convergence settings
    settings.convergence_settings.put_all_local_residual_norms =
        FLAGS_enable_put_all_local_residual_norms;
    settings.convergence_settings.enable_global_check_iter_offset =
        FLAGS_enable_global_check_iter_offset;
    settings.convergence_settings.enable_global_check =
        FLAGS_enable_global_check;
    if (FLAGS_global_convergence_type == "centralized-tree") {
        settings.convergence_settings.enable_global_simple_tree = true;
    } else if (FLAGS_global_convergence_type == "decentralized") {
        settings.convergence_settings.enable_decentralized_leader_election =
            true;
        settings.convergence_settings.enable_accumulate =
            FLAGS_enable_decentralized_accumulate;
    }

    // General solver settings
    metadata.local_solver_tolerance = FLAGS_local_tol;
    metadata.local_precond = FLAGS_local_precond;
    metadata.local_max_iters = FLAGS_local_max_iters;
    metadata.updated_max_iters = FLAGS_updated_max_iters;
    settings.non_symmetric_matrix = FLAGS_non_symmetric_matrix;
    settings.restart_iter = FLAGS_restart_iter;
    settings.enable_logging = FLAGS_enable_logging;
    metadata.precond_max_block_size = FLAGS_precond_max_block_size;
    settings.matrix_filename = FLAGS_matrix_filename;
    settings.explicit_laplacian = FLAGS_explicit_laplacian;
    settings.enable_random_rhs = FLAGS_enable_random_rhs;
    settings.use_mixed_precision = FLAGS_use_mixed_precision;
    settings.overlap = FLAGS_overlap;
    settings.naturally_ordered_factor = FLAGS_factor_ordering_natural;
    settings.reorder = FLAGS_local_reordering;
    settings.factorization = FLAGS_local_factorization;
    if (FLAGS_partition == "metis") {
        settings.partition =
            schwz::Settings::partition_settings::partition_metis;
        settings.metis_objtype = FLAGS_metis_objtype;
    } else if (FLAGS_partition == "regular") {
        settings.partition =
            schwz::Settings::partition_settings::partition_regular;
    } else if (FLAGS_partition == "regular2d") {
        settings.partition =
            schwz::Settings::partition_settings::partition_regular2d;
    }
    if (FLAGS_local_solver == "iterative-ginkgo") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::iterative_solver_ginkgo;
    } else if (FLAGS_local_solver == "direct-cholmod") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::direct_solver_cholmod;
    } else if (FLAGS_local_solver == "direct-umfpack") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::direct_solver_umfpack;
    } else if (FLAGS_local_solver == "direct-ginkgo") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::direct_solver_ginkgo;
    }
    settings.debug_print = FLAGS_debug;
    MixedValueType dummy = 0.0;
    int gsize = 0;
    if (metadata.my_rank == 0) {
        metadata.global_size = system_matrix.m();
        std::cout << " Running on the " << FLAGS_executor << " executor on "
                  << metadata.num_subdomains << " ranks with "
                  << FLAGS_num_threads << " threads" << std::endl;
        std::cout << " MixedValueType: " << typeid(dummy).name() << std::endl;
        std::cout << " Problem Size: " << metadata.global_size
                  << " Number of non-zeros: "
                  << system_matrix.n_nonzero_elements() << std::endl;
        gsize = metadata.global_size;
    }
    MPI_Bcast(&gsize, 1, MPI_INT, 0, MPI_COMM_WORLD);
    metadata.global_size = gsize;
    if (FLAGS_print_config) {
        if (metadata.my_rank == 0) {
            this->print_config();
        }
    }
    using vec_vtype = gko::matrix::Dense<ValueType>;
    std::shared_ptr<vec_vtype> solution_vector;
    schwz::SolverRAS<ValueType, IndexType, MixedValueType> solver(settings,
                                                                  metadata);
    solver.initialize(system_matrix, system_rhs);
    auto start_time = std::chrono::steady_clock::now();
    solver.run(solution_vector);
    auto elapsed_time = std::chrono::duration<double>(
        std::chrono::steady_clock::now() - start_time);
    if (metadata.my_rank == 0) {
        std::cout << "Time for solve only: " << elapsed_time.count()
                  << std::endl;
    }
    if (FLAGS_timings_file != "null") {
        std::string rank_string = std::to_string(metadata.my_rank);
        if (metadata.my_rank < 10) {
            rank_string = "0" + std::to_string(metadata.my_rank);
        }
        std::string filename = FLAGS_timings_file + "_" + rank_string + ".csv";
        this->write_timings(metadata.time_struct, filename,
                            settings.comm_settings.enable_onesided);
    }
    if (FLAGS_write_comm_data) {
        std::string rank_string = std::to_string(metadata.my_rank);
        if (metadata.my_rank < 10) {
            rank_string = "0" + std::to_string(metadata.my_rank);
        }
        std::string filename_send = "num_send_" + rank_string + ".csv";
        std::string filename_recv = "num_recv_" + rank_string + ".csv";
        this->write_comm_data(metadata.num_subdomains, metadata.my_rank,
                              metadata.comm_data_struct, filename_send,
                              filename_recv);
    }

    if (metadata.my_rank == 0) {
        std::copy(solution_vector->get_values(),
                  solution_vector->get_values() + metadata.global_size,
                  solution.begin());
        constraints.distribute(solution);
    }
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType,
                        MixedValueType>::refine_grid()
{
    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());
    KellyErrorEstimator<dim>::estimate(
        dof_handler, QGauss<dim - 1>(fe.degree + 1),
        std::map<types::boundary_id, const Function<dim> *>(), solution,
        estimated_error_per_cell);
    GridRefinement::refine_and_coarsen_fixed_number(
        triangulation, estimated_error_per_cell, 0.3, 0.03);
    triangulation.execute_coarsening_and_refinement();
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType, MixedValueType>::
    output_results(const unsigned int cycle) const
{
    {
        GridOut grid_out;
        std::ofstream output("grid-" + std::to_string(cycle) + ".gnuplot");
        GridOutFlags::Gnuplot gnuplot_flags(false, 5);
        grid_out.set_flags(gnuplot_flags);
        MappingQGeneric<dim> mapping(3);
        grid_out.write_gnuplot(triangulation, output, &mapping);
    }
    {
        DataOut<dim> data_out;
        data_out.attach_dof_handler(dof_handler);
        data_out.add_data_vector(solution, "solution");
        data_out.build_patches();
        std::ofstream output("solution-" + std::to_string(cycle) + ".vtu");
        data_out.write_vtu(output);
    }
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType, MixedValueType>::run()
{
    int num_cycles = FLAGS_num_refine_cycles;

    for (unsigned int cycle = 0; cycle < num_cycles; ++cycle) {
        std::cout << "Cycle " << cycle << ':' << std::endl;

        if (cycle == 0) {
            GridGenerator::hyper_cube(triangulation);
            triangulation.refine_global(FLAGS_init_refine_level);
        } else
            refine_grid();
        std::cout << "   Number of active cells:       "
                  << triangulation.n_active_cells() << std::endl;
        setup_system();
        std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
                  << std::endl;
        assemble_system();
        this->solve();
        if (FLAGS_vis_sol) {
            output_results(cycle);
        }
    }
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType, MixedValueType>::run(
    MPI_Comm mpi_communicator)
{
    int num_cycles = FLAGS_num_refine_cycles;
    int mpi_size, mpi_rank;
    MPI_Comm_size(mpi_communicator, &mpi_size);
    MPI_Comm_rank(mpi_communicator, &mpi_rank);

    for (unsigned int cycle = 0; cycle < num_cycles; ++cycle) {
        if (mpi_rank == 0) {
            std::cout << "Cycle " << cycle << ':' << std::endl;

            if (cycle == 0) {
                GridGenerator::hyper_cube(triangulation);
                triangulation.refine_global(FLAGS_init_refine_level);
            } else
                refine_grid();
            std::cout << "   Number of active cells:       "
                      << triangulation.n_active_cells() << std::endl;
            setup_system();
            std::cout << "   Number of degrees of freedom: "
                      << dof_handler.n_dofs() << std::endl;
            assemble_system();
        }
        this->solve(MPI_COMM_WORLD);
        if (mpi_rank == 0) {
            if (FLAGS_vis_sol) {
                output_results(cycle);
            }
        }
    }
}

int main(int argc, char **argv)
{
    try {
        initialize_argument_parsing(&argc, &argv);
        BenchDealiiLaplace<3, double, int, float> laplace_problem;
        if (FLAGS_num_threads > 1) {
            int req_thread_support = MPI_THREAD_MULTIPLE;
            int prov_thread_support = MPI_THREAD_MULTIPLE;

            MPI_Init_thread(&argc, &argv, req_thread_support,
                            &prov_thread_support);
            if (prov_thread_support != req_thread_support) {
                std::cout << "Required thread support is " << req_thread_support
                          << " but provided thread support is only "
                          << prov_thread_support << std::endl;
            }
        } else {
            MPI_Init(&argc, &argv);
        }

        int rank = 0;
        MPI_Comm_rank(MPI_COMM_WORLD, &rank);
        if (FLAGS_dealii_orig) {
            if (rank == 0) {
                auto start_time = std::chrono::steady_clock::now();
                laplace_problem.run();
                auto elapsed_time = std::chrono::duration<double>(
                    std::chrono::steady_clock::now() - start_time);
                std::cout << "Total Time for setup+solve: "
                          << elapsed_time.count() << std::endl;
            }
        } else {
            auto start_time = std::chrono::steady_clock::now();
            laplace_problem.run(MPI_COMM_WORLD);
            auto elapsed_time = std::chrono::duration<double>(
                std::chrono::steady_clock::now() - start_time);
            if (rank == 0) {
                std::cout << "Total Time for setup+solve: "
                          << elapsed_time.count() << std::endl;
            }
        }
        MPI_Finalize();
    } catch (std::exception &exc) {
        std::cerr << std::endl
                  << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        std::cerr << "Exception on processing: " << std::endl
                  << exc.what() << std::endl
                  << "Aborting!" << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        return 1;
    } catch (...) {
        std::cerr << std::endl
                  << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        std::cerr << "Unknown exception!" << std::endl
                  << "Aborting!" << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        return 1;
    }
    return 0;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Wakker, K. F. (2007), Lecture Notes astro II (Chapter 18), TU Delft course AE4-874,
 *          Delft University of technology, Delft, The Netherlands.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "tudat/astro/mission_segments/escapeAndCapture.h"

namespace tudat
{
namespace unit_tests
{

//! Test patched conics implementation.
BOOST_AUTO_TEST_SUITE( test_escape_and_capture )

//! Test delta-V computation for the escape phase.
BOOST_AUTO_TEST_CASE( testDeltaVEscape )
{
    // Set tolerance.
    const double tolerance = 1.0e-15;

    // Expected test result based on Table 18.2 of [Wakker, 2007]. Escape velocity to go to Mars
    // starting from a parking orbit around Earth at 185 km altitude. Result calculated with more
    // precision separately using Equation 18.25 of [Wakker, 2007].
    const double expectedDeltaVEscape = 3614.64460281887;

    // Set required parameters.
    const double gravitationalParameterEarth = 3.986e14;
    const double semiMajorAxis = 6.378e6 + 1.85e5;
    const double eccentricity = 0.0;
    const double excessVelocity = 2944.61246668719;

    // Compute delta-V of escape phase.
    const double deltaVEscape = mission_segments::computeEscapeOrCaptureDeltaV(
                gravitationalParameterEarth, semiMajorAxis, eccentricity, excessVelocity );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( expectedDeltaVEscape, deltaVEscape, tolerance );
}

//! Test delta-V computation for the capture phase.
BOOST_AUTO_TEST_CASE( testDeltaVCapture )
{
    // Set tolerance.
    const double tolerance = 1.0e-15;

    // Expected test result based on Table 18.2 of [Wakker, 2007]. Capture velocity to arrive at
    // a parking orbit around Mars of 1.1 times the planetary radius starting from Earth. Result
    // calculated with more precision separately using Equation 18.25 of [Wakker, 2007].
    const double expectedDeltaVCapture = 2087.1062716740798;

    // Set required parameters.
    const double gravitationalParameterMars = 4.2830e13;
    const double semiMajorAxis = 1.1 * 3.3895e6;
    const double eccentricity = 0.0;
    const double excessVelocity = 2648.83359973278;

    // Compute delta-V of escape phase.
    const double deltaVCapture = mission_segments::computeEscapeOrCaptureDeltaV(
                gravitationalParameterMars, semiMajorAxis, eccentricity, excessVelocity );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( expectedDeltaVCapture, deltaVCapture, tolerance );
}

//! Test delta-V computation for an escape from an eccentric parking orbit.
BOOST_AUTO_TEST_CASE( testDeltaVEscapeElliptical )
{
    // Set tolerance.
    const double tolerance = 1.0e-15;

    // Departure from 200 km perigee, 1800 km apogee parking orbit from Earth to Mars transfer.
    // Result calculated to high precision separately using Equation 18.25 of [Wakker, 2007].
    const double expectedDeltaVEscape = 3200.2178506657729;

    // Set required parameters.
    const double gravitationalParameterEarth = 3.986e14;
    const double semiMajorAxis = 6.378e6 + 1.0e6;
    const double eccentricity = 0.108430468961778;
    const double excessVelocity = 2944.61246668719;

    // Compute delta-V of escape phase.
    const double deltaVEscape = mission_segments::computeEscapeOrCaptureDeltaV(
                gravitationalParameterEarth, semiMajorAxis, eccentricity, excessVelocity );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( expectedDeltaVEscape, deltaVEscape, tolerance );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat