#include "OGR.h"
#include <boost/math/constants/constants.hpp>
#include <macgyver/Exception.h>
#include <ogr_geometry.h>

// ----------------------------------------------------------------------
/*!
 * \brief Calculate area in m^2 for a geography
 *
 * Refs: Some algorithms for polygons on a sphere
 *       http://hdl.handle.net/2014/40409
 *
 *       https://trac.osgeo.org/openlayers/browser/trunk/openlayers/lib/OpenLayers/Geometry/LinearRing.js
 *
 * Note: The algorithm returns a positive number for clockwise rings.
 */
// ----------------------------------------------------------------------

double geographic_area(const OGRLineString *theGeom)
{
  try
  {
    double area = 0;

    std::size_t npoints = theGeom->getNumPoints();

    for (std::size_t i = 0; i < npoints - 1; i++)
    {
      double x1 = theGeom->getX(i) * boost::math::double_constants::degree;
      double y1 = theGeom->getY(i) * boost::math::double_constants::degree;
      double x2 = theGeom->getX(i + 1) * boost::math::double_constants::degree;
      double y2 = theGeom->getY(i + 1) * boost::math::double_constants::degree;

      area += (x2 - x1) * (2 + sin(y1) + sin(y2));
    }
    area *= 6378137.0 * 6378137.0 / 2.0;

    return std::abs(area);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double geographic_area(const OGRLinearRing *theGeom)
{
  try
  {
    return geographic_area(static_cast<const OGRLineString *>(theGeom));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief get_Area substitute for OGRLineString copied from OGRLinearRing::get_Area()
 *
 * The area is computed according to Green's Theorem:
 *
 * Area is "Sum(x(i)*(y(i+1) - y(i-1)))/2" for i = 0 to pointCount-1,
 * assuming the last point is a duplicate of the first.
 *
 */
// ----------------------------------------------------------------------

double metric_area(const OGRLineString *theGeom)
{
  try
  {
    std::size_t npoints = theGeom->getNumPoints();
    if (npoints < 2)
      return 0;

    double area = theGeom->getX(0) * (theGeom->getY(1) - theGeom->getY(npoints - 1));

    for (std::size_t i = 1; i < npoints - 1; i++)
      area += theGeom->getX(i) * (theGeom->getY(i + 1) - theGeom->getY(i - 1));

    area += theGeom->getX(npoints - 1) * (theGeom->getY(0) - theGeom->getY(npoints - 2));

    return 0.5 * std::abs(area);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry
 */
// ----------------------------------------------------------------------

OGRPolygon *despeckle_polygon(const OGRPolygon *theGeom, double theLimit, bool theGeogFlag)
{
  try
  {
    // Quick exit if the exterior is too small

    const auto *exterior = theGeom->getExteriorRing();
    double area = (theGeogFlag ? geographic_area(exterior) : exterior->get_Area());

    if (area < theLimit)
      return nullptr;

    // We have at least a valid exterior

    auto *out = new OGRPolygon;
    out->addRingDirectly(dynamic_cast<OGRLinearRing *>(exterior->clone()));

    // Remove too small holes too

    for (int i = 0, n = theGeom->getNumInteriorRings(); i < n; ++i)
    {
      const auto *hole = theGeom->getInteriorRing(i);
      area = (theGeogFlag ? geographic_area(hole) : hole->get_Area());
      if (area >= theLimit)
        out->addRingDirectly(dynamic_cast<OGRLinearRing *>(hole->clone()));
    }

    return out;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry
 */
// ----------------------------------------------------------------------

OGRLineString *despeckle_linestring(const OGRLineString *theGeom, double theLimit, bool theGeogFlag)
{
  try
  {
    if (theGeom == nullptr || theGeom->IsEmpty() != 0)
      return nullptr;

    if (!theGeom->get_IsClosed())
      return dynamic_cast<OGRLineString *>(theGeom->clone());

    // Despeckle closed linestrings only

    // TODO: Old GDAL does not have get_Area for linestrings
    // double area = (theGeogFlag ? geographic_area(theGeom) : geom->get_Area());
    double area = (theGeogFlag ? geographic_area(theGeom) : metric_area(theGeom));

    if (area < theLimit)
      return nullptr;

    return dynamic_cast<OGRLineString *>(theGeom->clone());
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry
 */
// ----------------------------------------------------------------------

OGRPoint *despeckle_point(const OGRPoint *theGeom)
{
  try
  {
    if (theGeom == nullptr || theGeom->IsEmpty() != 0)
      return nullptr;

    return dynamic_cast<OGRPoint *>(theGeom->clone());
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry
 */
// ----------------------------------------------------------------------

OGRMultiPoint *despeckle_multipoint(const OGRMultiPoint *theGeom)
{
  try
  {
    if (theGeom == nullptr || theGeom->IsEmpty() != 0)
      return nullptr;

    return dynamic_cast<OGRMultiPoint *>(theGeom->clone());
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry
 */
// ----------------------------------------------------------------------

OGRMultiLineString *despeckle_multilinestring(const OGRMultiLineString *theGeom,
                                              double theLimit,
                                              bool theGeogFlag)
{
  try
  {
    if (theGeom == nullptr || theGeom->IsEmpty() != 0)
      return nullptr;
    ;

    auto *out = new OGRMultiLineString();

    for (int i = 0, n = theGeom->getNumGeometries(); i < n; ++i)
    {
      auto *geom = despeckle_linestring(
          dynamic_cast<const OGRLineString *>(theGeom->getGeometryRef(i)), theLimit, theGeogFlag);
      if (geom != nullptr)
        out->addGeometryDirectly(geom);
    }

    if (out->IsEmpty() == 0)
      return out;

    delete out;
    return nullptr;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry
 */
// ----------------------------------------------------------------------

OGRMultiPolygon *despeckle_multipolygon(const OGRMultiPolygon *theGeom,
                                        double theLimit,
                                        bool theGeogFlag)
{
  try
  {
    if (theGeom == nullptr || theGeom->IsEmpty() != 0)
      return nullptr;

    auto *out = new OGRMultiPolygon();

    for (int i = 0, n = theGeom->getNumGeometries(); i < n; ++i)
    {
      auto *geom = despeckle_polygon(
          dynamic_cast<const OGRPolygon *>(theGeom->getGeometryRef(i)), theLimit, theGeogFlag);
      if (geom != nullptr)
        out->addGeometryDirectly(geom);
    }

    if (out->IsEmpty() == 0)
      return out;

    delete out;
    return nullptr;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry
 */
// ----------------------------------------------------------------------

// Needed since two functions call each other

OGRGeometry *despeckle_geom(const OGRGeometry *theGeom, double theLimit, bool theGeogFlag);

OGRGeometryCollection *despeckle_geometrycollection(const OGRGeometryCollection *theGeom,
                                                    double theLimit,
                                                    bool theGeogFlag)
{
  try
  {
    if (theGeom == nullptr || theGeom->IsEmpty() != 0)
      return nullptr;

    auto *out = new OGRGeometryCollection;

    for (int i = 0, n = theGeom->getNumGeometries(); i < n; ++i)
    {
      auto *geom = despeckle_geom(theGeom->getGeometryRef(i), theLimit, theGeogFlag);
      if (geom != nullptr)
        out->addGeometryDirectly(geom);
    }

    if (out->IsEmpty() == 0)
      return out;

    delete out;
    return nullptr;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry to output geometry
 */
// ----------------------------------------------------------------------

OGRGeometry *despeckle_geom(const OGRGeometry *theGeom, double theLimit, bool theGeogFlag)
{
  try
  {
    OGRwkbGeometryType id = theGeom->getGeometryType();

    switch (id)
    {
      case wkbPoint:
        return despeckle_point(dynamic_cast<const OGRPoint *>(theGeom));
      case wkbLineString:
        return despeckle_linestring(
            dynamic_cast<const OGRLineString *>(theGeom), theLimit, theGeogFlag);
      case wkbPolygon:
        return despeckle_polygon(dynamic_cast<const OGRPolygon *>(theGeom), theLimit, theGeogFlag);
      case wkbMultiPoint:
        return despeckle_multipoint(dynamic_cast<const OGRMultiPoint *>(theGeom));
      case wkbMultiLineString:
        return despeckle_multilinestring(
            dynamic_cast<const OGRMultiLineString *>(theGeom), theLimit, theGeogFlag);
      case wkbMultiPolygon:
        return despeckle_multipolygon(
            dynamic_cast<const OGRMultiPolygon *>(theGeom), theLimit, theGeogFlag);
      case wkbGeometryCollection:
        return despeckle_geometrycollection(
            dynamic_cast<const OGRGeometryCollection *>(theGeom), theLimit, theGeogFlag);
      case wkbLinearRing:
        throw Fmi::Exception::Trace(BCP, "Direct despeckling of LinearRings is not supported");
      case wkbNone:
        throw Fmi::Exception::Trace(
            BCP, "Encountered a 'none' geometry component when despeckling a geometry");
      default:
        throw Fmi::Exception::Trace(
            BCP, "Encountered an unknown geometry component when clipping polygons");
    }

    // NOT REACHED
    return nullptr;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle a geometry so that small polygons are removed
 *
 * \return Empty GeometryCollection if the result is empty
 */
// ----------------------------------------------------------------------

OGRGeometry *Fmi::OGR::despeckle(const OGRGeometry &theGeom, double theAreaLimit)
{
  try
  {
    // Area calculations for geographies must be done by ourselves, OGR
    // does it in the native system and hence would produce square degrees.

    OGRSpatialReference *crs = theGeom.getSpatialReference();
    bool geographic = (crs != nullptr ? (crs->IsGeographic() != 0) : false);

    // Actual despeckling

    auto *geom =
        despeckle_geom(&theGeom, theAreaLimit * 1000 * 1000, geographic);  // from m^2 to km^2

    if (geom != nullptr)
      geom->assignSpatialReference(theGeom.getSpatialReference());  // SR is ref. counted

    return geom;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}


/////////////////////////////////////////////////////////////////////////
// Copyright (C) 2016 Sergey Koshelev
/////////////////////////////////////////////////////////////////////////

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "HammingDistanceClass"

#include <boost/test/unit_test.hpp>

#include "../lib/HammingDistance.h"

BOOST_AUTO_TEST_CASE( calculateBlobsEqSz )
{
   // 2 bytes completely different
   unsigned int a = 0xFFFF;
   unsigned int b = 0x0000;
   BOOST_CHECK( HammingDistance::calculate( &a, &b, 1 ) == 16 );

   // move 1 set bit through integer and calculate Hamming distance. Must be always 1
   for ( int i = 0; i < sizeof( int ) * 8; ++i )
   {
      unsigned int a = 1 << i;
      unsigned int b = 0;
      BOOST_CHECK( HammingDistance::calculate( &a, &b, 1 ) == 1 );
   }

   // check array
   int aa[] = { 0xFFFF, 0x0000 };
   int bb[] = { 0xFFFE, 0x0010 };
   BOOST_CHECK( HammingDistance::calculate( aa, bb, 2 ) == 2 );
}

BOOST_AUTO_TEST_CASE( calculateBlobsDifSz )
{
   int aa[] = { 0xFFFF, 0x0000 };
   int bb[] = { 0xFFFF };
   BOOST_CHECK( HammingDistance::calculate( aa, bb, 2, 1 ) == sizeof(int)*8 );
}

BOOST_AUTO_TEST_CASE( calculateBlobsZeroTerm )
{
   int aa[] = { 0xFFFF, 0xFFFF, 0x1000, 0 };
   int bb[] = { 0xFFFF, 0xFEFF, 0x0100, 0 };
   BOOST_CHECK( HammingDistance::calculate( aa, bb ) == 3 );
}

BOOST_AUTO_TEST_CASE( calculateStrings )
{
   std::string a = "1011101";
   std::string b = "1001001";
   BOOST_CHECK( HammingDistance::calculate( a, b ) == 2 );

   a = "2173896";
   b = "2233796";
   BOOST_CHECK( HammingDistance::calculate( a, b ) == 3 );

   a = "2173896";
   b = "223379";
   BOOST_CHECK( HammingDistance::calculate( a, b ) == 4 );

}


#ifndef STAN_MATH_PRIM_MAT_FUN_DIAG_PRE_MULTIPLY_HPP
#define STAN_MATH_PRIM_MAT_FUN_DIAG_PRE_MULTIPLY_HPP

#include <stan/math/prim/mat/err/check_vector.hpp>
#include <stan/math/prim/scal/err/check_size_match.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <boost/math/tools/promotion.hpp>

namespace stan {
  namespace math {

    template <typename T1, typename T2, int R1, int C1, int R2, int C2>
    Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
                  R2, C2>
    diag_pre_multiply(const Eigen::Matrix<T1, R1, C1>& m1,
                  const Eigen::Matrix<T2, R2, C2>& m2) {
      check_vector("diag_pre_multiply", "m1", m1);
      int m2_rows = m2.rows();
      check_size_match("diag_pre_multiply",
                       "m1.size()", m1.size(),
                       "m2.rows()", m2_rows);
      int m2_cols = m2.cols();
      Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
                    R2, C2>
        result(m2_rows, m2_cols);
      for (int j = 0; j < m2_cols; ++j)
        for (int i = 0; i < m2_rows; ++i)
          result(i, j) = m1(i) * m2(i, j);
      return result;
    }

  }
}
#endif


//This file was modified slightly from
//  http://boost.2283326.n4.nabble.com/accumulators-histogram-td2639100.html
//I just added the IsDensity flag
#pragma once
#include <vector>
#include <limits>
#include <functional>
#include <boost/range.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/min.hpp>

namespace boost { namespace accumulators
{

///////////////////////////////////////////////////////////////////////////////
// cache_size and num_bins named parameters
//
BOOST_PARAMETER_NESTED_KEYWORD(tag, histogram_num_bins, num_bins)
BOOST_PARAMETER_NESTED_KEYWORD(tag, histogram_min_range, min_range)
BOOST_PARAMETER_NESTED_KEYWORD(tag, histogram_max_range, max_range)

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // histogram_impl
    //  histogram histogram
    /**
        @brief Histogram histogram estimator

        The histogram histogram estimator returns a histogram of the sample distribution. The positions and sizes of the bins
        are determined using a specifiable number of cached samples (cache_size). The range between the minimum and the
        maximum of the cached samples is subdivided into a specifiable number of bins (num_bins) of same size. Additionally,
        an under- and an overflow bin is added to capture future under- and overflow samples. Once the bins are determined,
        the cached samples and all subsequent samples are added to the correct bins. At the end, a range of std::pair is
        return, where each pair contains the position of the bin (lower bound) and the samples count (normalized with the
        total number of samples).

        @param  histogram_cache_size Number of first samples used to determine min and max.
        @param  histogram_num_bins Number of bins (two additional bins collect under- and overflow samples).
    */
    template<typename Sample, bool IsDensity>
    struct histogram_impl
      : accumulator_base
    {
        // typedef typename numeric::functional::average<Sample, std::size_t>::result_type float_type;
        using float_type = double;
        typedef std::vector<std::pair<float_type, float_type> > histogram_type;
        typedef std::vector<float_type> array_type;
        // for boost::result_of
        typedef iterator_range<typename histogram_type::iterator> result_type;

      template<typename Args>
      histogram_impl(Args const& args) :
		num_bins(args[histogram_num_bins]),
		minimum (args[histogram_min_range]),
		maximum (args[histogram_max_range]),
		bin_size (numeric::average(args[histogram_max_range] - args[histogram_min_range], args[histogram_num_bins])),
		samples_in_bin(args[histogram_num_bins] + 2, 0.),
		bin_positions(args[histogram_num_bins] + 2),
		_histogram(
		   args[histogram_num_bins] + 2,
		   std::make_pair(0,1)
		   ),
	     is_dirty(true)
      {
			// determine bin positions (their lower bounds)
            for (std::size_t i = 0; i < this->num_bins + 2; ++i)
            {
                this->bin_positions[i] = minimum + (i - 1.0) * bin_size;
            }
      }

      template<typename Args>
      void operator ()(Args const &args)
      {
			// std::size_t cnt = count(args);
            {
                if (args[sample] < this->bin_positions[1])
                {
                    ++(this->samples_in_bin[0]);
                }
                else if (args[sample] >= this->bin_positions[this->num_bins + 1])
                {
                    ++(this->samples_in_bin[this->num_bins + 1]);
                }
                else
                {
                    typename array_type::iterator it = std::upper_bound(
                        this->bin_positions.begin()
                      , this->bin_positions.end()
                      , args[sample]
                    );

                    std::size_t d = std::distance(this->bin_positions.begin(), it);
                    ++(this->samples_in_bin[d - 1]);
                }
            }
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            {
                // creates a vector of std::pair where each pair i holds
                // the values bin_positions[i] (x-axis of histogram) and
                // samples_in_bin[i] / cnt (y-axis of histogram).

                for (std::size_t i = 0; i < this->num_bins + 2; ++i)
                {
                    if(IsDensity)
                       this->_histogram[i] = std::make_pair(this->bin_positions[i], numeric::average(this->samples_in_bin[i], count(args)));
                    else
                       this->_histogram[i] = std::make_pair(this->bin_positions[i], this->samples_in_bin[i]);
                }
            }
            // returns a range of pairs
            return make_iterator_range(this->_histogram);
        }

    private:
        std::size_t				num_bins;        // number of bins
		float_type				minimum;
		float_type				maximum;
		float_type				bin_size;
        array_type				samples_in_bin;  // number of samples in each bin
        array_type				bin_positions;   // lower bounds of bins
        mutable histogram_type	_histogram;       // histogram
		mutable	bool			is_dirty;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::histogram_density and tag::histogram_count
//
namespace tag
{
    template <bool IsDensity = false>
    struct histogram_generic
      : depends_on<count>
	      , histogram_num_bins
		  , histogram_min_range
		  , histogram_max_range
    {
        /// INTERNAL ONLY
        ///
	struct impl {
	  template<typename Sample, typename Weight>
	  struct apply {
        typedef boost::accumulators::impl::histogram_impl<Sample, IsDensity> type;
	  };
	};
    };

    using histogram_density = histogram_generic<true>;
    using histogram_count = histogram_generic<false>;
}

///////////////////////////////////////////////////////////////////////////////
// extract::histogram
//
namespace extract
{
   extractor<tag::histogram_density> const histogram_density = {};
   extractor<tag::histogram_count> const histogram_count = {};
}

using extract::histogram_count;
using extract::histogram_density;

// // So that histogram can be automatically substituted
// // with weighted_histogram when the weight parameter is non-void.
// template<>
// struct as_weighted_feature<tag::histogram>
// {
//     typedef tag::weighted_histogram type;
// };

// template<>
// struct feature_of<tag::weighted_histogram>
//   : feature_of<tag::histogram>
// {
// };

}} // namespace boost::accumulators


#include "Integrator21GL.hh"

#include <Eigen/Dense>
#include <map>
#include <iterator>

#include "GSLSamplerGL.hh"
#include "TypesFunctions.hh"

using namespace Eigen;
using namespace std;

Integrator21GL::Integrator21GL(size_t xbins, int xorders, double* xedges, int yorder, double ymin, double ymax) :
Integrator21Base(xbins, xorders, xedges, yorder, ymin, ymax)
{
  init_sampler();
}

Integrator21GL::Integrator21GL(size_t xbins, int* xorders, double* xedges, int yorder, double ymin, double ymax) :
Integrator21Base(xbins, xorders, xedges, yorder, ymin, ymax)
{
  init_sampler();
}

void Integrator21GL::sample(FunctionArgs& fargs){
  auto& rets=fargs.rets;
  GSLSamplerGL sampler;
  auto& x=rets[0];
  auto& y=rets[1];
  sampler.fill_bins(m_xorders.size(), m_xorders.data(), m_xedges.data(), x.buffer, m_xweights.data());
  sampler.fill(m_yorder, m_ymin, m_ymax, y.buffer, m_yweights.data());

  m_weights = m_xweights.matrix() * m_yweights.matrix().transpose();

  rets[2].x = m_xedges.cast<double>();

  auto npoints=m_xedges.size()-1;
  rets[3].x = 0.5*(m_xedges.tail(npoints)+m_xedges.head(npoints));
  rets[4].mat = x.vec.replicate(1, m_yweights.size());
  rets[5].mat = y.vec.transpose().replicate(m_xweights.size(), 1);
  rets[6].x = 0.0;

  rets.untaint();
  rets.freeze();
}


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ASEC_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ASEC_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/function/acsc.hpp>
#include <boost/simd/function/minus.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF ( asec_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::double_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const A0& a0) const BOOST_NOEXCEPT
    {
      A0 tmp =  (Pio_2<A0>()-acsc(a0)) +  Constant<A0, 0x3c91a62633145c07ll>();
      return if_zero_else(is_equal(a0, One<A0>()), tmp);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF( asec_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return (bs::Pio_2<A0>()-bs::acsc(a0));
      }
   };

} } }

#endif


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#ifndef __IS_KURATOWSKI_SUBGRAPH_HPP__
#define __IS_KURATOWSKI_SUBGRAPH_HPP__

#include <boost/config.hpp>
#include <boost/utility.hpp> //for next/prior
#include <boost/tuple/tuple.hpp>   //for tie
#include <boost/property_map/property_map.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/isomorphism.hpp>
#include <boost/graph/adjacency_list.hpp>

#include <algorithm>
#include <vector>
#include <set>

namespace boost
{

  namespace detail
  {

    template <typename Graph>
    Graph make_K_5()
    {
      typename graph_traits<Graph>::vertex_iterator vi, vi_end, inner_vi;
      Graph K_5(5);
      for(tie(vi,vi_end) = vertices(K_5); vi != vi_end; ++vi)
        for(inner_vi = next(vi); inner_vi != vi_end; ++inner_vi)
          add_edge(*vi, *inner_vi, K_5);
      return K_5;
    }

    template <typename Graph>
    Graph make_K_3_3()
    {
      typename graph_traits<Graph>::vertex_iterator
        vi, vi_end, bipartition_start, inner_vi;
      Graph K_3_3(6);
      bipartition_start = next(next(next(vertices(K_3_3).first)));
      for(tie(vi, vi_end) = vertices(K_3_3); vi != bipartition_start; ++vi)
        for(inner_vi= bipartition_start; inner_vi != vi_end; ++inner_vi)
          add_edge(*vi, *inner_vi, K_3_3);
      return K_3_3;
    }

    template <typename AdjacencyList, typename Vertex>
    void contract_edge(AdjacencyList& neighbors, Vertex u, Vertex v)
    {
      // Remove u from v's neighbor list
      neighbors[v].erase(std::remove(neighbors[v].begin(),
                                     neighbors[v].end(), u
                                     ),
                         neighbors[v].end()
                         );

      // Replace any references to u with references to v
      typedef typename AdjacencyList::value_type::iterator
        adjacency_iterator_t;

      adjacency_iterator_t u_neighbor_end = neighbors[u].end();
      for(adjacency_iterator_t u_neighbor_itr = neighbors[u].begin();
          u_neighbor_itr != u_neighbor_end; ++u_neighbor_itr
          )
        {
          Vertex u_neighbor(*u_neighbor_itr);
          std::replace(neighbors[u_neighbor].begin(),
                       neighbors[u_neighbor].end(), u, v
                       );
        }

      // Remove v from u's neighbor list
      neighbors[u].erase(std::remove(neighbors[u].begin(),
                                     neighbors[u].end(), v
                                     ),
                         neighbors[u].end()
                         );

      // Add everything in u's neighbor list to v's neighbor list
      std::copy(neighbors[u].begin(),
                neighbors[u].end(),
                std::back_inserter(neighbors[v])
                );

      // Clear u's neighbor list
      neighbors[u].clear();

    }

    enum target_graph_t { tg_k_3_3, tg_k_5};

  } // namespace detail

  template <typename Graph, typename ForwardIterator, typename VertexIndexMap>
  bool is_kuratowski_subgraph(const Graph& g,
                              ForwardIterator begin,
                              ForwardIterator end,
                              VertexIndexMap vm
                              )
  {

    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator_t;
    typedef typename graph_traits<Graph>::edge_descriptor edge_t;
    typedef typename graph_traits<Graph>::edges_size_type e_size_t;
    typedef typename graph_traits<Graph>::vertices_size_type v_size_t;
    typedef typename std::vector<vertex_t> v_list_t;
    typedef typename v_list_t::iterator v_list_iterator_t;
    typedef iterator_property_map
      <typename std::vector<v_list_t>::iterator, VertexIndexMap>
      vertex_to_v_list_map_t;

    typedef adjacency_list<vecS, vecS, undirectedS> small_graph_t;

    detail::target_graph_t target_graph = detail::tg_k_3_3; //unless we decide otherwise later

    static small_graph_t K_5(detail::make_K_5<small_graph_t>());

    static small_graph_t K_3_3(detail::make_K_3_3<small_graph_t>());

    v_size_t n_vertices(num_vertices(g));
    v_size_t max_num_edges(3*n_vertices - 5);

    std::vector<v_list_t> neighbors_vector(n_vertices);
    vertex_to_v_list_map_t neighbors(neighbors_vector.begin(), vm);

    e_size_t count = 0;
    for(ForwardIterator itr = begin; itr != end; ++itr)
      {

        if (count++ > max_num_edges)
          return false;

        edge_t e(*itr);
        vertex_t u(source(e,g));
        vertex_t v(target(e,g));

        neighbors[u].push_back(v);
        neighbors[v].push_back(u);

      }

    for(v_size_t max_size = 2; max_size < 5; ++max_size)
      {

        vertex_iterator_t vi, vi_end;
        for(tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
          {
            vertex_t v(*vi);

            //a hack to make sure we don't contract the middle edge of a path
            //of four degree-3 vertices
            if (max_size == 4 && neighbors[v].size() == 3)
              {
                if (neighbors[neighbors[v][0]].size() +
                    neighbors[neighbors[v][1]].size() +
                    neighbors[neighbors[v][2]].size()
                    < 11 // so, it has two degree-3 neighbors
                    )
                  continue;
              }

            while (neighbors[v].size() > 0 && neighbors[v].size() < max_size)
              {
                // Find one of v's neighbors u such that that v and u
                // have no neighbors in common. We'll look for such a
                // neighbor with a naive cubic-time algorithm since the
                // max size of any of the neighbor sets we'll consider
                // merging is 3

                bool neighbor_sets_intersect = false;

                vertex_t min_u = graph_traits<Graph>::null_vertex();
                vertex_t u;
                v_list_iterator_t v_neighbor_end = neighbors[v].end();
                for(v_list_iterator_t v_neighbor_itr = neighbors[v].begin();
                    v_neighbor_itr != v_neighbor_end;
                    ++v_neighbor_itr
                    )
                  {
                    neighbor_sets_intersect = false;
                    u = *v_neighbor_itr;
                    v_list_iterator_t u_neighbor_end = neighbors[u].end();
                    for(v_list_iterator_t u_neighbor_itr =
                          neighbors[u].begin();
                        u_neighbor_itr != u_neighbor_end &&
                          !neighbor_sets_intersect;
                        ++u_neighbor_itr
                        )
                      {
                        for(v_list_iterator_t inner_v_neighbor_itr =
                              neighbors[v].begin();
                            inner_v_neighbor_itr != v_neighbor_end;
                            ++inner_v_neighbor_itr
                            )
                          {
                            if (*u_neighbor_itr == *inner_v_neighbor_itr)
                              {
                                neighbor_sets_intersect = true;
                                break;
                              }
                          }

                      }
                    if (!neighbor_sets_intersect &&
                        (min_u == graph_traits<Graph>::null_vertex() ||
                         neighbors[u].size() < neighbors[min_u].size())
                        )
                      {
                        min_u = u;
                      }

                  }

                if (min_u == graph_traits<Graph>::null_vertex())
                  // Exited the loop without finding an appropriate neighbor of
                  // v, so v must be a lost cause. Move on to other vertices.
                  break;
                else
                  u = min_u;

                detail::contract_edge(neighbors, u, v);

              }//end iteration over v's neighbors

          }//end iteration through vertices v

        if (max_size == 3)
          {
            // check to see whether we should go on to find a K_5
            for(tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
              if (neighbors[*vi].size() == 4)
                {
                  target_graph = detail::tg_k_5;
                  break;
                }

            if (target_graph == detail::tg_k_3_3)
              break;
          }

      }//end iteration through max degree 2,3, and 4

    //Now, there should only be 5 or 6 vertices with any neighbors. Find them.

    v_list_t main_vertices;
    vertex_iterator_t vi, vi_end;

    for(tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
      {
        if (!neighbors[*vi].empty())
          main_vertices.push_back(*vi);
      }

    // create a graph isomorphic to the contracted graph to test
    // against K_5 and K_3_3
    small_graph_t contracted_graph(main_vertices.size());
    std::map<vertex_t,typename graph_traits<small_graph_t>::vertex_descriptor>
      contracted_vertex_map;

    typename v_list_t::iterator itr, itr_end;
    itr_end = main_vertices.end();
    typename graph_traits<small_graph_t>::vertex_iterator
      si = vertices(contracted_graph).first;

    for(itr = main_vertices.begin(); itr != itr_end; ++itr, ++si)
      {
        contracted_vertex_map[*itr] = *si;
      }

    typename v_list_t::iterator jtr, jtr_end;
    for(itr = main_vertices.begin(); itr != itr_end; ++itr)
      {
        jtr_end = neighbors[*itr].end();
        for(jtr = neighbors[*itr].begin(); jtr != jtr_end; ++jtr)
          {
            if (get(vm,*itr) < get(vm,*jtr))
              {
                add_edge(contracted_vertex_map[*itr],
                         contracted_vertex_map[*jtr],
                         contracted_graph
                         );
              }
          }
      }

    if (target_graph == detail::tg_k_5)
      {
        return isomorphism(K_5,contracted_graph);
      }
    else //target_graph == tg_k_3_3
      {
        return isomorphism(K_3_3,contracted_graph);
      }

  }

  template <typename Graph, typename ForwardIterator>
  bool is_kuratowski_subgraph(const Graph& g,
                              ForwardIterator begin,
                              ForwardIterator end
                              )
  {
    return is_kuratowski_subgraph(g, begin, end, get(vertex_index,g));
  }

}

#endif //__IS_KURATOWSKI_SUBGRAPH_HPP__


//
// Created by Alex Beccaro on 21/03/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/101-150/101/problem101.hpp"

BOOST_AUTO_TEST_SUITE( Problem101 )

    BOOST_AUTO_TEST_CASE( Example1 ) {
        auto res = problems::problem101::solve([](int32_t x) { return x * x * x; });
        BOOST_CHECK_EQUAL(res, 74);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem101::solve();
        BOOST_CHECK_EQUAL(res, 37076114526);
    }

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"

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

using namespace pinocchio;

template<typename D>
void addJointAndBody(Model & model,
                     const JointModelBase<D> & jmodel,
                     const Model::JointIndex parent_id,
                     const SE3 & joint_placement,
                     const std::string & joint_name,
                     const Inertia & Y)
{
  Model::JointIndex idx;

  idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
  model.appendBodyToJoint(idx,Y);
}

BOOST_AUTO_TEST_SUITE(JointSpherical)

BOOST_AUTO_TEST_CASE(spatial)
{
  SE3 M(SE3::Random());
  Motion v(Motion::Random());

  MotionSpherical mp(MotionSpherical::Vector3(1.,2.,3.));
  Motion mp_dense(mp);

  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));

  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
}

BOOST_AUTO_TEST_CASE(vsFreeFlyer)
{
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef Eigen::Matrix <double, 6, 1> Vector6;
  typedef Eigen::Matrix <double, 7, 1> VectorFF;
  typedef SE3::Matrix3 Matrix3;

  Model modelSpherical, modelFreeflyer;

  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  addJointAndBody(modelSpherical,JointModelSpherical(),0,pos,"spherical",inertia);
  addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,pos,"free-flyer",inertia);

  Data dataSpherical(modelSpherical);
  Data dataFreeFlyer(modelFreeflyer);

  Eigen::VectorXd q = Eigen::VectorXd::Ones(modelSpherical.nq);q.normalize();
  VectorFF qff; qff << 0, 0, 0, q[0], q[1], q[2], q[3];
  Eigen::VectorXd v = Eigen::VectorXd::Ones(modelSpherical.nv);
  Vector6 vff; vff << 0, 0, 0, 1, 1, 1;
  Eigen::VectorXd tauSpherical = Eigen::VectorXd::Ones(modelSpherical.nv);
  Eigen::VectorXd tauff; tauff.resize(7); tauff << 0,0,0,1,1,1,1;
  Eigen::VectorXd aSpherical = Eigen::VectorXd::Ones(modelSpherical.nv);
  Eigen::VectorXd aff(vff);

  forwardKinematics(modelSpherical, dataSpherical, q, v);
  forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);

  computeAllTerms(modelSpherical, dataSpherical, q, v);
  computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);

  BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataSpherical.oMi[1]));
  BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataSpherical.liMi[1]));
  BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataSpherical.Ycrb[1].matrix()));
  BOOST_CHECK(dataFreeFlyer.f[1].toVector().isApprox(dataSpherical.f[1].toVector()));

  Eigen::VectorXd nle_expected_ff(3); nle_expected_ff << dataFreeFlyer.nle[3],
                                                         dataFreeFlyer.nle[4],
                                                         dataFreeFlyer.nle[5]
                                                         ;
  BOOST_CHECK(nle_expected_ff.isApprox(dataSpherical.nle));
  BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataSpherical.com[0]));

  // InverseDynamics == rnea
  tauSpherical = rnea(modelSpherical, dataSpherical, q, v, aSpherical);
  tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);

  Vector3 tau_expected; tau_expected << tauff(3), tauff(4), tauff(5);
  BOOST_CHECK(tauSpherical.isApprox(tau_expected));

  // ForwardDynamics == aba
  Eigen::VectorXd aAbaSpherical = aba(modelSpherical,dataSpherical, q, v, tauSpherical);
  Eigen::VectorXd aAbaFreeFlyer = aba(modelFreeflyer,dataFreeFlyer, qff, vff, tauff);
  Vector3 a_expected; a_expected << aAbaFreeFlyer[3],
                                    aAbaFreeFlyer[4],
                                    aAbaFreeFlyer[5]
                                    ;
  BOOST_CHECK(aAbaSpherical.isApprox(a_expected));

  // crba
  crba(modelSpherical, dataSpherical,q);
  crba(modelFreeflyer, dataFreeFlyer, qff);

  Eigen::Matrix<double, 3, 3> M_expected(dataFreeFlyer.M.bottomRightCorner<3,3>());

  BOOST_CHECK(dataSpherical.M.isApprox(M_expected));

  // Jacobian
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_planar;jacobian_planar.resize(6,3); jacobian_planar.setZero();
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero();
  computeJointJacobians(modelSpherical, dataSpherical, q);
  computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
  getJointJacobian(modelSpherical, dataSpherical, 1, LOCAL, jacobian_planar);
  getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, jacobian_ff);

  Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(3),
                                                                      jacobian_ff.col(4),
                                                                      jacobian_ff.col(5)
                                                                      ;

  BOOST_CHECK(jacobian_planar.isApprox(jacobian_expected));

}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(JointSphericalZYX)

BOOST_AUTO_TEST_CASE(spatial)
{
  SE3 M(SE3::Random());
  Motion v(Motion::Random());

  MotionSpherical mp(MotionSpherical::Vector3(1.,2.,3.));
  Motion mp_dense(mp);

  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));

  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
}

BOOST_AUTO_TEST_CASE(vsFreeFlyer)
{
  // WARNIG : Dynamic algorithm's results cannot be compared to FreeFlyer's ones because
  // of the representation of the rotation and the ConstraintSubspace difference.
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef Eigen::Matrix <double, 6, 1> Vector6;
  typedef Eigen::Matrix <double, 7, 1> VectorFF;
  typedef SE3::Matrix3 Matrix3;

  Model modelSphericalZYX, modelFreeflyer;

  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  addJointAndBody(modelSphericalZYX,JointModelSphericalZYX(),0,pos,"spherical-zyx",inertia);
  addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,pos,"free-flyer",inertia);

  Data dataSphericalZYX(modelSphericalZYX);
  Data dataFreeFlyer(modelFreeflyer);

  Eigen::AngleAxisd rollAngle(1, Eigen::Vector3d::UnitZ());
  Eigen::AngleAxisd yawAngle(1, Eigen::Vector3d::UnitY());
  Eigen::AngleAxisd pitchAngle(1, Eigen::Vector3d::UnitX());
  Eigen::Quaterniond q_sph = rollAngle * yawAngle * pitchAngle;

  Eigen::VectorXd q = Eigen::VectorXd::Ones(modelSphericalZYX.nq);
  VectorFF qff; qff << 0, 0, 0, q_sph.x(), q_sph.y(), q_sph.z(), q_sph.w();
  Eigen::VectorXd v = Eigen::VectorXd::Ones(modelSphericalZYX.nv);
  Vector6 vff; vff << 0, 0, 0, 1, 1, 1;
  Eigen::VectorXd tauSpherical = Eigen::VectorXd::Ones(modelSphericalZYX.nv);
  Eigen::VectorXd tauff; tauff.resize(6); tauff << 0,0,0,1,1,1;
  Eigen::VectorXd aSpherical = Eigen::VectorXd::Ones(modelSphericalZYX.nv);
  Eigen::VectorXd aff(vff);

  forwardKinematics(modelSphericalZYX, dataSphericalZYX, q, v);
  forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);

  computeAllTerms(modelSphericalZYX, dataSphericalZYX, q, v);
  computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);

  BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataSphericalZYX.oMi[1]));
  BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataSphericalZYX.liMi[1]));
  BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataSphericalZYX.Ycrb[1].matrix()));

  BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataSphericalZYX.com[0]));
}

BOOST_AUTO_TEST_CASE(test_rnea)
{
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef SE3::Matrix3 Matrix3;

  Model model;
  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());

  addJointAndBody(model,JointModelSphericalZYX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);

  Data data(model);

  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);

  rnea(model, data, q, v, a);
  Vector3 tau_expected(0., -4.905, 0.);

  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-14));

  q = Eigen::VectorXd::Ones(model.nq);
  v = Eigen::VectorXd::Ones(model.nv);
  a = Eigen::VectorXd::Ones(model.nv);

  rnea(model, data, q, v, a);
  tau_expected << -0.53611600195085, -0.74621832606188, -0.38177329067604;

  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));

  q << 3, 2, 1;
  v = Eigen::VectorXd::Ones(model.nv);
  a = Eigen::VectorXd::Ones(model.nv);

  rnea(model, data, q, v, a);
  tau_expected << 0.73934458094049,  2.7804530848031, 0.50684940972146;

  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));
}

BOOST_AUTO_TEST_CASE(test_crba)
{
  using namespace pinocchio;
  using namespace std;
  typedef SE3::Vector3 Vector3;
  typedef SE3::Matrix3 Matrix3;

  Model model;
  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());

  addJointAndBody(model,JointModelSphericalZYX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);

  Data data(model);

  Eigen::VectorXd q(Eigen::VectorXd::Zero(model.nq));
  Eigen::MatrixXd M_expected(model.nv,model.nv);

  crba(model, data, q);
  M_expected <<
  1.25,    0,    0,
  0, 1.25,    0,
  0,    0,    1;

  BOOST_CHECK(M_expected.isApprox(data.M, 1e-14));

  q = Eigen::VectorXd::Ones(model.nq);

  crba(model, data, q);
  M_expected <<
  1.0729816454316, -5.5511151231258e-17,     -0.8414709848079,
  -5.5511151231258e-17,                 1.25,                    0,
  -0.8414709848079,                    0,                    1;

  BOOST_CHECK(M_expected.isApprox(data.M, 1e-12));

  q << 3, 2, 1;

  crba(model, data, q);
  M_expected <<
  1.043294547392, 2.7755575615629e-17,   -0.90929742682568,
  0,                1.25,                   0,
  -0.90929742682568,                   0,                  1;

  BOOST_CHECK(M_expected.isApprox(data.M, 1e-10));
}

BOOST_AUTO_TEST_SUITE_END()


#include<iostream>
#include<cmath>
#include<algorithm>
#include<vector>
#include<ctime>
#include <boost/random.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include<fstream>
#include<string>
#include <numeric>
using namespace std;
// Function to print the vector
void PrintData(vector<double>& data);
// Function to print the matrix
void PrintData(vector< vector<double> >& data);

// Function to do Cholesky decomposition
vector< vector<double> > Cholesky(vector< vector<double> >& data);

//Function to write matrix into csv file
void WriteToCsv(string CsvFile, vector< vector<double> >& data);

//Function to write vector into csv file
void WriteToCsv(string CsvFile, vector<double>& data);

// Function to calculate standard deviation
double CaclVariance(vector<double>& data);


#include <catch.hpp>
#include <Eigen/Core>
#include <random>

#include "check_adjoint.h"
#include "test_utils.h"
#include "renderer_blending.cuh"

template<kernel::BlendMode blendMode>
void testAdjointBlending()
{
	typedef empty TmpStorage_t;
	typedef VectorXr Vector_t;

	auto forward = [](const Vector_t& x, TmpStorage_t* tmp) -> Vector_t
	{
		const real4 acc = fromEigen4(x.segment(0, 4));
		const real4 current = fromEigen4(x.segment(4, 4));
		const real_t stepsize = x[8];

		const real4 output = kernel::Blending<blendMode>::blend(
			acc, current, stepsize);

		return toEigen(output);
	};
	auto adjoint = [](const Vector_t& x, const Vector_t& e, const Vector_t& g,
		Vector_t& z, const TmpStorage_t& tmp)
	{
		const real4 acc = fromEigen4(x.segment(0, 4));
		const real4 current = fromEigen4(x.segment(4, 4));
		const real_t stepsize = x[8];

		const real4 output = fromEigen4(e);
		const real4 adj_output = fromEigen4(g);

		real4 adj_acc, adj_current;
		real_t adj_stepsize;

		real4 acc_in = kernel::Blending<blendMode>::adjoint(
			output, current, stepsize, adj_output,
			adj_acc, adj_current, adj_stepsize);

		INFO("input-acc: " << acc);
		INFO("reconstructed-acc: " << acc_in);
		REQUIRE(acc.x == Approx(acc_in.x));
		REQUIRE(acc.y == Approx(acc_in.y));
		REQUIRE(acc.z == Approx(acc_in.z));
		REQUIRE(acc.w == Approx(acc_in.w));

		z.segment<4>(0) = toEigen(adj_acc);
		z.segment<4>(4) = toEigen(adj_current);
		z[8] = adj_stepsize;
	};

	std::default_random_engine rnd(42);
	std::uniform_real_distribution<real_t> distr(0.01, 0.99);
	int N = 20;
	for (int i = 0; i < N; ++i)
	{
		INFO("N=" << i);
		Vector_t x(9);
		for (int j = 0; j < 9; ++j) x[j] = distr(rnd);

		checkAdjoint<Vector_t, TmpStorage_t>(x, forward, adjoint,
			1e-5, 1e-5, 1e-6);
	}
}

TEST_CASE("Adjoint-Blending-BeerLambert", "[adjoint]")
{
	testAdjointBlending<kernel::BlendBeerLambert>();
}

TEST_CASE("Adjoint-Blending-Alpha", "[adjoint]")
{
	testAdjointBlending<kernel::BlendAlpha>();
}


//============================================================================
// Daniel J. Greenhoe
// normed linear space R^2
//=============================================================================
//=====================================
// headers
//=====================================
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <Eigen/Dense>  // https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325
#include "r1.h"
#include "r4.h"
typedef Eigen::Matrix< double, 4, 1 > Vector4d;

//=====================================
// oquad
//=====================================
//-----------------------------------------------------------------------------
// oquad constructors
//-----------------------------------------------------------------------------
oquad::oquad(void)
{
  xx.at(0) = 0.0;
  xx.at(1) = 0.0;
  xx.at(2) = 0.0;
  xx.at(3) = 0.0;
}

oquad::oquad(double u0, double u1, double u2, double u3)
{
  xx.at(0) = u0;
  xx.at(1) = u1;
  xx.at(2) = u2;
  xx.at(3) = u3;
}

oquad::oquad(double u)
{
  xx.at(0) = u;
  xx.at(1) = u;
  xx.at(2) = u;
  xx.at(3) = u;
}

//-----------------------------------------------------------------------------
//! \brief oquad put member functions
//-----------------------------------------------------------------------------
void oquad::put(double u)
{
  xx.at(0) = u;
  xx.at(1) = u;
  xx.at(2) = u;
  xx.at(3) = u;
}

void oquad::put(oquad u)
{
  xx.at(0) = u.get(0);
  xx.at(1) = u.get(1);
  xx.at(2) = u.get(2);
  xx.at(3) = u.get(3);
}

//-----------------------------------------------------------------------------
//! \brief Write values to oquad
//-----------------------------------------------------------------------------
void oquad::put(double u0, double u1, double u2, double u3)
{
  xx.at(0) = u0;
  xx.at(1) = u1;
  xx.at(2) = u2;
  xx.at(3) = u3;
}

//-----------------------------------------------------------------------------
//! \brief Write values to oquad
//-----------------------------------------------------------------------------
void oquad::put(int n,double u)
{
  xx.at(n) = u;
}

//-----------------------------------------------------------------------------
//! \brief return the 4-tuple value
//-----------------------------------------------------------------------------
oquad oquad::get(void)
{
  oquad u;
  u.put( 0, get1() );
  u.put( 1, get2() );
  u.put( 2, get3() );
  u.put( 3, get4() );
  return u;
}

//-----------------------------------------------------------------------------
//! \brief return the minimum element of the 4 tupple
//-----------------------------------------------------------------------------
double oquad::min(void) const
{
  const Eigen::Map< const Vector4d > a( getdata() );
  const double minVal = a.minCoeff();
  return minVal;
}

//-----------------------------------------------------------------------------
//! \brief return the maximum element of the 4 tupple
//-----------------------------------------------------------------------------
double oquad::max(void) const
{
  const Eigen::Map< const Vector4d > a( getdata() );
  const double maxVal = a.maxCoeff();
  return maxVal;
}

//-----------------------------------------------------------------------------
//! \brief print the tuple
//-----------------------------------------------------------------------------
void oquad::list(const char *str1, const char *str2) const
{
  if(strlen(str1)!=0)printf("%s",str1);
  putchar('(');
  printf("%9.6lf,", get1() );
  printf("%9.6lf,", get2() );
  printf("%9.6lf,", get3() );
  printf("%9.6lf)", get4() );
  if(strlen(str2)!=0)printf("%s",str2);
}

//=====================================
// vectR4 functions
//=====================================
//-----------------------------------------------------------------------------
//! \brief return the 4-tuple value
//-----------------------------------------------------------------------------
const vectR4 vectR4::get(void)
{
  vectR4 u;
  u.put( 0, get1() );
  u.put( 1, get2() );
  u.put( 2, get3() );
  u.put( 3, get4() );
  return u;
}

//-----------------------------------------------------------------------------
//! \brief magnitude
//-----------------------------------------------------------------------------
double vectR4::mag(void) const
{
  const Eigen::Map< const Vector4d > a( getdata() );
  return a.norm();
}

//-----------------------------------------------------------------------------
//! \brief Multiply the vector by a scalar a
//-----------------------------------------------------------------------------
vectR4 vectR4::mpy(const double a)
{
  vectR4 w;
  const Eigen::Map< const Vector4d > vv( getdata() );
  Eigen::Map< Vector4d > ww( w.getdataa() );
  ww = a * vv;
  return w;
}

//-----------------------------------------------------------------------------
//! \brief operator: +=
//-----------------------------------------------------------------------------
void vectR4::operator+=(vectR4 q)
{
  Eigen::Map< Vector4d > pp( getdataa() );
  const Eigen::Map< const Vector4d > qq( q.getdata() );
  pp = pp + qq;
}

//-----------------------------------------------------------------------------
//! \brief operator: -=
//-----------------------------------------------------------------------------
void vectR4::operator-=(vectR4 q)
{
  Eigen::Map< Vector4d > pp( getdataa() );
  const Eigen::Map< const Vector4d > qq( q.getdata() );
  pp = pp - qq;
}

//-----------------------------------------------------------------------------
//! \brief operator: -=
//-----------------------------------------------------------------------------
void vectR4::operator*=(double a)
{
  vectR4  p=get();
  p = a*p;
  put(p);
}

//-----------------------------------------------------------------------------
//! \brief operator: a*y
//-----------------------------------------------------------------------------
vectR4 operator*(const double a, const vectR4 x)
{
  vectR4 y;
  const Eigen::Map< const Vector4d > xx( x.getdata() );
  Eigen::Map< Vector4d > yy( y.getdataa() );
  yy = a * xx;
  return y;
}

//-----------------------------------------------------------------------------
//! \brief operator: dot product of p and q
//-----------------------------------------------------------------------------
double operator^(vectR4 p,vectR4 q)
{
  const  Eigen::Map< const Vector4d > pp( p.getdata() );
  const  Eigen::Map< const Vector4d > qq( q.getdata() );
  double innerProduct = pp.adjoint() * qq;
  return innerProduct;
}

//=====================================
//! \brief seqR4
//=====================================
//-----------------------------------------------------------------------------
//! \brief constructor initializing seqR1 to 0
//-----------------------------------------------------------------------------
seqR4::seqR4(long M)
{
  N=M;
  x = new vectR4[N];
  clear();
}

//-----------------------------------------------------------------------------
//! \brief constructor initializing seqR1 to <u>
//-----------------------------------------------------------------------------
seqR4::seqR4( long M, double u )
{
  N=M;
  x = new vectR4[N];
  fill( u );
}

//-----------------------------------------------------------------------------
//! \brief Fill the seqR3 with a value 0
//-----------------------------------------------------------------------------
void seqR4::clear(void)
{
  fill( 0.0 );
}

//-----------------------------------------------------------------------------
//! \brief fill the seqR4 with a value <u>
//-----------------------------------------------------------------------------
void seqR4::fill(double u)
{
  for(long n=0; n<N; n++) put( n, u );
}

//-----------------------------------------------------------------------------
//! \brief put a single value u=(u1,u2,u3,u4) into the seqR4 x at location n
//-----------------------------------------------------------------------------
int seqR4::put(long n, double u1,double u2,double u3,double u4)
{
  int retval=0;
  if(n<N)x[n].put(u1,u2,u3,u4);
  else{
    fprintf(stderr,"n=%ld larger than seqR1 size N=%ld\n",n,N);
    retval=-1;
    }
  return retval;
}

//-----------------------------------------------------------------------------
//! \brief put a single value (u,u,u,u) into the seqR4 x at location n
//-----------------------------------------------------------------------------
int seqR4::put(long n, vectR4 u)
{
  int retval=0;
  if(n<N)x[n].put(u);
  else{
    fprintf(stderr,"n=%ld larger than seqR4 size N=%ld\n",n,N);
    retval=-1;
    }
  return retval;
}

//-----------------------------------------------------------------------------
//! \brief list contents of sequence
//-----------------------------------------------------------------------------
void seqR4::list(const long start, const long end, const char* str1, const char *str2, FILE *fptr){
  long n,m;
  vectR4 p;
  if(strlen(str1)>0){
    printf("%s",str1);
    if(fptr!=NULL)fprintf(fptr,"%s",str1);
    }
  for(n=start,m=1; n<=end; n++,m++){
    p=x[n];
    printf("(%5.2lf,%5.2lf,%5.2lf,%5.2lf) ",p.get1(),p.get2(),p.get3(),p.get4());
    if(m%2==0)printf("\n");
    if(fptr!=NULL){
      fprintf(fptr,"(%5.2lf,%5.2lf,%5.2lf,%5.2lf) ",p.get1(),p.get2(),p.get3(),p.get4());
      if(m%2==0)fprintf(fptr,"\n");
      }
    }
  if(strlen(str2)>0){
    printf("%s",str2);
    if(fptr!=NULL)fprintf(fptr,"%s",str2);
    }
  }

//-----------------------------------------------------------------------------
//! \brief list contents of seqR1 using 1 digit per element
//-----------------------------------------------------------------------------
void seqR4::list1(const long start, const long end, const char *str1, const char *str2,FILE *fptr){
  long n,m;
  vectR4 p;
  if(strlen(str1)>0){
    printf("%s",str1);
    if(fptr!=NULL)fprintf(fptr,"%s",str1);
    }
  for(n=start,m=1; n<=end; n++,m++){
    p=x[n];
    printf(" %1.0lf%1.0lf%1.0lf%1.0lf",p.get1(),p.get2(),p.get3(),p.get4());
    if(fptr!=NULL)fprintf(fptr," %1.0lf%1.0lf%1.0lf%1.0lf",p.get1(),p.get2(),p.get3(),p.get4());
    if(m%10==0)printf("\n");
    if(fptr!=NULL)if(m%10==0)fprintf(fptr,"\n");
    }
  if(strlen(str2)>0){
    printf("%s",str2);
    if(fptr!=NULL)fprintf(fptr,"%s",str2);
    }
  }

/*=====================================
//! \brief external operations
 *=====================================*/
//-----------------------------------------------------------------------------
//! \brief operator: return p+q
//-----------------------------------------------------------------------------
vectR4 operator+(vectR4 p, vectR4 q){
  int i;
  vectR4 y;
  for(i=0;i<4;i++)y.put(i,p.get(i)+q.get(i));
  return y;
  }

//-----------------------------------------------------------------------------
//! \brief operator: return p-q
//-----------------------------------------------------------------------------
vectR4 operator-(vectR4 p, vectR4 q){
  int i;
  vectR4 y;
  for(i=0;i<4;i++)y.put(i,p.get(i)-q.get(i));
  return y;
  }

//-----------------------------------------------------------------------------
//! \brief operator: return -p
//-----------------------------------------------------------------------------
vectR4 operator-(vectR4 p){
  vectR4 q;
  int i;
  for(i=0;i<4;i++)q.put(i,-p.get(i));
  return q;
  }

//-----------------------------------------------------------------------------
//! \brief return the angle theta in radians between the two vectors induced by
//! \brief the points <p> and <q> in the space R^4.
//! \brief on SUCCESS return theta in the closed interval [0:PI]
//! \brief on ERROR   return negative value or exit with value EXIT_FAILURE
//-----------------------------------------------------------------------------
double pqtheta(const vectR4 p, const vectR4 q){
  const double rp = p.r();
  const double rq = q.r();
  double y,theta;
  if(rp==0) return -1;
  if(rq==0) return -2;
  y = (p^q)/(rp*rq);
  if(y>+1)  {fprintf(stderr,"\nERROR using pqtheta(vectR4 p, vectR4 q): (p^q)/(rp*rq)=%lf>+1\n",y); exit(EXIT_FAILURE);}
  if(y<-1)  {fprintf(stderr,"\nERROR using pqtheta(vectR4 p, vectR4 q): (p^q)/(rp*rq)=%lf<-1\n",y); exit(EXIT_FAILURE);}
  theta = acos(y);
  return theta;
  }

/*=====================================
//! \brief external operations
 *=====================================*/
//-----------------------------------------------------------------------------
//! \brief compute magnitude of R^1 sequence
//-----------------------------------------------------------------------------
//int mag(seqR4 *xR4, seqR1 *ymag){
//  const long Nx=xR4->getN();
//  const long Ny=ymag->getN();
//  long n;
//  int retval=0;
//  vectR4 u;
//  ymag->clear();
//  if(Nx!=Ny){
//    fprintf(stderr,"ERROR using y=mag(xR4): lengths of xR4 (%ld) and ymag (%ld) differ.\n",Nx,Ny);
//    exit(EXIT_FAILURE);
//    }
//  for(n=0;n<Nx;n++){
//    u=xR4->get(n);
//    ymag->put(n,u.mag());
//    }
//  return retval;
//  }


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

#define BOOST_TEST_MAIN

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

#include "Tudat/Mathematics/BasicMathematics/leastSquaresEstimation.h"
#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/SimulationSetup/tudatSimulationHeader.h"
#include "Tudat/Mathematics/Statistics/basicStatistics.h"
namespace tudat
{

namespace unit_tests
{

using namespace tudat::simulation_setup;
using namespace tudat::propagators;
using namespace tudat::numerical_integrators;
using namespace tudat::orbital_element_conversions;
using namespace tudat::basic_mathematics;
using namespace tudat::unit_conversions;

double computeLenseThirringPericenterPrecession(
        const double gravitationalParameter,
        const double angularMomentum,
        const double semiMajorAxis,
        const double eccentricity,
        const double inclination )
{
    return - 6.0 * gravitationalParameter * angularMomentum * std::cos(inclination  ) /
            ( physical_constants::SPEED_OF_LIGHT * physical_constants::SPEED_OF_LIGHT * semiMajorAxis * semiMajorAxis * semiMajorAxis *
              std::pow( 1.0 - eccentricity * eccentricity, 1.5 ) );
}

double computeLenseThirringNodePrecession(
        const double gravitationalParameter,
        const double angularMomentum,
        const double semiMajorAxis,
        const double eccentricity )
{
    return 2.0 * gravitationalParameter * angularMomentum /
            ( physical_constants::SPEED_OF_LIGHT * physical_constants::SPEED_OF_LIGHT * semiMajorAxis * semiMajorAxis * semiMajorAxis *
              std::pow( 1.0 - eccentricity * eccentricity, 1.5 ) );
}

double computeSchwarzschildPericenterPrecession(
        const double gravitationalParameter,
        const double semiMajorAxis,
        const double eccentricity )
{
    return 3.0 * std::pow( gravitationalParameter, 1.5 ) /
            ( physical_constants::SPEED_OF_LIGHT * physical_constants::SPEED_OF_LIGHT * std::pow(
                  semiMajorAxis, 2.5 ) *( 1.0 - eccentricity * eccentricity ) );
}

double computeDeSitterPericenterPrecession( const double meanDistanceEarthToSun,
                                            const double meanEccentricity )
{
    return 1.5 * 1.327124E20 / ( physical_constants::SPEED_OF_LIGHT * physical_constants::SPEED_OF_LIGHT * meanDistanceEarthToSun ) * 2.0 * mathematical_constants::PI /
            ( physical_constants::JULIAN_YEAR ) * std::sqrt( 1.0 - meanEccentricity * meanEccentricity );
}

BOOST_AUTO_TEST_SUITE( test_relativistic_acceleration_corrections )

void testControlPropagation(
        Eigen::Vector6d asterixInitialStateInKeplerianElements,
        std::vector< std::map< double, double > > elementMaps,
        double earthGravitationalParameter )
{
    std::vector< double > polynomialPowers = { 0, 1 };
    for( unsigned elementIndex = 0; elementIndex < 5; elementIndex++ )
    {
        std::vector< double > fitOutput = linear_algebra::getLeastSquaresPolynomialFit(
                    elementMaps[ elementIndex ], polynomialPowers );
        BOOST_CHECK_CLOSE_FRACTION( asterixInitialStateInKeplerianElements( elementIndex ), fitOutput.at( 0 ), 1.0E-10 );
        if( elementIndex == 1 )
        {
            BOOST_CHECK_SMALL( fitOutput.at( 1 ), 1.0E-18 );
        }
        else
        {
            BOOST_CHECK_SMALL( fitOutput.at( 1 ), 1.0E-12 );
        }
    }
}

void testLenseThirringPropagation(
        Eigen::Vector6d asterixInitialStateInKeplerianElements,
        std::vector< std::map< double, double > > elementMaps,
        double earthGravitationalParameter )
{
    double theoreticalLenseThirringPericenterPrecession =
            computeLenseThirringPericenterPrecession(
                earthGravitationalParameter, 1.0E9,  asterixInitialStateInKeplerianElements( semiMajorAxisIndex ),
                asterixInitialStateInKeplerianElements( eccentricityIndex ),
                asterixInitialStateInKeplerianElements( inclinationIndex ) );
    double theoreticalLenseThirringNodePrecession =
            computeLenseThirringNodePrecession(
                earthGravitationalParameter, 1.0E9,  asterixInitialStateInKeplerianElements( semiMajorAxisIndex ),
                asterixInitialStateInKeplerianElements( eccentricityIndex ) );

    std::vector< double > polynomialPowers = { 0, 1 };
    for( unsigned elementIndex = 0; elementIndex < 5; elementIndex++ )
    {
        std::vector< double > fitOutput = linear_algebra::getLeastSquaresPolynomialFit(
                    elementMaps[ elementIndex ], polynomialPowers );
        BOOST_CHECK_CLOSE_FRACTION( asterixInitialStateInKeplerianElements( elementIndex ), fitOutput.at( 0 ), 1.0E-10 );
        if( elementIndex == 1 )
        {
            BOOST_CHECK_SMALL( fitOutput.at( 1 ), 1.0E-18 );
        }
        else if( elementIndex == 3 )
        {
            BOOST_CHECK_CLOSE_FRACTION( fitOutput.at( 1 ), theoreticalLenseThirringPericenterPrecession, 1.0E-5 );
        }
        else if( elementIndex == 4 )
        {
            BOOST_CHECK_CLOSE_FRACTION( fitOutput.at( 1 ), theoreticalLenseThirringNodePrecession, 1.0E-5 );
        }
        else
        {
            BOOST_CHECK_SMALL( fitOutput.at( 1 ), 1.0E-12 );
        }
    }
}

void testSchwarzschildPropagation(
        Eigen::Vector6d asterixInitialStateInKeplerianElements,
        std::vector< std::map< double, double > > elementMaps,
        double earthGravitationalParameter )
{
    double theoreticalSchwarzschildPericenterPrecession =
            computeSchwarzschildPericenterPrecession(
                earthGravitationalParameter, asterixInitialStateInKeplerianElements( semiMajorAxisIndex ),
                asterixInitialStateInKeplerianElements( eccentricityIndex ) );

    std::vector< double > polynomialPowers = { 0, 1 };
    for( unsigned elementIndex = 0; elementIndex < 5; elementIndex++ )
    {
        std::vector< double > fitOutput = linear_algebra::getLeastSquaresPolynomialFit(
                    elementMaps[ elementIndex ], polynomialPowers );
        if( elementIndex != 1 )
        {
            BOOST_CHECK_CLOSE_FRACTION( asterixInitialStateInKeplerianElements( elementIndex ), fitOutput.at( 0 ), 1.0E-8 );
        }
        else
        {
            BOOST_CHECK_CLOSE_FRACTION( asterixInitialStateInKeplerianElements( elementIndex ), fitOutput.at( 0 ), 1.0E-7 );
        }
        if( elementIndex == 1 )
        {
            BOOST_CHECK_SMALL( fitOutput.at( 1 ), 1.0E-16 );
        }
        else if( elementIndex == 3 )
        {
            BOOST_CHECK_CLOSE_FRACTION( fitOutput.at( 1 ), theoreticalSchwarzschildPericenterPrecession, 1.0E-5 );
        }
        else
        {
            BOOST_CHECK_SMALL( fitOutput.at( 1 ), 1.0E-10 );
        }
    }
}

void testDeSitterPropagation(
        Eigen::Vector6d asterixInitialStateInKeplerianElements,
        std::vector< std::map< double, double > > elementMaps,
        double meanDistanceEarthToSun,
        double meanEarthEccentricity )
{
    std::vector< double > polynomialPowers = { 0, 1 };
    for( unsigned elementIndex = 0; elementIndex < 5; elementIndex++ )
    {
        std::vector< double > fitOutput = linear_algebra::getLeastSquaresPolynomialFit(
                    elementMaps[ elementIndex ], polynomialPowers );
        if( elementIndex != 4 )
        {
            BOOST_CHECK_CLOSE_FRACTION( asterixInitialStateInKeplerianElements( elementIndex ), fitOutput.at( 0 ), 1.0E-10 );
        }
        else
        {
            BOOST_CHECK_CLOSE_FRACTION( asterixInitialStateInKeplerianElements( elementIndex ), fitOutput.at( 0 ), 1.0E-8 );
        }
        if( elementIndex == 1 )
        {
            BOOST_CHECK_SMALL( fitOutput.at( 1 ), 1.0E-18 );
        }
        else if( elementIndex == 4 )
        {
           BOOST_CHECK_CLOSE_FRACTION( fitOutput.at( 1 ), computeDeSitterPericenterPrecession(
                                           meanDistanceEarthToSun, meanEarthEccentricity  ), 2.5E-2 );
        }
        else
        {
            BOOST_CHECK_SMALL( fitOutput.at( 1 ), 1.0E-12 );
        }
    }
}
BOOST_AUTO_TEST_CASE( testLenseThirring )
{
    // Load Spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Set simulation end epoch.
    const double simulationEndEpoch = 0.25 * tudat::physical_constants::JULIAN_YEAR;

    // Create body objects.
    std::vector< std::string > bodiesToCreate;
    bodiesToCreate.push_back( "Earth" );
    bodiesToCreate.push_back( "Sun" );

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

    // Create Earth object
    NamedBodyMap bodyMap = createBodies( bodySettings );

    // Create spacecraft object.
    bodyMap[ "Asterix" ] = std::make_shared< simulation_setup::Body >( );

    // Finalize body creation.
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////            CREATE ACCELERATIONS          //////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    for( unsigned int testCase = 0; testCase < 4; testCase++ )
    {
        // Define propagator settings variables.
        SelectedAccelerationMap accelerationMap;
        std::vector< std::string > bodiesToPropagate;
        std::vector< std::string > centralBodies;

        bodiesToPropagate.push_back( "Asterix" );
        centralBodies.push_back( "Earth" );

        // Define propagation settings.
        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfAsterix;
        accelerationsOfAsterix[ "Earth" ].push_back( std::make_shared< AccelerationSettings >(
                                                         basic_astrodynamics::central_gravity ) );
        if( testCase == 1 )
        {
            accelerationsOfAsterix[ "Earth" ].push_back( std::make_shared< RelativisticAccelerationCorrectionSettings >(
                                                             false, true, false, "", 1.0E9 * Eigen::Vector3d::UnitZ( ) ) );
        }
        if( testCase == 2 )
        {
            accelerationsOfAsterix[ "Earth" ].push_back( std::make_shared< RelativisticAccelerationCorrectionSettings >(
                                                             true, false, false ) );
        }
        if( testCase == 3 )
        {
            accelerationsOfAsterix[ "Earth" ].push_back( std::make_shared< RelativisticAccelerationCorrectionSettings >(
                                                             false, false, true, "Sun" ) );
        }
        accelerationMap[ "Asterix" ] = accelerationsOfAsterix;

        // Create acceleration models and propagation settings.
        basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                    bodyMap, accelerationMap, bodiesToPropagate, centralBodies );

        ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ///////////////////////             CREATE PROPAGATION SETTINGS            ////////////////////////////////////////////
        ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        // Set initial conditions for the Asterix satellite that will be propagated in this simulation.
        // The initial conditions are given in Keplerian elements and later on converted to Cartesian
        // elements.

        // Set Keplerian elements for Asterix.
        Eigen::Vector6d asterixInitialStateInKeplerianElements;
        asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 5000.0E3;
        asterixInitialStateInKeplerianElements( eccentricityIndex ) = 0.2;
        asterixInitialStateInKeplerianElements( inclinationIndex ) = convertDegreesToRadians( 65.3 );
        asterixInitialStateInKeplerianElements( argumentOfPeriapsisIndex )
                = convertDegreesToRadians( 235.7 );
        asterixInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex )
                = convertDegreesToRadians( 23.4 );
        asterixInitialStateInKeplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 0.0 );

        // Convert Asterix state from Keplerian elements to Cartesian elements.
        double earthGravitationalParameter = bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
        Eigen::VectorXd systemInitialState = convertKeplerianToCartesianElements(
                    asterixInitialStateInKeplerianElements,
                    earthGravitationalParameter );

        std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
                std::make_shared< TranslationalStatePropagatorSettings< double > >
                ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, simulationEndEpoch, encke );

        // Create numerical integrator.
        std::shared_ptr< IntegratorSettings< > > integratorSettings =
                std::make_shared< RungeKuttaVariableStepSizeSettings< > >
                ( 0.0, 10.0,
                  RungeKuttaCoefficients::rungeKuttaFehlberg78, 1.0E-3, 1.0E3, 1.0E-12, 1.0E-12 );

        ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ///////////////////////             PROPAGATE ORBIT            ////////////////////////////////////////////////////////
        ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        // Create simulation object and propagate dynamics.
        SingleArcDynamicsSimulator< > dynamicsSimulator(
                    bodyMap, integratorSettings, propagatorSettings );
        std::map< double, Eigen::VectorXd > integrationResult = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );
        std::map< double, Eigen::VectorXd > keplerianIntegrationResult;

        // Compute map of Kepler elements
        Eigen::Vector6d currentCartesianState;
        std::vector< std::map< double, double > > elementMaps;
        elementMaps.resize( 6 );

        std::vector< double > solarDistances;
        std::vector< double > earthSemiMajorAxes;
        std::vector< double > earthEccentricities;

        Eigen::Vector6d earthKeplerianState;
        Eigen::Vector6d earthCartesianState;

        for( std::map< double, Eigen::VectorXd >::const_iterator stateIterator = integrationResult.begin( );
             stateIterator != integrationResult.end( ); stateIterator++ )
        {
            // Retrieve current Cartesian state (convert to Moon-centered frame if needed)
            currentCartesianState = stateIterator->second;
            keplerianIntegrationResult[ stateIterator->first ] =
                    convertCartesianToKeplerianElements(
                        currentCartesianState, earthGravitationalParameter );
            for( unsigned elementIndex = 0; elementIndex < 6; elementIndex++ )
            {
                elementMaps[ elementIndex ][ stateIterator->first ] = keplerianIntegrationResult[ stateIterator->first ]( elementIndex );
            }

            if( testCase == 3 )
            {
                earthCartesianState = spice_interface:: getBodyCartesianStateAtEpoch(
                            "Earth", "Sun", "ECLIPJ2000", "None", stateIterator->first );
                earthKeplerianState  = convertCartesianToKeplerianElements(
                            earthCartesianState, spice_interface::getBodyGravitationalParameter( "Sun" ) );

                earthSemiMajorAxes.push_back( earthKeplerianState( 0 ) );
                earthEccentricities.push_back( earthKeplerianState( 1 ) );

                solarDistances.push_back( earthCartesianState.segment( 0, 3 ).norm( ) );
            }
        }

        if( testCase == 0 )
        {
            testControlPropagation( asterixInitialStateInKeplerianElements, elementMaps, earthGravitationalParameter );
        }
        else if( testCase == 1 )
        {
            testLenseThirringPropagation( asterixInitialStateInKeplerianElements, elementMaps, earthGravitationalParameter );
        }
        else if( testCase == 2 )
        {
            testSchwarzschildPropagation( asterixInitialStateInKeplerianElements, elementMaps, earthGravitationalParameter );
        }
        else if( testCase == 3 )
        {
            testDeSitterPropagation(
                        asterixInitialStateInKeplerianElements, elementMaps, statistics::computeSampleMean( solarDistances ),
                        statistics::computeSampleMean( earthEccentricities ) );
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


// $Id$
//
//  Copyright (C) 2003-2009 Greg Landrum and Rational Discovery LLC
//
//   @@ All Rights Reserved @@
//  This file is part of the RDKit.
//  The contents are covered by the terms of the BSD license
//  which is included in the file license.txt, found at the root
//  of the RDKit source tree.
//
#include "MolOps.h"
#include "RDKitBase.h"
#include <RDGeneral/Invariant.h>
#include <RDGeneral/RDLog.h>

#include <boost/dynamic_bitset.hpp>
#include <iomanip>

namespace RDKit {
namespace MolOps {
double computeBalabanJ(double *distMat, int nb, int nAts) {
  // NOTE that the distance matrix is modified here for the sake of
  // efficiency
  PRECONDITION(distMat, "bogus distance matrix")
  double sum = 0.0;
  int nActive = nAts;
  int mu = nb - nActive + 1;

  if (mu == -1) return 0.0;

  for (int i = 0; i < nAts; i++) {
    int iTab = i * nAts;
    sum = 0.0;
    for (int j = 0; j < nAts; j++) {
      if (j != i) {
        sum += distMat[iTab + j];
      }
    }
    distMat[iTab + i] *= sum;
  }
  double accum = 0.0;
  for (int i = 0; i < nAts; i++) {
    int iTab = i * nAts + i;
    for (int j = i + 1; j < nAts; j++) {
      // NOTE: this isn't strictly the Balaban J value, because we
      // aren't only adding in adjacent atoms.  Since we're doing a
      // discriminator, that shouldn't be a problem.
      if (j != i) {
        accum += (1.0 / sqrt(distMat[iTab] * distMat[j * nAts + j]));
      }
    }
  }
  return nActive / ((mu + 1) * accum);
}

double computeBalabanJ(const ROMol &mol, bool useBO, bool force,
                       const std::vector<int> *bondPath, bool cacheIt) {
  RDUNUSED_PARAM(useBO);
  double res = 0.0;
  if (!force && mol.hasProp(common_properties::BalabanJ)) {
    mol.getProp(common_properties::BalabanJ, res);
  } else {
    double *dMat;
    int nb = 0, nAts = 0;
    if (bondPath) {
      boost::dynamic_bitset<> atomsUsed(mol.getNumAtoms());
      boost::dynamic_bitset<> bondsUsed(mol.getNumBonds());
      for (int ci : *bondPath) {
        bondsUsed[ci] = 1;
      }
      std::vector<const Bond *> bonds;
      bonds.reserve(bondPath->size());
      std::vector<int> atomsInPath;
      atomsInPath.reserve(bondPath->size() + 1);

      ROMol::EDGE_ITER beg, end;
      boost::tie(beg, end) = mol.getEdges();
      while (beg != end) {
        const Bond *bond = mol[*beg];
        if (bondsUsed[bond->getIdx()]) {
          int begIdx = bond->getBeginAtomIdx();
          int endIdx = bond->getEndAtomIdx();
          bonds.push_back(bond);
          if (!atomsUsed[begIdx]) {
            atomsInPath.push_back(begIdx);
            atomsUsed[begIdx] = 1;
          }
          if (!atomsUsed[endIdx]) {
            atomsInPath.push_back(endIdx);
            atomsUsed[endIdx] = 1;
          }
        }
        beg++;
      }
      nb = rdcast<int>(bondPath->size());
      nAts = rdcast<int>(atomsInPath.size());
      dMat = MolOps::getDistanceMat(mol, atomsInPath, bonds, true, true);
      res = computeBalabanJ(dMat, nb, nAts);
      delete[] dMat;
    } else {
      nb = mol.getNumBonds();
      nAts = mol.getNumAtoms();
      dMat = MolOps::getDistanceMat(mol, true, true, true, nullptr);
      res = computeBalabanJ(dMat, nb, nAts);
      delete[] dMat;
    }

    if (cacheIt) mol.setProp(common_properties::BalabanJ, res, true);
  }
  return res;
}
}  // end of namespace MolOps
}  // end of namespace RDKit


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

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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SIGNIFICANTS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SIGNIFICANTS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-exponential
    Function object implementing significants capabilities

    Compute the rounding to n significants digits

    @par Semantic:

    For every parameter of floating type T and strictly positive integer n

    @code
    T r = significants(x, n);
    @endcode

    is equivalent to round(x, m) where m is n-iceil(log10(abs(x)))

    @see round,  iceil, log10

  **/
  const boost::dispatch::functor<tag::significants_> significants = {};
} }
#endif

#include <boost/simd/function/scalar/significants.hpp>
#include <boost/simd/function/simd/significants.hpp>

#endif


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifdef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
#error The vector_as_graph.hpp header requires partial specialization
#endif

#include <vector>
#include <list>
#include <iostream> // needed by graph_utility. -Jeremy
#include <boost/graph/vector_as_graph.hpp>
#include <boost/graph/graph_utility.hpp>

int
main()
{
  enum
  { r, s, t, u, v, w, x, y, N };
  char name[] = "rstuvwxy";
  typedef std::vector < std::list < int > > Graph;
  Graph g(N);
  g[r].push_back(v);
  g[s].push_back(r);
  g[s].push_back(r);
  g[s].push_back(w);
  g[t].push_back(x);
  g[u].push_back(t);
  g[w].push_back(t);
  g[w].push_back(x);
  g[x].push_back(y);
  g[y].push_back(u);
  boost::print_graph(g, name);
  return 0;
}


#include "smooth_pair_finder.h"

#include <NTL/RR.h>

#include "norm_finding.h"
#include "util.h"

namespace {
    using namespace NTL;
}

namespace gnfs {
    SmoothPairFinder::SmoothPairFinder(const ZZ& n, long B, const ZZX& f, const ZZ& m)
      : n_(n), B_(B), f_(f), m_(m), M_(B), Mold_(0), a_(B), b_(-1) {
        ZZX fprime = diff(f);

        // Figure out what the factorbase columns represent.
        // Each one is a pair (p,r), where p is a prime up to B
        // and r is something where f(r) \equiv 0 (mod p).
        PrimeSeq pseq;
        long p;
        while ((p = pseq.next()) <= B) {
            cols_modular_.push_back(p);
            for (long r = 0; r < p; r++) {
                ZZ res = eval(f, ZZ(r));
                if (res % p == 0)
                    cols_algebraic_.emplace_back(p, r);
            }
        }

        // Generate pairs (q,s) for Adleman columns.
        // For each q (named p here), find an s where
        // f(s) \equiv 0 (mod p) and f'(s) \not\equiv 0 (mod p).
        long k = conv<long>(3 * log(conv<RR>(n)));
        long k_so_far = 0;
        while (k_so_far < k) {
            for (long s = 0; s < p; s++) {
                ZZ zs(s);
                if (eval(f, zs) % p == 0 && eval(fprime, zs) % p != 0) {
                    cols_adleman_.emplace_back(p, s);
                    k_so_far++;
                }
            }

            p = pseq.next();
        }
    }

    bool SmoothPairFinder::add_cols_modular(long a, long b, vec_GF2& ret) {
        ZZ ammb = a - m_ * b;
        ammb %= n_;

        if (ammb < 0) {
            ammb *= -1;
            ret.append(GF2(1));
        } else {
            ret.append(GF2(0));
        }

        for (const auto& p : cols_modular_) {
            bool divcount = false;
            while (ammb % p == 0) {
                ammb /= p;
                divcount ^= true;
            }
            ret.append(divcount ? GF2(1) : GF2(0));
        }

        return ammb == 1;
    }

    bool SmoothPairFinder::add_cols_algebraic(long a, long b, vec_GF2& ret) {
        ZZ namab = eval_norm(f_, ZZ(a), ZZ(b));

        // Iterate through each prime.
        // For each one, find r first,
        // then set the appropriate entry in the index vector.

        // Here, we'll keep a cursor into cols_algebraic_.
        auto alg_it = cols_algebraic_.cbegin();

        for (const auto& p : cols_modular_) {
            long r = lincon(a, b, p);

            // Get the divcount as before
            bool divcount = false;
            while (namab % p == 0) {
                namab /= p;
                divcount ^= true;
            }
            GF2 to_add = divcount ? GF2(1) : GF2(0);

            // Scroll past algebraic factorbase entries
            // until we find where we want to add this item.
            // Then add it.
            while (alg_it->first != p || alg_it->second != r) {
                ++alg_it;
                ret.append(GF2(0));
            }
            ++alg_it;
            ret.append(to_add);
        }

        // Add the rest of the zeros to the vector that we return.
        while (alg_it != cols_algebraic_.cend()) {
            ++alg_it;
            ret.append(GF2(0));
        }

        return namab == 1;
    }

    bool SmoothPairFinder::add_cols_adleman(long a, long b, NTL::vec_GF2& ret) {
        for (const auto& qs : cols_adleman_) {
            long q = qs.first;
            long s = qs.second;
            long jacobi_top = a - b * s;
            long jacobi = Jacobi(ZZ(jacobi_top % q), ZZ(q));
            ret.append(jacobi == -1 ? GF2(1) : GF2(0));
        }

        return true;
    }

    vec_GF2 SmoothPairFinder::generate_row(long a, long b) {
        vec_GF2 ret;  // if this length != num_cols, invalid
        // we always return ret so we can have the return value optimzn
        // even if ret ends up being invalid

        // Modular factorbase and -1.
        if (!add_cols_modular(a, b, ret)) {
            // ammb is not B-smooth
            // make SURE the length is invalid
            if (ret.length() == num_cols()) ret.append(GF2(0));
            return ret;
        }

        // Algebraic factorbase.
        if (!add_cols_algebraic(a, b, ret)) {
            // namab is not B-smooth
            // make SURE the length is invalid
            if (ret.length() == num_cols()) ret.append(GF2(0));
            return ret;
        }

        // Adleman columns.
        if (!add_cols_adleman(a, b, ret)) {
            // result is wrong
            // make SURE the length is invalid
            if (ret.length() == num_cols()) ret.append(GF2(0));
            return ret;
        }

        return ret;
    }

    std::pair<std::pair<long, long>, vec_GF2> SmoothPairFinder::get() {
        // TODO
        return std::make_pair(std::make_pair(0, 0), vec_GF2(INIT_SIZE, num_cols()));
    }
}


#include "stdafx.h"
#include "Math.h"
#include <boost/math/constants/constants.hpp>

namespace
{
constexpr double PI = boost::math::constants::pi<double>();
}

double DegreesToRadians(double degrees)
{
	return degrees * PI / 180.0;
}

double RadiansToDegrees(double radians)
{
	return radians * 180.0 / PI;
}


 // graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// 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
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include <boost/math/special_functions/zeta.hpp>
#include <boost/math/special_functions/gamma.hpp>

#include "graph_tool.hh"
#include "hash_map_wrap.hh"
#include "int_part.hh"
#include "util.hh"

double spence(double);

namespace graph_tool
{

using namespace std;

boost::multi_array<double, 2> __q_cache;

double log_sum(double a, double b)
{
    return std::max(a, b) + std::log1p(exp(-abs(a-b)));
}

void init_q_cache(size_t n_max)
{
    size_t old_n = __q_cache.shape()[0];
    if (old_n >= n_max)
        return;

    __q_cache.resize(boost::extents[n_max + 1][n_max + 1]);
    std::fill(__q_cache.data(), __q_cache.data() + __q_cache.num_elements(),
              -std::numeric_limits<double>::infinity());

    for (size_t n = 1; n <= n_max; ++n)
    {
        __q_cache[n][1] = 0;
        for (size_t k = 2; k <= n; ++k)
        {
            __q_cache[n][k] = log_sum(__q_cache[n][k], __q_cache[n][k - 1]);
            if (n > k)
                __q_cache[n][k] = log_sum(__q_cache[n][k], __q_cache[n - k][k]);
        }
    }
}

double q_rec(int n, int k)
{
    if (n <= 0 || k < 1)
        return 0;
    if (k > n)
        k = n;
    if (k == 1)
        return 1;
    return q_rec(n, k - 1) + q_rec(n - k, k);
}

gt_hash_map<pair<int, int>, double> __q_memo;

double q_rec_memo(int n, int k)
{
    if (k > n || n <= 0 || k < 1)
        return 0;
    if (k > n)
        k = n;
    if (k == 1)
        return 1;
    auto key = make_pair(n, k);
    auto iter = __q_memo.find(key);
    if (iter != __q_memo.end())
        return iter->second;
    auto res = q_rec_memo(n, k - 1) + q_rec_memo(n - k, k);
    __q_memo[key] = res;
    return res;
}

double log_q_approx_big(size_t n, size_t k)
{
    double C = M_PI * sqrt(2/3.);
    double S = C * sqrt(n) - log(4 * sqrt(3) * n);
    if (k < n)
    {
        double x = k / sqrt(n) - log(n) / C;
        S -= (2 / C) * exp(-C * x / 2);
    }
    return S;
}

double log_q_approx_small(size_t n, size_t k)
{
    return lbinom_fast(n - 1, k - 1) - lgamma_fast(k + 1);
}

double get_v(double u, double epsilon=1e-8)
{
    double v = u;
    double delta = 1;
    while (delta > epsilon)
    {
        // spence(exp(v)) = -spence(exp(-v)) - (v*v)/2
        double n_v = u * sqrt(spence(exp(-v)));
        delta = abs(n_v - v);
        v = n_v;
    }
    return v;
}

double log_q_approx(size_t n, size_t k)
{
    if (k < pow(n, 1/4.))
        return log_q_approx_small(n, k);
    double u = k / sqrt(n);
    double v = get_v(u);
    double lf = log(v) - log1p(- exp(-v) * (1 + u * u/2)) / 2 - log(2) * 3 / 2.
        - log(u) - log(M_PI);
    double g = 2 * v / u - u * log1p(-exp(-v));
    return lf - log(n) + sqrt(n) * g;
}

} // namespace graph_tool