// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-

// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright (c) 2018, FZI Forschungszentrum Informatik
//
// Redistribution and use in source and binary forms, with or without modification, are permitted
// provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this list of conditions
//    and the following disclaimer.
//
// 2. 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.
//
// 3. Neither the name of the copyright holder 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.
// -- END LICENSE BLOCK ------------------------------------------------

//----------------------------------------------------------------------
/*!\file
 *
 * \author  Philipp Bender <philipp.bender@fzi.de>
 * \date    2014-01-01
 *
 */
//----------------------------------------------------------------------

#pragma once

#include <boost/math/special_functions.hpp>
#include <cmath>
#include <boost/tuple/tuple.hpp>
#include <boost/variant/get.hpp>
#include "LocalGeographicCS.hpp"
#include "normalize_angle.hpp"
#include "Attribute.hpp"

namespace LLet
{

enum LATLON_COORDS
{
    LAT = 0,
    LON = 1,
    ID = 2
};

enum XY_COORDS
{
    X = 0,
    Y = 1
};

class point_with_id_t : public boost::tuple<double, double, int64_t>, public HasAttributes {
public:
  point_with_id_t(const boost::tuple<double, double, int64_t> &tuple = boost::tuple<double, double, int64_t>(0.0, 0.0, -1)) :
    boost::tuple<double, double, int64_t>(tuple)
  {
    // nothing to do
  }

#ifdef _WIN32 // The tuple implementation of MSVC10 fails on the auto-generated copy ctor
    point_with_id_t(const point_with_id_t &other) :
      HasAttributes(other)
    {
        boost::get<LLet::LAT>(*this) = boost::get<LLet::LAT>(other);
        boost::get<LLet::LON>(*this) = boost::get<LLet::LON>(other);
        boost::get<LLet::ID>(*this) = boost::get<LLet::ID>(other);
    }
#endif

};

inline bool operator==(const point_with_id_t& lhs, const point_with_id_t& rhs)
{
  return boost::get<0>(lhs) == boost::get<0>(rhs)
      && boost::get<1>(lhs) == boost::get<1>(rhs)
      && boost::get<2>(lhs) == boost::get<2>(rhs);
}
inline bool operator!=(const point_with_id_t& lhs, const point_with_id_t& rhs){return !(lhs == rhs);}

typedef boost::tuple< double, double > point_xy_t;
typedef std::vector<point_xy_t> vertex_container_t;

inline
boost::tuple< double, double > vec( const point_with_id_t& a, const point_with_id_t& b )
{
    using boost::get;

    LocalGeographicCS cs(get<LAT>(a), get<LON>(a));

    double ax, ay, bx, by;
    boost::tie(ax, ay) = cs.ll2xy(get<LAT>(a), get<LON>(a));
    boost::tie(bx, by) = cs.ll2xy(get<LAT>(b), get<LON>(b));

    double dx = bx -  ax;
    double dy = by -  ay;

    return boost::make_tuple(dx, dy);
}

//! Calculate the absolute point from absolute point \a a with relative offset \a vec and the given id \a id
inline point_with_id_t from_vec(const point_with_id_t& a, const point_xy_t& vec, const int64_t id = -1)
{
  using boost::get;

  LocalGeographicCS cs(get<LAT>(a), get<LON>(a));
  const boost::tuple<double, double>& ll = cs.xy2ll(get<X>(vec), get<Y>(vec));
  return boost::make_tuple(get<LAT>(ll), get<LON>(ll), id);
}

inline
double abs( const boost::tuple< double, double > &v )
{
    using boost::get;
    using boost::math::hypot;
    return hypot( get<X>(v), get<Y>(v) );
}

//! Calculate the distance between (metric) a and b
inline double dist(const point_xy_t& a, const point_xy_t& b)
{
  using boost::get;
  return abs(boost::make_tuple(get<X>(b) - get<X>(a), get<Y>(b) - get<Y>(a)));
}

//! Normalize the vector \a vec
inline void normalize(boost::tuple<double, double>& vec)
{
  using boost::get;
  const double length = abs(vec);
  assert(length != 0 && "The given vector's length is 0.");
  vec = boost::make_tuple(get<X>(vec) / length, get<Y>(vec) / length);
}

//! Calculate the distance between (gnss) a and b
inline double dist( const point_with_id_t& a, const point_with_id_t& b )
{
    return abs(vec(a, b));
}

inline
double scalar_product( const boost::tuple< double, double >& a, const boost::tuple< double, double >& b )
{
    using boost::get;
    return get<X>(a) * get<X>(b) + get<Y>(a) * get<Y>(b);
}

//! Project p on line
inline
point_xy_t projected(const point_xy_t& p, const vertex_container_t &line, double* angle=0, std::size_t* index=0, std::size_t* previous_index=0, std::size_t* subsequent_index=0)
{
  double min_distance = -1.0;
  double const px = boost::get<LLet::X>(p);
  double const py = boost::get<LLet::Y>(p);
  point_xy_t min_point = p;
  size_t const size = line.size();
  for (std::size_t i=1; i<size; ++i)
  {
    const double& ax = boost::get<LLet::X>(line[i-1]);
    const double& ay = boost::get<LLet::Y>(line[i-1]);
    const double& bx = boost::get<LLet::X>(line[i]);
    const double& by = boost::get<LLet::Y>(line[i]);

    point_xy_t const AP = point_xy_t(px-ax, py-ay);
    point_xy_t const AB = point_xy_t(bx-ax, by-ay);
    double const scalar_product_ab = scalar_product(AB, AB);
    double const lambda = scalar_product(AP, AB) / ( scalar_product_ab == 0.0 ? 1.0 : scalar_product_ab );

    double distance = 0.0;
    point_xy_t c;
    if (lambda <= 0.0)
    {
      c = line[i-1];
    }
    else if (lambda < 1.0)
    {
      c = point_xy_t(ax + lambda*(bx-ax), ay + lambda*(by-ay));
    }
    else
    {
      c = line[i];
    }
    distance = dist(p, c);
    if (min_distance < 0.0 || distance < min_distance)
    {
      min_distance = distance;
      min_point = c;
      if (angle)
      {
        *angle = atan2(boost::get<Y>(line[i])-boost::get<Y>(line[i-1]), boost::get<X>(line[i])-boost::get<X>(line[i-1]));
      }
      if (index)
      {
        *index = lambda < 0.5? i-1 : i;
      }
      if (subsequent_index)
      {
        *subsequent_index = i;
      }
      if (previous_index)
      {
        *previous_index = i-1;
      }
    }
  }

  return min_point;
}

inline
double angle( const boost::tuple< double, double >& a, const boost::tuple< double, double >& b )
{
    using boost::get;

    double sp = scalar_product(a, b);
    double cos_phi = sp / (abs(a) * abs(b));

    // sign for angle: test cross product
    double crossp_z = get<X>(a) * get<Y>(b) - get<Y>(a) * get<X>(b);
    double signum = boost::math::sign(crossp_z);
    double phi = normalize_angle(signum * std::acos(cos_phi));
    return phi;
}

template< typename T1, typename T2 >
inline
bool inrange(const T1& val, const T2& lo, const T2& hi)
{
    return val >= lo && val <= hi;
}

/*! Interpolate the given points using a Catmull-Rom polygon.
 *  Any \a ratio between 0 and 1 will interpolate in between \a p1 and \a p2.
 */
inline point_xy_t interpolate_spline(const point_xy_t& p0, const point_xy_t& p1,
                                     const point_xy_t& p2, const point_xy_t& p3, double ratio)
{
  /* Catmull-Rom coefficients:
   *
   * a0 = -0.5*p0 + 1.5*p1 - 1.5*p2 + 0.5*p3
   * a1 = p0 - 2.5*p1 + 2*p2 - 0.5*p3
   * a2 = -0.5*p0 + 0.5*p2
   */

  const point_xy_t a0 = boost::make_tuple(-0.5 * boost::get<X>(p0) + 1.5 * boost::get<X>(p1) - 1.5 * boost::get<X>(p2) + 0.5 * boost::get<X>(p3),
                                       -0.5 * boost::get<Y>(p0) + 1.5 * boost::get<Y>(p1) - 1.5 * boost::get<Y>(p2) + 0.5 * boost::get<Y>(p3));

  const point_xy_t a1 = boost::make_tuple(boost::get<X>(p0) - 2.5 * boost::get<X>(p1) + 2. * boost::get<X>(p2) - 0.5 * boost::get<X>(p3),
                                       boost::get<Y>(p0) - 2.5 * boost::get<Y>(p1) + 2. * boost::get<Y>(p2) - 0.5 * boost::get<Y>(p3));

  const point_xy_t a2 = boost::make_tuple(-0.5 * boost::get<X>(p0) + 0.5 * boost::get<X>(p2),
                                       -0.5 * boost::get<Y>(p0) + 0.5 * boost::get<Y>(p2));
  const double ratio_square = ratio * ratio;

  // Catmull-Rom polynom: result = a0 * ratio³ + a1 * ratio² + a2 * ratio + p1
  return boost::make_tuple(boost::get<X>(a0) * ratio * ratio_square + boost::get<X>(a1) * ratio_square + boost::get<X>(a2) * ratio + boost::get<X>(p1),
                        boost::get<Y>(a0) * ratio * ratio_square + boost::get<Y>(a1) * ratio_square + boost::get<Y>(a2) * ratio + boost::get<Y>(p1));
}

//! Calculate the dot product for vector \a and vector \b
inline double dot(const point_xy_t& a, const point_xy_t& b)
{
  return boost::get<LLet::X>(a) * boost::get<LLet::X>(b) +
      boost::get<LLet::Y>(a) * boost::get<LLet::Y>(b);
}

}


/*
 * This file belongs to the Galois project, a C++ library for exploiting parallelism.
 * The code is being released under the terms of the 3-Clause BSD License (a
 * copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */

#include "galois/Galois.h"
#include "galois/graphs/LCGraph.h"

#include "llvm/Support/CommandLine.h"
#include "Lonestar/BoilerPlate.h"

#include <boost/iterator/filter_iterator.hpp>

#include <iomanip>
#include <fstream>

static const char* name = "Betweenness Centrality";
static const char* desc = "Computes the betweenness centrality of all nodes in "
                          "a graph";
static const char* url = "betweenness_centrality";

static llvm::cl::opt<std::string> filename(llvm::cl::Positional,
                                           llvm::cl::desc("<input file>"),
                                           llvm::cl::Required);
static llvm::cl::opt<int> iterLimit("limit",
                                    llvm::cl::desc("Limit number of iterations "
                                                   "to value (0 is all nodes)"),
                                    llvm::cl::init(0));
static llvm::cl::opt<unsigned int> startNode("startNode",
                                             llvm::cl::desc("Node to start "
                                                            "search from"),
                                             llvm::cl::init(0));
static llvm::cl::opt<bool> forceVerify("forceVerify",
                                       llvm::cl::desc("Abort if not verified; "
                                                      "only makes sense for "
                                                      "torus graphs"));
static llvm::cl::opt<bool> printAll("printAll",
                                    llvm::cl::desc("Print betweenness values "
                                                   "for all nodes"));

using Graph = galois::graphs::LC_CSR_Graph<void, void>::with_no_lockable<
    true>::type ::with_numa_alloc<true>::type;
using GNode = Graph::GraphNode;

class BCOuter {
  Graph* G;
  int NumNodes;

  galois::substrate::PerThreadStorage<double*> CB; // betweeness measure
  galois::substrate::PerThreadStorage<double*> perThreadSigma;
  galois::substrate::PerThreadStorage<int*> perThreadD;
  galois::substrate::PerThreadStorage<double*> perThreadDelta;
  galois::substrate::PerThreadStorage<galois::gdeque<GNode>*> perThreadSucc;

public:
  /**
   * Constructor initializes thread local storage.
   */
  BCOuter(Graph& g) : G(&g), NumNodes(g.size()) { InitializeLocal(); }

  /**
   * Constructor destroys thread local storage.
   */
  ~BCOuter(void) { DeleteLocal(); }

  /**
   * Runs betweeness-centrality proper.
   *
   * @tparam Cont type of the data structure that holds the nodes to treat
   * as a source during betweeness-centrality.
   *
   * @param v Data structure that holds nodes to treat as a source during
   * betweeness-centrality
   */
  template <typename Cont>
  void run(const Cont& v) {
    // Each thread works on an individual source node
    galois::do_all(
        galois::iterate(v),
        [&](const GNode& curSource) {
          galois::gdeque<GNode> SQ;

          double* sigma               = *perThreadSigma.getLocal();
          int* d                      = *perThreadD.getLocal();
          double* delta               = *perThreadDelta.getLocal();
          galois::gdeque<GNode>* succ = *perThreadSucc.getLocal();

          sigma[curSource] = 1;
          d[curSource]     = 1;

          SQ.push_back(curSource);

          // Do bfs while computing number of shortest paths (saved into sigma)
          // and successors of nodes;
          // Note this bfs makes it so source has distance of 1 instead of 0
          for (auto qq = SQ.begin(), eq = SQ.end(); qq != eq; ++qq) {
            int src = *qq;

            for (auto edge : G->edges(src, galois::MethodFlag::UNPROTECTED)) {
              int dest = G->getEdgeDst(edge);

              if (!d[dest]) {
                SQ.push_back(dest);
                d[dest] = d[src] + 1;
              }

              if (d[dest] == d[src] + 1) {
                sigma[dest] = sigma[dest] + sigma[src];
                succ[src].push_back(dest);
              }
            }
          }

          // Back-propogate the dependency values (delta) along the BFS DAG
          // ignore the source (hence SQ.size > 1 and not SQ.empty)
          while (SQ.size() > 1) {
            int leaf = SQ.back();
            SQ.pop_back();

            double sigma_leaf = sigma[leaf]; // has finalized short path value
            double delta_leaf = delta[leaf];
            auto& succ_list   = succ[leaf];

            for (auto succ = succ_list.begin(), succ_end = succ_list.end();
                 succ != succ_end; ++succ) {
              delta_leaf += (sigma_leaf / sigma[*succ]) * (1.0 + delta[*succ]);
            }
            delta[leaf] = delta_leaf;
          }

          // save result of this source's BC, reset all local values for next
          // source
          double* Vec = *CB.getLocal();
          for (int i = 0; i < NumNodes; ++i) {
            Vec[i] += delta[i];
            delta[i] = 0;
            sigma[i] = 0;
            d[i]     = 0;
            succ[i].clear();
          }
        },
        galois::steal(), galois::loopname("Main"));
  }

  /**
   * Verification for reference torus graph inputs.
   * All nodes should have the same betweenness value up to
   * some tolerance.
   */
  void verify() {
    double sampleBC = 0.0;
    bool firstTime  = true;
    for (int i = 0; i < NumNodes; ++i) {
      double bc = (*CB.getRemote(0))[i];

      for (unsigned j = 1; j < galois::getActiveThreads(); ++j)
        bc += (*CB.getRemote(j))[i];

      if (firstTime) {
        sampleBC = bc;
        galois::gInfo("BC: ", sampleBC);
        firstTime = false;
      } else {
        // check if over some tolerance value
        if ((bc - sampleBC) > 0.0001) {
          galois::gInfo("If torus graph, verification failed ",
                        (bc - sampleBC));
          if (forceVerify)
            abort();
          return;
        }
      }
    }
  }

  /**
   * Print betweeness-centrality measures.
   *
   * @param begin first node to print BC measure of
   * @param end iterator after last node to print
   * @param out stream to output to
   * @param precision precision of the floating points outputted by the function
   */
  void printBCValues(size_t begin, size_t end, std::ostream& out,
                     int precision = 6) {
    for (; begin != end; ++begin) {
      double bc = (*CB.getRemote(0))[begin];

      for (unsigned j = 1; j < galois::getActiveThreads(); ++j)
        bc += (*CB.getRemote(j))[begin];

      out << begin << " " << std::setiosflags(std::ios::fixed)
          << std::setprecision(precision) << bc << "\n";
    }
  }

  /**
   * Print all betweeness centrality values in the graph.
   */
  void printBCcertificate() {
    std::stringstream foutname;
    foutname << "outer_certificate_" << galois::getActiveThreads();

    std::ofstream outf(foutname.str().c_str());
    galois::gInfo("Writing certificate...");

    printBCValues(0, NumNodes, outf, 9);

    outf.close();
  }

private:
  /**
   * Initialize an array at some provided address.
   *
   * @param addr Address to initialize array at
   */
  template <typename T>
  void initArray(T** addr) {
    *addr = new T[NumNodes]();
  }

  /**
   * Destroy an array at some provided address.
   *
   * @param addr Address to destroy array at
   */
  template <typename T>
  void deleteArray(T** addr) {
    delete[] * addr;
  }

  /**
   * Initialize local thread storage.
   */
  void InitializeLocal(void) {
    galois::on_each([this](unsigned, unsigned) {
      this->initArray(CB.getLocal());
      this->initArray(perThreadSigma.getLocal());
      this->initArray(perThreadD.getLocal());
      this->initArray(perThreadDelta.getLocal());
      this->initArray(perThreadSucc.getLocal());
    });
  }

  /**
   * Destroy local thread storage.
   */
  void DeleteLocal(void) {
    galois::on_each([this](unsigned, unsigned) {
      this->deleteArray(CB.getLocal());
      this->deleteArray(perThreadSigma.getLocal());
      this->deleteArray(perThreadD.getLocal());
      this->deleteArray(perThreadDelta.getLocal());
      this->deleteArray(perThreadSucc.getLocal());
    });
  }
};

/**
 * Functor that indicates if a node contains outgoing edges
 */
struct HasOut : public std::unary_function<GNode, bool> {
  Graph* graph;
  HasOut(Graph* g) : graph(g) {}

  bool operator()(const GNode& n) const {
    return graph->edge_begin(n) != graph->edge_end(n);
  }
};

int main(int argc, char** argv) {
  galois::SharedMemSys Gal;
  LonestarStart(argc, argv, name, desc, url);

  Graph g;
  galois::graphs::readGraph(g, filename);

  BCOuter bcOuter(g);

  size_t NumNodes = g.size();

  // preallocate pages for use in algorithm
  galois::reportPageAlloc("MeminfoPre");
  galois::preAlloc(galois::getActiveThreads() * NumNodes / 1650);
  galois::reportPageAlloc("MeminfoMid");

  // preprocessing: find the nodes with out edges we will process and skip
  // over nodes with no out edges

  // find first node with out edges
  boost::filter_iterator<HasOut, Graph::iterator> begin =
      boost::make_filter_iterator(HasOut(&g), g.begin(), g.end());
  boost::filter_iterator<HasOut, Graph::iterator> end =
      boost::make_filter_iterator(HasOut(&g), g.end(), g.end());
  // adjustedEnd = last node we will process based on how many iterations
  // (i.e. sources) we want to do
  boost::filter_iterator<HasOut, Graph::iterator> adjustedEnd =
      iterLimit ? galois::safe_advance(begin, end, (int)iterLimit) : end;

  size_t iterations = std::distance(begin, adjustedEnd);

  // vector of nodes we want to process
  std::vector<GNode> v(begin, adjustedEnd);

  galois::gPrint("Num Nodes: ", NumNodes, " Start Node: ", startNode,
                 " Iterations: ", iterations, "\n");

  // execute algorithm
  galois::StatTimer T;
  T.start();
  bcOuter.run(v);
  T.stop();

  bcOuter.printBCValues(0, std::min(10ul, NumNodes), std::cout, 6);

  if (printAll)
    bcOuter.printBCcertificate();
  if (forceVerify || !skipVerify)
    bcOuter.verify();

  galois::reportPageAlloc("MeminfoPost");

  return 0;
}


// r_c_shortest_paths.hpp header file

// Copyright Michael Drexl 2005, 2006.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GRAPH_R_C_SHORTEST_PATHS_HPP
#define BOOST_GRAPH_R_C_SHORTEST_PATHS_HPP

#include <map>
#include <queue>
#include <vector>

#include <boost/graph/graph_traits.hpp>

namespace boost {

// r_c_shortest_paths_label struct
template<class Graph, class Resource_Container>
struct r_c_shortest_paths_label
{
  r_c_shortest_paths_label
  ( const unsigned long n,
    const Resource_Container& rc = Resource_Container(),
    const r_c_shortest_paths_label* const pl = 0,
    const typename graph_traits<Graph>::edge_descriptor& ed =
      graph_traits<Graph>::edge_descriptor(),
    const typename graph_traits<Graph>::vertex_descriptor& vd =
      graph_traits<Graph>::vertex_descriptor() )
  : num( n ),
    cumulated_resource_consumption( rc ),
    p_pred_label( pl ),
    pred_edge( ed ),
    resident_vertex( vd ),
    b_is_dominated( false ),
    b_is_processed( false )
  {}
  r_c_shortest_paths_label& operator=( const r_c_shortest_paths_label& other )
  {
    if( this == &other )
      return *this;
    this->~r_c_shortest_paths_label();
    new( this ) r_c_shortest_paths_label( other );
    return *this;
  }
  const unsigned long num;
  Resource_Container cumulated_resource_consumption;
  const r_c_shortest_paths_label* const p_pred_label;
  const typename graph_traits<Graph>::edge_descriptor pred_edge;
  const typename graph_traits<Graph>::vertex_descriptor resident_vertex;
  bool b_is_dominated;
  bool b_is_processed;
}; // r_c_shortest_paths_label

template<class Graph, class Resource_Container>
inline bool operator==
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return
    l1.cumulated_resource_consumption == l2.cumulated_resource_consumption;
}

template<class Graph, class Resource_Container>
inline bool operator!=
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return
    !( l1 == l2 );
}

template<class Graph, class Resource_Container>
inline bool operator<
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return
    l1.cumulated_resource_consumption < l2.cumulated_resource_consumption;
}

template<class Graph, class Resource_Container>
inline bool operator>
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return
    l2.cumulated_resource_consumption < l1.cumulated_resource_consumption;
}

template<class Graph, class Resource_Container>
inline bool operator<=
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return
    l1 < l2 || l1 == l2;
}

template<class Graph, class Resource_Container>
inline bool operator>=
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return l2 < l1 || l1 == l2;
}

namespace detail {

// ks_smart_pointer class
// from:
// Kuhlins, S.; Schader, M. (1999):
// Die C++-Standardbibliothek
// Springer, Berlin
// p. 333 f.
template<class T>
class ks_smart_pointer
{
public:
  ks_smart_pointer( T* ptt = 0 ) : pt( ptt ) {}
  ks_smart_pointer( const ks_smart_pointer& other ) : pt( other.pt ) {}
  ks_smart_pointer& operator=( const ks_smart_pointer& other )
    { pt = other.pt; return *this; }
  ~ks_smart_pointer() {}
  T& operator*() const { return *pt; }
  T* operator->() const { return pt; }
  T* get() const { return pt; }
  operator T*() const { return pt; }
  friend bool operator==( const ks_smart_pointer& t,
                          const ks_smart_pointer& u )
    { return *t.pt == *u.pt; }
  friend bool operator!=( const ks_smart_pointer& t,
                          const ks_smart_pointer& u )
    { return *t.pt != *u.pt; }
  friend bool operator<( const ks_smart_pointer& t,
                         const ks_smart_pointer& u )
    { return *t.pt < *u.pt; }
  friend bool operator>( const ks_smart_pointer& t,
                         const ks_smart_pointer& u )
    { return *t.pt > *u.pt; }
  friend bool operator<=( const ks_smart_pointer& t,
                          const ks_smart_pointer& u )
    { return *t.pt <= *u.pt; }
  friend bool operator>=( const ks_smart_pointer& t,
                          const ks_smart_pointer& u )
    { return *t.pt >= *u.pt; }
private:
  T* pt;
}; // ks_smart_pointer

// r_c_shortest_paths_dispatch function (body/implementation)
template<class Graph,
         class VertexIndexMap,
         class EdgeIndexMap,
         class Resource_Container,
         class Resource_Extension_Function,
         class Dominance_Function,
         class Label_Allocator,
         class Visitor>
void r_c_shortest_paths_dispatch
( const Graph& g,
  const VertexIndexMap& vertex_index_map,
  const EdgeIndexMap& /*edge_index_map*/,
  typename graph_traits<Graph>::vertex_descriptor s,
  typename graph_traits<Graph>::vertex_descriptor t,
  // each inner vector corresponds to a pareto-optimal path
  std::vector
    <std::vector
      <typename graph_traits
        <Graph>::edge_descriptor> >& pareto_optimal_solutions,
  std::vector
    <Resource_Container>& pareto_optimal_resource_containers,
  bool b_all_pareto_optimal_solutions,
  // to initialize the first label/resource container
  // and to carry the type information
  const Resource_Container& rc,
  Resource_Extension_Function& ref,
  Dominance_Function& dominance,
  // to specify the memory management strategy for the labels
  Label_Allocator /*la*/,
  Visitor vis )
{
  pareto_optimal_resource_containers.clear();
  pareto_optimal_solutions.clear();

  unsigned long i_label_num = 0;
  typedef
    typename
      Label_Allocator::template rebind
        <r_c_shortest_paths_label
          <Graph, Resource_Container> >::other LAlloc;
  LAlloc l_alloc;
  typedef
    ks_smart_pointer
      <r_c_shortest_paths_label<Graph, Resource_Container> > Splabel;
  std::priority_queue<Splabel, std::vector<Splabel>, std::greater<Splabel> >
    unprocessed_labels;

  bool b_feasible = true;
  r_c_shortest_paths_label<Graph, Resource_Container>* first_label =
    l_alloc.allocate( 1 );
  l_alloc.construct
    ( first_label,
      r_c_shortest_paths_label
        <Graph, Resource_Container>( i_label_num++,
                                     rc,
                                     0,
                                     typename graph_traits<Graph>::
                                       edge_descriptor(),
                                     s ) );

  Splabel splabel_first_label = Splabel( first_label );
  unprocessed_labels.push( splabel_first_label );
  std::vector<std::list<Splabel> > vec_vertex_labels( num_vertices( g ) );
  vec_vertex_labels[vertex_index_map[s]].push_back( splabel_first_label );
  std::vector<typename std::list<Splabel>::iterator>
    vec_last_valid_positions_for_dominance( num_vertices( g ) );
  for( int i = 0; i < static_cast<int>( num_vertices( g ) ); ++i )
    vec_last_valid_positions_for_dominance[i] = vec_vertex_labels[i].begin();
  std::vector<int> vec_last_valid_index_for_dominance( num_vertices( g ), 0 );
  std::vector<bool>
    b_vec_vertex_already_checked_for_dominance( num_vertices( g ), false );
  while( !unprocessed_labels.empty()  && vis.on_enter_loop(unprocessed_labels, g) )
  {
    Splabel cur_label = unprocessed_labels.top();
    unprocessed_labels.pop();
    vis.on_label_popped( *cur_label, g );
    // an Splabel object in unprocessed_labels and the respective Splabel
    // object in the respective list<Splabel> of vec_vertex_labels share their
    // embedded r_c_shortest_paths_label object
    // to avoid memory leaks, dominated
    // r_c_shortest_paths_label objects are marked and deleted when popped
    // from unprocessed_labels, as they can no longer be deleted at the end of
    // the function; only the Splabel object in unprocessed_labels still
    // references the r_c_shortest_paths_label object
    // this is also for efficiency, because the else branch is executed only
    // if there is a chance that extending the
    // label leads to new undominated labels, which in turn is possible only
    // if the label to be extended is undominated
    if( !cur_label->b_is_dominated )
    {
      int i_cur_resident_vertex_num = cur_label->resident_vertex;
      std::list<Splabel>& list_labels_cur_vertex =
        vec_vertex_labels[i_cur_resident_vertex_num];
      if( static_cast<int>( list_labels_cur_vertex.size() ) >= 2
          && vec_last_valid_index_for_dominance[i_cur_resident_vertex_num]
               < static_cast<int>( list_labels_cur_vertex.size() ) )
      {
        typename std::list<Splabel>::iterator outer_iter =
          list_labels_cur_vertex.begin();
        bool b_outer_iter_at_or_beyond_last_valid_pos_for_dominance = false;
        while( outer_iter != list_labels_cur_vertex.end() )
        {
          Splabel cur_outer_splabel = *outer_iter;
          typename std::list<Splabel>::iterator inner_iter = outer_iter;
          if( !b_outer_iter_at_or_beyond_last_valid_pos_for_dominance
              && outer_iter ==
                   vec_last_valid_positions_for_dominance
                     [i_cur_resident_vertex_num] )
            b_outer_iter_at_or_beyond_last_valid_pos_for_dominance = true;
          if( !b_vec_vertex_already_checked_for_dominance
                [i_cur_resident_vertex_num]
              || b_outer_iter_at_or_beyond_last_valid_pos_for_dominance )
          {
            ++inner_iter;
          }
          else
          {
            inner_iter =
              vec_last_valid_positions_for_dominance
                [i_cur_resident_vertex_num];
            ++inner_iter;
          }
          bool b_outer_iter_erased = false;
          while( inner_iter != list_labels_cur_vertex.end() )
          {
            Splabel cur_inner_splabel = *inner_iter;
            if( dominance( cur_outer_splabel->
                             cumulated_resource_consumption,
                           cur_inner_splabel->
                             cumulated_resource_consumption ) )
            {
              typename std::list<Splabel>::iterator buf = inner_iter;
              ++inner_iter;
              list_labels_cur_vertex.erase( buf );
              if( cur_inner_splabel->b_is_processed )
              {
                l_alloc.destroy( cur_inner_splabel.get() );
                l_alloc.deallocate( cur_inner_splabel.get(), 1 );
              }
              else
                cur_inner_splabel->b_is_dominated = true;
              continue;
            }
            else
              ++inner_iter;
            if( dominance( cur_inner_splabel->
                             cumulated_resource_consumption,
                           cur_outer_splabel->
                             cumulated_resource_consumption ) )
            {
              typename std::list<Splabel>::iterator buf = outer_iter;
              ++outer_iter;
              list_labels_cur_vertex.erase( buf );
              b_outer_iter_erased = true;
              if( cur_outer_splabel->b_is_processed )
              {
                l_alloc.destroy( cur_outer_splabel.get() );
                l_alloc.deallocate( cur_outer_splabel.get(), 1 );
              }
              else
                cur_outer_splabel->b_is_dominated = true;
              break;
            }
          }
          if( !b_outer_iter_erased )
            ++outer_iter;
        }
        if( static_cast<int>( list_labels_cur_vertex.size() ) > 1 )
          vec_last_valid_positions_for_dominance[i_cur_resident_vertex_num] =
            (--(list_labels_cur_vertex.end()));
        else
          vec_last_valid_positions_for_dominance[i_cur_resident_vertex_num] =
            list_labels_cur_vertex.begin();
        b_vec_vertex_already_checked_for_dominance
          [i_cur_resident_vertex_num] = true;
        vec_last_valid_index_for_dominance[i_cur_resident_vertex_num] =
          static_cast<int>( list_labels_cur_vertex.size() ) - 1;
      }
    }
    if( !b_all_pareto_optimal_solutions && cur_label->resident_vertex == t )
    {
      // the devil don't sleep
      if( cur_label->b_is_dominated )
      {
        l_alloc.destroy( cur_label.get() );
        l_alloc.deallocate( cur_label.get(), 1 );
      }
      while( unprocessed_labels.size() )
      {
        Splabel l = unprocessed_labels.top();
        unprocessed_labels.pop();
        // delete only dominated labels, because nondominated labels are
        // deleted at the end of the function
        if( l->b_is_dominated )
        {
          l_alloc.destroy( l.get() );
          l_alloc.deallocate( l.get(), 1 );
        }
      }
      break;
    }
    if( !cur_label->b_is_dominated )
    {
      cur_label->b_is_processed = true;
      vis.on_label_not_dominated( *cur_label, g );
      typename graph_traits<Graph>::vertex_descriptor cur_vertex =
        cur_label->resident_vertex;
      typename graph_traits<Graph>::out_edge_iterator oei, oei_end;
      for( boost::tie( oei, oei_end ) = out_edges( cur_vertex, g );
           oei != oei_end;
           ++oei )
      {
        b_feasible = true;
        r_c_shortest_paths_label<Graph, Resource_Container>* new_label =
          l_alloc.allocate( 1 );
        l_alloc.construct( new_label,
                           r_c_shortest_paths_label
                             <Graph, Resource_Container>
                               ( i_label_num++,
                                 cur_label->cumulated_resource_consumption,
                                 cur_label.get(),
                                 *oei,
                                 target( *oei, g ) ) );
        b_feasible =
          ref( g,
               new_label->cumulated_resource_consumption,
               new_label->p_pred_label->cumulated_resource_consumption,
               new_label->pred_edge );

        if( !b_feasible )
        {
          vis.on_label_not_feasible( *new_label, g );
          l_alloc.destroy( new_label );
          l_alloc.deallocate( new_label, 1 );
        }
        else
        {
          const r_c_shortest_paths_label<Graph, Resource_Container>&
            ref_new_label = *new_label;
          vis.on_label_feasible( ref_new_label, g );
          Splabel new_sp_label( new_label );
          vec_vertex_labels[vertex_index_map[new_sp_label->resident_vertex]].
            push_back( new_sp_label );
          unprocessed_labels.push( new_sp_label );
        }
      }
    }
    else
    {
      vis.on_label_dominated( *cur_label, g );
      l_alloc.destroy( cur_label.get() );
      l_alloc.deallocate( cur_label.get(), 1 );
    }
  }
  std::list<Splabel> dsplabels = vec_vertex_labels[vertex_index_map[t]];
  typename std::list<Splabel>::const_iterator csi = dsplabels.begin();
  typename std::list<Splabel>::const_iterator csi_end = dsplabels.end();
  // if d could be reached from o
  if( !dsplabels.empty() )
  {
    for( ; csi != csi_end; ++csi )
    {
      std::vector<typename graph_traits<Graph>::edge_descriptor>
        cur_pareto_optimal_path;
      const r_c_shortest_paths_label<Graph, Resource_Container>* p_cur_label =
        (*csi).get();
      pareto_optimal_resource_containers.
        push_back( p_cur_label->cumulated_resource_consumption );
      while( p_cur_label->num != 0 )
      {
        cur_pareto_optimal_path.push_back( p_cur_label->pred_edge );
        p_cur_label = p_cur_label->p_pred_label;
      }
      pareto_optimal_solutions.push_back( cur_pareto_optimal_path );
      if( !b_all_pareto_optimal_solutions )
        break;
    }
  }

  int i_size = static_cast<int>( vec_vertex_labels.size() );
  for( int i = 0; i < i_size; ++i )
  {
    const std::list<Splabel>& list_labels_cur_vertex = vec_vertex_labels[i];
    csi_end = list_labels_cur_vertex.end();
    for( csi = list_labels_cur_vertex.begin(); csi != csi_end; ++csi )
    {
      l_alloc.destroy( (*csi).get() );
      l_alloc.deallocate( (*csi).get(), 1 );
    }
  }
} // r_c_shortest_paths_dispatch

} // detail

// default_r_c_shortest_paths_visitor struct
struct default_r_c_shortest_paths_visitor
{
  template<class Label, class Graph>
  void on_label_popped( const Label&, const Graph& ) {}
  template<class Label, class Graph>
  void on_label_feasible( const Label&, const Graph& ) {}
  template<class Label, class Graph>
  void on_label_not_feasible( const Label&, const Graph& ) {}
  template<class Label, class Graph>
  void on_label_dominated( const Label&, const Graph& ) {}
  template<class Label, class Graph>
  void on_label_not_dominated( const Label&, const Graph& ) {}
  template<class Queue, class Graph>
  bool on_enter_loop(const Queue& queue, const Graph& graph) {return true;}
}; // default_r_c_shortest_paths_visitor

// default_r_c_shortest_paths_allocator
typedef
  std::allocator<int> default_r_c_shortest_paths_allocator;
// default_r_c_shortest_paths_allocator

// r_c_shortest_paths functions (handle/interface)
// first overload:
// - return all pareto-optimal solutions
// - specify Label_Allocator and Visitor arguments
template<class Graph,
         class VertexIndexMap,
         class EdgeIndexMap,
         class Resource_Container,
         class Resource_Extension_Function,
         class Dominance_Function,
         class Label_Allocator,
         class Visitor>
void r_c_shortest_paths
( const Graph& g,
  const VertexIndexMap& vertex_index_map,
  const EdgeIndexMap& edge_index_map,
  typename graph_traits<Graph>::vertex_descriptor s,
  typename graph_traits<Graph>::vertex_descriptor t,
  // each inner vector corresponds to a pareto-optimal path
  std::vector<std::vector<typename graph_traits<Graph>::edge_descriptor> >&
    pareto_optimal_solutions,
  std::vector<Resource_Container>& pareto_optimal_resource_containers,
  // to initialize the first label/resource container
  // and to carry the type information
  const Resource_Container& rc,
  const Resource_Extension_Function& ref,
  const Dominance_Function& dominance,
  // to specify the memory management strategy for the labels
  Label_Allocator la,
  Visitor vis )
{
  r_c_shortest_paths_dispatch( g,
                               vertex_index_map,
                               edge_index_map,
                               s,
                               t,
                               pareto_optimal_solutions,
                               pareto_optimal_resource_containers,
                               true,
                               rc,
                               ref,
                               dominance,
                               la,
                               vis );
}

// second overload:
// - return only one pareto-optimal solution
// - specify Label_Allocator and Visitor arguments
template<class Graph,
         class VertexIndexMap,
         class EdgeIndexMap,
         class Resource_Container,
         class Resource_Extension_Function,
         class Dominance_Function,
         class Label_Allocator,
         class Visitor>
void r_c_shortest_paths
( const Graph& g,
  const VertexIndexMap& vertex_index_map,
  const EdgeIndexMap& edge_index_map,
  typename graph_traits<Graph>::vertex_descriptor s,
  typename graph_traits<Graph>::vertex_descriptor t,
  std::vector<typename graph_traits<Graph>::edge_descriptor>&
    pareto_optimal_solution,
  Resource_Container& pareto_optimal_resource_container,
  // to initialize the first label/resource container
  // and to carry the type information
  const Resource_Container& rc,
  const Resource_Extension_Function& ref,
  const Dominance_Function& dominance,
  // to specify the memory management strategy for the labels
  Label_Allocator la,
  Visitor vis )
{
  // each inner vector corresponds to a pareto-optimal path
  std::vector<std::vector<typename graph_traits<Graph>::edge_descriptor> >
    pareto_optimal_solutions;
  std::vector<Resource_Container> pareto_optimal_resource_containers;
  r_c_shortest_paths_dispatch( g,
                               vertex_index_map,
                               edge_index_map,
                               s,
                               t,
                               pareto_optimal_solutions,
                               pareto_optimal_resource_containers,
                               false,
                               rc,
                               ref,
                               dominance,
                               la,
                               vis );
  if (!pareto_optimal_solutions.empty()) {
    pareto_optimal_solution = pareto_optimal_solutions[0];
    pareto_optimal_resource_container = pareto_optimal_resource_containers[0];
  }
}

// third overload:
// - return all pareto-optimal solutions
// - use default Label_Allocator and Visitor
template<class Graph,
         class VertexIndexMap,
         class EdgeIndexMap,
         class Resource_Container,
         class Resource_Extension_Function,
         class Dominance_Function>
void r_c_shortest_paths
( const Graph& g,
  const VertexIndexMap& vertex_index_map,
  const EdgeIndexMap& edge_index_map,
  typename graph_traits<Graph>::vertex_descriptor s,
  typename graph_traits<Graph>::vertex_descriptor t,
  // each inner vector corresponds to a pareto-optimal path
  std::vector<std::vector<typename graph_traits<Graph>::edge_descriptor> >&
    pareto_optimal_solutions,
  std::vector<Resource_Container>& pareto_optimal_resource_containers,
  // to initialize the first label/resource container
  // and to carry the type information
  const Resource_Container& rc,
  const Resource_Extension_Function& ref,
  const Dominance_Function& dominance )
{
  r_c_shortest_paths_dispatch( g,
                               vertex_index_map,
                               edge_index_map,
                               s,
                               t,
                               pareto_optimal_solutions,
                               pareto_optimal_resource_containers,
                               true,
                               rc,
                               ref,
                               dominance,
                               default_r_c_shortest_paths_allocator(),
                               default_r_c_shortest_paths_visitor() );
}

// fourth overload:
// - return only one pareto-optimal solution
// - use default Label_Allocator and Visitor
template<class Graph,
         class VertexIndexMap,
         class EdgeIndexMap,
         class Resource_Container,
         class Resource_Extension_Function,
         class Dominance_Function>
void r_c_shortest_paths
( const Graph& g,
  const VertexIndexMap& vertex_index_map,
  const EdgeIndexMap& edge_index_map,
  typename graph_traits<Graph>::vertex_descriptor s,
  typename graph_traits<Graph>::vertex_descriptor t,
  std::vector<typename graph_traits<Graph>::edge_descriptor>&
    pareto_optimal_solution,
  Resource_Container& pareto_optimal_resource_container,
  // to initialize the first label/resource container
  // and to carry the type information
  const Resource_Container& rc,
  const Resource_Extension_Function& ref,
  const Dominance_Function& dominance )
{
  // each inner vector corresponds to a pareto-optimal path
  std::vector<std::vector<typename graph_traits<Graph>::edge_descriptor> >
    pareto_optimal_solutions;
  std::vector<Resource_Container> pareto_optimal_resource_containers;
  r_c_shortest_paths_dispatch( g,
                               vertex_index_map,
                               edge_index_map,
                               s,
                               t,
                               pareto_optimal_solutions,
                               pareto_optimal_resource_containers,
                               false,
                               rc,
                               ref,
                               dominance,
                               default_r_c_shortest_paths_allocator(),
                               default_r_c_shortest_paths_visitor() );
  if (!pareto_optimal_solutions.empty()) {
    pareto_optimal_solution = pareto_optimal_solutions[0];
    pareto_optimal_resource_container = pareto_optimal_resource_containers[0];
  }
}
// r_c_shortest_paths

// check_r_c_path function
template<class Graph,
         class Resource_Container,
         class Resource_Extension_Function>
void check_r_c_path( const Graph& g,
                     const std::vector
                       <typename graph_traits
                         <Graph>::edge_descriptor>& ed_vec_path,
                     const Resource_Container& initial_resource_levels,
                     // if true, computed accumulated final resource levels must
                     // be equal to desired_final_resource_levels
                     // if false, computed accumulated final resource levels must
                     // be less than or equal to desired_final_resource_levels
                     bool b_result_must_be_equal_to_desired_final_resource_levels,
                     const Resource_Container& desired_final_resource_levels,
                     Resource_Container& actual_final_resource_levels,
                     const Resource_Extension_Function& ref,
                     bool& b_is_a_path_at_all,
                     bool& b_feasible,
                     bool& b_correctly_extended,
                     typename graph_traits<Graph>::edge_descriptor&
                       ed_last_extended_arc )
{
  int i_size_ed_vec_path = static_cast<int>( ed_vec_path.size() );
  std::vector<typename graph_traits<Graph>::edge_descriptor> buf_path;
  if( i_size_ed_vec_path == 0 )
    b_feasible = true;
  else
  {
    if( i_size_ed_vec_path == 1
        || target( ed_vec_path[0], g ) == source( ed_vec_path[1], g ) )
      buf_path = ed_vec_path;
    else
      for( int i = i_size_ed_vec_path - 1; i >= 0; --i )
        buf_path.push_back( ed_vec_path[i] );
    for( int i = 0; i < i_size_ed_vec_path - 1; ++i )
    {
      if( target( buf_path[i], g ) != source( buf_path[i + 1], g ) )
      {
        b_is_a_path_at_all = false;
        b_feasible = false;
        b_correctly_extended = false;
        return;
      }
    }
  }
  b_is_a_path_at_all = true;
  b_feasible = true;
  b_correctly_extended = false;
  Resource_Container current_resource_levels = initial_resource_levels;
  actual_final_resource_levels = current_resource_levels;
  for( int i = 0; i < i_size_ed_vec_path; ++i )
  {
    ed_last_extended_arc = buf_path[i];
    b_feasible = ref( g,
                      actual_final_resource_levels,
                      current_resource_levels,
                      buf_path[i] );
    current_resource_levels = actual_final_resource_levels;
    if( !b_feasible )
      return;
  }
  if( b_result_must_be_equal_to_desired_final_resource_levels )
    b_correctly_extended =
     actual_final_resource_levels == desired_final_resource_levels ?
       true : false;
  else
  {
    if( actual_final_resource_levels < desired_final_resource_levels
        || actual_final_resource_levels == desired_final_resource_levels )
      b_correctly_extended = true;
  }
} // check_path

} // namespace

#endif // BOOST_GRAPH_R_C_SHORTEST_PATHS_HPP


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

/*
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003, 2004, 2005, 2006, 2007 StatPro Italia srl
 Copyright (C) 2004, 2005, 2006 Ferdinando Ametrano
 Copyright (C) 2006 Katiuscia Manzoni
 Copyright (C) 2015 Maddalena Zanzi

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

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

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

#include <ql/time/asx.hpp>
#include "ql_settings.hpp"
#include "ql_utilities_dataparsers.hpp"
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
//#include <boost/algorithm/string/case_conv.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

//using boost::algorithm::to_upper_copy;
//using std::string;

namespace QuantLib {
    template <class ExtDate> inline
    bool ASX<ExtDate>::isASXdate(const ExtDate& dat, bool mainCycle) {
        auto date = to_DateLike(dat);
        auto s = date.serialNumber();
        if (date.weekday(s)!=Friday)
            return false;

        Day d = date.dayOfMonth(s);
        if (d<8 || d>14)
            return false;

        if (!mainCycle) return true;

        switch (date.month(s)) {
          case March:
          case June:
          case September:
          case December:
            return true;
          default:
            return false;
        }
    }
    template <class ExtDate> inline
    bool ASX<ExtDate>::isASXcode(const std::string& in, bool mainCycle) {
        if (in.length() != 2)
            return false;

        std::string str1("0123456789");
        std::string::size_type loc = str1.find(in.substr(1, 1), 0);
        if (loc == std::string::npos)
            return false;

        if (mainCycle) str1 = "hmzuHMZU";
        else           str1 = "fghjkmnquvxzFGHJKMNQUVXZ";
        loc = str1.find(in.substr(0,1), 0);
        return loc != std::string::npos;
    }
    template <class ExtDate> inline
    std::string ASX<ExtDate>::code(const ExtDate& date) {
        QL_REQUIRE(isASXdate(date, false),
                   "{} is not an ASX date",date);

        std::ostringstream ASXcode;
        unsigned int y = to_DateLike(date).year() % 10;
        switch(to_DateLike(date).month()) {
          case January:
            ASXcode << 'F' << y;
            break;
          case February:
            ASXcode << 'G' << y;
            break;
          case March:
            ASXcode << 'H' << y;
            break;
          case April:
            ASXcode << 'J' << y;
            break;
          case May:
            ASXcode << 'K' << y;
            break;
          case June:
            ASXcode << 'M' << y;
            break;
          case July:
            ASXcode << 'N' << y;
            break;
          case August:
            ASXcode << 'Q' << y;
            break;
          case September:
            ASXcode << 'U' << y;
            break;
          case October:
            ASXcode << 'V' << y;
            break;
          case November:
            ASXcode << 'X' << y;
            break;
          case December:
            ASXcode << 'Z' << y;
            break;
          default:
            QL_FAIL("not an ASX month (and it should have been)");
        }

        #if defined(QL_EXTRA_SAFETY_CHECKS)
        QL_ENSURE(isASXcode(ASXcode.str(), false),
                  "the result {} is an invalid ASX code", ASXcode.str() );
        #endif
        return ASXcode.str();
    }
    template <class ExtDate> inline
    ExtDate ASX<ExtDate>::date(const std::string& asxCode,
                   const ExtDate& refExtDate) {
        QL_REQUIRE(isASXcode(asxCode, false), "{} is not a valid ASX code", asxCode );

        DateLike<ExtDate> referenceExtDate{
            (to_DateLike(refExtDate) != ExtDate() ?
                 refExtDate :
                 ExtDate(Settings<ExtDate>::instance().evaluationDate()))};
        auto to_upper_copy = [](const std::string& s) { std::string res; for (auto i:s) res.push_back(std::toupper(i)); return res;};
        std::string code = to_upper_copy(asxCode);
        std::string ms = code.substr(0,1);
        QuantLib::Month m;
        if (ms=="F")      m = January;
        else if (ms=="G") m = February;
        else if (ms=="H") m = March;
        else if (ms=="J") m = April;
        else if (ms=="K") m = May;
        else if (ms=="M") m = June;
        else if (ms=="N") m = July;
        else if (ms=="Q") m = August;
        else if (ms=="U") m = September;
        else if (ms=="V") m = October;
        else if (ms=="X") m = November;
        else if (ms=="Z") m = December;
        else QL_FAIL("invalid ASX month letter");

//        Year y = boost::lexical_cast<Year>(); // lexical_cast causes compilation errors with x64

        Year y= io::to_integer(code.substr(1,1));
        /* year<1900 are not valid QuantLib years: to avoid a run-time
           exception few lines below we need to add 10 years right away */
        if (y==0 && referenceExtDate.year()<=1909) y+=10;
        Year referenceYear = (referenceExtDate.year() % 10);
        y += referenceExtDate.year() - referenceYear;
        ExtDate result = ASX<ExtDate>::nextDate(DateAdaptor<ExtDate>::Date(1, m, y), false);
        if (result<referenceExtDate)
            return ASX<ExtDate>::nextDate(DateAdaptor<ExtDate>::Date(1, m, y + 10), false);

        return result;
    }
    template <class ExtDate> inline
    ExtDate ASX<ExtDate>::nextDate(const ExtDate& date, bool mainCycle) {
        DateLike<ExtDate> refExtDate{(to_DateLike(date) == ExtDate() ?
                                          ExtDate(Settings<ExtDate>::instance().evaluationDate()) :
                                          date)};
        Year y = refExtDate.year();
        QuantLib::Month m = refExtDate.month();

        Size offset = mainCycle ? 3 : 1;
        Size skipMonths = offset-(m%offset);
        if (skipMonths != offset || refExtDate.dayOfMonth() > 14) {
            skipMonths += Size(m);
            if (skipMonths<=12) {
                m = QuantLib::Month(skipMonths);
            } else {
                m = QuantLib::Month(skipMonths-12);
                y += 1;
            }
        }

        ExtDate result = DateLike<ExtDate>::nthWeekday(2, Friday, m, y);
        if (result<=refExtDate)
            result = nextDate(DateAdaptor<ExtDate>::Date(15, m, y), mainCycle);
        return result;
    }
    template <class ExtDate> inline
    ExtDate ASX<ExtDate>::nextDate(const std::string& ASXcode,
                       bool mainCycle,
                       const ExtDate& referenceExtDate)  {
        ExtDate asxExtDate = date(ASXcode, referenceExtDate);
        return nextDate(asxExtDate+1, mainCycle);
    }
    template <class ExtDate> inline
    std::string ASX<ExtDate>::nextCode(const ExtDate& d,
                              bool mainCycle) {
        ExtDate date = nextDate(d, mainCycle);
        return code(date);
    }
    template <class ExtDate> inline
    std::string ASX<ExtDate>::nextCode(const std::string& asxCode,
                              bool mainCycle,
                              const ExtDate& referenceExtDate) {
        ExtDate date = nextDate(asxCode, mainCycle, referenceExtDate);
        return code(date);
    }

}


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

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

#include <Eigen/Core>
#include <tudat/basics/testMacros.h>

#include <tudat/simulation/simulation.h>
#include <tudat/io/basicInputOutput.h>
#include <tudat/io/applicationOutput.h>
#include "tudat/astro/LowThrustTrajectories/ShapeBasedMethods/compositeFunctionHodographicShaping.h"
#include "tudat/astro/LowThrustTrajectories/ShapeBasedMethods/hodographicShaping.h"
#include "tudat/astro/LowThrustTrajectories/ShapeBasedMethods/baseFunctionsHodographicShaping.h"
#include "tudat/astro/LowThrustTrajectories/ShapeBasedMethods/createBaseFunctionHodographicShaping.h"
#include "tudat/astro/LowThrustTrajectories/ShapeBasedMethods/baseFunctionsSphericalShaping.h"
#include "tudat/astro/LowThrustTrajectories/ShapeBasedMethods/compositeFunctionSphericalShaping.h"
#include "tudat/astro/LowThrustTrajectories/ShapeBasedMethods/sphericalShaping.h"
#include "tudat/astro/ephemerides/approximatePlanetPositions.h"
#include "tudat/simulation/simulation.h"
#include "tudat/interface/spice/spiceEphemeris.h"

using namespace tudat;
using namespace tudat::input_output;
using namespace tudat::simulation_setup;
using namespace tudat::shape_based_methods;

SystemOfBodies getBetBodyMap( )
{
    // Create central, departure and arrival bodies.
    std::vector< std::string > bodiesToCreate;
    bodiesToCreate.push_back( "Sun" );
    bodiesToCreate.push_back( "Earth" );
    bodiesToCreate.push_back( "Mars" );
    bodiesToCreate.push_back( "Jupiter" );

    std::map< std::string, std::shared_ptr< simulation_setup::BodySettings > > bodySettings =
            simulation_setup::getDefaultBodySettings( bodiesToCreate );

    std::string frameOrigin = "SSB";
    std::string frameOrientation = "ECLIPJ2000";

    // Define central body ephemeris settings.
    bodySettings[ "Sun" ]->ephemerisSettings = std::make_shared< simulation_setup::ConstantEphemerisSettings >(
                ( Eigen::Vector6d( ) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ).finished( ), frameOrigin, frameOrientation );

    bodySettings[ "Sun" ]->ephemerisSettings->resetFrameOrientation( frameOrientation );
    bodySettings[ "Sun" ]->rotationModelSettings->resetOriginalFrame( frameOrientation );

    // Create system of bodies.
    simulation_setup::SystemOfBodies bodies = createBodies( bodySettings );

    bodies[ "Borzi" ] = std::make_shared< simulation_setup::Body >( );
    bodies.at( "Borzi" )->setSuppressDependentOrientationCalculatorWarning( true );
    bodies.at( "Borzi" )->setEphemeris( std::make_shared< ephemerides::TabulatedCartesianEphemeris< > >(
                                                         std::shared_ptr< interpolators::OneDimensionalInterpolator
                                                         < double, Eigen::Vector6d > >( ), frameOrigin, frameOrientation ) );

    // Create radiation pressure settings
    double referenceAreaRadiation = 4.0;
    double radiationPressureCoefficient = 1.2;
    std::vector< std::string > occultingBodies;
    occultingBodies.push_back( "Earth" );
    std::shared_ptr< RadiationPressureInterfaceSettings > asterixRadiationPressureSettings =
            std::make_shared< CannonBallRadiationPressureInterfaceSettings >(
                "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies );

    // Create and set radiation pressure settings
    bodies[ "Borzi" ]->setRadiationPressureInterface(
                "Sun", createRadiationPressureInterface(
                    asterixRadiationPressureSettings, "Borzi", bodies ) );

    setGlobalFrameBodyEphemerides( bodies, frameOrigin, frameOrientation );

    return bodies;
}

int main( )
{

    std::string outputSubFolder = "ShapeBasedTrajectoriesExample/";
    spice_interface::loadStandardSpiceKernels( );

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////           DEFINE TRAJECTORY GLOBAL PARAMETERS      ////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    int numberOfRevolutions = 1;
    double julianDateAtDeparture = 8174.5 * physical_constants::JULIAN_DAY;
    double timeOfFlight = 580.0 * physical_constants::JULIAN_DAY;
    double vehicleInitialMass = 2000.0;
    double specificImpulse = 3000.0;

    std::function< double( const double ) > specificImpulseFunction = [ = ]( const double ){ return specificImpulse; };

    // Retrieve cartesian state at departure and arrival.
    ephemerides::EphemerisPointer pointerToDepartureBodyEphemeris = std::make_shared< ephemerides::ApproximatePlanetPositions>(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter );
    ephemerides::EphemerisPointer pointerToArrivalBodyEphemeris = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::mars );
    Eigen::Vector6d cartesianStateAtDeparture = pointerToDepartureBodyEphemeris->getCartesianState( julianDateAtDeparture );
    Eigen::Vector6d cartesianStateAtArrival = pointerToArrivalBodyEphemeris->getCartesianState( julianDateAtDeparture + timeOfFlight );

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////////////             DEFINE HODOGRAPHIC SHAPING LEG          ////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    double frequency = 2.0 * mathematical_constants::PI / timeOfFlight;
    double scaleFactor = 1.0 / timeOfFlight;

    // Create base function settings for the components of the radial velocity composite function.
    std::shared_ptr< BaseFunctionHodographicShapingSettings > firstRadialVelocityBaseFunctionSettings =
            std::make_shared< BaseFunctionHodographicShapingSettings >( );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > secondRadialVelocityBaseFunctionSettings =
            std::make_shared< PowerFunctionHodographicShapingSettings >( 1.0, scaleFactor );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > thirdRadialVelocityBaseFunctionSettings =
            std::make_shared< PowerFunctionHodographicShapingSettings >( 2.0, scaleFactor );

    // Create components of the radial velocity composite function.
    std::vector< std::shared_ptr< BaseFunctionHodographicShaping > > radialVelocityFunctionComponents;
    radialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( constant, firstRadialVelocityBaseFunctionSettings ) );
    radialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( scaledPower, secondRadialVelocityBaseFunctionSettings ) );
    radialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( scaledPower, thirdRadialVelocityBaseFunctionSettings ) );

    // Create base function settings for the components of the normal velocity composite function.
    std::shared_ptr< BaseFunctionHodographicShapingSettings > firstNormalVelocityBaseFunctionSettings =
            std::make_shared< BaseFunctionHodographicShapingSettings >( );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > secondNormalVelocityBaseFunctionSettings =
            std::make_shared< PowerFunctionHodographicShapingSettings >( 1.0, scaleFactor );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > thirdNormalVelocityBaseFunctionSettings =
            std::make_shared< PowerFunctionHodographicShapingSettings >( 2.0, scaleFactor );

    // Create components of the normal velocity composite function.
    std::vector< std::shared_ptr< shape_based_methods::BaseFunctionHodographicShaping > > normalVelocityFunctionComponents;
    normalVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( constant, firstNormalVelocityBaseFunctionSettings ) );
    normalVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( scaledPower, secondNormalVelocityBaseFunctionSettings ) );
    normalVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( scaledPower, thirdNormalVelocityBaseFunctionSettings ) );

    // Create base function settings for the components of the axial velocity composite function.
    std::shared_ptr< BaseFunctionHodographicShapingSettings > firstAxialVelocityBaseFunctionSettings =
            std::make_shared< TrigonometricFunctionHodographicShapingSettings >( ( numberOfRevolutions + 0.5 ) * frequency );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > secondAxialVelocityBaseFunctionSettings =
            std::make_shared< PowerTimesTrigonometricFunctionHodographicShapingSettings >
            ( 3.0, ( numberOfRevolutions + 0.5 ) * frequency, scaleFactor );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > thirdAxialVelocityBaseFunctionSettings =
            std::make_shared< PowerTimesTrigonometricFunctionHodographicShapingSettings >(
                3.0, ( numberOfRevolutions + 0.5 ) * frequency, scaleFactor );

    // Create components for the axial velocity composite function.
    std::vector< std::shared_ptr< shape_based_methods::BaseFunctionHodographicShaping > > axialVelocityFunctionComponents;
    axialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( cosine, firstAxialVelocityBaseFunctionSettings ) );
    axialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( scaledPowerCosine, secondAxialVelocityBaseFunctionSettings ) );
    axialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( scaledPowerSine, thirdAxialVelocityBaseFunctionSettings ) );

    // Initialize free coefficients vector for radial velocity function (empty here, only 3 base functions).
    Eigen::VectorXd freeCoefficientsRadialVelocityFunction = Eigen::VectorXd::Zero( 0 );

    // Initialize free coefficients vector for normal velocity function (empty here, only 3 base functions).
    Eigen::VectorXd freeCoefficientsNormalVelocityFunction = Eigen::VectorXd::Zero( 0 );

    // Initialize free coefficients vector for axial velocity function (empty here, only 3 base functions).
    Eigen::VectorXd freeCoefficientsAxialVelocityFunction = Eigen::VectorXd::Zero( 0 );

    // Create hodographic-shaping object with defined velocity functions and boundary conditions.
    shape_based_methods::HodographicShaping hodographicShaping(
                cartesianStateAtDeparture, cartesianStateAtArrival, timeOfFlight,
                spice_interface::getBodyGravitationalParameter( "Sun" ), 1,
                radialVelocityFunctionComponents, normalVelocityFunctionComponents, axialVelocityFunctionComponents,
                freeCoefficientsRadialVelocityFunction, freeCoefficientsNormalVelocityFunction, freeCoefficientsAxialVelocityFunction,
                vehicleInitialMass );

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////////////             DEFINE SPHERICAL SHAPING LEG            ////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Define root finder settings (used to update the updated value of the free coefficient, so that it matches the required time of flight).
    std::shared_ptr< root_finders::RootFinderSettings > rootFinderSettings =
            std::make_shared< root_finders::RootFinderSettings >( root_finders::bisection_root_finder, 1.0e-6, 30 );

    // Compute shaped trajectory.
    shape_based_methods::SphericalShaping sphericalShaping = shape_based_methods::SphericalShaping(
                cartesianStateAtDeparture, cartesianStateAtArrival, timeOfFlight,
                spice_interface::getBodyGravitationalParameter( "Sun" ),
                numberOfRevolutions, 0.000703,
                rootFinderSettings, 1.0e-6, 1.0e-1, vehicleInitialMass );

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //////////////////////////////         CREATE ENVIRONMENT                              /////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Set vehicle mass.
    SystemOfBodies bodies = getBetBodyMap( );
    bodies[ "Borzi" ]->setConstantBodyMass( vehicleInitialMass );

    // Define body to propagate and central body.
    std::vector< std::string > bodiesToPropagate;
    std::vector< std::string > centralBodies;
    bodiesToPropagate.push_back( "Borzi" );
    centralBodies.push_back( "Sun" );

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////                 DEFINE PROPAGATION SETTINGS                   /////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Define integrator settings.
    double stepSize = timeOfFlight / static_cast< double >( 8000.0 );
    std::shared_ptr< numerical_integrators::IntegratorSettings< double > > integratorSettings =
            std::make_shared< numerical_integrators::IntegratorSettings< double > > ( numerical_integrators::rungeKutta4, 0.0, stepSize );

    // Define list of dependent variables to save.
    std::vector< std::shared_ptr< propagators::SingleDependentVariableSaveSettings > > dependentVariablesList;

    // Create object with list of dependent variables
    std::shared_ptr< propagators::DependentVariableSaveSettings > dependentVariablesToSave =
            std::make_shared< propagators::DependentVariableSaveSettings >( dependentVariablesList, false );

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////       NUMERICALLY PROPAGATE THE SIMPLIFIED PROBLEM            /////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    std::map< double, Eigen::VectorXd > hodographicShapingPropagationUnperturbedCase;
    std::map< double, Eigen::Vector6d > hodographicShapingAnalyticalResults;
    std::map< double, Eigen::VectorXd > hodographicShapingDependentVariablesHistory;

    // Create propagator settings for hodographic shaping.
    std::pair< std::shared_ptr< propagators::PropagatorSettings< double > >, std::shared_ptr< propagators::PropagatorSettings< double > > >
            hodographicShapingPropagatorSettings = hodographicShaping.createLowThrustPropagatorSettings(
                bodies, bodiesToPropagate.at( 0 ), centralBodies.at( 0 ), specificImpulseFunction,
                basic_astrodynamics::AccelerationMap( ), integratorSettings, dependentVariablesToSave );

    // Compute shaped trajectory and propagated trajectory.
    hodographicShaping.computeSemiAnalyticalAndFullPropagation(
                bodies, integratorSettings, hodographicShapingPropagatorSettings, hodographicShapingPropagationUnperturbedCase,
                hodographicShapingAnalyticalResults, hodographicShapingDependentVariablesHistory );

    std::map< double, Eigen::VectorXd > sphericalShapingPropagationUnperturbedCase;
    std::map< double, Eigen::Vector6d > sphericalShapingAnalyticalResults;
    std::map< double, Eigen::VectorXd > sphericalShapingDependentVariablesHistory;

    // Create propagator settings for spherical shaping.
    std::pair< std::shared_ptr< propagators::PropagatorSettings< double > >, std::shared_ptr< propagators::PropagatorSettings< double > > >
            sphericalShapingPropagatorSettings = sphericalShaping.createLowThrustPropagatorSettings(
                bodies, bodiesToPropagate.at( 0 ), centralBodies.at( 0 ), specificImpulseFunction,
                basic_astrodynamics::AccelerationMap( ), integratorSettings, dependentVariablesToSave );

    // Compute shaped trajectory and propagated trajectory.
    sphericalShaping.computeSemiAnalyticalAndFullPropagation(
                bodies, integratorSettings, sphericalShapingPropagatorSettings, sphericalShapingPropagationUnperturbedCase,
                sphericalShapingAnalyticalResults, sphericalShapingDependentVariablesHistory );

    input_output::writeDataMapToTextFile( hodographicShapingAnalyticalResults,
                                          "hodographicShapingAnalyticalResults.dat",
                                          tudat_applications::getOutputPath( ) + outputSubFolder,
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( sphericalShapingAnalyticalResults,
                                          "sphericalShapingAnalyticalResults.dat",
                                          tudat_applications::getOutputPath( ) + outputSubFolder,
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( hodographicShapingPropagationUnperturbedCase,
                                          "hodographicShapingPropagationUnperturbedCase.dat",
                                          tudat_applications::getOutputPath( ) + outputSubFolder,
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( sphericalShapingPropagationUnperturbedCase,
                                          "sphericalShapingPropagationUnperturbedCase.dat",
                                          tudat_applications::getOutputPath( ) + outputSubFolder,
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ////////////////////         RETRIEVE TRAJECTORY, MASS, THRUST AND THRUST ACCELERATION PROFILES           //////////////////
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Hodographic shaping
    std::vector< double > epochsVectorHodographicShaping;
    for ( std::map< double, Eigen::Vector6d >::iterator itr = hodographicShapingAnalyticalResults.begin( ) ;
          itr != hodographicShapingAnalyticalResults.end( ) ; itr++ )
    {
        epochsVectorHodographicShaping.push_back( itr->first );
    }

    std::map< double, Eigen::VectorXd > hodographicShapingMassProfile;
    std::map< double, Eigen::VectorXd > hodographicShapingThrustProfile;
    std::map< double, Eigen::VectorXd > hodographicShapingThrustAccelerationProfile;

    hodographicShaping.getMassProfile(
                epochsVectorHodographicShaping, hodographicShapingMassProfile, specificImpulseFunction, integratorSettings );
    hodographicShaping.getThrustForceProfile(
                epochsVectorHodographicShaping, hodographicShapingThrustProfile, specificImpulseFunction, integratorSettings );
    hodographicShaping.getThrustAccelerationProfile(
                epochsVectorHodographicShaping, hodographicShapingThrustAccelerationProfile, specificImpulseFunction, integratorSettings );

    // Spherical shaping
    std::vector< double > epochsVectorSphericalShaping;
    for ( std::map< double, Eigen::Vector6d >::iterator itr = sphericalShapingAnalyticalResults.begin( ) ;
          itr != sphericalShapingAnalyticalResults.end( ) ; itr++ )
    {
        epochsVectorSphericalShaping.push_back( itr->first );
    }

    std::map< double, Eigen::VectorXd > sphericalShapingMassProfile;
    std::map< double, Eigen::VectorXd > sphericalShapingThrustProfile;
    std::map< double, Eigen::VectorXd > sphericalShapingThrustAccelerationProfile;

    sphericalShaping.getMassProfile(
                epochsVectorSphericalShaping, sphericalShapingMassProfile, specificImpulseFunction, integratorSettings );
    sphericalShaping.getThrustForceProfile(
                epochsVectorSphericalShaping, sphericalShapingThrustProfile, specificImpulseFunction, integratorSettings );
    sphericalShaping.getThrustAccelerationProfile(
                epochsVectorSphericalShaping, sphericalShapingThrustAccelerationProfile, specificImpulseFunction, integratorSettings );

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //////////////////////////////         DEFINE PERTURBED DYNAMICAL ENVIRONMENT          /////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Define propagation settings.
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationSettingsPerturbedProblem;
    accelerationSettingsPerturbedProblem[ "Earth" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::central_gravity ) );
    accelerationSettingsPerturbedProblem[ "Mars" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::central_gravity ) );
    accelerationSettingsPerturbedProblem[ "Jupiter" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::central_gravity ) );
    accelerationSettingsPerturbedProblem[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::cannon_ball_radiation_pressure ) );

    SelectedAccelerationMap accelerationMap;

    accelerationMap[ "Borzi" ] = accelerationSettingsPerturbedProblem;
    bodiesToPropagate.push_back( "Borzi" );
    centralBodies.push_back( "Sun" );

    basic_astrodynamics::AccelerationMap perturbingAccelerationsMapPertubedProblem = createAccelerationModelsMap(
                bodies, accelerationMap, bodiesToPropagate, centralBodies );

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //////////////////////////////         PROPAGATE THE FULLY PERTURBED PROBLEM           /////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    std::map< double, Eigen::VectorXd > hodographicShapingPropagationPerturbedCase;
    std::map< double, Eigen::Vector6d > hodographicShapingAnalyticalResultsPerturbedCase;
    std::map< double, Eigen::VectorXd > hodographicShapingDependentVariablesHistoryPerturbedCase;

    std::map< double, Eigen::VectorXd > sphericalShapingPropagationPerturbedCase;
    std::map< double, Eigen::Vector6d > sphericalShapingAnalyticalResultsPerturbedCase;
    std::map< double, Eigen::VectorXd > sphericalShapingDependentVariablesHistoryPerturbedCase;

    // Create propagator settings for hodographic shaping.
    std::pair< std::shared_ptr< propagators::PropagatorSettings< double > >, std::shared_ptr< propagators::PropagatorSettings< double > > >
            hodographicShapingPropagatorSettingsPerturbedCase = hodographicShaping.createLowThrustPropagatorSettings(
                bodies, bodiesToPropagate.at( 0 ), centralBodies.at( 0 ), specificImpulseFunction,
                perturbingAccelerationsMapPertubedProblem, integratorSettings, dependentVariablesToSave );

    // Compute shaped trajectory and propagated trajectory.
    hodographicShaping.computeSemiAnalyticalAndFullPropagation(
                bodies, integratorSettings, hodographicShapingPropagatorSettingsPerturbedCase, hodographicShapingPropagationPerturbedCase,
                hodographicShapingAnalyticalResultsPerturbedCase, hodographicShapingDependentVariablesHistoryPerturbedCase );

    // Create propagator settings for spherical shaping.
    std::pair< std::shared_ptr< propagators::PropagatorSettings< double > >, std::shared_ptr< propagators::PropagatorSettings< double > > >
            sphericalShapingPropagatorSettingsPerturbedCase = sphericalShaping.createLowThrustPropagatorSettings(
                bodies, bodiesToPropagate.at( 0 ), centralBodies.at( 0 ), specificImpulseFunction,
                perturbingAccelerationsMapPertubedProblem, integratorSettings, dependentVariablesToSave );

    // Compute shaped trajectory and propagated trajectory.
    sphericalShaping.computeSemiAnalyticalAndFullPropagation(
                bodies, integratorSettings, sphericalShapingPropagatorSettingsPerturbedCase, sphericalShapingPropagationPerturbedCase,
                sphericalShapingAnalyticalResultsPerturbedCase, sphericalShapingDependentVariablesHistoryPerturbedCase );

    input_output::writeDataMapToTextFile( hodographicShapingPropagationPerturbedCase,
                                          "hodographicShapingPropagationPerturbedCase.dat",
                                          tudat_applications::getOutputPath( ) + outputSubFolder,
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( sphericalShapingPropagationPerturbedCase,
                                          "sphericalShapingPropagationPerturbedCase.dat",
                                          tudat_applications::getOutputPath( ) + outputSubFolder,
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( hodographicShapingMassProfile,
                                          "hodographicShapingMassProfile.dat",
                                          tudat_applications::getOutputPath( ) + outputSubFolder,
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( hodographicShapingThrustProfile,
                                          "hodographicShapingThrustProfile.dat",
                                          tudat_applications::getOutputPath( ) + outputSubFolder,
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( hodographicShapingThrustAccelerationProfile,
                                          "hodographicShapingThrustAccelerationProfile.dat",
                                          tudat_applications::getOutputPath( ) + outputSubFolder,
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( sphericalShapingMassProfile,
                                          "sphericalShapingMassProfile.dat",
                                          tudat_applications::getOutputPath( ) + outputSubFolder,
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( sphericalShapingThrustProfile,
                                          "sphericalShapingThrustProfile.dat",
                                          tudat_applications::getOutputPath( ) + outputSubFolder,
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( sphericalShapingThrustAccelerationProfile,
                                          "sphericalShapingThrustAccelerationProfile.dat",
                                          tudat_applications::getOutputPath( ) + outputSubFolder,
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    std::cout << "deltaV hodographic shaping: " << hodographicShaping.computeDeltaV( ) << "\n\n";
    std::cout << "deltaV spherical shaping: " << sphericalShaping.computeDeltaV( ) << "\n\n";

    // Final statement.
    // The exit code EXIT_SUCCESS indicates that the program was successfully executed.
    return EXIT_SUCCESS;
}