/**
 * @file   CamPose.hpp
 * @brief  Header of CamPose class for camera pose representation.
 * @author Charlie Li
 * @date   2019.08.23
 */

#ifndef CAMPOSE_HPP
#define CAMPOSE_HPP

#include <iostream>

#include <opencv2/core.hpp>
#include <Eigen/Core>

namespace SLAM_demo {

/**
 * @class CamPose
 * @brief Store camera pose of each frame.
 */
class CamPose {
public: // public members
    /// Store default pose and its inverse \f$[I|0]\f$.
    CamPose();
    /**
     * @brief Construct pose-related data using the camera pose itself.
     * @param[in] Tcw \f$3 \times 4\f$ camera pose \f$[R_{cw}|t_{cw}]\f$,
     */
    CamPose(const cv::Mat& Tcw);
    /**
     * @brief Construct pose-related data using camera rotation and
     *        translation matrices.
     * @param[in] Rcw \f$3 \times 3\f$ camera rotation matrix.
     * @param[in] tcw \f$3 \times 1\f$ camera translation matrix.
     */
    CamPose(const cv::Mat& Rcw, const cv::Mat& tcw);
    /// Copy constructor.
    CamPose(const CamPose& pose);
    /// Copy-assignment operator.
    CamPose& operator=(const CamPose& pose);
    /// Default destructor.
    ~CamPose() = default;
    /// Set camera pose \f$[R|t]\f$ of current frame using an input pose.
    void setPose(const cv::Mat& Tcw);
    /**
     * @brief Set camera pose \f$[R|t]\f$ of current frame using input
     *        rotation and translation matrices.
     */
    void setPose(const cv::Mat& Rcw, const cv::Mat& tcw);
    /**
     * @name Getters for Pose-related Data
     * @brief A group of getters for retrieving pose-related data.
     * @return Matrix representation of pose-related data of type cv::Mat.
     */
    ///@{
    CamPose getCamPoseInv() const { return CamPose(getPoseInv()); }
    cv::Mat getPose() const { return mTcw.clone(); }
    cv::Mat getRotation() const { return mTcw.colRange(0, 3).clone(); }
    cv::Mat getRotationAngleAxis() const;
    cv::Mat getTranslation() const {
        return mTcw.rowRange(0, 3).col(3).clone();
    }
    /**
     * @brief Get \f$3 \times 3\f$ skew-symmetric matrix \f$[t]_x\f$ based on
     *        \f$3 \times 1\f$ translation vector \f$t = (t_1, t_2, t_3)^T\f$.
     *
     * \f[ [t]_x = \begin{bmatrix}
     *               0 & -t_3 & t_2 \\ t_3 & 0 & -t_1 \\ -t_2 & t_1 & 0
     *             \end{bmatrix}
     * \f]
     */
    cv::Mat getTranslationSS() const;
    cv::Mat getPoseInv() const { return mTwc.clone(); }
    cv::Mat getRotationInv() const {
        return mTwc.rowRange(0, 3).colRange(0, 3).clone();
    }
    //cv::Mat getRotationInvAngleAxis() const;
    /// Get \f$t_{wc}\f$, which is the camera origin in world coordinate system.
    cv::Mat getCamOrigin() const { return mTwc.rowRange(0, 3).col(3).clone(); }
    ///@}
    /**
     * @brief  Get Euler angle representation of rotation matrix \f$R_{cw}\f$
     *         as \f$3 \times 1\f$ vector \f$(yaw, pitch, roll)^T\f$.
     *         Unit: degree.
     */
    Eigen::Matrix<float, 3, 1> getREulerAngleEigen() const;
    /**
     * @brief  Get quaternion representation of rotation matrix \f$R_{cw}\f$
     *         as \f$4 \times 1\f$ vector \f$(qw, qx, qy, qz)^T\f$.
     */
    Eigen::Quaternion<float> getRQuatEigen() const;
    /// Get Quaternion representation of R^T.
    Eigen::Quaternion<float> getRInvQuatEigen() const;
    /// Pose multiplication & assignment.
    CamPose& operator*=(const CamPose& rhs);
    /// Pose multiplication.
    const CamPose operator*(const CamPose& rhs) const;
private: // private data
    /**
     * @brief \f$3 \times 4\f$ camera pose \f$[R_{cw}|t_{cw}]\f$, i.e., the
     *        transformation from world to camera coordinate system.
     */
    cv::Mat mTcw;
    /**
     * @brief \f$3 \times 4\f$ transformation matrix \f$[R_{wc}|t_{wc}]\f$
     *        from camera to world coordinate system.
     *        \f$ T_{cw, 4 \times 4} = T_{wc, 4 \times 4}^{-1}\f$.
     */
    cv::Mat mTwc;
private: // private members
    /**
     * @brief Update rotation matrix within \f$T_{cw}\f$, and at the same
     *        time update corresponding inverse transformation \f$T_{wc}\f$.
     */
    void setRotation(const cv::Mat& Rcw);
    /**
     * @brief Update translation matrix within \f$T_{cw}\f$, and at the same
     *        time update corresponding inverse transformation \f$T_{wc}\f$.
     */
    void setTranslation(const cv::Mat& tcw);
    /// Set inverse pose \f$T_{wc} = [R_{cw}^T | -R_{cw}^T t_{cw}]\f$.
    void setPoseInv();
};

/// Display pose info.
std::ostream& operator<<(std::ostream& os, const CamPose& pose);

} // namespace SLAM_demo

#endif // CAMPOSE_HPP


#include "Polyline.h"
#include "Vertex.h"
#include <boost/math/constants/constants.hpp>
#include <boost/optional.hpp>
#include <fmt/format.h>
#include <smartmet/macgyver/Exception.h>
#include <algorithm>
#include <stdexcept>

using boost::math::double_constants::radian;

namespace Trax
{
// Begin a new polyline
Polyline::Polyline(std::initializer_list<double> init_list)
{
  if (init_list.size() == 0 || init_list.size() % 2 != 0)
    throw Fmi::Exception(BCP, "Invalid initialization of Polyline from elements");
  const auto* iter = init_list.begin();
  while (iter != init_list.end())
  {
    double x = *iter++;
    double y = *iter++;
    m_points.emplace_back(x, y, false);
  }
  update_bbox();
}

// A polyline is closed if it contains at least 3 points and the end points are equal
bool Polyline::closed() const
{
  if (size() < 3)
    return false;
  return (xbegin() == xend() && ybegin() == yend());
}

// A polyline is clockwise if it is closed and the signed area is positive.
bool Polyline::clockwise() const
{
  if (!closed())
    return false;

  double area = 0;
  auto n = size();
  for (std::size_t i = 0; i < n - 1; i++)
    area += (m_points[i + 1].x - m_points[i].x) * (m_points[i].y + m_points[i + 1].y);

  // The true area is |area/2|, but we care only about the orientation

  // Treating zero-size polygons as anti-clockwise lead to troubled code elsewhere,
  // hence we use equality as well.
  return area >= 0;
}

std::vector<double> Polyline::xcoordinates() const
{
  std::vector<double> ret;
  const auto n = m_points.size();
  ret.reserve(n);
  for (auto i = 0UL; i < n; i++)
    ret.push_back(m_points[i].x);
  return ret;
}

std::vector<double> Polyline::ycoordinates() const
{
  std::vector<double> ret;
  const auto n = m_points.size();
  ret.reserve(n);
  for (auto i = 0UL; i < n; i++)
    ret.push_back(m_points[i].y);
  return ret;
}

// Polygon exteriors and holes may share only singular vertices and not
// whole edges. Hence any point along any edge should be inside an exterior
// if the hole is inside it.

std::pair<double, double> Polyline::inside_point() const
{
  const auto x = 0.5 * (m_points[0].x + m_points[1].x);
  const auto y = 0.5 * (m_points[0].y + m_points[1].y);
  return std::make_pair(x, y);
}

// Polyline exit angle
double Polyline::end_angle() const
{
  const auto n = m_points.size() - 2;
  auto x1 = m_points[n].x;
  auto y1 = m_points[n].y;
  auto x2 = m_points[n + 1].x;
  auto y2 = m_points[n + 1].y;
  return atan2(y2 - y1, x2 - x1) * radian;
}

// Polyline start angle
double Polyline::start_angle() const
{
  auto x1 = m_points[0].x;
  auto y1 = m_points[0].y;
  auto x2 = m_points[1].x;
  auto y2 = m_points[1].y;
  return atan2(y2 - y1, x2 - x1) * radian;
}

bool Polyline::bbox_contains(const Polyline& other) const
{
  return bbox().contains(other.bbox());
}

// Does the (closed) polyline contain the other (closed) polyline
bool Polyline::contains(const Polyline& other) const
{
  // Quick exit based on bounding box containment
  if (!bbox_contains(other))
  {
    return false;
  }

  // A hole may touch the exterior at a vertex, but may not share
  // an edge with it. Hence choosing the middle of a vertex as a test
  // point is safe provided there are no rounding errors, huge values
  // causing problems, highly distorted grids or something similar
  // making the calculations not robust.

  const auto test_point = other.inside_point();
  const auto x = test_point.first;
  const auto y = test_point.second;

  // Refs 1-2 are for clarity since ref 3 only provides an idea for optimization by Stuart MacMartin
  //
  // 1: http://www.faqs.org/faqs/graphics/algorithms-faq/ question 2.03
  // 2: https://wrf.ecse.rpi.edu/Research/Short_Notes/pnpoly.html
  // 3: http://www.realtimerendering.com/resources/RTNews/html//rtnv5n3.html#art3

  const auto n = m_points.size();
  bool inside = false;
  for (std::size_t i = 1; i < n; i++)
  {
    // Skip continuously while below or above y
    if (m_points[i - 1].y < y)
    {
      while (i < n && m_points[i].y < y)
        i++;
    }
    else if (m_points[i - 1].y > y)
    {
      while (i < n && m_points[i].y > y)
        i++;
    }

    if (i >= n)
      break;  // reached end without crossing y

    auto x1 = m_points[i - 1].x;
    auto y1 = m_points[i - 1].y;
    auto x2 = m_points[i].x;
    auto y2 = m_points[i].y;
    if ((y1 > y) != (y2 > y))
      if (x < (x1 - x2) * (y - y2) / (y1 - y2) + x2)
        inside = !inside;
  }

  return inside;
}

// Append a new coordinate. Discard duplicates which appear from apthological cases
// such as an isoband range value appearing only in one corner of a grid cell.
void Polyline::append(const Vertex& vertex)
{
  if (m_points.empty())
  {
    m_points.reserve(256);  // do not settle for a small initial allocation
    m_points.emplace_back(vertex.x, vertex.y, vertex.ghost);
  }
  else if (m_points.back().x != vertex.x || m_points.back().y != vertex.y)  // ignore duplicates
    m_points.emplace_back(vertex.x, vertex.y, vertex.ghost);
}

// Append another polyline directly
void Polyline::append(const Polyline& other)
{
  m_points.insert(m_points.end(), ++other.m_points.begin(), other.m_points.end());
}

// Reverse the winding order
void Polyline::reverse()
{
  const auto n = size();
  for (auto i = 0UL, j = n - 1; i < j; i++, j--)
    std::swap(m_points[i], m_points[j]);
}

// Export normal WKT
std::string Polyline::wkt() const
{
  std::string ret = "LINESTRING ";
  ret += wkt_body();
  return ret;
}

// Export WKT body only for use in multilinestrings etc
std::string Polyline::wkt_body() const
{
  std::string ret = "(";

  const auto n = size();
  for (auto i = 0UL; i < n; i++)
  {
    if (i > 0)
      ret += ',';
    ret += fmt::format("{} {}", m_points[i].x, m_points[i].y);
  }
  ret += ')';
  return ret;
}

void Polyline::wkb(std::ostringstream& out) const
{
  unsigned char byteOrder = 1;
  int n = 1;
  if(*(char *)&n == 0)
    byteOrder = 0;

  out.write((const char*)&byteOrder,sizeof(byteOrder));
  uint type = 2; // Line
  out.write((const char*)&type,sizeof(type));
  wkb_body(out);
}

void Polyline::wkb_body(std::ostringstream& out) const
{
  uint n = size();
  out.write((const char*)&n,sizeof(n));

  for (uint i = 0; i < n; i++)
  {
    double x = m_points[i].x;
    double y = m_points[i].y;

    //printf("Point %f,%f\n",x,y);
    out.write((const char*)&x,8);
    out.write((const char*)&y,8);
  }
}

// Normalize coordinates to lexicographic order for testing purposes
Polyline& Polyline::normalize()
{
  if (!closed())
  {
    const auto n = size() - 1;
    // Lexicographically smallest end vertex first
    if (m_points[0].x > m_points[n].x ||
        (m_points[0].x == m_points[n].x && m_points[0].y > m_points[n].y))
      reverse();
    return *this;
  }

  // Find lexicographically smallest element

  auto n = size() - 1;  // minus one since the last vertex is the same as the first one
  auto best = 0UL;
  for (auto i = 1UL; i < n; i++)
  {
    if ((m_points[i].x < m_points[best].x) ||
        (m_points[i].x == m_points[best].x && m_points[i].y < m_points[best].y))
      best = i;
  }

  // Rotate until it is the first element
  if (best != 0)
  {
    auto pos = m_points.begin();
    std::advance(pos, best);
    std::rotate(m_points.begin(), pos, --m_points.end());

    // Fix closing vertex
    m_points[n] = m_points[0];
  }
  return *this;
}

// For normalizing collections of polylines
bool Polyline::operator<(const Polyline& other) const
{
  if (this == &other)
    return false;

  const auto n = std::min(size(), other.size());
  for (auto i = 0UL; i < n; i++)
  {
    if (m_points[i].x != other.m_points[i].x)
      return m_points[i].x < other.m_points[i].x;
    if (m_points[i].y != other.m_points[i].y)
      return m_points[i].y < other.m_points[i].y;
  }
  return false;
}

void Polyline::update_bbox()
{
  m_bbox.init(m_points);
}

bool Polyline::has_ghosts() const
{
  return std::any_of(m_points.begin(), m_points.end(), [](const Point& p) { return p.ghost; });
}

struct ValidRange
{
  std::size_t begin;
  std::size_t end;
};

boost::optional<ValidRange> find_valid_range(const Points& points, std::size_t startpos)
{
  const auto n = points.size();
  while (startpos < n && points[startpos].ghost)
    ++startpos;
  if (startpos >= n)
    return {};

  auto endpos = startpos;
  while (endpos < n && !points[endpos].ghost)
    ++endpos;

  return ValidRange{startpos, endpos};
}

// precondition: has_ghosts is true, closed is true
void Polyline::remove_ghosts(Polylines& new_polylines)
{
#if 0
  std::cout << "Removing ghosts from " << wkt() << "\n";
  for (auto i = 0UL; i < m_points.size(); i++)
    std::cout << fmt::format(
        "\t{} : {},{} {}\n", i, m_points[i].x, m_points[i].y, m_points[i].ghost ? "?" : "-");
#endif

  // Extract valid ranges
  std::vector<ValidRange> ranges;
  const auto n = m_points.size();
  auto startpos = 0UL;
  while (startpos < n)
  {
    auto range = find_valid_range(m_points, startpos);
    if (!range)
      break;
    if (range->end - range->begin > 1)
      ranges.emplace_back(*range);
    startpos = range->end;
  }

  // Detect whether there is a wraparound
  const bool wraparound =
      (ranges.size() > 1 && ranges.front().begin == 0 && ranges.back().end == n);

  auto sz = ranges.size();
  if (wraparound)  // if there is wraparound, the last range is handled along with the first one
    --sz;

  for (auto i = 0UL; i < sz; i++)
  {
    Polyline line;
    const auto& range = ranges[i];
    if (i == 0 && wraparound)
    {
      const auto& range2 = ranges.back();
      // std::cout << "Keeping wraparound range " << range2.begin << "..." << range2.end << "\n";
      for (auto j = range2.begin; j < range2.end - 1; j++)
        line.m_points.push_back(m_points[j]);
    }
    // std::cout << "Keeping  range " << range.begin << "..." << range.end << "\n";
    for (auto j = range.begin; j < range.end; j++)
      line.m_points.push_back(m_points[j]);
    new_polylines.emplace_back(std::move(line));
  }
}

}  // namespace Trax


/* Name: Minimum Gradient Tests
 * Number: N0720144 (James Doyle)
 */

// Includes:
#include <boost/test/unit_test.hpp>

#include "logs.h"
#include "route.h"
#include "track.h"

// Using:
using namespace GPS;

BOOST_AUTO_TEST_SUITE( Route_minGradient_N0720144 )

const bool isFileName = true;

/* Test 1: Tests if function outputs the largest negative gradient in a set of points
 * that gives multiple negative gradient differences
 */

BOOST_AUTO_TEST_CASE( LargestNegativeGradient )
{
    Route route = Route(LogFiles::GPXRoutesDir + "LargestNegativeGradient.gpx",isFileName);
    BOOST_CHECK_CLOSE(route.minGradient(),-220513.18751,1);
}

/* Test 2: Tests if it throws the exception std::invalid_argument when there is a single point
 * in the track
 */

BOOST_AUTO_TEST_CASE( Invalid_Amount_of_Points )
{
    BOOST_CHECK_THROW(Route route = Route(LogFiles::GPXRoutesDir + "Invalid_Amount_of_Points.gpx",isFileName),std::invalid_argument);
}

/* Test 3: Tests if the elevations are zero it outputs a minimum gradient of 0
 */

BOOST_AUTO_TEST_CASE( Elevations_Eqaul_to_Zero )
{
    Route route = Route(LogFiles::GPXRoutesDir + "Elevations_Equal_to_Zero.gpx", isFileName);
    BOOST_CHECK_EQUAL(route.minGradient(),0);
}

/* Test 4: Tests if the gradient is outputted if all the latitudes of the points within the track
 * the same
 */

BOOST_AUTO_TEST_CASE( No_Change_Latitude )
{
    Route route = Route(LogFiles::GPXRoutesDir + "No_Change_Latitude.gpx",isFileName);
    BOOST_CHECK_CLOSE(route.minGradient(),235274.33443,1);
}

/* Test 4: Tests if the gradient is outputted if all the longitudes of the points within the track
 * the same
 */

BOOST_AUTO_TEST_CASE( No_Change_Longitude )
{
    Route route = Route(LogFiles::GPXRoutesDir + "No_Change_Longitude.gpx",isFileName);
    BOOST_CHECK_CLOSE(route.minGradient(),235274.33443,1);
}

BOOST_AUTO_TEST_SUITE_END()


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

  Copyright 2016 NumScale SAS

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

STF_CASE_TPL (" inrad",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::inrad;

  using r_t = decltype(inrad(T()));

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

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(inrad(bs::Inf<T>()), bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(inrad(bs::Minf<T>()), bs::Minf<r_t>(), 0.5);
  STF_ULP_EQUAL(inrad(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(inrad(T(-180)), -bs::Pi<r_t>(), 0.5);
  STF_ULP_EQUAL(inrad(T(-45)), -bs::Pio_4<r_t>(), 0.5);
  STF_ULP_EQUAL(inrad(T(-90)), -bs::Pio_2<r_t>(), 0.5);
  STF_ULP_EQUAL(inrad(bs::Zero<T>()), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(inrad(T(180)), bs::Pi<r_t>(), 0.5);
  STF_ULP_EQUAL(inrad(T(45)), bs::Pio_4<r_t>(), 0.5);
  STF_ULP_EQUAL(inrad(T(90)), bs::Pio_2<r_t>(), 0.5);
}


//
// Copyright 2005-2007 Adobe Systems Incorporated
// Copyright 2018 Mateusz Loskot <mateusz at loskot dot net>
//
// Distribtted 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
//
#include <boost/gil/channel.hpp>
#include <boost/gil/channel_algorithm.hpp>
#include <boost/gil/typedefs.hpp>
#include <cstdint>
#include <limits>

#define BOOST_TEST_MODULE test_scoped_channel_value
#include "unit_test.hpp"

namespace gil = boost::gil;

struct int_minus_value  { static std::int8_t apply() { return -64; } };
struct int_plus_value   { static std::int8_t apply() { return  64; } };
using fixture = gil::scoped_channel_value
    <
        std::uint8_t, int_minus_value, int_plus_value
    >;

BOOST_AUTO_TEST_CASE(scoped_channel_value_default_constructor)
{
    fixture f;
    std::uint8_t v = f;
    BOOST_TEST(v == std::uint8_t{0});
}

BOOST_AUTO_TEST_CASE(scoped_channel_value_user_defined_constructors)
{
    fixture f{1};
    std::uint8_t v = f;
    BOOST_TEST(v == std::uint8_t{1});
}

BOOST_AUTO_TEST_CASE(scoped_channel_value_copy_constructors)
{
    fixture f1{128};
    fixture f2{f1};

    BOOST_TEST(std::uint8_t{f1} == std::uint8_t{128});
    BOOST_TEST(std::uint8_t{f1} == std::uint8_t{f2});
}

BOOST_AUTO_TEST_CASE(scoped_channel_value_assignment)
{
    fixture f;
    f = 64;
    std::uint8_t v = f;
    BOOST_TEST(v == std::uint8_t{64});
}

BOOST_AUTO_TEST_CASE(scoped_channel_value_float32_t)
{
    auto const tolerance = btt::tolerance(std::numeric_limits<float>::epsilon());
    // min
    BOOST_TEST(gil::float_point_zero<float>::apply() == 0.0, tolerance);
    BOOST_TEST(gil::channel_traits<gil::float32_t>::min_value() == 0.0);
    // max
    BOOST_TEST(gil::float_point_one<float>::apply() == 1.0, tolerance);
    BOOST_TEST(gil::channel_traits<gil::float32_t>::max_value() == 1.0);
}

BOOST_AUTO_TEST_CASE(scoped_channel_value_float64_t)
{
    auto const tolerance = btt::tolerance(std::numeric_limits<double>::epsilon());
    // min
    BOOST_TEST(gil::float_point_zero<double>::apply() == 0.0, tolerance);
    BOOST_TEST(gil::channel_traits<gil::float64_t>::min_value() == 0.0, tolerance);
    // max
    BOOST_TEST(gil::float_point_one<double>::apply() == 1.0, tolerance);
    BOOST_TEST(gil::channel_traits<gil::float64_t>::max_value() == 1.0, tolerance);
}

BOOST_AUTO_TEST_CASE(scoped_channel_value_halfs)
{
    // Create a double channel with range [-0.5 .. 0.5]
    struct minus_half { static double apply() { return -0.5; } };
    struct plus_half { static double apply() { return 0.5; } };
    using halfs = gil::scoped_channel_value<double, minus_half, plus_half>;

    auto const tolerance = btt::tolerance(std::numeric_limits<double>::epsilon());
    BOOST_TEST(gil::channel_traits<halfs>::min_value() == minus_half::apply(), tolerance);
    BOOST_TEST(gil::channel_traits<halfs>::max_value() == plus_half::apply(), tolerance);
    // scoped channel maximum should map to the maximum
    BOOST_TEST(gil::channel_convert<std::uint16_t>(
        gil::channel_traits<halfs>::max_value()) == 65535, tolerance);
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/orderable.hpp>
#include <boost/hana/tuple.hpp>

#include <test/auto/base.hpp>
#include <test/auto/comparable.hpp>
#include <test/auto/orderable.hpp>

#include <string>
#include <type_traits>
using namespace std::literals;
using namespace boost::hana;

// Minimal LessThanComparable types
struct ord1 { int value; };
struct ord2 {
    int value;
    constexpr operator ord1() const { return {value}; }
};

template <typename T, typename U, typename = std::enable_if_t<
    (std::is_same<T, ord1>{} || std::is_same<T, ord2>{}) &&
    (std::is_same<U, ord1>{} || std::is_same<U, ord2>{})
>>
constexpr bool operator<(T a, U b)
{ return a.value < b.value; }

namespace boost { namespace hana {
    template <typename T, typename U>
    struct equal_impl<T, U, when<
        (std::is_same<T, ord1>{} || std::is_same<T, ord2>{}) &&
        (std::is_same<U, ord1>{} || std::is_same<U, ord2>{})
    >> {
        static constexpr bool apply(T a, U b)
        { return a.value == b.value; }
    };
}}

namespace boost { namespace hana { namespace test {
    template <> auto objects<int> = make<Tuple>(0,1,2,3,4,5);
    template <> auto objects<unsigned int> = make<Tuple>(0u,1u,2u,3u,4u,5u);
    template <> auto objects<long> = make<Tuple>(0l,1l,2l,3l,4l,5l);
    template <> auto objects<unsigned long> = make<Tuple>(0ul,1ul,2ul,3ul,4ul,5ul);
    template <> auto objects<ord1> = make<Tuple>(ord1{0}, ord1{1}, ord1{2}, ord1{3}, ord1{4});
}}}

int main() {
    // laws
    test::laws<Orderable, int>();
    test::laws<Orderable, unsigned int>();
    test::laws<Orderable, long>();
    test::laws<Orderable, unsigned long>();
    test::laws<Orderable, ord1>();

    // less
    {
        BOOST_HANA_CONSTEXPR_CHECK(less(5, 6));
        BOOST_HANA_CONSTEXPR_CHECK(not_(less(6, 6)));
        BOOST_HANA_CONSTEXPR_CHECK(not_(less(7, 6)));

        // Provided model for LessThanComparable types
        BOOST_HANA_CONSTEXPR_CHECK(less(ord1{0}, ord1{1}));
        BOOST_HANA_CONSTEXPR_CHECK(not_(less(ord1{0}, ord1{0})));
        BOOST_HANA_CONSTEXPR_CHECK(not_(less(ord1{1}, ord1{0})));

        BOOST_HANA_CONSTEXPR_CHECK(less(ord1{0}, ord2{1}));
        BOOST_HANA_CONSTEXPR_CHECK(not_(less(ord1{0}, ord2{0})));
        BOOST_HANA_CONSTEXPR_CHECK(not_(less(ord1{1}, ord2{0})));

        BOOST_HANA_CONSTEXPR_CHECK(less(ord2{0}, ord1{1}));
        BOOST_HANA_CONSTEXPR_CHECK(not_(less(ord2{0}, ord1{0})));
        BOOST_HANA_CONSTEXPR_CHECK(not_(less(ord2{1}, ord1{0})));

        BOOST_HANA_RUNTIME_CHECK(less("ab", "abc"s));
        BOOST_HANA_RUNTIME_CHECK(less("abc"s, "abcde"));
    }

    // greater
    {
        BOOST_HANA_CONSTEXPR_CHECK(not_(greater(5, 6)));
        BOOST_HANA_CONSTEXPR_CHECK(not_(greater(6, 6)));
        BOOST_HANA_CONSTEXPR_CHECK(greater(7, 6));

        BOOST_HANA_RUNTIME_CHECK(greater("abcd", "ab"s));
        BOOST_HANA_RUNTIME_CHECK(greater("abc"s, "abb"));
    }

    // less_equal
    {
        BOOST_HANA_CONSTEXPR_CHECK(less_equal(5, 6));
        BOOST_HANA_CONSTEXPR_CHECK(less_equal(6, 6));
        BOOST_HANA_CONSTEXPR_CHECK(not_(less_equal(7, 6)));

        BOOST_HANA_RUNTIME_CHECK(less_equal("ab", "abcd"s));
        BOOST_HANA_RUNTIME_CHECK(less_equal("abc"s, "abc"));
    }

    // greater_equal
    {
        BOOST_HANA_CONSTEXPR_CHECK(not_(greater_equal(5, 6)));
        BOOST_HANA_CONSTEXPR_CHECK(greater_equal(6, 6));
        BOOST_HANA_CONSTEXPR_CHECK(greater_equal(7, 6));

        BOOST_HANA_RUNTIME_CHECK(greater_equal("abcd", "ab"s));
        BOOST_HANA_RUNTIME_CHECK(greater_equal("abc"s, "abc"));
    }

    // min
    {
        BOOST_HANA_CONSTEXPR_CHECK(equal(min(5, 6), 5));
        BOOST_HANA_CONSTEXPR_CHECK(equal(min(6, 5), 5));
    }

    // max
    {
        BOOST_HANA_CONSTEXPR_CHECK(equal(max(5, 6), 6));
        BOOST_HANA_CONSTEXPR_CHECK(equal(max(6, 5), 6));
    }
}


// Copyright 2004 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#include <boost/graph/betweenness_centrality.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <vector>
#include <stack>
#include <queue>
#include <boost/property_map/property_map.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/lexical_cast.hpp>

using namespace boost;

const double error_tolerance = 0.001;

typedef property< edge_weight_t, double, property< edge_index_t, std::size_t > >
    EdgeProperties;

struct weighted_edge
{
    int source, target;
    double weight;
};

template < typename Graph >
void run_weighted_test(Graph*, int V, weighted_edge edge_init[], int E,
    double correct_centrality[])
{
    Graph g(V);
    typedef typename graph_traits< Graph >::vertex_descriptor Vertex;
    typedef typename graph_traits< Graph >::vertex_iterator vertex_iterator;
    typedef typename graph_traits< Graph >::edge_descriptor Edge;

    std::vector< Vertex > vertices(V);
    {
        vertex_iterator v, v_end;
        int index = 0;
        for (boost::tie(v, v_end) = boost::vertices(g); v != v_end;
             ++v, ++index)
        {
            put(vertex_index, g, *v, index);
            vertices[index] = *v;
        }
    }

    std::vector< Edge > edges(E);
    for (int e = 0; e < E; ++e)
    {
        edges[e] = add_edge(
            vertices[edge_init[e].source], vertices[edge_init[e].target], g)
                       .first;
        put(edge_weight, g, edges[e], 1.0);
    }

    std::vector< double > centrality(V);
    brandes_betweenness_centrality(g,
        centrality_map(make_iterator_property_map(
                           centrality.begin(), get(vertex_index, g), double()))
            .vertex_index_map(get(vertex_index, g))
            .weight_map(get(edge_weight, g)));

    for (int v = 0; v < V; ++v)
    {
        BOOST_TEST(centrality[v] == correct_centrality[v]);
    }
}

struct unweighted_edge
{
    int source, target;
};

template < typename Graph >
void run_unweighted_test(Graph*, int V, unweighted_edge edge_init[], int E,
    double correct_centrality[], double* correct_edge_centrality = 0)
{
    Graph g(V);
    typedef typename graph_traits< Graph >::vertex_descriptor Vertex;
    typedef typename graph_traits< Graph >::vertex_iterator vertex_iterator;
    typedef typename graph_traits< Graph >::edge_descriptor Edge;

    std::vector< Vertex > vertices(V);
    {
        vertex_iterator v, v_end;
        int index = 0;
        for (boost::tie(v, v_end) = boost::vertices(g); v != v_end;
             ++v, ++index)
        {
            put(vertex_index, g, *v, index);
            vertices[index] = *v;
        }
    }

    std::vector< Edge > edges(E);
    for (int e = 0; e < E; ++e)
    {
        edges[e] = add_edge(
            vertices[edge_init[e].source], vertices[edge_init[e].target], g)
                       .first;
        put(edge_weight, g, edges[e], 1.0);
        put(edge_index, g, edges[e], e);
    }

    std::vector< double > centrality(V);
    std::vector< double > edge_centrality1(E);

    brandes_betweenness_centrality(g,
        centrality_map(make_iterator_property_map(
                           centrality.begin(), get(vertex_index, g), double()))
            .edge_centrality_map(make_iterator_property_map(
                edge_centrality1.begin(), get(edge_index, g), double()))
            .vertex_index_map(get(vertex_index, g)));

    std::vector< double > centrality2(V);
    std::vector< double > edge_centrality2(E);
    brandes_betweenness_centrality(g,
        vertex_index_map(get(vertex_index, g))
            .weight_map(get(edge_weight, g))
            .centrality_map(make_iterator_property_map(
                centrality2.begin(), get(vertex_index, g), double()))
            .edge_centrality_map(make_iterator_property_map(
                edge_centrality2.begin(), get(edge_index, g), double())));

    std::vector< double > edge_centrality3(E);
    brandes_betweenness_centrality(g,
        edge_centrality_map(make_iterator_property_map(
            edge_centrality3.begin(), get(edge_index, g), double())));

    for (int v = 0; v < V; ++v)
    {
        BOOST_TEST(centrality[v] == centrality2[v]);

        double relative_error = correct_centrality[v] == 0.0
            ? centrality[v]
            : (centrality[v] - correct_centrality[v]) / correct_centrality[v];
        if (relative_error < 0)
            relative_error = -relative_error;
        BOOST_TEST(relative_error < error_tolerance);
    }

    for (int e = 0; e < E; ++e)
    {
        BOOST_TEST(edge_centrality1[e] == edge_centrality2[e]);
        BOOST_TEST(edge_centrality1[e] == edge_centrality3[e]);

        if (correct_edge_centrality)
        {
            double relative_error = correct_edge_centrality[e] == 0.0
                ? edge_centrality1[e]
                : (edge_centrality1[e] - correct_edge_centrality[e])
                    / correct_edge_centrality[e];
            if (relative_error < 0)
                relative_error = -relative_error;
            BOOST_TEST(relative_error < error_tolerance);

            if (relative_error >= error_tolerance)
            {
                std::cerr << "Edge " << e << " has edge centrality "
                          << edge_centrality1[e] << ", should be "
                          << correct_edge_centrality[e] << std::endl;
            }
        }
    }
}

template < typename Graph > void run_wheel_test(Graph*, int V)
{
    typedef typename graph_traits< Graph >::vertex_descriptor Vertex;
    typedef typename graph_traits< Graph >::vertex_iterator vertex_iterator;
    typedef typename graph_traits< Graph >::edge_descriptor Edge;

    Graph g(V);
    Vertex center = *boost::vertices(g).first;

    std::vector< Vertex > vertices(V);
    {
        vertex_iterator v, v_end;
        int index = 0;
        for (boost::tie(v, v_end) = boost::vertices(g); v != v_end;
             ++v, ++index)
        {
            put(vertex_index, g, *v, index);
            vertices[index] = *v;
            if (*v != center)
            {
                Edge e = add_edge(*v, center, g).first;
                put(edge_weight, g, e, 1.0);
            }
        }
    }

    std::vector< double > centrality(V);
    brandes_betweenness_centrality(g,
        make_iterator_property_map(
            centrality.begin(), get(vertex_index, g), double()));

    std::vector< double > centrality2(V);
    brandes_betweenness_centrality(g,
        centrality_map(make_iterator_property_map(
                           centrality2.begin(), get(vertex_index, g), double()))
            .vertex_index_map(get(vertex_index, g))
            .weight_map(get(edge_weight, g)));

    relative_betweenness_centrality(g,
        make_iterator_property_map(
            centrality.begin(), get(vertex_index, g), double()));

    relative_betweenness_centrality(g,
        make_iterator_property_map(
            centrality2.begin(), get(vertex_index, g), double()));

    for (int v = 0; v < V; ++v)
    {
        BOOST_TEST(centrality[v] == centrality2[v]);
        BOOST_TEST(
            (v == 0 && centrality[v] == 1) || (v != 0 && centrality[v] == 0));
    }

    double dominance = central_point_dominance(g,
        make_iterator_property_map(
            centrality2.begin(), get(vertex_index, g), double()));
    BOOST_TEST(dominance == 1.0);
}

template < typename MutableGraph >
void randomly_add_edges(MutableGraph& g, double edge_probability)
{
    typedef typename graph_traits< MutableGraph >::directed_category
        directed_category;

    minstd_rand gen;
    uniform_01< minstd_rand, double > rand_gen(gen);

    typedef typename graph_traits< MutableGraph >::vertex_descriptor vertex;
    typename graph_traits< MutableGraph >::vertex_iterator vi, vi_end;
    for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    {
        vertex v = *vi;
        typename graph_traits< MutableGraph >::vertex_iterator wi
            = is_same< directed_category, undirected_tag >::value
            ? vi
            : vertices(g).first;
        while (wi != vi_end)
        {
            vertex w = *wi++;
            if (v != w)
            {
                if (rand_gen() < edge_probability)
                    add_edge(v, w, g);
            }
        }
    }
}

template < typename Graph, typename VertexIndexMap, typename CentralityMap >
void simple_unweighted_betweenness_centrality(
    const Graph& g, VertexIndexMap index, CentralityMap centrality)
{
    typedef typename boost::graph_traits< Graph >::vertex_descriptor vertex;
    typedef
        typename boost::graph_traits< Graph >::vertex_iterator vertex_iterator;
    typedef typename boost::graph_traits< Graph >::adjacency_iterator
        adjacency_iterator;
    typedef typename boost::graph_traits< Graph >::vertices_size_type
        vertices_size_type;
    typedef typename boost::property_traits< CentralityMap >::value_type
        centrality_type;

    vertex_iterator vi, vi_end;
    for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
        put(centrality, *vi, 0);

    vertex_iterator si, si_end;
    for (boost::tie(si, si_end) = vertices(g); si != si_end; ++si)
    {
        vertex s = *si;

        // S <-- empty stack
        std::stack< vertex > S;

        // P[w] <-- empty list, w \in V
        typedef std::vector< vertex > Predecessors;
        std::vector< Predecessors > predecessors(num_vertices(g));

        // sigma[t] <-- 0, t \in V
        std::vector< vertices_size_type > sigma(num_vertices(g), 0);

        // sigma[s] <-- 1
        sigma[get(index, s)] = 1;

        // d[t] <-- -1, t \in V
        std::vector< int > d(num_vertices(g), -1);

        // d[s] <-- 0
        d[get(index, s)] = 0;

        // Q <-- empty queue
        std::queue< vertex > Q;

        // enqueue s --> Q
        Q.push(s);

        while (!Q.empty())
        {
            // dequeue v <-- Q
            vertex v = Q.front();
            Q.pop();

            // push v --> S
            S.push(v);

            adjacency_iterator wi, wi_end;
            for (boost::tie(wi, wi_end) = adjacent_vertices(v, g); wi != wi_end;
                 ++wi)
            {
                vertex w = *wi;

                // w found for the first time?
                if (d[get(index, w)] < 0)
                {
                    // enqueue w --> Q
                    Q.push(w);

                    // d[w] <-- d[v] + 1
                    d[get(index, w)] = d[get(index, v)] + 1;
                }

                // shortest path to w via v?
                if (d[get(index, w)] == d[get(index, v)] + 1)
                {
                    // sigma[w] = sigma[w] + sigma[v]
                    sigma[get(index, w)] += sigma[get(index, v)];

                    // append v --> P[w]
                    predecessors[get(index, w)].push_back(v);
                }
            }
        }

        // delta[v] <-- 0, v \in V
        std::vector< centrality_type > delta(num_vertices(g), 0);

        // S returns vertices in order of non-increasing distance from s
        while (!S.empty())
        {
            // pop w <-- S
            vertex w = S.top();
            S.pop();

            const Predecessors& w_preds = predecessors[get(index, w)];
            for (typename Predecessors::const_iterator vi = w_preds.begin();
                 vi != w_preds.end(); ++vi)
            {
                vertex v = *vi;
                // delta[v] <-- delta[v] + (sigma[v]/sigma[w])*(1 + delta[w])
                delta[get(index, v)] += ((centrality_type)sigma[get(index, v)]
                                            / sigma[get(index, w)])
                    * (1 + delta[get(index, w)]);
            }

            if (w != s)
            {
                // C_B[w] <-- C_B[w] + delta[w]
                centrality[w] += delta[get(index, w)];
            }
        }
    }

    typedef typename graph_traits< Graph >::directed_category directed_category;
    const bool is_undirected
        = is_same< directed_category, undirected_tag >::value;
    if (is_undirected)
    {
        vertex_iterator v, v_end;
        for (boost::tie(v, v_end) = vertices(g); v != v_end; ++v)
        {
            put(centrality, *v, get(centrality, *v) / centrality_type(2));
        }
    }
}

template < typename Graph > void random_unweighted_test(Graph*, int n)
{
    Graph g(n);

    {
        typename graph_traits< Graph >::vertex_iterator v, v_end;
        int index = 0;
        for (boost::tie(v, v_end) = boost::vertices(g); v != v_end;
             ++v, ++index)
        {
            put(vertex_index, g, *v, index);
        }
    }

    randomly_add_edges(g, 0.20);

    std::cout << "Random graph with " << n << " vertices and " << num_edges(g)
              << " edges.\n";

    std::cout << "  Direct translation of Brandes' algorithm...";
    std::vector< double > centrality(n);
    simple_unweighted_betweenness_centrality(g, get(vertex_index, g),
        make_iterator_property_map(
            centrality.begin(), get(vertex_index, g), double()));
    std::cout << "DONE.\n";

    std::cout << "  Real version, unweighted...";
    std::vector< double > centrality2(n);
    brandes_betweenness_centrality(g,
        make_iterator_property_map(
            centrality2.begin(), get(vertex_index, g), double()));
    std::cout << "DONE.\n";

    if (!std::equal(centrality.begin(), centrality.end(), centrality2.begin()))
    {
        for (std::size_t v = 0; v < centrality.size(); ++v)
        {
            double relative_error = centrality[v] == 0.0
                ? centrality2[v]
                : (centrality2[v] - centrality[v]) / centrality[v];
            if (relative_error < 0)
                relative_error = -relative_error;
            BOOST_TEST(relative_error < error_tolerance);
        }
    }

    std::cout << "  Real version, weighted...";
    std::vector< double > centrality3(n);

    for (typename graph_traits< Graph >::edge_iterator ei = edges(g).first;
         ei != edges(g).second; ++ei)
        put(edge_weight, g, *ei, 1);

    brandes_betweenness_centrality(g,
        weight_map(get(edge_weight, g))
            .centrality_map(make_iterator_property_map(
                centrality3.begin(), get(vertex_index, g), double())));
    std::cout << "DONE.\n";

    if (!std::equal(centrality.begin(), centrality.end(), centrality3.begin()))
    {
        for (std::size_t v = 0; v < centrality.size(); ++v)
        {
            double relative_error = centrality[v] == 0.0
                ? centrality3[v]
                : (centrality3[v] - centrality[v]) / centrality[v];
            if (relative_error < 0)
                relative_error = -relative_error;
            BOOST_TEST(relative_error < error_tolerance);
        }
    }
}

int main(int argc, char* argv[])
{
    int random_test_num_vertices = 300;
    if (argc >= 2)
        random_test_num_vertices = boost::lexical_cast< int >(argv[1]);
    typedef adjacency_list< listS, listS, undirectedS,
        property< vertex_index_t, int >, EdgeProperties >
        Graph;
    typedef adjacency_list< listS, listS, directedS,
        property< vertex_index_t, int >, EdgeProperties >
        Digraph;

    struct unweighted_edge ud_edge_init1[5]
        = { { 0, 1 }, { 0, 3 }, { 1, 2 }, { 3, 2 }, { 2, 4 } };
    double ud_centrality1[5] = { 0.5, 1.0, 3.5, 1.0, 0.0 };
    run_unweighted_test((Graph*)0, 5, ud_edge_init1, 5, ud_centrality1);

    // Example borrowed from the JUNG test suite
    struct unweighted_edge ud_edge_init2[10] = {
        { 0, 1 },
        { 0, 6 },
        { 1, 2 },
        { 1, 3 },
        { 2, 4 },
        { 3, 4 },
        { 4, 5 },
        { 5, 8 },
        { 7, 8 },
        { 6, 7 },
    };
    double ud_centrality2[9]
        = { 0.2142 * 28, 0.2797 * 28, 0.0892 * 28, 0.0892 * 28, 0.2797 * 28,
              0.2142 * 28, 0.1666 * 28, 0.1428 * 28, 0.1666 * 28 };
    double ud_edge_centrality2[10] = { 10.66666, 9.33333, 6.5, 6.5, 6.5, 6.5,
        10.66666, 9.33333, 8.0, 8.0 };

    run_unweighted_test(
        (Graph*)0, 9, ud_edge_init2, 10, ud_centrality2, ud_edge_centrality2);

    weighted_edge dw_edge_init1[6] = { { 0, 1, 1.0 }, { 0, 3, 1.0 },
        { 1, 2, 0.5 }, { 3, 1, 1.0 }, { 3, 4, 1.0 }, { 4, 2, 0.5 } };
    double dw_centrality1[5] = { 0.0, 1.5, 0.0, 1.0, 0.5 };
    run_weighted_test((Digraph*)0, 5, dw_edge_init1, 6, dw_centrality1);

    run_wheel_test((Graph*)0, 15);

    random_unweighted_test((Graph*)0, random_test_num_vertices);

    return boost::report_errors();
}


/**
 * @file sdms_vector.hpp
 * @author David Albert (david.albert@insa-lyon.fr)
 * @brief
 * @version 0.1
 * @date 07/01/2021
 *
 * @copyright Copyright (c) 2021
 *
 */
#pragma once

#include <vector>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <assert.h>

#include <sdm/utils/linear_algebra/vector_interface.hpp>

namespace sdm
{
  /**
   * @brief Create a SDMS Vector. A SMDS Vector is used to optimize the calculation, however, you have to be careful when using it because it's not possible to add element after the initialization
   *
   * @tparam I Type of index
   * @tparam T Type of value
   */
  template <class I, class T, class TBaseVector>
  class sdmsVector : public VectorInterface<I, T>
  {
  public:
    // using array_type = typename TBaseVector::array_type;
    // using value_type = typename array_type::value_type;

    static double PRECISION;

    sdmsVector();

    /**
     * @brief Create a SDMS Vector. In order to create a vector, it's necessary to provide a map that associated the I element with a specific index and a specific Value.
     *
     * @param std::shared_ptr<std::unordered_map<I, size_t>> : A map that associate the element I with a specific index
     * @param std::shared_ptr<std::unordered_map<I, T>> : A map that associate the element I with a specific value
     * @param double : the default value
     *
     */
    sdmsVector(std::shared_ptr<std::unordered_map<I, size_t>>, std::shared_ptr<std::unordered_map<I, T>>, double = 0);

    /**
     * @brief Create a SDMS Vector.
     *
     * @param std::vector<I>> : A vector for each element I
     * @param std::vector<T> : A vector for each value associate at the same position of the element I in vector
     * @param double : the default value
     *
     */
    sdmsVector(std::vector<I>, std::vector<T>, double = 0);

    virtual ~sdmsVector() {}

    T at(const I &) const;
    T getValueAt(const I &) const;
    void setValueAt(const I &, const T &value);
    void addValueAt(const I &, const T &value); // ps: added by baris, not tested.

    T sum() const;
    T norm_1() const;
    T norm_2() const;

    T min();
    I argmin();
    T max();
    I argmax();

    // sdmsVector transpose() const;

    const std::vector<I> &getIndexes() const;

    /**
     * @brief Compare two vectors. Return true if all values are lower or equal to the second vector.
     *
     * @return true
     * @return false
     */
    bool operator<=(const sdmsVector &) const;
    bool operator==(const sdmsVector &) const;
    bool operator!=(const sdmsVector &) const;
    bool isEqual(const sdmsVector &other, double precision) const;

    // template <class AE>
    T dot(const sdmsVector &v2) const;

    // template <class AE>
    T operator^(const sdmsVector &v2) const;

    std::string str() const;

    auto begin() const { return this->iterator_.begin(); }
    auto end() const { return this->iterator_.end(); }

    template <class Archive>
    void serialize(Archive &archive, const unsigned int);

    size_t size() const;
    std::shared_ptr<std::unordered_map<I, size_t>> getMapElementToIndex() const;

  protected:
    std::vector<I> vector_element_;
    // std::shared_ptr<std::unordered_map<I, size_t>> map_index_to_value_;
    std::shared_ptr<std::unordered_map<I, size_t>> map_element_to_index_;

    // std::map<I, T> iterator_;

    TBaseVector tbasevector_;

    std::pair<I, T> getMin() const;
    std::pair<I, T> getMax() const;

    // friend sdmsVector operator*(const T &arg1, const sdmsVector &arg2)
    // {
    //   sdmsVector vnew = arg1 * static_cast<TBaseVector>(arg2);
    //   vnew.setIndexes(arg2.getIndexes());
    //   return vnew;
    // }
  };

  /**
   * @brief Sparse vector are vectors that store only non-zero values.
   *
   * @tparam I Type of index
   * @tparam T Type of value
   */
  template <typename I = size_t, typename T = double>
  using SparseVector = sdmsVector<I, T, boost::numeric::ublas::mapped_vector<T>>;

  /**
   * @brief Dense vector are standard vector.
   *
   * @tparam I Type of index
   * @tparam T Type of value
   */
  template <typename I = size_t, typename T = double>
  using DenseVector = sdmsVector<I, T, boost::numeric::ublas::vector<T>>;

} // namespace sdm

#include <sdm/utils/linear_algebra/sdms_vector.tpp>


/*
 * test_pose.cpp
 *
 *  Created on: Mar 15, 2012
 *      Author: Pablo Iñigo Blasco
 */

#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/math/CQuaternion.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Quaternion.h>
#include <tf/tf.h>
#include <mrpt_bridge/pose.h>
#include <gtest/gtest.h>
#include <Eigen/Dense>

using namespace std;

#if MRPT_VERSION >= 0x199
#define getAsVectorVal asVectorVal
using mrpt::DEG2RAD;
#else
using mrpt::utils::DEG2RAD;
#endif

void checkPoseMatrixFromRotationParameters(
	const double roll, const double pitch, const double yaw)
{
	// TF-BULLET ROTATION
	tf::Pose original_pose;
	tf::Quaternion rotation;
	rotation.setRPY(roll, pitch, yaw);
	original_pose.setRotation(rotation);
	tf::Matrix3x3 basis = original_pose.getBasis();

	// MRPT-ROTATION
	mrpt::poses::CPose3D mrpt_original_pose;
	mrpt_original_pose.setYawPitchRoll(yaw, pitch, roll);
	mrpt::math::CMatrixDouble33 mrpt_basis =
		mrpt_original_pose.getRotationMatrix();

	for (int i = 0; i < 3; i++)
		for (int j = 0; j < 3; j++)
			EXPECT_NEAR(basis[i][j], mrpt_basis(i, j), 0.01);
}

TEST(PoseConversions, copyMatrix3x3ToCMatrixDouble33)
{
	tf::Matrix3x3 src(0, 1, 2, 3, 4, 5, 6, 7, 8);
	mrpt::math::CMatrixDouble33 des;
	mrpt_bridge::convert(src, des);
	for (int r = 0; r < 3; r++)
		for (int c = 0; c < 3; c++) EXPECT_FLOAT_EQ(des(r, c), src[r][c]);
}
TEST(PoseConversions, copyCMatrixDouble33ToMatrix3x3)
{
	mrpt::math::CMatrixDouble33 src;
	for (int r = 0; r < 3; r++)
		for (int c = 0; c < 3; c++) src(r, c) = 12.0 + r * 4 - c * 2 + r * c;

	tf::Matrix3x3 des;
	mrpt_bridge::convert(src, des);
	for (int r = 0; r < 3; r++)
		for (int c = 0; c < 3; c++) EXPECT_FLOAT_EQ(des[r][c], src(r, c));
}

TEST(PoseConversions, checkPoseMatrixFromRotationParameters)
{
	checkPoseMatrixFromRotationParameters(0, 0, 0);
	checkPoseMatrixFromRotationParameters(0.2, 0, 0);
	checkPoseMatrixFromRotationParameters(0, 0.2, 0);
	checkPoseMatrixFromRotationParameters(0, 0, 0.2);
	checkPoseMatrixFromRotationParameters(0.4, 0.3, 0.2);
	checkPoseMatrixFromRotationParameters(M_PI, -M_PI / 2.0, 0);
}

// Declare a test
TEST(PoseConversions, reference_frame_change_with_rotations)
{
	geometry_msgs::PoseWithCovariance ros_msg_original_pose;

	ros_msg_original_pose.pose.position.x = 1;
	ros_msg_original_pose.pose.position.y = 0;
	ros_msg_original_pose.pose.position.z = 0;
	ros_msg_original_pose.pose.orientation.x = 0;
	ros_msg_original_pose.pose.orientation.y = 0;
	ros_msg_original_pose.pose.orientation.z = 0;
	ros_msg_original_pose.pose.orientation.w = 1;

	// to mrpt
	mrpt::poses::CPose3DPDFGaussian mrpt_original_pose;
	mrpt_bridge::convert(ros_msg_original_pose, mrpt_original_pose);
	EXPECT_EQ(
		ros_msg_original_pose.pose.position.x, mrpt_original_pose.mean[0]);

	// to tf
	tf::Pose tf_original_pose;
	tf::poseMsgToTF(ros_msg_original_pose.pose, tf_original_pose);

	// rotate yaw pi in MRPT
	mrpt::poses::CPose3D rotation_mrpt;
	double yaw = M_PI / 2.0;
	rotation_mrpt.setFromValues(0, 0, 0, yaw, 0, 0);
	mrpt::poses::CPose3D mrpt_result = rotation_mrpt + mrpt_original_pose.mean;
	EXPECT_NEAR(mrpt_result[1], 1.0, 0.01);

	// rotate yaw pi in TF
	tf::Quaternion rotation_tf;
	rotation_tf.setRPY(0, 0, yaw);
	tf::Pose rotation_pose_tf;
	rotation_pose_tf.setIdentity();
	rotation_pose_tf.setRotation(rotation_tf);
	tf::Pose tf_result = rotation_pose_tf * tf_original_pose;
	EXPECT_NEAR(tf_result.getOrigin()[1], 1.0, 0.01);

	geometry_msgs::Pose mrpt_ros_result;
	mrpt_bridge::convert(mrpt_result, mrpt_ros_result);

	EXPECT_NEAR(mrpt_ros_result.position.x, tf_result.getOrigin()[0], 0.01);
	EXPECT_NEAR(mrpt_ros_result.position.y, tf_result.getOrigin()[1], 0.01);
	EXPECT_NEAR(mrpt_ros_result.position.z, tf_result.getOrigin()[2], 0.01);
}

void check_CPose3D_tofrom_ROS(
	double x, double y, double z, double yaw, double pitch, double roll)
{
	const mrpt::poses::CPose3D p3D(x, y, z, yaw, pitch, roll);

	// Convert MRPT->ROS
	geometry_msgs::Pose ros_p3D;
	mrpt_bridge::convert(p3D, ros_p3D);

	// Compare ROS quat vs. MRPT quat:
	mrpt::math::CQuaternionDouble q;
	p3D.getAsQuaternion(q);

	EXPECT_NEAR(ros_p3D.position.x, p3D.x(), 1e-4) << "p: " << p3D << endl;
	EXPECT_NEAR(ros_p3D.position.y, p3D.y(), 1e-4) << "p: " << p3D << endl;
	EXPECT_NEAR(ros_p3D.position.z, p3D.z(), 1e-4) << "p: " << p3D << endl;

	EXPECT_NEAR(ros_p3D.orientation.x, q.x(), 1e-4) << "p: " << p3D << endl;
	EXPECT_NEAR(ros_p3D.orientation.y, q.y(), 1e-4) << "p: " << p3D << endl;
	EXPECT_NEAR(ros_p3D.orientation.z, q.z(), 1e-4) << "p: " << p3D << endl;
	EXPECT_NEAR(ros_p3D.orientation.w, q.r(), 1e-4) << "p: " << p3D << endl;

	// Test the other path: ROS->MRPT
	mrpt::poses::CPose3D p_bis;
	mrpt_bridge::convert(ros_p3D, p_bis);

	// p_bis==p3D?
	EXPECT_NEAR(
		(p_bis.getAsVectorVal() - p3D.getAsVectorVal())
			.array()
			.abs()
			.maxCoeff(),
		0, 1e-4)
		<< "p_bis: " << p_bis << endl
		<< "p3D: " << p3D << endl;
}

// Declare a test
TEST(PoseConversions, check_CPose3D_tofrom_ROS)
{
	check_CPose3D_tofrom_ROS(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
	check_CPose3D_tofrom_ROS(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));

	check_CPose3D_tofrom_ROS(1, 2, 3, DEG2RAD(30), DEG2RAD(0), DEG2RAD(0));
	check_CPose3D_tofrom_ROS(1, 2, 3, DEG2RAD(0), DEG2RAD(30), DEG2RAD(0));
	check_CPose3D_tofrom_ROS(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(30));

	check_CPose3D_tofrom_ROS(1, 2, 3, DEG2RAD(-5), DEG2RAD(15), DEG2RAD(-30));

	check_CPose3D_tofrom_ROS(0, 0, 0, DEG2RAD(0), DEG2RAD(90), DEG2RAD(0));
	check_CPose3D_tofrom_ROS(0, 0, 0, DEG2RAD(0), DEG2RAD(-90), DEG2RAD(0));
}

// Declare a test
TEST(PoseConversions, check_CPose2D_to_ROS)
{
	const mrpt::poses::CPose2D p2D(1, 2, 0.56);

	// Convert MRPT->ROS
	geometry_msgs::Pose ros_p2D;
	mrpt_bridge::convert(p2D, ros_p2D);

	// Compare vs. 3D pose:
	const mrpt::poses::CPose3D p3D = mrpt::poses::CPose3D(p2D);
	mrpt::poses::CPose3D p3D_ros;
	mrpt_bridge::convert(ros_p2D, p3D_ros);

	// p3D_ros should equal p3D
	EXPECT_NEAR(
		(p3D_ros.getAsVectorVal() - p3D.getAsVectorVal())
			.array()
			.abs()
			.maxCoeff(),
		0, 1e-4)
		<< "p3D_ros: " << p3D_ros << endl
		<< "p3D: " << p3D << endl;
}


#ifndef CPPMATH_GEOMETROY_GEOMETRY_HPP_
#define CPPMATH_GEOMETROY_GEOMETRY_HPP_

#include <cmath>  // fabs()
#include <limits>  // limits<double>

#include <Eigen/Core>

namespace cppmath
{
    /**
     * Helper functions for geometric computations.
     *
     * \author cpieloth
     * \copyright Copyright 2014 Christof Pieloth, Licensed under the Apache License, Version 2.0
     */
    namespace geometry
    {
        typedef Eigen::Vector3d Vector3T;
        typedef Eigen::Matrix3Xd PointsT;
        typedef Eigen::Matrix3d Matrix3T;

        Matrix3T getRotationXYZMatrix( double x, double y, double z );

        /**
         * Calculates an orthogonal vector to v.
         *
         * \param o An orthogonal vector of v.
         * \param v Input vector.
         * \return True if o contains an orthogonal vector of v.
         */
        bool findOrthogonalVector( Vector3T* const o, const Vector3T& v );

        /**
         * Calculates an tangent plane for the normal vector n. u, v and n are orthogonal to each other.
         *
         * \param u Plane vector 1 for parametrically description.
         * \param v Plane vector 2 for parametrically description.
         * \param n Normal vector.
         * \return True if u and v contain a plane vector.
         */
        bool findTangentPlane( Vector3T* const u, Vector3T* const v, const Vector3T& n );
    }
}

#endif  // CPPMATH_GEOMETROY_GEOMETRY_HPP_


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#include <cmath>
#include <numeric>

#include <boost/math/special_functions/erf.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/lognormal.hpp>

#include "tudat/math/statistics/multiVariateGaussianProbabilityDistributions.h"
namespace tudat
{
namespace statistics
{

//! Function to evaluate pdf of Gaussian cupola distribution
double GaussianCopulaDistributionXd::evaluatePdf(
        const Eigen::VectorXd& independentVariables )
{
    double probabilityDensity = 0.0 ;

    // Check if vector independentVariables is inside [0,1]
    int inBound = 0 ;
    for( int i = 0 ; i < dimension_ ; i++ )
    {
        if( independentVariables(i) > 0.0 && independentVariables(i) < 1.0 )
        {
            inBound++;
        }
    }

    // If data is in bounds
    if( inBound == dimension_ )
    {
        // Convert U[0,1] to N[0,1] using inverse CDF of standard normal distribution
        Eigen::VectorXd gaussianQuantiles( dimension_ ) ;
        boost::math::normal distribution( 0.0 , 1.0 );

        for( int i = 0 ; i < dimension_ ; i++ )
        {
            gaussianQuantiles( i ) = boost::math::quantile( distribution , independentVariables( i ) ); // Inverse cdf
        }

        // Calculate probability density
        Eigen::MatrixXd location = - 0.5 * ( gaussianQuantiles.transpose() *
                       ( inverseCorrelationMatrix_ - Eigen::MatrixXd::Identity( dimension_ , dimension_ ) ) *
                                             gaussianQuantiles );

        probabilityDensity = ( ( 1.0 / ( std::sqrt( determinant_ ) ) ) * std::exp( location( 0, 0 ) ) ) ;
    }

    return probabilityDensity ;
}

} // namespace statistics
} // namespace tudat


#include <algorithm>
#include <iostream>
#include <math.h>
#include <malloc.h>
#include <NTL/ZZ.h>

inline bool incstates (int *states,int fields,int avoid){
    states[--fields]++;
	if(states[fields]==avoid)states[fields]++;
	while (fields&&states[fields]>9){
		states[fields]=0;
		states[--fields]++;
		if(states[fields]==avoid)states[fields]++;
	}
	return (states[0] > 9)?(states[0]=0,false):true;
}

int main()
{
	ZZ totalsum = ZZ::zero();
	ZZ zzOne = to_ZZ(1);
	ZZ mul;
	ZZ num;
	ZZ min = to_ZZ(1000000000);
	int a[] = {0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1};
    int states[] = {0,0,0,0,0,0,0,0};
	int nxt;
	for (int i = 0;i < 10;i++){
		bool found = false;
		for(int rep=1;rep<4&&!found;replaced++){
			do{
				do{
					nxt = 0;
					num = ZZ::zero();
					mul = min * 10;
					for(int k = rep;k<rep+10;k++)num+=((mul/=10)*((a[k])?states[nxt++]:i));
					if(ProbPrime(num) && num>min)found = (totalsum+=num,true);
				}while(incstates(states,rep,i));
			}while (std::next_permutation(a+rep,a+10+rep));
		}
	}
	cout << totalsum << "\n";
	return free(a);
}


/*
 [auto_generated]
 boost/numeric/odeint/stepper/bulirsch_stoer_dense_out.hpp

 [begin_description]
 Implementaiton of the Burlish-Stoer method with dense output
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

 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 BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_DENSE_OUT_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_DENSE_OUT_HPP_INCLUDED

#include <iostream>

#include <algorithm>

#include <boost/config.hpp> // for min/max guidelines

#include <boost/numeric/odeint/util/bind.hpp>

#include <boost/math/special_functions/binomial.hpp>

#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/modified_midpoint.hpp>
#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/unit_helper.hpp>

#include <boost/type_traits.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template<
    class State ,
    class Value = double ,
    class Deriv = State ,
    class Time = Value ,
    class Algebra = typename algebra_dispatcher< State >::algebra_type ,
    class Operations = typename operations_dispatcher< State >::operations_type ,
    class Resizer = initially_resizer
    >
class bulirsch_stoer_dense_out {

public:

    typedef State state_type;
    typedef Value value_type;
    typedef Deriv deriv_type;
    typedef Time time_type;
    typedef Algebra algebra_type;
    typedef Operations operations_type;
    typedef Resizer resizer_type;
    typedef dense_output_stepper_tag stepper_category;
#ifndef DOXYGEN_SKIP
    typedef state_wrapper< state_type > wrapped_state_type;
    typedef state_wrapper< deriv_type > wrapped_deriv_type;

    typedef bulirsch_stoer_dense_out< State , Value , Deriv , Time , Algebra , Operations , Resizer > controlled_error_bs_type;

    typedef typename inverse_time< time_type >::type inv_time_type;

    typedef std::vector< value_type > value_vector;
    typedef std::vector< time_type > time_vector;
    typedef std::vector< inv_time_type > inv_time_vector;  //should be 1/time_type for boost.units
    typedef std::vector< value_vector > value_matrix;
    typedef std::vector< size_t > int_vector;
    typedef std::vector< wrapped_state_type > state_vector_type;
    typedef std::vector< wrapped_deriv_type > deriv_vector_type;
    typedef std::vector< deriv_vector_type > deriv_table_type;
#endif //DOXYGEN_SKIP

    const static size_t m_k_max = 8;

    bulirsch_stoer_dense_out(
        value_type eps_abs = 1E-6 , value_type eps_rel = 1E-6 ,
        value_type factor_x = 1.0 , value_type factor_dxdt = 1.0 ,
        bool control_interpolation = false )
        : m_error_checker( eps_abs , eps_rel , factor_x, factor_dxdt ) ,
          m_control_interpolation( control_interpolation) ,
          m_last_step_rejected( false ) , m_first( true ) ,
          m_current_state_x1( true ) ,
          m_error( m_k_max ) ,
          m_interval_sequence( m_k_max+1 ) ,
          m_coeff( m_k_max+1 ) ,
          m_cost( m_k_max+1 ) ,
          m_table( m_k_max ) ,
          m_mp_states( m_k_max+1 ) ,
          m_derivs( m_k_max+1 ) ,
          m_diffs( 2*m_k_max+1 ) ,
          STEPFAC1( 0.65 ) , STEPFAC2( 0.94 ) , STEPFAC3( 0.02 ) , STEPFAC4( 4.0 ) , KFAC1( 0.8 ) , KFAC2( 0.9 )
    {
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();

        for( unsigned short i = 0; i < m_k_max+1; i++ )
        {
            /* only this specific sequence allows for dense output */
            m_interval_sequence[i] = 2 + 4*i;  // 2 6 10 14 ...
            m_derivs[i].resize( m_interval_sequence[i] );
            if( i == 0 )
                m_cost[i] = m_interval_sequence[i];
            else
                m_cost[i] = m_cost[i-1] + m_interval_sequence[i];
            m_coeff[i].resize(i);
            for( size_t k = 0 ; k < i ; ++k  )
            {
                const value_type r = static_cast< value_type >( m_interval_sequence[i] ) / static_cast< value_type >( m_interval_sequence[k] );
                m_coeff[i][k] = 1.0 / ( r*r - static_cast< value_type >( 1.0 ) ); // coefficients for extrapolation
            }
            // crude estimate of optimal order

            m_current_k_opt = 4;
            /* no calculation because log10 might not exist for value_type!
            const value_type logfact( -log10( max BOOST_PREVENT_MACRO_SUBSTITUTION( eps_rel , static_cast< value_type >( 1.0E-12 ) ) ) * 0.6 + 0.5 );
            m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( 1 , min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>( m_k_max-1 ) , static_cast<int>( logfact ) ));
            */
        }
        int num = 1;
        for( int i = 2*(m_k_max) ; i >=0  ; i-- )
        {
            m_diffs[i].resize( num );
            num += (i+1)%2;
        }
    }

    template< class System , class StateIn , class DerivIn , class StateOut , class DerivOut >
    controlled_step_result try_step( System system , const StateIn &in , const DerivIn &dxdt , time_type &t , StateOut &out , DerivOut &dxdt_new , time_type &dt )
    {
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        using std::pow;

        static const value_type val1( 1.0 );

        typename odeint::unwrap_reference< System >::type &sys = system;

        bool reject( true );

        time_vector h_opt( m_k_max+1 );
        inv_time_vector work( m_k_max+1 );

        m_k_final = 0;
        time_type new_h = dt;

        //std::cout << "t=" << t <<", dt=" << dt << ", k_opt=" << m_current_k_opt << ", first: " << m_first << std::endl;

        for( size_t k = 0 ; k <= m_current_k_opt+1 ; k++ )
        {
            m_midpoint.set_steps( m_interval_sequence[k] );
            if( k == 0 )
            {
                m_midpoint.do_step( sys , in , dxdt , t , out , dt , m_mp_states[k].m_v , m_derivs[k]);
            }
            else
            {
                m_midpoint.do_step( sys , in , dxdt , t , m_table[k-1].m_v , dt , m_mp_states[k].m_v , m_derivs[k] );
                extrapolate( k , m_table , m_coeff , out );
                // get error estimate
                m_algebra.for_each3( m_err.m_v , out , m_table[0].m_v ,
                                     typename operations_type::template scale_sum2< value_type , value_type >( val1 , -val1 ) );
                const value_type error = m_error_checker.error( m_algebra , in , dxdt , m_err.m_v , dt );
                h_opt[k] = calc_h_opt( dt , error , k );
                work[k] = static_cast<value_type>( m_cost[k] ) / h_opt[k];

                m_k_final = k;

                if( (k == m_current_k_opt-1) || m_first )
                { // convergence before k_opt ?
                    if( error < 1.0 )
                    {
                        //convergence
                        reject = false;
                        if( (work[k] < KFAC2*work[k-1]) || (m_current_k_opt <= 2) )
                        {
                            // leave order as is (except we were in first round)
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(k)+1 ) );
                            new_h = h_opt[k] * static_cast<value_type>( m_cost[k+1] ) / static_cast<value_type>( m_cost[k] );
                        } else {
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(k) ) );
                            new_h = h_opt[k];
                        }
                        break;
                    }
                    else if( should_reject( error , k ) && !m_first )
                    {
                        reject = true;
                        new_h = h_opt[k];
                        break;
                    }
                }
                if( k == m_current_k_opt )
                { // convergence at k_opt ?
                    if( error < 1.0 )
                    {
                        //convergence
                        reject = false;
                        if( (work[k-1] < KFAC2*work[k]) )
                        {
                            m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(m_current_k_opt)-1 );
                            new_h = h_opt[m_current_k_opt];
                        }
                        else if( (work[k] < KFAC2*work[k-1]) && !m_last_step_rejected )
                        {
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , static_cast<int>(m_current_k_opt)+1 );
                            new_h = h_opt[k]*static_cast<value_type>( m_cost[m_current_k_opt] ) / static_cast<value_type>( m_cost[k] );
                        } else
                            new_h = h_opt[m_current_k_opt];
                        break;
                    }
                    else if( should_reject( error , k ) )
                    {
                        reject = true;
                        new_h = h_opt[m_current_k_opt];
                        break;
                    }
                }
                if( k == m_current_k_opt+1 )
                { // convergence at k_opt+1 ?
                    if( error < 1.0 )
                    {   //convergence
                        reject = false;
                        if( work[k-2] < KFAC2*work[k-1] )
                            m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(m_current_k_opt)-1 );
                        if( (work[k] < KFAC2*work[m_current_k_opt]) && !m_last_step_rejected )
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , static_cast<int>(k) );
                        new_h = h_opt[m_current_k_opt];
                    } else
                    {
                        reject = true;
                        new_h = h_opt[m_current_k_opt];
                    }
                    break;
                }
            }
        }

        if( !reject )
        {

            //calculate dxdt for next step and dense output
            sys( out , dxdt_new , t+dt );

            //prepare dense output
            value_type error = prepare_dense_output( m_k_final , in , dxdt , out , dxdt_new , dt );

            if( error > static_cast<value_type>(10) ) // we are not as accurate for interpolation as for the steps
            {
                reject = true;
                new_h = dt * pow BOOST_PREVENT_MACRO_SUBSTITUTION( error , static_cast<value_type>(-1)/(2*m_k_final+2) );
            } else {
                t += dt;
            }
        }
        //set next stepsize
        if( !m_last_step_rejected || (new_h < dt) )
            dt = new_h;

        m_last_step_rejected = reject;
        if( reject )
            return fail;
        else
            return success;
    }

    template< class StateType >
    void initialize( const StateType &x0 , const time_type &t0 , const time_type &dt0 )
    {
        m_resizer.adjust_size( x0 , detail::bind( &controlled_error_bs_type::template resize_impl< StateType > , detail::ref( *this ) , detail::_1 ) );
        boost::numeric::odeint::copy( x0 , get_current_state() );
        m_t = t0;
        m_dt = dt0;
        reset();
    }

    /*  =======================================================
     *  the actual step method that should be called from outside (maybe make try_step private?)
     */
    template< class System >
    std::pair< time_type , time_type > do_step( System system )
    {
        const size_t max_count = 1000;

        if( m_first )
        {
            typename odeint::unwrap_reference< System >::type &sys = system;
            sys( get_current_state() , get_current_deriv() , m_t );
        }

        controlled_step_result res = fail;
        m_t_last = m_t;
        size_t count = 0;
        while( res == fail )
        {
            res = try_step( system , get_current_state() , get_current_deriv() , m_t , get_old_state() , get_old_deriv() , m_dt );
            m_first = false;
            if( count++ == max_count )
                throw std::overflow_error( "bulirsch_stoer : too much iterations!");
        }
        toggle_current_state();
        return std::make_pair( m_t_last , m_t );
    }

    /* performs the interpolation from a calculated step */
    template< class StateOut >
    void calc_state( time_type t , StateOut &x ) const
    {
        do_interpolation( t , x );
    }

    const state_type& current_state( void ) const
    {
        return get_current_state();
    }

    time_type current_time( void ) const
    {
        return m_t;
    }

    const state_type& previous_state( void ) const
    {
        return get_old_state();
    }

    time_type previous_time( void ) const
    {
        return m_t_last;
    }

    time_type current_time_step( void ) const
    {
        return m_dt;
    }

    /** \brief Resets the internal state of the stepper. */
    void reset()
    {
        m_first = true;
        m_last_step_rejected = false;
    }

    template< class StateIn >
    void adjust_size( const StateIn &x )
    {
        resize_impl( x );
        m_midpoint.adjust_size();
    }

private:

    template< class StateInOut , class StateVector >
    void extrapolate( size_t k , StateVector &table , const value_matrix &coeff , StateInOut &xest , size_t order_start_index = 0 )
    //polynomial extrapolation, see http://www.nr.com/webnotes/nr3web21.pdf
    {
        static const value_type val1( 1.0 );
        for( int j=k-1 ; j>0 ; --j )
        {
            m_algebra.for_each3( table[j-1].m_v , table[j].m_v , table[j-1].m_v ,
                                 typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k + order_start_index][j + order_start_index] ,
                                                                                -coeff[k + order_start_index][j + order_start_index] ) );
        }
        m_algebra.for_each3( xest , table[0].m_v , xest ,
                             typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k + order_start_index][0 + order_start_index] ,
                                                                                -coeff[k + order_start_index][0 + order_start_index]) );
    }

    template< class StateVector >
    void extrapolate_dense_out( size_t k , StateVector &table , const value_matrix &coeff , size_t order_start_index = 0 )
    //polynomial extrapolation, see http://www.nr.com/webnotes/nr3web21.pdf
    {
        // result is written into table[0]
        static const value_type val1( 1.0 );
        for( int j=k ; j>1 ; --j )
        {
            m_algebra.for_each3( table[j-1].m_v , table[j].m_v , table[j-1].m_v ,
                                 typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k + order_start_index][j + order_start_index - 1] ,
                                                                                -coeff[k + order_start_index][j + order_start_index - 1] ) );
        }
        m_algebra.for_each3( table[0].m_v , table[1].m_v , table[0].m_v ,
                             typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k + order_start_index][order_start_index] ,
                                                                                -coeff[k + order_start_index][order_start_index]) );
    }

    time_type calc_h_opt( time_type h , value_type error , size_t k ) const
    {
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        using std::pow;

        value_type expo=1.0/(m_interval_sequence[k-1]);
        value_type facmin = pow BOOST_PREVENT_MACRO_SUBSTITUTION( STEPFAC3 , expo );
        value_type fac;
        if (error == 0.0)
            fac=1.0/facmin;
        else
        {
            fac = STEPFAC2 / pow BOOST_PREVENT_MACRO_SUBSTITUTION( error / STEPFAC1 , expo );
            fac = max BOOST_PREVENT_MACRO_SUBSTITUTION( facmin/STEPFAC4 , min BOOST_PREVENT_MACRO_SUBSTITUTION( 1.0/facmin , fac ) );
        }
        return h*fac;
    }

    bool in_convergence_window( size_t k ) const
    {
        if( (k == m_current_k_opt-1) && !m_last_step_rejected )
            return true; // decrease order only if last step was not rejected
        return ( (k == m_current_k_opt) || (k == m_current_k_opt+1) );
    }

    bool should_reject( value_type error , size_t k ) const
    {
        if( k == m_current_k_opt-1 )
        {
            const value_type d = m_interval_sequence[m_current_k_opt] * m_interval_sequence[m_current_k_opt+1] /
                (m_interval_sequence[0]*m_interval_sequence[0]);
            //step will fail, criterion 17.3.17 in NR
            return ( error > d*d );
        }
        else if( k == m_current_k_opt )
        {
            const value_type d = m_interval_sequence[m_current_k_opt+1] / m_interval_sequence[0];
            return ( error > d*d );
        } else
            return error > 1.0;
    }

    template< class StateIn1 , class DerivIn1 , class StateIn2 , class DerivIn2 >
    value_type prepare_dense_output( int k , const StateIn1 &x_start , const DerivIn1 &dxdt_start ,
                                     const StateIn2 & /* x_end */ , const DerivIn2 & /*dxdt_end */ , time_type dt )
    /* k is the order to which the result was approximated */
    {

        /* compute the coefficients of the interpolation polynomial
         * we parametrize the interval t .. t+dt by theta = -1 .. 1
         * we use 2k+3 values at the interval center theta=0 to obtain the interpolation coefficients
         * the values are x(t+dt/2) and the derivatives dx/dt , ... d^(2k+2) x / dt^(2k+2) at the midpoints
         * the derivatives are approximated via finite differences
         * all values are obtained from interpolation of the results from the increasing orders of the midpoint calls
         */

        // calculate finite difference approximations to derivatives at the midpoint
        for( int j = 0 ; j<=k ; j++ )
        {
            /* not working with boost units... */
            const value_type d = m_interval_sequence[j] / ( static_cast<value_type>(2) * dt );
            value_type f = 1.0; //factor 1/2 here because our interpolation interval has length 2 !!!
            for( int kappa = 0 ; kappa <= 2*j+1 ; ++kappa )
            {
                calculate_finite_difference( j , kappa , f , dxdt_start );
                f *= d;
            }

            if( j > 0 )
                extrapolate_dense_out( j , m_mp_states , m_coeff );
        }

        time_type d = dt/2;

        // extrapolate finite differences
        for( int kappa = 0 ; kappa<=2*k+1 ; kappa++ )
        {
            for( int j=1 ; j<=(k-kappa/2) ; ++j )
                extrapolate_dense_out( j , m_diffs[kappa] , m_coeff , kappa/2 );

            // extrapolation results are now stored in m_diffs[kappa][0]

            // divide kappa-th derivative by kappa because we need these terms for dense output interpolation
            m_algebra.for_each1( m_diffs[kappa][0].m_v , typename operations_type::template scale< time_type >( static_cast<time_type>(d) ) );

            d *= dt/(2*(kappa+2));
        }

        // dense output coefficients a_0 is stored in m_mp_states[0], a_i for i = 1...2k are stored in m_diffs[i-1][0]

        // the error is just the highest order coefficient of the interpolation polynomial
        // this is because we use only the midpoint theta=0 as support for the interpolation (remember that theta = -1 .. 1)

        value_type error = 0.0;
        if( m_control_interpolation )
        {
            boost::numeric::odeint::copy( m_diffs[2*k+1][0].m_v , m_err.m_v );
            error = m_error_checker.error( m_algebra , x_start , dxdt_start , m_err.m_v , dt );
        }

        return error;
    }

    template< class DerivIn >
    void calculate_finite_difference( size_t j , size_t kappa , value_type fac , const DerivIn &dxdt )
    {
        const int m = m_interval_sequence[j]/2-1;
        if( kappa == 0) // no calculation required for 0th derivative of f
        {
            m_algebra.for_each2( m_diffs[0][j].m_v , m_derivs[j][m].m_v ,
                                 typename operations_type::template scale_sum1< value_type >( fac ) );
        }
        else
        {
            // calculate the index of m_diffs for this kappa-j-combination
            const int j_diffs = j - kappa/2;

            m_algebra.for_each2( m_diffs[kappa][j_diffs].m_v , m_derivs[j][m+kappa].m_v ,
                                 typename operations_type::template scale_sum1< value_type >( fac ) );
            value_type sign = -1.0;
            int c = 1;
            //computes the j-th order finite difference for the kappa-th derivative of f at t+dt/2 using function evaluations stored in m_derivs
            for( int i = m+static_cast<int>(kappa)-2 ; i >= m-static_cast<int>(kappa) ; i -= 2 )
            {
                if( i >= 0 )
                {
                    m_algebra.for_each3( m_diffs[kappa][j_diffs].m_v , m_diffs[kappa][j_diffs].m_v , m_derivs[j][i].m_v ,
                                         typename operations_type::template scale_sum2< value_type , value_type >( 1.0 ,
                                                                                sign * fac * boost::math::binomial_coefficient< value_type >( kappa , c ) ) );
                }
                else
                {
                    m_algebra.for_each3( m_diffs[kappa][j_diffs].m_v , m_diffs[kappa][j_diffs].m_v , dxdt ,
                                         typename operations_type::template scale_sum2< value_type , value_type >( 1.0 , sign * fac ) );
                }
                sign *= -1;
                ++c;
            }
        }
    }

    template< class StateOut >
    void do_interpolation( time_type t , StateOut &out ) const
    {
        // interpolation polynomial is defined for theta = -1 ... 1
        // m_k_final is the number of order-iterations done for the last step - it governs the order of the interpolation polynomial
        const value_type theta = 2 * get_unit_value( (t - m_t_last) / (m_t - m_t_last) ) - 1;
        // we use only values at interval center, that is theta=0, for interpolation
        // our interpolation polynomial is thus of order 2k+2, hence we have 2k+3 terms

        boost::numeric::odeint::copy( m_mp_states[0].m_v , out );
        // add remaining terms: x += a_1 theta + a2 theta^2 + ... + a_{2k} theta^{2k}
        value_type theta_pow( theta );
        for( size_t i=0 ; i<=2*m_k_final+1 ; ++i )
        {
            m_algebra.for_each3( out , out , m_diffs[i][0].m_v ,
                                 typename operations_type::template scale_sum2< value_type >( static_cast<value_type>(1) , theta_pow ) );
            theta_pow *= theta;
        }
    }

    /* Resizer methods */
    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized( false );

        resized |= adjust_size_by_resizeability( m_x1 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_x2 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_dxdt1 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_dxdt2 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_err , x , typename is_resizeable<state_type>::type() );

        for( size_t i = 0 ; i < m_k_max ; ++i )
            resized |= adjust_size_by_resizeability( m_table[i] , x , typename is_resizeable<state_type>::type() );
        for( size_t i = 0 ; i < m_k_max+1 ; ++i )
            resized |= adjust_size_by_resizeability( m_mp_states[i] , x , typename is_resizeable<state_type>::type() );
        for( size_t i = 0 ; i < m_k_max+1 ; ++i )
            for( size_t j = 0 ; j < m_derivs[i].size() ; ++j )
                resized |= adjust_size_by_resizeability( m_derivs[i][j] , x , typename is_resizeable<deriv_type>::type() );
        for( size_t i = 0 ; i < 2*m_k_max+1 ; ++i )
            for( size_t j = 0 ; j < m_diffs[i].size() ; ++j )
                resized |= adjust_size_by_resizeability( m_diffs[i][j] , x , typename is_resizeable<deriv_type>::type() );

        return resized;
    }

    state_type& get_current_state( void )
    {
        return m_current_state_x1 ? m_x1.m_v : m_x2.m_v ;
    }

    const state_type& get_current_state( void ) const
    {
        return m_current_state_x1 ? m_x1.m_v : m_x2.m_v ;
    }

    state_type& get_old_state( void )
    {
        return m_current_state_x1 ? m_x2.m_v : m_x1.m_v ;
    }

    const state_type& get_old_state( void ) const
    {
        return m_current_state_x1 ? m_x2.m_v : m_x1.m_v ;
    }

    deriv_type& get_current_deriv( void )
    {
        return m_current_state_x1 ? m_dxdt1.m_v : m_dxdt2.m_v ;
    }

    const deriv_type& get_current_deriv( void ) const
    {
        return m_current_state_x1 ? m_dxdt1.m_v : m_dxdt2.m_v ;
    }

    deriv_type& get_old_deriv( void )
    {
        return m_current_state_x1 ? m_dxdt2.m_v : m_dxdt1.m_v ;
    }

    const deriv_type& get_old_deriv( void ) const
    {
        return m_current_state_x1 ? m_dxdt2.m_v : m_dxdt1.m_v ;
    }

    void toggle_current_state( void )
    {
        m_current_state_x1 = ! m_current_state_x1;
    }

    default_error_checker< value_type, algebra_type , operations_type > m_error_checker;
    modified_midpoint_dense_out< state_type , value_type , deriv_type , time_type , algebra_type , operations_type , resizer_type > m_midpoint;

    bool m_control_interpolation;

    bool m_last_step_rejected;
    bool m_first;

    time_type m_t;
    time_type m_dt;
    time_type m_dt_last;
    time_type m_t_last;

    size_t m_current_k_opt;
    size_t m_k_final;

    algebra_type m_algebra;

    resizer_type m_resizer;

    wrapped_state_type m_x1 , m_x2;
    wrapped_deriv_type m_dxdt1 , m_dxdt2;
    wrapped_state_type m_err;
    bool m_current_state_x1;

    value_vector m_error; // errors of repeated midpoint steps and extrapolations
    int_vector m_interval_sequence; // stores the successive interval counts
    value_matrix m_coeff;
    int_vector m_cost; // costs for interval count

    state_vector_type m_table; // sequence of states for extrapolation

    //for dense output:
    state_vector_type m_mp_states; // sequence of approximations of x at distance center
    deriv_table_type m_derivs; // table of function values
    deriv_table_type m_diffs; // table of function values

    //wrapped_state_type m_a1 , m_a2 , m_a3 , m_a4;

    const value_type STEPFAC1 , STEPFAC2 , STEPFAC3 , STEPFAC4 , KFAC1 , KFAC2;
};

/********** DOXYGEN **********/

/**
 * \class bulirsch_stoer_dense_out
 * \brief The Bulirsch-Stoer algorithm.
 *
 * The Bulirsch-Stoer is a controlled stepper that adjusts both step size
 * and order of the method. The algorithm uses the modified midpoint and
 * a polynomial extrapolation compute the solution. This class also provides
 * dense output facility.
 *
 * \tparam State The state type.
 * \tparam Value The value type.
 * \tparam Deriv The type representing the time derivative of the state.
 * \tparam Time The time representing the independent variable - the time.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

    /**
     * \fn bulirsch_stoer_dense_out::bulirsch_stoer_dense_out( value_type eps_abs , value_type eps_rel , value_type factor_x , value_type factor_dxdt , bool control_interpolation )
     * \brief Constructs the bulirsch_stoer class, including initialization of
     * the error bounds.
     *
     * \param eps_abs Absolute tolerance level.
     * \param eps_rel Relative tolerance level.
     * \param factor_x Factor for the weight of the state.
     * \param factor_dxdt Factor for the weight of the derivative.
     * \param control_interpolation Set true to additionally control the error of
     * the interpolation.
     */

    /**
     * \fn bulirsch_stoer_dense_out::try_step( System system , const StateIn &in , const DerivIn &dxdt , time_type &t , StateOut &out , DerivOut &dxdt_new , time_type &dt )
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed. Also, the internal order of the stepper is adjusted if required.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE.
     * It must fulfill the Simple System concept.
     * \param in The state of the ODE which should be solved.
     * \param dxdt The derivative of state.
     * \param t The value of the time. Updated if the step is successful.
     * \param out Used to store the result of the step.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */

    /**
     * \fn bulirsch_stoer_dense_out::initialize( const StateType &x0 , const time_type &t0 , const time_type &dt0 )
     * \brief Initializes the dense output stepper.
     *
     * \param x0 The initial state.
     * \param t0 The initial time.
     * \param dt0 The initial time step.
     */

    /**
     * \fn bulirsch_stoer_dense_out::do_step( System system )
     * \brief Does one time step. This is the main method that should be used to
     * integrate an ODE with this stepper.
     * \note initialize has to be called before using this method to set the
     * initial conditions x,t and the stepsize.
     * \param system The system function to solve, hence the r.h.s. of the
     * ordinary differential equation. It must fulfill the Simple System concept.
     * \return Pair with start and end time of the integration step.
     */

    /**
     * \fn bulirsch_stoer_dense_out::calc_state( time_type t , StateOut &x ) const
     * \brief Calculates the solution at an intermediate point within the last step
     * \param t The time at which the solution should be calculated, has to be
     * in the current time interval.
     * \param x The output variable where the result is written into.
     */

    /**
     * \fn bulirsch_stoer_dense_out::current_state( void ) const
     * \brief Returns the current state of the solution.
     * \return The current state of the solution x(t).
     */

    /**
     * \fn bulirsch_stoer_dense_out::current_time( void ) const
     * \brief Returns the current time of the solution.
     * \return The current time of the solution t.
     */

    /**
     * \fn bulirsch_stoer_dense_out::previous_state( void ) const
     * \brief Returns the last state of the solution.
     * \return The last state of the solution x(t-dt).
     */

    /**
     * \fn bulirsch_stoer_dense_out::previous_time( void ) const
     * \brief Returns the last time of the solution.
     * \return The last time of the solution t-dt.
     */

    /**
     * \fn bulirsch_stoer_dense_out::current_time_step( void ) const
     * \brief Returns the current step size.
     * \return The current step size.
     */

    /**
     * \fn bulirsch_stoer_dense_out::adjust_size( const StateIn &x )
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */

}
}
}

#endif // BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_HPP_INCLUDED


#include <gurobi_c++.h>
#include <iostream>
#include <cassert>
#include <boost/filesystem.hpp>
using namespace std;
namespace fs = boost::filesystem;

int main() {
  fs::remove(fs::path("example.log"));

  try {
    GRBEnv env("example.log");
    GRBModel model(env);
    model.set(GRB_StringAttr_ModelName, "model1");

    auto x = model.addVar(0.0, 1.0, 0.0, GRB_BINARY, "x");
    auto y = model.addVar(0.0, 1.0, 0.0, GRB_BINARY, "y");
    auto z = model.addVar(0.0, 1.0, 0.0, GRB_BINARY, "z");
    model.update();

    model.setObjective(x + 2.0 * y + z, GRB_MINIMIZE);

    model.addConstr(x + y + z <= 2.0, "c0");
    model.addConstr(x + y == 1.0, "c1");

    model.optimize();

    assert(model.get(GRB_DoubleAttr_ObjVal) == 1.0);
    assert(x.get(GRB_DoubleAttr_X) == 1);
    assert(y.get(GRB_DoubleAttr_X) == 0);
    assert(z.get(GRB_DoubleAttr_X) == 0);

  } catch (GRBException e) {
    cout << e.getErrorCode() << endl;
    cout << e.getMessage() << endl;
    return 1;
  }

  assert(fs::exists(fs::path("example.log")));
}