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

#ifndef SMOOTH__DIFF_HPP_
#define SMOOTH__DIFF_HPP_

/**
 * @file
 * @brief Differentiation on Lie groups.
 */

#include <Eigen/Core>
#include <type_traits>

#include "concepts.hpp"
#include "internal/utils.hpp"
#include "tn.hpp"

namespace smooth {

/**
 * @brief Grouping of function arguments.
 *
 * A tuple of references is created from the input arguments,
 * which is the expected format in e.g. dr() and minimize().
 */
template<typename... _Args>
auto wrt(_Args &&... args)
{
  return std::forward_as_tuple(std::forward<_Args>(args)...);
}

// differentiation module
namespace diff {
namespace detail {

/**
 * @brief Numerical differentiation in tangent space.
 *
 * @param f function to differentiate
 * @param x reference tuple of function arguments
 * @return \p std::pair containing value and right derivative: \f$(f(x), \mathrm{d}^r f_x)\f$
 *
 * @note All arguments in x as well as the return type \f$f(x)\f$ must satisfy
 * the Manifold concept.
 */
template<typename _F, typename _Wrt>
auto dr_numerical(_F && f, _Wrt && x)
{
  using Result = typename decltype(std::apply(f, x))::PlainObject;
  using Scalar = typename Result::Scalar;

  // arguments are modified below, so we create a copy of those that come in as const
  auto x_nc = utils::tuple_copy_if_const(std::forward<_Wrt>(x));

  // static sizes
  static constexpr Eigen::Index Nx = utils::tuple_dof<std::decay_t<_Wrt>>::value;
  static constexpr Eigen::Index Ny = Result::SizeAtCompileTime;

  const Scalar eps = std::sqrt(Eigen::NumTraits<Scalar>::epsilon());

  const Result val = std::apply(f, x_nc);

  // dynamic sizes
  Eigen::Index nx = std::apply([](auto &&... args) { return (args.size() + ...); }, x_nc);
  Eigen::Index ny = val.size();

  // output variable
  Eigen::Matrix<Scalar, Ny, Nx> jac(ny, nx);

  Eigen::Index index_pos = 0;

  utils::static_for<std::tuple_size_v<std::decay_t<_Wrt>>>([&](auto i) {
    static constexpr Eigen::Index Nx_j =
      std::decay_t<std::tuple_element_t<i, std::decay_t<_Wrt>>>::SizeAtCompileTime;
    auto & w       = std::get<i>(x_nc);
    const int nx_j = w.size();

    using W = std::decay_t<decltype(w)>;

    for (auto j = 0; j != nx_j; ++j) {
      Scalar eps_j = eps;
      if constexpr (std::is_base_of_v<Eigen::MatrixBase<W>, W>) {
        // scale step size if we are in Rn
        eps_j *= abs(w[j]);
        if (eps_j == 0.) { eps_j = eps; }
      } else if constexpr (std::is_base_of_v<smooth::TnBase<W>, W>) {
        // or Tn
        eps_j *= abs(w.rn()[j]);
        if (eps_j == 0.) { eps_j = eps; }
      }
      // const cast needed in case argument is const (value is restored two lines below)
      w += (eps_j * Eigen::Matrix<Scalar, Nx_j, 1>::Unit(nx_j, j));
      jac.col(index_pos + j) = (std::apply(f, x_nc) - val) / eps_j;
      w += (-eps_j * Eigen::Matrix<Scalar, Nx_j, 1>::Unit(nx_j, j));
    }
    index_pos += nx_j;
  });

  return std::make_pair(val, jac);
}

}  // namespace detail

/**
 * @enum smooth::diff::Type
 * @brief Differentiation methods
 */
enum class Type {
  NUMERICAL,  ///< Numerical (forward) derivatives
  AUTODIFF,   ///< Uses the autodiff (https://autodiff.github.io) library; requires  \p
              ///< compat/autodiff.hpp
  CERES,      ///< Uses the Ceres (http://ceres-solver.org) built-in autodiff; requires \p
              ///< compat/ceres.hpp
  ANALYTIC,   ///< Hand-coded derivative, requires that function returns \p std::pair \f$(f(x),
              ///< \mathrm{d}^r f_x) \f$
  DEFAULT     ///< Automatically select type based on availability
};

static constexpr Type DefaultType =
#ifdef SMOOTH_DIFF_AUTODIFF
  Type::AUTODIFF;
#elif defined SMOOTH_DIFF_CERES
  Type::CERES;
#else
  Type::NUMERICAL;
#endif

/**
 * @brief Differentiation in tangent space
 *
 * @tparam dm differentiation method to use
 *
 * @param f function to differentiate
 * @param x reference tuple of function arguments
 * @return \p std::pair containing value and right derivative: \f$(f(x), \mathrm{d}^r f_x)\f$
 *
 * @note All arguments in x as well as the return type \f$f(x)\f$ must satisfy
 * the Manifold concept.
 */
template<Type dm, typename _F, typename _Wrt>
auto dr(_F && f, _Wrt && x)
{
  if constexpr (dm == Type::NUMERICAL) {
    return detail::dr_numerical(std::forward<_F>(f), std::forward<_Wrt>(x));
  } else if constexpr (dm == Type::AUTODIFF) {
#ifdef SMOOTH_DIFF_AUTODIFF
    return dr_autodiff(std::forward<_F>(f), std::forward<_Wrt>(x));
#else
    static_assert(dm != Type::AUTODIFF, "compat/autodiff.hpp header not included");
#endif
  } else if constexpr (dm == Type::CERES) {
#ifdef SMOOTH_DIFF_CERES
    return dr_ceres(std::forward<_F>(f), std::forward<_Wrt>(x));
#else
    static_assert(dm != Type::CERES, "compat/ceres.hpp header not included");
#endif
  } else if constexpr (dm == Type::ANALYTIC) {
    return std::apply(f, std::forward<_Wrt>(x));
  } else if constexpr (dm == Type::DEFAULT) {
    return dr<DefaultType>(std::forward<_F>(f), std::forward<_Wrt>(x));
  }
}

/**
 * @brief Differentiation in tangent space using default method
 *
 * @param f function to differentiate
 * @param x reference tuple of function arguments
 * @return \p std::pair containing value and right derivative: \f$(f(x), \mathrm{d}^r f_x)\f$
 *
 * @note All arguments in x as well as the return type \f$f(x)\f$ must satisfy
 * the Manifold concept.
 */
template<typename _F, typename _Wrt>
auto dr(_F && f, _Wrt && x)
{
  return dr<Type::DEFAULT>(std::forward<_F>(f), std::forward<_Wrt>(x));
}

}  // namespace diff
}  // namespace smooth

#endif  // SMOOTH__DIFF_HPP_


// Copyright 2014, Max Planck Society.
// Distributed under the BSD 3-Clause license.
// (See accompanying file LICENSE.txt or copy at
// http://opensource.org/licenses/BSD-3-Clause)

#ifndef GRASSMANN_AVERAGES_PCA_UTILITIES_HPP__
#define GRASSMANN_AVERAGES_PCA_UTILITIES_HPP__

/*!@file
 * Grassmann averages for robust PCA, companion functions.
 *
 * This file contains some utility function for multithreading, norm computation, convergence check
 *
 */

#include <boost/numeric/conversion/bounds.hpp>

#include <boost/asio/io_service.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/recursive_mutex.hpp>

#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>

#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/vector.hpp>

#include <numeric>

// lock free queue, several producers, one consumer
//#include <boost/lockfree/queue.hpp>

namespace grassmann_averages_pca
{

  //! A callback class for monitoring the advance of the algorithm
  //!
  //! All calls are made in the main thread: there is no thread-safety issue.
  template <class data_t>
  struct grassmann_trivial_callback
  {

    //! Called to provide important messages/logs
    void log_error_message(const char* message) const
    {
      std::cout << message << std::endl;
    }

    //! This is called after centering the data in order to keep track
    //! of the mean of the dataset
    void signal_mean(const data_t& mean) const
    {}

    //! Called after the computation of the PCA
    void signal_pca(const data_t& mean,
                    size_t current_eigenvector_dimension) const
    {}

    //! Called each time a new eigenvector is computed
    void signal_eigenvector(const data_t& current_eigenvector,
                            size_t current_eigenvector_dimension) const
    {}

    //! Called at every step of the algorithm, at the end of the step
    void signal_intermediate_result(
      const data_t& current_eigenvector_state,
      size_t current_eigenvector_dimension,
      size_t current_iteration_step) const
    {}

  };

  namespace details
  {

    //! Wrapper object for infinity/max @f$\ell_\infty@f$ norm.
    struct norm_infinity
    {
      template <class vector_t>
      double operator()(vector_t const& v) const
      {
        return boost::numeric::ublas::norm_inf(v);
      }
    };

    //! Returns the square of the @f$\ell_2@f$ norm.
    struct norm_ell2_square
    {
      template <class vector_t>
      double operator()(vector_t const& v) const
      {
        double acc(0);
        for(typename vector_t::const_iterator it(v.begin()), ite(v.end());
            it < ite;
            ++it)
        {
          typename vector_t::const_reference v(*it);
          acc += v * v;
        }
        return acc;
      }
    };

    //! Returns the @f$\ell_2@f$ norm of a vector.
    struct norm2
    {
      typedef double result_type;
      norm_ell2_square op;
      template <class vector_t>
      result_type operator()(vector_t const& v) const
      {
        return std::sqrt(op(v));
      }
    };

    /*!@brief Gram Schmidt orthonormalisation of a collection of vectors.
     * @tparam it_t iterator on the collection of vectors. Should model a forward input iterator.
     * @tparam norm_t type of the norm operator.
     *
     * @param it beginning of the collection of vectors
     * @param ite end of the collection of vectors
     * @param start first element of the collection to be orthonormalized. start should be inside the range given by it and ite.
     * @param norm_op the norm used to normalise the vectors
     */
    template <class it_t, class norm_t>
    bool gram_schmidt_orthonormalisation(it_t it, it_t ite, it_t start, norm_t const &norm_op)
    {

      if(start == it)
      {
        *start *= typename it_t::value_type::value_type(1./norm_op(*start));
        ++start;
      }

      it_t previous(start);

      for(; start != ite; ++previous, ++start)
      {
        typename it_t::reference current = *start;
        for(it_t it_orthonormalised_element(it); it_orthonormalised_element < previous; ++it_orthonormalised_element)
        {
          current -= boost::numeric::ublas::inner_prod(current, *it_orthonormalised_element) * (*it_orthonormalised_element);
        }
        current *= typename it_t::value_type::value_type(1./norm_op(current));

      }
      return true;
    }

    /*!@brief Computes the mean of a data set after having removed the lower and upper k first elements.
     *
     * This function computes @f[\sum_{k \leq i < N-k} p_{o(i)}@f] where
     * - @f$p@f$ is the data set of size @f$N@f$, and @f$p_i@f$ is its ith element
     * - @f$o@f$ is a function ordering the data set: @f$\forall i, p_{o(i)} \leq p_{o(i+1)}, 0 \leq i < N @f$

     * @tparam T type of the data set. All internal accumulations will be performed with this type. T should not be const as
     *         the data set will be modified in place.
     *
     * @param p_data the data set composed of nb_total_elements of type T.
     * @param nb_total_elements number of elements of the data set
     * @param k_first_last number of elements to remove from the lower and upper distributions.
     *
     * @pre @f$ \text{k_first_last} \leq \frac{\text{nb_total_elements}}{2}@f$
     */
    template <class T>
    T compute_mean_within_bounds(T *p_data, size_t nb_total_elements, size_t k_first_last)
    {
      if(k_first_last < nb_total_elements / 2)
      {
        std::nth_element(p_data, p_data + k_first_last, p_data + nb_total_elements);
        std::nth_element(p_data + k_first_last+1, p_data + nb_total_elements - k_first_last-1, p_data + nb_total_elements);
        T acc = std::accumulate(p_data + k_first_last, p_data + nb_total_elements - k_first_last, T(0));

        return acc / (nb_total_elements - 2*k_first_last);
      }
      else
      {
        assert(k_first_last == nb_total_elements / 2);
        std::nth_element(p_data, p_data + k_first_last, p_data + nb_total_elements);

        if(nb_total_elements & 1)
        {
          return *(p_data + k_first_last);
        }
        else
        {
          return (*(p_data + k_first_last) + *std::max_element(p_data, p_data + k_first_last)) / 2;
        }
      }
    }

    /*!@brief Checks the convergence of a sequence.
     *
     * @tparam data_t: type of the data.
     * @tparam norm_t: the norm used in order to compare the closeness of two successive results.
     *
     * The type of the data should meet the following requirements:
     * - data_t should be copy constructible and assignable.
     * - operator- is defined between two instances of data_t and return a type compatible with the input of the norm operator (usually a data_t).
     *
     * The convergence is assumed as soon as the norm between two subsequent states is less than a certain @f$\epsilon@f$, that is
     * the functor returns true if:
     * @f[\left\|v_t - v_{t-1}\right\| < \epsilon@f]
     *
     * @note Once the convergence is reached, the internal states are not updated anymore (the calling algorithm is supposed to stop).
     */
    template <class data_t, class norm_t = norm_infinity>
    struct convergence_check
    {
      //! The amount of change below which the sequence is considered as having reached a steady point.
      const double epsilon;

      //! Holds an instance of the norm used for checking the convergence.
      norm_t norm_comparison;

      //! The previous value
      data_t previous_state;

      //! Initialise the instance with the initial state of the vector.
      convergence_check(data_t const& current_state, double epsilon_ = 1E-5) :
        epsilon(epsilon_),
        previous_state(current_state)
      {}

      //! Returns true on convergence.
      bool operator()(data_t const& current_state)
      {
        bool ret = norm_comparison(current_state - previous_state) < epsilon;
        if(!ret)
        {
          previous_state = current_state;
        }
        return ret;
      }

    };

    // some issues with the random number generator
    const double fVeryBigButStillComputable = 1E10;
    const double fVerySmallButStillComputable = -1E10;

    /*!@brief Used to initialize the initial guess to some random data.
     *
     * @tparam data_t the type of the data returned. It should model a vector:
     *  - default, copy constructible and constructible with a size
     *  - has a member @c size returning a value of type @c data_t::Size_type
     *  - has a field @c data_t::Value_type
     */
    template <
      class data_t,
      class random_distribution_t =
        typename boost::mpl::if_<
          boost::is_floating_point<typename data_t::value_type>,
          boost::random::uniform_real_distribution<typename data_t::value_type>,
          boost::random::uniform_int_distribution<typename data_t::value_type>
        >::type,
      class random_number_generator_t = boost::random::mt19937
    >
    struct random_data_generator
    {
      typedef typename data_t::value_type value_type;
      mutable random_number_generator_t rng;
      value_type min_bound;
      value_type max_bound;

      //! Default construction
      random_data_generator(
        value_type min_value_ = boost::numeric::bounds<value_type>::lowest(),
        value_type max_value_ = boost::numeric::bounds<value_type>::highest()) :
        rng(),
        min_bound(min_value_),
        max_bound(max_value_)
      {}

      //! Constructs from the specified seeded random number generator.
      random_data_generator(
        random_number_generator_t const &r,
        value_type min_value_ = boost::numeric::bounds<value_type>::lowest(),
        value_type max_value_ = boost::numeric::bounds<value_type>::highest()) :
        rng(r),
        min_bound(min_value_),
        max_bound(max_value_)
      {}

      data_t operator()(const data_t& v) const
      {
        data_t out(v.size());
        random_distribution_t dist(min_bound, max_bound);
        for(typename data_t::size_type i(0), j(v.size()); i < j; i++)
        {
          out[i] = dist(rng);
        }
        return out;
      }
    };

    //! @namespace
    namespace threading
    {

      //! Ensures the proper stop of the processing pool and the finalisation of all threads.
      struct safe_stop
      {
      private:
        boost::asio::io_service& io_service;
        boost::thread_group& thread_group;

      public:
        safe_stop(boost::asio::io_service& ios, boost::thread_group& tg) : io_service(ios), thread_group(tg)
        {}

        ~safe_stop()
        {
          io_service.stop();
          thread_group.join_all();
        }
      };

      //! @brief Helper structure for managing additions on standard uBlas vectors.
      //!
      //! This class is intended to be used with asynchronous_results_merger. It just adds an update to the current state.
      //! @tparam data_t type of the vectors. It is supposed that data_t implements in-place addition (@c data_t::operator+=).
      template <class data_t>
      struct merger_addition
      {
        bool operator()(data_t &current_state, data_t const& update_value) const
        {
          current_state += update_value;
          return true;
        }
      };

      //! Helper structure for managing initialisations of standard uBlas vectors.
      //!
      //! This class is intended to be used with asynchronous_results_merger.
      //! @tparam data_t type of the vectors
      //! @note This implementation supposes that the type is compatible with boost::numeric::ublas::vector
      template <class data_t>
      struct initialisation_vector_specific_dimension
      {
      private:
        const size_t data_dimension;                    //!< Dimension of the vectors
        typedef typename data_t::value_type scalar_t;   //<! Scalar type

      public:
        //! Initialise the instance with the dimension of the data.
        //! The dimension is fixed.
        initialisation_vector_specific_dimension(size_t dimension) : data_dimension(dimension)
        {}

        //! Initialise the current state a null (0) vector of the dimension guiven at construction.
        bool operator()(data_t & current_state) const
        {
          current_state = boost::numeric::ublas::scalar_vector<scalar_t>(data_dimension, 0);
          return true;
        }
      };

      /*!@brief Merges the result of all workers and signals the results to the main thread.
       *
       * The purpose of this class is to gather the computation results coming from several threads into one unique result seen by the main calling thread.
       * Each thread computes a partial update of the final result. These partial update are signalled to this instance via @c asynchronous_results_merger::update (thread safe).
       * These updates are gathered/merged to the final result through the "merger" instance (of type @c merger_type) in a thread safe manner.
       * The number of updates is also signalled to the main thread via a call to @c asynchronous_results_merger::notify. The main thread supposes the computation over/in sync if it received
       * an amount of notification through the @c asynchronous_results_merger::wait function.
       *
       * @tparam result_type_ the type of the final result.
       * @tparam merger_type the type of the merger. The merger should be a callable with two arguments: result_type_ and update_element_
       * @tparam init_result_type the type of the initialiser. The initialiser should be a callable with one argument of type result_type_.
       * @tparam update_element_ the type of the update. These updates are provided by the several workers to this merger.
       *
       * @note This implementation supposes that the pointers to the update elements remain after the call to @c asynchronous_results_merger::notify. This is because
       * the implementation tries to avoid any "long" or time consuming lock. If the merge cannot be performed in the asynchronous_results_merger::update call itself,
       * then the update element is queued and the merge is performed in the main calling thread (the wait function).
       */
      template <class result_type_, class merger_type, class init_result_type, class update_element_ = result_type_>
      struct asynchronous_results_merger : boost::noncopyable
      {
      public:

        typedef result_type_ result_type;         //!< The type returned by asynchronous_results_merger::get_merged_result
        typedef update_element_ update_element;   //!< The type used for the updates.

      protected:
        typedef boost::recursive_mutex mutex_t;     //!< Type of the mutex. This one is re-entrant/recursive in order to allow the same thread locking it several times.
        typedef boost::lock_guard<mutex_t> lock_t;  //!< Exclusive lock

        //! Mutex for critical sections.
        //!@note This mutex is re-entrant.
        mutable mutex_t internal_mutex;

        //! Holds the current value of the merge.
        //! This variable is constantly updated as chunk processed finish.
        result_type current_value;

        //! Holds the instance of the class responsible for merging new values (updates) to the
        //! current instance (current_value).
        merger_type merger_instance;

        //! Holds the instance of the class responsible for initialising the current value to
        //! an initial state (before any merge arrives).
        init_result_type initialisation_instance;

        //! Number of updates after the initialisation
        volatile int nb_updates;

        //! Thread synchronisation (event sent after an update, for counting).
        boost::condition_variable_any condition_;

        std::list<update_element const*> lf_queue;
        //boost::lockfree::queue<update_element const*> lf_queue;

      public:

        /*!Constructor
         *
         * @param initialisation_instance_ an instance of the class initialising the current state.
         */
        asynchronous_results_merger(init_result_type const &initialisation_instance_) :
          initialisation_instance(initialisation_instance_),
          nb_updates(0),
          lf_queue()
        {}

        //! Initializes the internal states
        void init()
        {
          init_results();
          init_notifications();
        }

        //! Initialises the internal state of the accumulator
        void init_results()
        {
          initialisation_instance(current_value);
        }

        //! Initialises the number of notifications.
        //! Also called by init.
        void init_notifications()
        {
          nb_updates = 0;
        }

        /*! Receives the update element from each worker.
         *
         * The update element is passed to the merger in order to create an updated value of the internal result.
         * @note The call is thread safe.
         */
        void update(update_element const* updated_value)
        {
          boost::unique_lock<mutex_t> lock(internal_mutex);//, boost::try_to_lock);

          if(lock.owns_lock())
          {
            merger_instance(current_value, *updated_value);
            while(!lf_queue.empty())
            {
              updated_value = lf_queue.back();
              lf_queue.pop_back();
              merger_instance(current_value, *updated_value);
            }
          }
          else
          {
            //while(!lf_queue.push(updated_value))
            //  ;
          }
        }

        /*! Function receiving the update notification.
         *
         *  @note The call is thread safe.
         */
        void notify()
        {
          lock_t guard(internal_mutex);
          ++nb_updates;

          //
          // The condition functions are not async-signal safe, and should not be called from a signal handler.
          // In particular, calling pthread_cond_signal or pthread_cond_broadcast from a signal handler may
          // deadlock the calling thread.
          //
          // Raffi: the notification outside the lock above causes a deadlock, apparently it should be protected
          // from simultaneous access.
          condition_.notify_one();
        }

        //! Returns once the number of updates reaches the number in argument.
        //!
        //!@warning if an inappropriate number is given, the method might never return.
        bool wait_notifications(size_t nb_notifications)
        {
          boost::unique_lock<mutex_t> lock(internal_mutex);
          while (nb_updates < nb_notifications)
          {
            // when entering wait, the lock is unlocked and made available to other threads.
            // when awakened, the lock is locked before wait returns.
            condition_.wait(lock);

            //assert(nb_updates);           // cannot be awakened if there is no update
            assert(lock.owns_lock());

            // consumes what was under a collision in the update
            update_element const* updated_value(0);
            if(!lf_queue.empty())
            {
              updated_value = lf_queue.back();
              lf_queue.pop_back();
              merger_instance(current_value, *updated_value);
            }
          }

          assert(lock.owns_lock());

          // consumes what was under a collision in the update
          // we might end up here if there was at least one collision, and everything was finished before the line "while (nb_updates < nb_notifications)"
          {
            update_element const* updated_value(0);
            while(!lf_queue.empty())
            {
              updated_value = lf_queue.back();
              lf_queue.pop_back();
              merger_instance(current_value, *updated_value);
            }
          }

          return true;
        }

        //! Returns the current merged results.
        //! @warning the call is not thread safe (intended to be called once the wait_notifications returned and no
        //! other thread is working).
        result_type const& get_merged_result() const
        {
          return current_value;
        }

        //! Returns the current merged results.
        //! @warning the call is not thread safe (intended to be called once the wait_notifications returned and no
        //! other thread is working).
        result_type & get_merged_result()
        {
          return current_value;
        }
      };

    } // namespace threading
  } // namespace details
} // namespace grassmann_averages_pca

#endif /* GRASSMANN_AVERAGES_PCA_UTILITIES_HPP__*/


// Ben Martin
// December 3, 2005

// This currently works only on undirected graphs...
// Graph must model Adjacency Graph, Incidence Graph, VertexListGraph

#ifndef BOOST_GRAPH_SPECTRUM_HPP
#define BOOST_GRAPH_SPECTRUM_HPP

#define USE_IETL 0

#if USE_IETL
#else
#define USE_LAPACK 1
#endif

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/properties.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <utility> // for pair

//#include <iostream.h>
#ifdef USE_LAPACK
#ifdef _APPLE_
#include <vecLib/clapack.h>
#endif
//#include "f2c.h"
//#include "cblas.h"
//#include "clapack.h"
#elif USE_IETL
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <ietl/vectorspace.h>
#include <ietl/lanczos.h>
#include <ietl/iteration.h>
#include <boost/random/linear_congruential.hpp>
#include <ietl/interface/ublas.h>
#endif

typedef long int integer;
typedef double doublereal;

#ifdef USE_LAPACK
extern "C" int dsyevr_(char *jobz, char *range, char *uplo, integer *n,
		       doublereal *a, integer *lda, doublereal *vl, doublereal *vu, integer *
		       il, integer *iu, doublereal *abstol, integer *m, doublereal *w,
		       doublereal *z__, integer *ldz, integer *isuppz, doublereal *work,
		       integer *lwork, integer *iwork, integer *liwork, integer *info);
#endif

namespace boost {

  template <typename Graph, typename Matrix >
  void spectrum(Graph& g,
		int first_eigenvector_index,
		int num_eigenvectors,
		Matrix &eigenvectors,
		double rel_tol = 100,
		double abs_tol = 1000)
  {
    std::vector<double> evals(num_eigenvectors);
    spectrum(g, first_eigenvector_index, num_eigenvectors, eigenvectors, evals, rel_tol, abs_tol);
  }

  // Parameters:
  //   first_eigenvector_index:
  //     Since the smallest eigenvector is not useful, often this will
  //       be set to 1, for the "Fiedler vector," though if all
  //       eigenvectors are desired, 0 may be a more logical value.
  //     Negative values are interpreted as allowing a default choice of 1.
  //   num_eigenvectors:
  //     The number of eigencectors to return.

  template <typename Graph, typename Matrix , typename EVector>
  void spectrum(Graph& g,
		int first_eigenvector_index,
		int num_eigenvectors,
		//		std::vector<Vector> &eigenvectors,
		Matrix &eigenvectors,
		EVector &eigenvalues,
		double rel_tol = 100,
		double abs_tol = 1000)
  {

    //    Matrix eigenvectors = *(in_eigenvectors);

    if (first_eigenvector_index < 0)
      first_eigenvector_index = 1;

    typedef typename property_map<Graph, vertex_index_t>::const_type IndexMap;
    typedef typename Graph::vertex_iterator VertexIterator;
    typedef typename Graph::edge_iterator EdgeIterator;
    typedef typename boost::graph_traits<Graph>::adjacency_iterator AdjacencyIterator;

    IndexMap index_map = get(vertex_index, g);

    VertexIterator v, vs, ve;
    using std::pair;
    std::pair<VertexIterator, VertexIterator> p;
    p = vertices(g);
    vs = p.first;
    ve = p.second;

    EdgeIterator e, es, ee;
    using std::pair;
    std::pair<EdgeIterator, EdgeIterator> ep;
    ep = edges(g);
    es = ep.first;
    ee = ep.second;
    typename Graph::vertex_descriptor src, tgt;

    integer N = num_vertices(g);

#ifdef USE_LAPACK
    doublereal *A;
    A = new double[N * N];
#elif USE_IETL
    using namespace boost::numeric::ublas;
    typedef compressed_matrix<double> Matrix;
    Matrix A(N, N);
#endif

    int i;

    AdjacencyIterator a, as, ae;
    std::pair<AdjacencyIterator, AdjacencyIterator> ap;

#ifdef USE_LAPACK
    for (i = 0; i < N * N; i++)
      A[i] = 0;
#elif USE_IETL
#endif

    i = 0;
    /*
    for (v = vs; v != ve; ++v) {
#ifdef USE_LAPACK
      A[(index_map[*v] * N) + index_map[*v]] = (doublereal)out_degree(*v, g);
#elif USE_IETL
      A(index_map[*v], index_map[*v]) = (double)out_degree(*v, g);
#endif
    }
    */
    for (e = es; e != ee; ++e) {
      i += 1;
      src = source(*e, g);
      tgt = target(*e, g);
#ifdef USE_LAPACK
      if (src != tgt && A[index_map[src] + index_map[tgt]*N] != -1) {
	A[index_map[src] + index_map[tgt]*N] = (doublereal)(-1);
	A[index_map[src]*N + index_map[tgt]] = (doublereal)(-1);
	A[index_map[src] + index_map[src]*N] += (doublereal)1;
	A[index_map[tgt] + index_map[tgt]*N] += (doublereal)1;
      }
#elif USE_IETL
      if (src != tgt && A(index_map[src], index_map[tgt]) != -1) {
	A(index_map[src], index_map[tgt]) = (double)(-1);
	A(index_map[tgt], index_map[src]) = (double)(-1);
	A(index_map[src], index_map[src]) += (double)1;
	A(index_map[tgt], index_map[tgt]) += (double)1;
      }
#endif
    }

    /*
    cout << "index_map = " << endl;
    for (v = vs; v != ve; ++v) {
      cout << index_map[*v] << " ";
    }
    cout << endl;
    cout << "A = " << endl;
    for (i = 0; i < N; i++)
    {
      for (int j = 0; j < N; j++)
      {
        cout << A[i*N + j] << " ";
      }
        cout << endl;
    }
    */

#ifdef USE_LAPACK
    // Set up the 8 billion parameters for CLAPACK
    char   JOBZ   = 'v';
    char   RANGE  = 'i';
    char   UPLO   = 'l'; // arbitrary at the moment...
    // N is defined
    // A is defined
    integer    LDA    = N;
    doublereal VL     = 0; // not needed
    doublereal VU     = 0; // not needed
    integer    IL     = first_eigenvector_index + 1; //2 // first NON-ZERO eigenvalue
    integer    IU     = first_eigenvector_index + num_eigenvectors; // 4 for the 3d case
    //    char   dlamch_cmach = 's';

    //    doublereal ABSTOL = 0.001;// dlamch_(&dlamch_cmach); // most likely SEVERE OVERKILL
    doublereal ABSTOL = abs_tol*std::numeric_limits<double>::epsilon();
    integer    M = (IU - IL + 1); // N; // should come back 2 or 3 (for 2d, 3d respectively)
    doublereal *W = new double[N]; // the eigenvalues
    doublereal *Z = new double[N*M]; // the eigenvectors
    integer    LDZ = N;
    integer    *ISUPPZ = new integer[2*M];// (integer*)0; // = new int[2*(IU-IL+1)]; ??? Not needed?
    doublereal *WORK = new double[26*N]; // should this be allocated?
    integer    LWORK  = 26*N; // -1
    integer    *IWORK = new integer[10*N];
    integer    LIWORK = 10*N; // -1
    integer    INFO;

    dsyevr_(&JOBZ, &RANGE, &UPLO, &N, A, &LDA, &VL, &VU, &IL, &IU, &ABSTOL,
            &M, W, Z, &LDZ, ISUPPZ, WORK, &LWORK, IWORK, &LIWORK, &INFO);

    /*
    cout << "N = " << N << endl;
    cout << "IL = " << INFO << endl;
    cout << "IU = " << INFO << endl;

    cout << "INFO = " << INFO << endl;
    cout << "M = " << M << endl;

    cout << "W = ";
    for (i = 0; i < M; i++)
      cout << W[i] << " ";
    cout << endl;
    for (i = 0; i < M; i++)
      for (int j = 0; j < N; j++)
      {
        cout << Z[i*N + j] << "";
        cout << endl;
      }
    */
#elif USE_IETL

    using namespace ietl;

    vectorspace<boost::numeric::ublas::vector<double> > VS(N);
    lanczos<Matrix, vectorspace<boost::numeric::ublas::vector<double> > > LanczosObject(A, VS);

    double _rel_tol = 1000.*std::numeric_limits<double>::epsilon();
    double _abs_tol = 10000.*std::numeric_limits<double>::epsilon();

    //    _rel_tol = 100.*std::numeric_limits<double>::epsilon();
    //    _abs_tol = 100.*std::numeric_limits<double>::epsilon();

    // SO far these are the best I have found:
    //    _rel_tol = 1000.*std::numeric_limits<double>::epsilon();
    //    _abs_tol = 10000.*std::numeric_limits<double>::epsilon();

    _rel_tol = rel_tol*std::numeric_limits<double>::epsilon();
    _abs_tol = abs_tol*std::numeric_limits<double>::epsilon();

    lanczos_iteration_nlowest<double> LanczosIterationControl(1000000, first_eigenvector_index + num_eigenvectors, _rel_tol, _abs_tol);
    //    std::cout << std::numeric_limits<double>::epsilon() << std::endl;
    boost::minstd_rand gen(1);
    //boost::rand48 gen(1);
    LanczosObject.calculate_eigenvalues(LanczosIterationControl, gen);
    std::vector<double> evals = LanczosObject.eigenvalues();

    //    for (int i = 0; i < N; i++)
    //      std::cout << evals[i] << " ";
    //    std::cout << std::endl;

    /*
    std::vector<int> multiplicities = LanczosObject.multiplicities();
    int lowest_eval_multiplicity = multiplicities[1];
    int second_eval_multiplicity;
    if (lowest_eval_multiplicity == 1 && multiplicities[2] > 1)
      second_eval_multiplicity = 2;
    else
      second_eval_multiplicity = 1;

    for (int i = 0; i < N; i++)
      std::cout << multiplicities[i] << " ";
    std::cout << std::endl;
    */

    std::vector<boost::numeric::ublas::vector<double> > evecs(3);
    for (int j = 0; j < num_eigenvectors; j++)
      evecs[j] = *(new boost::numeric::ublas::vector<double>(N));
    std::vector<boost::numeric::ublas::vector<double> >::iterator out_it = evecs.begin();

    Info<double> info;
    std::vector<double>::iterator ebegin, eend;
    ebegin = evals.begin();
    ebegin += first_eigenvector_index;
    eend = evals.begin();
    //    eend += 4 + (1 - lowest_eval_multiplicity) + (1 - second_eval_multiplicity);
    eend += first_eigenvector_index + num_eigenvectors;
    LanczosObject.eigenvectors(ebegin, eend, out_it, info, gen, 100000);
    //    LanczosObject.eigenvectors(ebegin, eend, out_it, info, gen);

    /*
    // If eigenvalues are repeated, we need to copy eigenvectors
    if (lowest_eval_multiplicity > 1) {
      for (int i = 0; i < N; i++)
	evecs[1][i] = evecs[0][i];
      if (lowest_eval_multiplicity > 2) {
	for(int i = 0; i < N; i++)
	  evecs[2][i] = evecs[0][i];
      }
    }
    if (second_eval_multiplicity > 1) {
      for (int i = 0; i < N; i++)
	evecs[2][i] = evecs[1][i];
    }
    */

    //    for (int i = 0; i < 3; i++)
    //      for (int j = 0; j < N; j++)
    //	std::cout << evecs[i][j] << " ";
    //    cout << std::endl;

    //    std::cout << A << std::endl;
    //    std::cout << LanczosIterationControl.error_code() << endl;;
    //    std::cout << info.error_info(1) << " " << info.error_info(2) << " " << info.error_info(3) << std::endl;

#endif

    //    std::vector<Vector> retval(num_eigenvectors);
    for (int j = 0; j < num_eigenvectors; j++) {
      // retval[j] = *(new Vector(N));
      i = 0;
      for (v = vs; v != ve; ++v) {
#ifdef USE_LAPACK
	//	  retval[j][i] = Z[(first_eigenvector_index+j-1)*LDZ + i];
	eigenvectors[j][i] = Z[j*LDZ + i];
#elif USE_IETL
	//	  retval[j][i] = evecs[first_eigenvector_index+j-1][i];
	eigenvectors[j][i] = evecs[first_eigenvector_index+j-1][i];

#endif
	i++;
      }
    }
#ifdef USE_LAPACK
    for (int j = 0; j < M; j++) {
      eigenvalues[j] = W[j];
    }
#endif
    //    return retval;

  } // end spectrum()

} // end namespace boost

#endif // BOOST_GRAPH_SPECTRUM_HPP


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

// This program will make use of the ATOM solver to construct a transfer trajectory
// between two points in space. Only a single transfer segment is simulated here, that is, no multitargeting.
// There is just one departure and one arrival object considered in the simulation. The object TLEs are
// taken from a catalog file. The user can specify which object will be departure and which will be arrival
// along with the time of flight.

#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <fstream>
#include <exception>
#include <cstdlib>
#include <iterator>

#include <libsgp4/Globals.h>
#include <libsgp4/SGP4.h>
#include <libsgp4/Tle.h>

#include <Atom/atom.hpp>
#include <Atom/convertCartesianStateToTwoLineElements.hpp>

#include <Astro/orbitalElementConversions.hpp>

#include <SML/sml.hpp>
#include <SML/constants.hpp>
#include <SML/basicFunctions.hpp>
#include <SML/linearAlgebra.hpp>

#include <boost/array.hpp>

#include </usr/local/abhi/pykep/src/lambert_problem.cpp>
#include </usr/local/abhi/pykep/src/lambert_problem.h>
#include </usr/local/abhi/pykep/src/keplerian_toolbox.h>

typedef double Real;
typedef std::vector < Real > Vector6;
typedef std::vector < Real > Vector3;
typedef std::vector < Real > Vector2;
typedef std::vector < std::vector < Real > > Vector2D;
typedef boost::array < Real, 3 > array3;

//! Remove newline characters from string.
void removeNewline( std::string& string )
{
    string.erase( std::remove( string.begin( ), string.end( ), '\r' ), string.end( ) );
    string.erase( std::remove( string.begin( ), string.end( ), '\n' ), string.end( ) );
}

//! Convert SGP4 ECI object to state vector.
Vector6 getStateVector( const Eci state )
{
    Vector6 result( 6 );
    result[ 0 ] = state.Position( ).x;
    result[ 1 ] = state.Position( ).y;
    result[ 2 ] = state.Position( ).z;
    result[ 3 ] = state.Velocity( ).x;
    result[ 4 ] = state.Velocity( ).y;
    result[ 5 ] = state.Velocity( ).z;
    return result;
}

Vector6 getStateVectorInMetre( const Eci state )
{
    Vector6 result( 6 );
    result[ 0 ] = state.Position( ).x * 1000.0;
    result[ 1 ] = state.Position( ).y * 1000.0;
    result[ 2 ] = state.Position( ).z * 1000.0;
    result[ 3 ] = state.Velocity( ).x * 1000.0;
    result[ 4 ] = state.Velocity( ).y * 1000.0;
    result[ 5 ] = state.Velocity( ).z * 1000.0;
    return result;
}

int main( void )
{
    // conversion from km to m
    // const double km2m = 1000;
    const double Rearth = kXKMPER; // earth radius in km
    // earth radius and diameter
    // const double EarthRadius = kXKMPER * km2m; // unit m
    // const double EarthDiam = 2 * EarthRadius;

    // grav. parameter 'mu' of earth
    const double muEarth = kMU*( pow( 10, 9 ) ); // unit m^3/s^2

    // vectors to store arrival and departure velocities for the transfer trajectory
    Vector3 DepartureVelocity( 3 );
    Vector3 ArrivalVelocity( 3 );

    // read the TLE file. Line based parsing, using string streams
    std::string line;

    std::ifstream tlefile( "../../src/napa_prograde_catalog.txt" );
    const bool is_retro = false;

    if( !tlefile.is_open() )
        perror("error while opening file");

    std::vector < Tle > tleObjects; // vector of TLE objects

    while( !tlefile.eof( ) )
    {
        std::vector < std::string > tleStrings;

        std::getline( tlefile, line ); // read line from the catalog file
        removeNewline( line ); // remove new line characters such as '/n'
        tleStrings.push_back( line );
        // std::cout << line << std::endl;

        std::getline( tlefile, line );
        removeNewline( line );
        tleStrings.push_back( line );
        // std::cout << line << std::endl;

        std::getline( tlefile, line );
        removeNewline( line );
        tleStrings.push_back( line );
        // std::cout << line << std::endl << std::endl;

        tleObjects.push_back( Tle( tleStrings[ 0 ], tleStrings[ 1 ], tleStrings[ 2 ] ) );
    }
    tlefile.close( );

    const int DebrisObjects = tleObjects.size( );
    std::cout << "Total debris objects = " << DebrisObjects << std::endl;

    // some variables for the "catch" segment
    int failCount = 0;
    int catchDepartureID;
    int catchArrivalID;

    std::ofstream ephemerisfile;
    ephemerisfile.precision( 15 );
    std::ofstream osculatingfile;
    osculatingfile.precision( 15 );

    std::cout.precision( 15 );

    //*********************** Inputs *********************************************************************************************************//
    Tle departureObject = tleObjects[ 0 ];
    Tle arrivalObject = tleObjects[ 1 ];
    DateTime departureEpoch = DateTime( 2016, 1, 13, 22, 06, 45 );
    const double TOF = 24910.0; // time of flight
    ephemerisfile.open( "/home/abhishek/Dropbox/Dinamica Internship/16-03-01 Napa osculating elements and ephemeris/16-03-01 ephemeris transfer case 5.csv" );
    osculatingfile.open( "/home/abhishek/Dropbox/Dinamica Internship/16-03-01 Napa osculating elements and ephemeris/16-03-01 osculating transfer case 5.csv" );
    //***************************************************************************************************************************************//

    SGP4 sgp4Departure( departureObject );
    const Eci tleDepartureState = sgp4Departure.FindPosition( departureEpoch );
    // const Eci tleDepartureState = sgp4Departure.FindPosition( 0.0 );
    const Vector6 departureState = getStateVector( tleDepartureState );

    array3 departurePosition;
    array3 departureVelocity;
    for( int j = 0; j < 3; j++ )
    {
        departurePosition[ j ] = departureState[ j ];
        departureVelocity[ j ] = departureState[ j + 3 ];
    }

    const int departureObjectId = static_cast< int >( departureObject.NoradNumber( ) );
    // std::cout << "departure Object ID = " << departureObjectId << std::endl;
    catchDepartureID = departureObjectId; // for the catch segment of the program

    SGP4 sgp4Arrival( arrivalObject );
    const int arrivalObjectId = static_cast< int >( arrivalObject.NoradNumber( ) );
    // std::cout << "Arrival Object ID = " << arrivalObjectId << std::endl;
    catchArrivalID = arrivalObjectId;

    // DateTime arrivalEpoch = arrivalObject.Epoch( ); // take epoch from arrival TLE and then add TOF to it
    // arrivalEpoch = arrivalEpoch.AddSeconds( TOF );
    const DateTime arrivalEpoch = departureEpoch.AddSeconds( TOF );
    const Eci tleArrivalState = sgp4Arrival.FindPosition( arrivalEpoch );
    // const Eci tleArrivalState = sgp4Arrival.FindPosition( 0.0 );
    const Vector6 arrivalState = getStateVector( tleArrivalState );

    array3 arrivalPosition;
    array3 arrivalVelocity;
    for( int j = 0; j < 3; j++ )
    {
        arrivalPosition[ j ] = arrivalState[ j ];
        arrivalVelocity[ j ] = arrivalState[ j + 3 ];
    }

    kep_toolbox::lambert_problem targeter( departurePosition, arrivalPosition, TOF, kMU, is_retro, 5 );
    const int numberOfSolutions = targeter.get_v1( ).size( );
    std::vector< array3 > departureDeltaVs( numberOfSolutions ); // delta-V components at the departure point
    std::vector< array3 > arrivalDeltaVs( numberOfSolutions );
    std::vector< Real > transferDeltaVs( numberOfSolutions ); // magnitude of the total delta-V of one transfer between two points

    for ( int j = 0; j < numberOfSolutions; j++ )
    {
        array3 transferDepartureVelocity = targeter.get_v1( )[ j ]; // velocity of the s/c at the departure point in the transfer orbit
        array3 transferArrivalVelocity = targeter.get_v2( )[ j ];

        departureDeltaVs[ j ] = sml::add( transferDepartureVelocity, sml::multiply( departureVelocity, -1.0 ) );
        arrivalDeltaVs[ j ] = sml::add( transferArrivalVelocity, sml::multiply( arrivalVelocity, -1.0 ) );

        transferDeltaVs[ j ] = sml::norm< Real >( departureDeltaVs[ j ] ) + sml::norm< Real >( arrivalDeltaVs[ j ] );
    }

    const std::vector< Real >::iterator minDeltaVIterator = std::min_element( transferDeltaVs.begin( ), transferDeltaVs.end( ) );
    const int minimumDeltaVIndex = std::distance( transferDeltaVs.begin( ), minDeltaVIterator );

    array3 minIndexDepartureVelocity = targeter.get_v1( )[ minimumDeltaVIndex ]; // best guess for velocity in transfer orbit at the departure point
    Vector3 departureVelocityGuess( 3 );
    Vector3 atomDeparturePosition( 3 );
    Vector3 atomArrivalPosition( 3 );

    Vector6 LambertState( 6 );
    array3 minLambertDepVel = targeter.get_v1( )[ minimumDeltaVIndex ];
    for( int i = 0; i < 3; i++ )
    {
        LambertState[ i ] = departurePosition[ i ] * 1000.0; // metre
        LambertState[ i + 3 ] = minLambertDepVel[ i ] * 1000.0; // metre/sec
    }

    array3 LambertPropPosition;
    array3 LambertPropVelocity;
    for ( int i = 0; i < 3; i++ )
    {
        LambertPropPosition[ i ] = LambertState[ i ] / 1000.0; // km
        LambertPropVelocity[ i ] = LambertState[ i + 3 ] / 1000.0; // km/s
    }

    const Real tolerance = 10.0 * std::numeric_limits< Real >::epsilon( );
    Vector6 LambertKep = astro::convertCartesianToKeplerianElements( LambertState, muEarth, tolerance );

    for( int j = 0; j < 3; j++ )
    {
        departureVelocityGuess[ j ] = minIndexDepartureVelocity[ j ];
        atomDeparturePosition[ j ] = departurePosition[ j ];
        atomArrivalPosition[ j ] = arrivalPosition[ j ];
    }

    array3 atomDepartureVelocity;
    array3 atomArrivalVelocity;

    std::string SolverStatusSummary;
    int numberOfIterations;
    const int maxIterations = 100;
    const Tle referenceTle = Tle( );

    try
    {
        Vector3 outputDepartureVelocity( 3 );
        Vector3 outputArrivalVelocity( 3 );
        atom::executeAtomSolver< Real, Vector3 >( atomDeparturePosition,
                                                           departureEpoch,
                                                           atomArrivalPosition,
                                                           TOF,
                                                           departureVelocityGuess,
                                                           outputDepartureVelocity,
                                                           outputArrivalVelocity,
                                                           SolverStatusSummary,
                                                           numberOfIterations,
                                                           referenceTle,
                                                           kMU,
                                                           kXKMPER,
                                                           1.0e-10,
                                                           1.0e-5,
                                                           maxIterations );

        Vector6 atomDepartureState( 6 );
        Vector6 atomArrivalState( 6 );

        for( int i = 0; i < 3; i++ )
        {
            atomDepartureState[ i ] = atomDeparturePosition[ i ];
            atomDepartureState[ i + 3 ] = outputDepartureVelocity[ i ];
            atomArrivalState[ i ] = atomArrivalPosition[ i ];
            atomArrivalState[ i + 3 ] = outputArrivalVelocity[ i ];
        }

        Tle transferTLE = atom::convertCartesianStateToTwoLineElements< Real, Vector6>( atomDepartureState, departureEpoch );

        // std::cout << "Transfer orbit TLE:" << testDepartureTLE << std::endl << std::endl;
        // std::cout << "Departure object TLE:" << departureObject << std::endl << std::endl;

        // Generate Ephemeris for the transfer orbit obtained from the ATOM solver
        Real TimeStep = TOF/1000.0;
        Real EphemerisJD = 0.0;
        SGP4 sgp4Ephemeris( transferTLE );
        Real tsince = 0.0;

        ephemerisfile << "jd" << "," << "x" << "," << "y" << "," << "z" << "," << "xdot" << "," << "ydot" << "," << "zdot" << ",";
        ephemerisfile << "lx" << "," << "ly" << "," << "lz" << "," << "lxdot" << "," << "lydot" << "," << "lzdot" << std::endl;

        osculatingfile << "jd" << "," << "a" << "," << "e" << "," << "i" << "," << "aop" << "," << "raan" << "," << "TA" << ",";
        osculatingfile << "raan_dot_moon" << "," << "raan_dot_sun" << "," << "raan_dot_3b" << "," << "raan_dot_j2" << "," << "raan_dot_total" << "," << "aop_dot_moon" << ",";
        osculatingfile << "aop_dot_sun" << "," << "aop_dot_3b" << "," << "aop_dot_j2" << "," << "aop_dot_total" << ",";
        osculatingfile << "La" << "," << "Le" << "," << "Li" << "," << "Laop" << "," << "Lraan" << "," << "LTA" << std::endl;

        const Real j2 = 0.00108263;
        Vector6 atomDepartureStateMetre( 6 );

        for( int i = 0; i < 6; i++ )
            atomDepartureStateMetre[ i ] = atomDepartureState[ i ] * 1000;

        Vector6 nominalTransferKeplerian = astro::convertCartesianToKeplerianElements( atomDepartureStateMetre, muEarth, tolerance );
        double p_a = nominalTransferKeplerian[ 0 ];
        double p_a_km = p_a / 1000.0;
        std::cout << "semi major axis of transfer orbit = " << p_a / 1000.0 << std::endl;
        double p_e = nominalTransferKeplerian[ 1 ];
        double p_i = nominalTransferKeplerian[ 2 ];
        std::cout << "inclination = " << sml::convertRadiansToDegrees( p_i ) << std::endl;
        double p_T = 2 * sml::SML_PI * std::sqrt( std::pow( p_a, 3 ) / muEarth );
        std::cout << "time period for transfer orbit = " << p_T / 60 << std::endl;
        double p_n = ( 24 * 60 * 60 ) / p_T;
        std::cout << "mean motion for transfer orbit = " << p_n << std::endl;

        double p_aop = sml::convertRadiansToDegrees( nominalTransferKeplerian[ 3 ] );
        double p_raan = sml::convertRadiansToDegrees( nominalTransferKeplerian[ 4 ] );

        double raan_dot_moon = -0.00338 * ( std::cos( p_i ) / p_n ) / 86400.0;
        double raan_dot_sun = -0.00154 * ( std::cos( p_i ) / p_n ) / 86400.0;
        double raan_dot_j2 = -1.5 * p_n * 360.0 * j2 * std::pow( ( Rearth / p_a_km ), 2 ) * std::cos( p_i ) / ( std::pow( (1 - std::pow( p_e, 2 ) ), 2 ) * 86400.0 );
        double raan_dot_total = raan_dot_moon + raan_dot_sun + raan_dot_j2;
        std::cout << "raan_dot_total = " << raan_dot_total * 86400.0 << std::endl;

        double aop_dot_moon = 0.00169 * ( 4 - 5 * ( std::pow( std::sin( p_i ), 2 ) ) ) / ( p_n * 86400.0 );
        double aop_dot_sun = 0.00077 * ( 4 - 5 * ( std::pow( std::sin( p_i ), 2 ) ) ) / ( p_n * 86400.0 );
        double aop_dot_j2 = 0.75 * p_n * 360.0 * j2 * std::pow( ( Rearth / p_a_km ), 2 ) * ( 4 - 5 * ( std::pow( std::sin( p_i ), 2 ) ) )
                                                                        / ( std::pow( (1 - std::pow( p_e, 2 ) ), 2 ) * 86400.0 );
        double aop_dot_total = aop_dot_moon + aop_dot_sun + aop_dot_j2;
        std::cout << "aop_dot_total = " << aop_dot_total * 86400.0 << std::endl;

        for( int i = 0; i < 1001; i++ )
        {
            Real t = i;
            tsince = t * TimeStep;
            DateTime EphemerisEpoch = departureEpoch; // departure epoch for the transfer orbit arc
            EphemerisEpoch = EphemerisEpoch.AddSeconds( tsince );

            Eci AtomEphemeris = sgp4Ephemeris.FindPosition( EphemerisEpoch );

            EphemerisJD = EphemerisEpoch.ToJulian( ); // convert to Julian date

            Vector6 cart = getStateVectorInMetre( AtomEphemeris );

            //convert cartesian to keplerian elements using astro ( units are m and rads )
            Vector6 osc = astro::convertCartesianToKeplerianElements( cart, muEarth, tolerance );

            osculatingfile << EphemerisJD << ",";
            osculatingfile << osc[ 0 ] / 1000.0 << ",";
            osculatingfile << osc[ 1 ] << ",";
            osculatingfile << sml::convertRadiansToDegrees( osc[ 2 ] ) << ",";
            osculatingfile << sml::convertRadiansToDegrees( osc[ 3 ] ) << ",";
            osculatingfile << sml::convertRadiansToDegrees( osc[ 4 ] ) << ",";
            osculatingfile << sml::convertRadiansToDegrees( osc[ 5 ] ) << ",";
            osculatingfile << p_raan + raan_dot_moon * tsince << ",";
            osculatingfile << p_raan + raan_dot_sun * tsince << ",";
            osculatingfile << p_raan + ( raan_dot_sun + raan_dot_moon ) * tsince << ",";
            osculatingfile << p_raan + raan_dot_j2 * tsince << ",";
            osculatingfile << p_raan + raan_dot_total * tsince << ",";
            osculatingfile << p_aop + aop_dot_moon * tsince << ",";
            osculatingfile << p_aop + aop_dot_sun * tsince << ",";
            osculatingfile << p_aop + ( aop_dot_moon + aop_dot_sun ) * tsince << ",";
            osculatingfile << p_aop + aop_dot_j2 * tsince << ",";
            osculatingfile << p_aop + aop_dot_total * tsince << ",";
            osculatingfile << LambertKep[ 0 ] / 1000.0 << ",";
            osculatingfile << LambertKep[ 1 ] << ",";
            osculatingfile << sml::convertRadiansToDegrees( LambertKep[ 2 ] ) << ",";
            osculatingfile << sml::convertRadiansToDegrees( LambertKep[ 3 ] ) << ",";
            osculatingfile << sml::convertRadiansToDegrees( LambertKep[ 4 ] ) << ",";
            osculatingfile << sml::convertRadiansToDegrees( LambertKep[ 5 ] ) << std::endl;

            kep_toolbox::propagate_lagrangian( LambertPropPosition, LambertPropVelocity, TimeStep, kMU );
            for ( int j = 0; j < 3; j++ )
            {
                LambertState[ j ] = LambertPropPosition[ j ] * 1000.0; // metre
                LambertState[ j + 3 ] = LambertPropVelocity[ j ] * 1000.0; // metre/sec
            }
            LambertKep = astro::convertCartesianToKeplerianElements( LambertState, muEarth, tolerance );

            ephemerisfile << EphemerisJD << ",";
            ephemerisfile << AtomEphemeris.Position( ).x << ",";
            ephemerisfile << AtomEphemeris.Position( ).y << ",";
            ephemerisfile << AtomEphemeris.Position( ).z << ",";
            ephemerisfile << AtomEphemeris.Velocity( ).x << ",";
            ephemerisfile << AtomEphemeris.Velocity( ).y << ",";
            ephemerisfile << AtomEphemeris.Velocity( ).z << ",";
            ephemerisfile << LambertPropPosition[ 0 ] << ",";
            ephemerisfile << LambertPropPosition[ 1 ] << ",";
            ephemerisfile << LambertPropPosition[ 2 ] << ",";
            ephemerisfile << LambertPropVelocity[ 0 ] << ",";
            ephemerisfile << LambertPropVelocity[ 1 ] << ",";
            ephemerisfile << LambertPropVelocity[ 2 ] << std::endl;

            if( i == 1000 )
            {
                std::cout << "Difference in Actual Arrival Position and Final State taken from Trasnfer Orbit Ephemeris" << std::endl;
                std::cout << "X axis Difference = " << AtomEphemeris.Position( ).x - atomArrivalPosition[ 0 ] << std::endl;
                std::cout << "Y axis Difference = " << AtomEphemeris.Position( ).y - atomArrivalPosition[ 1 ] << std::endl;
                std::cout << "Z axis Difference = " << AtomEphemeris.Position( ).z - atomArrivalPosition[ 2 ] << std::endl << std::endl;
            }
        }

        for( int k = 0; k < 3; k++)
        {
            atomDepartureVelocity[ k ] = outputDepartureVelocity[ k ];
            atomArrivalVelocity[ k ] = outputArrivalVelocity[ k ];
        }

        array3 atomDepartureDeltaV;
        array3 atomArrivalDeltaV;
        Real AtomDeltaV;

        atomDepartureDeltaV = sml::add( atomDepartureVelocity, sml::multiply( departureVelocity, -1.0 ) );
        atomArrivalDeltaV = sml::add( atomArrivalVelocity, sml::multiply( arrivalVelocity, -1.0 ) );

        AtomDeltaV = sml::norm< Real >( atomDepartureDeltaV ) + sml::norm< Real >( atomArrivalDeltaV );
        std::cout << "Atom DeltaV = " << AtomDeltaV << std::endl;
    }

    catch( const std::exception& err )
    {
        ++failCount;
        std::cout << "Exception Caught = " << err.what( ) << std::endl;
        std::cout << "For departure ID = " << catchDepartureID << " " << "For Arrival ID = " << catchArrivalID << std::endl << std::endl;
        // std::cout << "Fail count = " << failCount << std::endl;
    }

   ephemerisfile.close( );
   osculatingfile.close( );
   return EXIT_SUCCESS;
}


#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>

#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"

namespace caffe {
//math_function 定义了caffe 中用到的一些矩阵操作和数值计算的一些函数

/*
 *功能： C=alpha*A*B+beta*C
 *A,B,C 是输入矩阵（一维数组格式）
 *CblasRowMajor :数据是行主序的（二维数据也是用一维数组储存的）
 *TransA, TransB：是否要对A和B做转置操作（CblasTrans CblasNoTrans）
 *M： A、C 的行数
 *N： B、C 的列数
 *K： A 的列数， B 的行数
 *lda ： A的列数（不做转置）行数（做转置）
 *ldb： B的列数（不做转置）行数（做转置）
*/
template<>
void caffe_cpu_gemm<float>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const float alpha, const float* A, const float* B, const float beta,
    float* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_sgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template<>
void caffe_cpu_gemm<double>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const double alpha, const double* A, const double* B, const double beta,
    double* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_dgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

/*
功能： y=alpha*A*x+beta*y
其中X和Y是向量，A 是矩阵
M：A 的行数
N：A 的列数
cblas_sgemv 中的 参数1 表示对X和Y的每个元素都进行操作
*/
template <>
void caffe_cpu_gemv<float>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const float alpha, const float* A, const float* x,
    const float beta, float* y) {
  cblas_sgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_cpu_gemv<double>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const double alpha, const double* A, const double* x,
    const double beta, double* y) {
  cblas_dgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

/*
功能： Y=alpha*X+Y
N：为X和Y中element的个数
*/
template <>
void caffe_axpy<float>(const int N, const float alpha, const float* X,
    float* Y) { cblas_saxpy(N, alpha, X, 1, Y, 1); }

template <>
void caffe_axpy<double>(const int N, const double alpha, const double* X,
    double* Y) { cblas_daxpy(N, alpha, X, 1, Y, 1); }

/*
功能：用常数 alpha 对 Y 进行初始化
函数 void *memset(void *buffer, char c, unsigned count) 一般为新申请的内存做初始化，
功能是将buffer所指向内存中的每个字节的内容全部设置为c指定的ASCII值, count为块的大小,
使用memset函数来初始化数组或者结构体比其他初始化方法更快一点
*/
template <typename Dtype>
void caffe_set(const int N, const Dtype alpha, Dtype* Y) {
  if (alpha == 0) {
    memset(Y, 0, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    return;
  }
  for (int i = 0; i < N; ++i) {
    Y[i] = alpha;
  }
}

template void caffe_set<int>(const int N, const int alpha, int* Y);
template void caffe_set<float>(const int N, const float alpha, float* Y);
template void caffe_set<double>(const int N, const double alpha, double* Y);

//功能： 给 Y 的每个 element 加上常数 alpha
template <>
void caffe_add_scalar(const int N, const float alpha, float* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <>
void caffe_add_scalar(const int N, const double alpha, double* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

/*
函数 void *memcpy(void *dest, void *src, unsigned int count) 把src所指向
的内存区域 copy到dest所指向的内存区域, count为块的大小
表头文件: #include <string.h>
定义函数: void *memcpy(void *dest, const void *src, size_t n)
函数说明: memcpy()用来拷贝src所指的内存内容前n个字节到dest所指的内存地址上。与strcpy()不同的是,memcpy()会完整的复制n个字节,不会因为遇到字符串结束'\0'而结束
返回值:   返回指向dest的指针
*/
template <typename Dtype>
void caffe_copy(const int N, const Dtype* X, Dtype* Y) {
  if (X != Y) {
    if (Caffe::mode() == Caffe::GPU) {
#ifndef CPU_ONLY
      // NOLINT_NEXT_LINE(caffe/alt_fn)
      CUDA_CHECK(cudaMemcpy(Y, X, sizeof(Dtype) * N, cudaMemcpyDefault));
#else
      NO_GPU;
#endif
    } else {
      memcpy(Y, X, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    }
  }
}

template void caffe_copy<int>(const int N, const int* X, int* Y);
template void caffe_copy<unsigned int>(const int N, const unsigned int* X,
    unsigned int* Y);
template void caffe_copy<float>(const int N, const float* X, float* Y);
template void caffe_copy<double>(const int N, const double* X, double* Y);

/*
功能：X = alpha*X
N： X中element的个数
*/
template <>
void caffe_scal<float>(const int N, const float alpha, float *X) {
  cblas_sscal(N, alpha, X, 1);
}

template <>
void caffe_scal<double>(const int N, const double alpha, double *X) {
  cblas_dscal(N, alpha, X, 1);
}

// axpby Y=alpha * X +beta*Y
template <>
void caffe_cpu_axpby<float>(const int N, const float alpha, const float* X,
                            const float beta, float* Y) {
  cblas_saxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_cpu_axpby<double>(const int N, const double alpha, const double* X,
                             const double beta, double* Y) {
  cblas_daxpby(N, alpha, X, 1, beta, Y, 1);
}

/*
功能：这四个函数分别实现element-wise的加减乘除（y[i] = a[i] + - * \ b[i]）
*/
template <>
void caffe_add<float>(const int n, const float* a, const float* b,
    float* y) {
  vsAdd(n, a, b, y);
}

template <>
void caffe_add<double>(const int n, const double* a, const double* b,
    double* y) {
  vdAdd(n, a, b, y);
}

template <>
void caffe_sub<float>(const int n, const float* a, const float* b,
    float* y) {
  vsSub(n, a, b, y);
}

template <>
void caffe_sub<double>(const int n, const double* a, const double* b,
    double* y) {
  vdSub(n, a, b, y);
}

template <>
void caffe_mul<float>(const int n, const float* a, const float* b,
    float* y) {
  vsMul(n, a, b, y);
}

template <>
void caffe_mul<double>(const int n, const double* a, const double* b,
    double* y) {
  vdMul(n, a, b, y);
}

template <>
void caffe_div<float>(const int n, const float* a, const float* b,
    float* y) {
  vsDiv(n, a, b, y);
}

template <>
void caffe_div<double>(const int n, const double* a, const double* b,
    double* y) {
  vdDiv(n, a, b, y);
}

template <>
void caffe_powx<float>(const int n, const float* a, const float b,
    float* y) {
  vsPowx(n, a, b, y);
}

template <>
void caffe_powx<double>(const int n, const double* a, const double b,
    double* y) {
  vdPowx(n, a, b, y);
}

template <>
void caffe_sqr<float>(const int n, const float* a, float* y) {
  vsSqr(n, a, y);
}

template <>
void caffe_sqr<double>(const int n, const double* a, double* y) {
  vdSqr(n, a, y);
}

template <>
void caffe_exp<float>(const int n, const float* a, float* y) {
  vsExp(n, a, y);
}

template <>
void caffe_exp<double>(const int n, const double* a, double* y) {
  vdExp(n, a, y);
}

template <>
void caffe_log<float>(const int n, const float* a, float* y) {
  vsLn(n, a, y);
}

template <>
void caffe_log<double>(const int n, const double* a, double* y) {
  vdLn(n, a, y);
}

template <>
void caffe_abs<float>(const int n, const float* a, float* y) {
    vsAbs(n, a, y);
}

template <>
void caffe_abs<double>(const int n, const double* a, double* y) {
    vdAbs(n, a, y);
}

/*
功能：返回一个随机数
*/
unsigned int caffe_rng_rand() {
  return (*caffe_rng())();
}

/*
功能 ： 返回 b 最大方向上可以表示的最接近的数值。
*/
template <typename Dtype>
Dtype caffe_nextafter(const Dtype b) {
  return boost::math::nextafter<Dtype>(
      b, std::numeric_limits<Dtype>::max());
}

template
float caffe_nextafter(const float b);

template
double caffe_nextafter(const double b);

template <typename Dtype>
void caffe_rng_uniform(const int n, const Dtype a, const Dtype b, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_LE(a, b);
  boost::uniform_real<Dtype> random_distribution(a, caffe_nextafter<Dtype>(b));
  boost::variate_generator<caffe::rng_t*, boost::uniform_real<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_uniform<float>(const int n, const float a, const float b,
                              float* r);

template
void caffe_rng_uniform<double>(const int n, const double a, const double b,
                               double* r);

template <typename Dtype>
void caffe_rng_gaussian(const int n, const Dtype a,
                        const Dtype sigma, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  boost::normal_distribution<Dtype> random_distribution(a, sigma);
  boost::variate_generator<caffe::rng_t*, boost::normal_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_gaussian<float>(const int n, const float mu,
                               const float sigma, float* r);

template
void caffe_rng_gaussian<double>(const int n, const double mu,
                                const double sigma, double* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, int* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, unsigned int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = static_cast<unsigned int>(variate_generator());
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, unsigned int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, unsigned int* r);

template <>
float caffe_cpu_strided_dot<float>(const int n, const float* x, const int incx,
    const float* y, const int incy) {
  return cblas_sdot(n, x, incx, y, incy);
}

/*
功能： 返回 vector X 和 vector Y 的内积。
incx， incy ： 步长，即每隔incx 或 incy 个element 进行操作。
*/
template <>
double caffe_cpu_strided_dot<double>(const int n, const double* x,
    const int incx, const double* y, const int incy) {
  return cblas_ddot(n, x, incx, y, incy);
}

template <typename Dtype>
Dtype caffe_cpu_dot(const int n, const Dtype* x, const Dtype* y) {
  return caffe_cpu_strided_dot(n, x, 1, y, 1);
}

template
float caffe_cpu_dot<float>(const int n, const float* x, const float* y);

template
double caffe_cpu_dot<double>(const int n, const double* x, const double* y);

template <>
float caffe_cpu_asum<float>(const int n, const float* x) {
  return cblas_sasum(n, x, 1);
}

template <>
double caffe_cpu_asum<double>(const int n, const double* x) {
  return cblas_dasum(n, x, 1);
}

template <>
void caffe_cpu_scale<float>(const int n, const float alpha, const float *x,
                            float* y) {
  cblas_scopy(n, x, 1, y, 1);
  cblas_sscal(n, alpha, y, 1);
}

template <>
void caffe_cpu_scale<double>(const int n, const double alpha, const double *x,
                             double* y) {
  cblas_dcopy(n, x, 1, y, 1);
  cblas_dscal(n, alpha, y, 1);
}

}  // namespace caffe