// Boost.Geometry Index
//
// R-tree distance (knn, path, etc. ) query visitor implementation
//
// Copyright (c) 2011-2014 Adam Wulkiewicz, Lodz, Poland.
//
// This file was modified by Oracle on 2019-2021.
// Modifications copyright (c) 2019-2021 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
//
// 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)

#ifndef BOOST_GEOMETRY_INDEX_DETAIL_RTREE_VISITORS_DISTANCE_QUERY_HPP
#define BOOST_GEOMETRY_INDEX_DETAIL_RTREE_VISITORS_DISTANCE_QUERY_HPP

#include <queue>

#include <boost/geometry/index/detail/distance_predicates.hpp>
#include <boost/geometry/index/detail/predicates.hpp>
#include <boost/geometry/index/detail/priority_dequeue.hpp>
#include <boost/geometry/index/detail/rtree/node/node_elements.hpp>
#include <boost/geometry/index/detail/translator.hpp>
#include <boost/geometry/index/parameters.hpp>

namespace boost { namespace geometry { namespace index {

namespace detail { namespace rtree { namespace visitors {

struct pair_first_less
{
    template <typename First, typename Second>
    inline bool operator()(std::pair<First, Second> const& p1,
                           std::pair<First, Second> const& p2) const
    {
        return p1.first < p2.first;
    }
};

struct pair_first_greater
{
    template <typename First, typename Second>
    inline bool operator()(std::pair<First, Second> const& p1,
                           std::pair<First, Second> const& p2) const
    {
        return p1.first > p2.first;
    }
};

template <typename T, typename Comp>
struct priority_dequeue : index::detail::priority_dequeue<T, std::vector<T>, Comp>
{
    priority_dequeue() = default;
    //void reserve(typename std::vector<T>::size_type n)
    //{
    //    this->c.reserve(n);
    //}
    //void clear()
    //{
    //    this->c.clear();
    //}
};

template <typename T, typename Comp>
struct priority_queue : std::priority_queue<T, std::vector<T>, Comp>
{
    priority_queue() = default;
    //void reserve(typename std::vector<T>::size_type n)
    //{
    //    this->c.reserve(n);
    //}
    void clear()
    {
        this->c.clear();
    }
};

struct branch_data_comp
{
    template <typename BranchData>
    bool operator()(BranchData const& b1, BranchData const& b2) const
    {
        return b1.distance > b2.distance || (b1.distance == b2.distance && b1.reverse_level > b2.reverse_level);
    }
};

template <typename DistanceType, typename Value>
class distance_query_result
{
    using neighbor_data = std::pair<DistanceType, const Value *>;
    using neighbors_type = std::vector<neighbor_data>;
    using size_type = typename neighbors_type::size_type;

public:
    inline distance_query_result(size_type k)
        : m_count(k)
    {
        m_neighbors.reserve(m_count);
    }

    // NOTE: Do not call if max_count() == 0
    inline void store(DistanceType const& distance, const Value * value_ptr)
    {
        if (m_neighbors.size() < m_count)
        {
            m_neighbors.push_back(std::make_pair(distance, value_ptr));

            if (m_neighbors.size() == m_count)
            {
                std::make_heap(m_neighbors.begin(), m_neighbors.end(), pair_first_less());
            }
        }
        else if (distance < m_neighbors.front().first)
        {
            std::pop_heap(m_neighbors.begin(), m_neighbors.end(), pair_first_less());
            m_neighbors.back().first = distance;
            m_neighbors.back().second = value_ptr;
            std::push_heap(m_neighbors.begin(), m_neighbors.end(), pair_first_less());
        }
    }

    // NOTE: Do not call if max_count() == 0
    inline bool ignore_branch(DistanceType const& distance) const
    {
        return m_neighbors.size() == m_count
            && m_neighbors.front().first <= distance;
    }

    template <typename OutIt>
    inline size_type finish(OutIt out_it) const
    {
        for (auto const& p : m_neighbors)
        {
            *out_it = *(p.second);
            ++out_it;
        }

        return m_neighbors.size();
    }

    size_type max_count() const
    {
        return m_count;
    }

private:
    size_type m_count;
    neighbors_type m_neighbors;
};

template <typename MembersHolder, typename Predicates>
class distance_query
{
    typedef typename MembersHolder::value_type value_type;
    typedef typename MembersHolder::box_type box_type;
    typedef typename MembersHolder::parameters_type parameters_type;
    typedef typename MembersHolder::translator_type translator_type;

    typedef typename index::detail::strategy_type<parameters_type>::type strategy_type;

    typedef typename MembersHolder::node node;
    typedef typename MembersHolder::internal_node internal_node;
    typedef typename MembersHolder::leaf leaf;

    typedef index::detail::predicates_element
        <
            index::detail::predicates_find_distance<Predicates>::value, Predicates
        > nearest_predicate_access;
    typedef typename nearest_predicate_access::type nearest_predicate_type;
    typedef typename indexable_type<translator_type>::type indexable_type;

    typedef index::detail::calculate_distance<nearest_predicate_type, indexable_type, strategy_type, value_tag> calculate_value_distance;
    typedef index::detail::calculate_distance<nearest_predicate_type, box_type, strategy_type, bounds_tag> calculate_node_distance;
    typedef typename calculate_value_distance::result_type value_distance_type;
    typedef typename calculate_node_distance::result_type node_distance_type;

    typedef typename MembersHolder::size_type size_type;
    typedef typename MembersHolder::node_pointer node_pointer;

    using neighbor_data = std::pair<value_distance_type, const value_type *>;
    using neighbors_type = std::vector<neighbor_data>;

    struct branch_data
    {
        branch_data(node_distance_type d, size_type rl, node_pointer p)
            : distance(d), reverse_level(rl), ptr(p)
        {}

        node_distance_type distance;
        size_type reverse_level;
        node_pointer ptr;
    };
    using branches_type = priority_queue<branch_data, branch_data_comp>;

public:
    distance_query(MembersHolder const& members, Predicates const& pred)
        : m_tr(members.translator())
        , m_strategy(index::detail::get_strategy(members.parameters()))
        , m_pred(pred)
    {
        m_neighbors.reserve((std::min)(members.values_count, size_type(max_count())));
        //m_branches.reserve(members.parameters().get_min_elements() * members.leafs_level); ?
        // min, max or average?
    }

    template <typename OutIter>
    size_type apply(MembersHolder const& members, OutIter out_it)
    {
        return apply(members.root, members.leafs_level, out_it);
    }

private:
    template <typename OutIter>
    size_type apply(node_pointer ptr, size_type reverse_level, OutIter out_it)
    {
        namespace id = index::detail;

        if (max_count() <= 0)
        {
            return 0;
        }

        for (;;)
        {
            if (reverse_level > 0)
            {
                internal_node& n = rtree::get<internal_node>(*ptr);
                // fill array of nodes meeting predicates
                for (auto const& p : rtree::elements(n))
                {
                    node_distance_type node_distance; // for distance predicate

                    // if current node meets predicates (0 is dummy value)
                    if (id::predicates_check<id::bounds_tag>(m_pred, 0, p.first, m_strategy)
                        // and if distance is ok
                        && calculate_node_distance::apply(predicate(), p.first, m_strategy, node_distance)
                        // and if current node is closer than the furthest neighbor
                        && ! ignore_branch(node_distance))
                    {
                        // add current node's data into the list
                        m_branches.push(branch_data(node_distance, reverse_level - 1, p.second));
                    }
                }
            }
            else
            {
                leaf& n = rtree::get<leaf>(*ptr);
                // search leaf for closest value meeting predicates
                for (auto const& v : rtree::elements(n))
                {
                    value_distance_type value_distance; // for distance predicate

                    // if value meets predicates
                    if (id::predicates_check<id::value_tag>(m_pred, v, m_tr(v), m_strategy)
                        // and if distance is ok
                        && calculate_value_distance::apply(predicate(), m_tr(v), m_strategy, value_distance))
                    {
                        // store value
                        store_value(value_distance, boost::addressof(v));
                    }
                }
            }

            if (m_branches.empty()
                || ignore_branch(m_branches.top().distance))
            {
                break;
            }

            ptr = m_branches.top().ptr;
            reverse_level = m_branches.top().reverse_level;
            m_branches.pop();
        }

        for (auto const& p : m_neighbors)
        {
            *out_it = *(p.second);
            ++out_it;
        }

        return m_neighbors.size();
    }

    bool ignore_branch(node_distance_type const& node_distance) const
    {
        return m_neighbors.size() == max_count()
            && m_neighbors.front().first <= node_distance;
    }

    void store_value(value_distance_type value_distance, const value_type * value_ptr)
    {
        if (m_neighbors.size() < max_count())
        {
            m_neighbors.push_back(std::make_pair(value_distance, value_ptr));

            if (m_neighbors.size() == max_count())
            {
                std::make_heap(m_neighbors.begin(), m_neighbors.end(), pair_first_less());
            }
        }
        else if (value_distance < m_neighbors.front().first)
        {
            std::pop_heap(m_neighbors.begin(), m_neighbors.end(), pair_first_less());
            m_neighbors.back() = std::make_pair(value_distance, value_ptr);
            std::push_heap(m_neighbors.begin(), m_neighbors.end(), pair_first_less());
        }
    }

    std::size_t max_count() const
    {
        return nearest_predicate_access::get(m_pred).count;
    }

    nearest_predicate_type const& predicate() const
    {
        return nearest_predicate_access::get(m_pred);
    }

    translator_type const& m_tr;
    strategy_type m_strategy;

    Predicates const& m_pred;

    branches_type m_branches;
    neighbors_type m_neighbors;
};

template <typename MembersHolder, typename Predicates>
class distance_query_incremental
{
    typedef typename MembersHolder::value_type value_type;
    typedef typename MembersHolder::box_type box_type;
    typedef typename MembersHolder::parameters_type parameters_type;
    typedef typename MembersHolder::translator_type translator_type;
    typedef typename MembersHolder::allocators_type allocators_type;

    typedef typename index::detail::strategy_type<parameters_type>::type strategy_type;

    typedef typename MembersHolder::node node;
    typedef typename MembersHolder::internal_node internal_node;
    typedef typename MembersHolder::leaf leaf;

    typedef index::detail::predicates_element
        <
            index::detail::predicates_find_distance<Predicates>::value, Predicates
        > nearest_predicate_access;
    typedef typename nearest_predicate_access::type nearest_predicate_type;
    typedef typename indexable_type<translator_type>::type indexable_type;

    typedef index::detail::calculate_distance<nearest_predicate_type, indexable_type, strategy_type, value_tag> calculate_value_distance;
    typedef index::detail::calculate_distance<nearest_predicate_type, box_type, strategy_type, bounds_tag> calculate_node_distance;
    typedef typename calculate_value_distance::result_type value_distance_type;
    typedef typename calculate_node_distance::result_type node_distance_type;

    typedef typename allocators_type::size_type size_type;
    typedef typename allocators_type::const_reference const_reference;
    typedef typename allocators_type::node_pointer node_pointer;

    typedef typename rtree::elements_type<internal_node>::type internal_elements;
    typedef typename internal_elements::const_iterator internal_iterator;
    typedef typename rtree::elements_type<leaf>::type leaf_elements;

    using neighbor_data = std::pair<value_distance_type, const value_type *>;
    using neighbors_type = priority_dequeue<neighbor_data, pair_first_greater>;

    struct branch_data
    {
        branch_data(node_distance_type d, size_type rl, node_pointer p)
            : distance(d), reverse_level(rl), ptr(p)
        {}

        node_distance_type distance;
        size_type reverse_level;
        node_pointer ptr;
    };
    using branches_type = priority_queue<branch_data, branch_data_comp>;

public:
    inline distance_query_incremental()
        : m_tr(nullptr)
//        , m_strategy()
//        , m_pred()
        , m_neighbors_count(0)
        , m_neighbor_ptr(nullptr)
    {}

    inline distance_query_incremental(Predicates const& pred)
        : m_tr(nullptr)
//        , m_strategy()
        , m_pred(pred)
        , m_neighbors_count(0)
        , m_neighbor_ptr(nullptr)
    {}

    inline distance_query_incremental(MembersHolder const& members, Predicates const& pred)
        : m_tr(::boost::addressof(members.translator()))
        , m_strategy(index::detail::get_strategy(members.parameters()))
        , m_pred(pred)
        , m_neighbors_count(0)
        , m_neighbor_ptr(nullptr)
    {}

    const_reference dereference() const
    {
        return *m_neighbor_ptr;
    }

    void initialize(MembersHolder const& members)
    {
        if (0 < max_count())
        {
            apply(members.root, members.leafs_level);
            increment();
        }
    }

    void increment()
    {
        for (;;)
        {
            if (m_branches.empty())
            {
                // there exists a next closest neighbor so we can increment
                if (! m_neighbors.empty())
                {
                    m_neighbor_ptr = m_neighbors.top().second;
                    ++m_neighbors_count;
                    m_neighbors.pop_top();
                }
                else
                {
                    // there aren't any neighbors left, end
                    m_neighbor_ptr = nullptr;
                    m_neighbors_count = max_count();
                }

                return;
            }
            else
            {
                branch_data const& closest_branch = m_branches.top();

                // if next neighbor is closer or as close as the closest branch, set next neighbor
                if (! m_neighbors.empty() && m_neighbors.top().first <= closest_branch.distance )
                {
                    m_neighbor_ptr = m_neighbors.top().second;
                    ++m_neighbors_count;
                    m_neighbors.pop_top();
                    return;
                }

                BOOST_GEOMETRY_INDEX_ASSERT(m_neighbors_count + m_neighbors.size() <= max_count(), "unexpected neighbors count");

                // if there is enough neighbors and there is no closer branch
                if (ignore_branch_or_value(closest_branch.distance))
                {
                    m_branches.clear();
                    continue;
                }
                else
                {
                    node_pointer ptr = closest_branch.ptr;
                    size_type reverse_level = closest_branch.reverse_level;
                    m_branches.pop();

                    apply(ptr, reverse_level);
                }
            }
        }
    }

    bool is_end() const
    {
        return m_neighbor_ptr == nullptr;
    }

    friend bool operator==(distance_query_incremental const& l, distance_query_incremental const& r)
    {
        return l.m_neighbors_count == r.m_neighbors_count;
    }

private:
    void apply(node_pointer ptr, size_type reverse_level)
    {
        namespace id = index::detail;
        // Put node's elements into the list of active branches if those elements meets predicates
        // and distance predicates(currently not used)
        // and aren't further than found neighbours (if there is enough neighbours)
        if (reverse_level > 0)
        {
            internal_node& n = rtree::get<internal_node>(*ptr);
            // fill active branch list array of nodes meeting predicates
            for (auto const& p : rtree::elements(n))
            {
                node_distance_type node_distance; // for distance predicate

                // if current node meets predicates (0 is dummy value)
                if (id::predicates_check<id::bounds_tag>(m_pred, 0, p.first, m_strategy)
                    // and if distance is ok
                    && calculate_node_distance::apply(predicate(), p.first, m_strategy, node_distance)
                    // and if current node is closer than the furthest neighbor
                    && ! ignore_branch_or_value(node_distance))
                {
                    // add current node into the queue
                    m_branches.push(branch_data(node_distance, reverse_level - 1, p.second));
                }
            }
        }
        // Put values into the list of neighbours if those values meets predicates
        // and distance predicates(currently not used)
        // and aren't further than already found neighbours (if there is enough neighbours)
        else
        {
            leaf& n = rtree::get<leaf>(*ptr);
            // search leaf for closest value meeting predicates
            for (auto const& v : rtree::elements(n))
            {
                value_distance_type value_distance; // for distance predicate

                // if value meets predicates
                if (id::predicates_check<id::value_tag>(m_pred, v, (*m_tr)(v), m_strategy)
                    // and if distance is ok
                    && calculate_value_distance::apply(predicate(), (*m_tr)(v), m_strategy, value_distance)
                    // and if current value is closer than the furthest neighbor
                    && ! ignore_branch_or_value(value_distance))
                {
                    // add current value into the queue
                    m_neighbors.push(std::make_pair(value_distance, boost::addressof(v)));

                    // remove unneeded value
                    if (m_neighbors_count + m_neighbors.size() > max_count())
                    {
                        m_neighbors.pop_bottom();
                    }
                }
            }
        }
    }

    template <typename Distance>
    bool ignore_branch_or_value(Distance const& distance)
    {
        return m_neighbors_count + m_neighbors.size() == max_count()
            && (m_neighbors.empty() || m_neighbors.bottom().first <= distance);
    }

    std::size_t max_count() const
    {
        return nearest_predicate_access::get(m_pred).count;
    }

    nearest_predicate_type const& predicate() const
    {
        return nearest_predicate_access::get(m_pred);
    }

    const translator_type * m_tr;
    strategy_type m_strategy;

    Predicates m_pred;

    branches_type m_branches;
    neighbors_type m_neighbors;
    size_type m_neighbors_count;
    const value_type * m_neighbor_ptr;
};

}}} // namespace detail::rtree::visitors

}}} // namespace boost::geometry::index

#endif // BOOST_GEOMETRY_INDEX_DETAIL_RTREE_VISITORS_DISTANCE_QUERY_HPP


#include "IPTVTopologyOracle.hpp"
#include "Scheduler.hpp"
#include "boost/random/discrete_distribution.hpp"
#include "boost/random/uniform_int.hpp"
#include "boost/random/normal_distribution.hpp"
#include "zipf_distribution.hpp"
#include <boost/lexical_cast.hpp>
#include <initializer_list>
#include <boost/random/mersenne_twister.hpp>

// random generator
extern boost::mt19937 gen;

IPTVTopologyOracle::IPTVTopologyOracle(Topology* topo, po::variables_map vm,
        uint roundDuration) : TopologyOracle(topo, vm, roundDuration) {
  this->contentNum = vm["contents"].as<uint>() * vm["channels"].as<uint>();
  double zmExp = vm["zm-exponent"].as<double>();
  this->shiftDist = new boost::random::uniform_int_distribution<>(0,50);
  this->expDist = new boost::random::uniform_real_distribution<>(0.4, zmExp);
  this->rankDist = new boost::random::zipf_distribution<>(contentNum,
          (*shiftDist)(gen), (*expDist)(gen));
  this->relDayDist = new boost::random::zipf_distribution<>(7, 0, (*expDist)(gen));
  // allocate memory for the dailyCatalog and initialize contentRateVec
  std::vector<double> dailyRank(contentNum, 0.0);
  contentRateVec.resize(7, dailyRank);
  for (uint i = 0; i < 7; i++) {
    std::vector<ContentElement*> vec(contentNum, nullptr);
    dailyCatalog.push_back(vec);
  }
}

IPTVTopologyOracle::~IPTVTopologyOracle() {
  BOOST_FOREACH(std::vector<ContentElement*> list, dailyCatalog) {
    BOOST_FOREACH(ContentElement* content, list) {
      delete content;
    }
    list.clear();
  }
  dailyCatalog.clear();
  userWatchMap.clear();
  delete this->relDayDist;
  delete this->rankDist;
  delete this->expDist;
  delete this->shiftDist;
}

void IPTVTopologyOracle::generateUserViewMap(Scheduler* scheduler) {
  BOOST_LOG_TRIVIAL(debug) << "entering generateUserViewMap";
  /* fetching video chunks takes time, so we get delays with respect to the
   * session defined here; hence I'm introducing a guardThreshold so that we
   * have enough time to complete all the flows before midnight (hopefully)

  uint guardThreshold = 60;
  uint roundDuration = scheduler->getRoundDuration() - guardThreshold;
   *  */
  uint roundDuration = scheduler->getRoundDuration();
  double totalHours = 0, randomHours;
  boost::random::normal_distribution<> userSessionDist(avgHoursPerUser);
  boost::random::discrete_distribution<> hourDist(usrPctgByHour);
  boost::random::uniform_int_distribution<> minSecDist(0, 3599);
  // Generate a session start time and session length for each user
  SimTime sessionStart, sessionLength;

  BOOST_FOREACH(Vertex v, topo->getPonNodes()) {
    for (uint i = 0; i < topo->getPonCustomers(v); i++) {
      PonUser user = std::make_pair(v,i);
      // populate userWatchMap for the first time if it's round 0
      if (scheduler->getCurrentRound() == 0) {
        std::pair<PonUser, UserWatchingInfo> userWatchMapEntry = std::make_pair(user, UserWatchingInfo());
        userWatchMap.insert(userWatchMapEntry);
      }
      else {
        userWatchMap.at(user).reset();
      }
      randomHours = userSessionDist(gen);
      // daily sessions can't be longer than a day, duh
      if (randomHours > 24)
        randomHours = 24;
      if (randomHours > 0)
        totalHours += randomHours;
      sessionLength = std::floor((randomHours * 3600) + 0.5);
      if (sessionLength <= 0) {
        BOOST_LOG_TRIVIAL(info) << "TopologyOracle::generateUserViewMap() - "
                  "non-positive sessionLength for user " << v << "," << i
                  << " - skipping it";
        continue;
      }
      /* the time we generate is assumed to be the middle point of the viewing
       * session, so we shift it of half sessionLength, but we need to ensure it
       * still fits inside the round
       */
      sessionStart = (hourDist(gen) * 3600 + minSecDist(gen)) -
              std::floor((sessionLength / 2) + 0.5);
      if (sessionStart < 0)
        sessionStart = 0;
      else if (sessionStart + sessionLength >= roundDuration)
        sessionStart = roundDuration - sessionLength;
      SimTimeInterval interval(sessionStart, sessionStart + sessionLength);
      // store the new interval
      userWatchMap.at(user).dailySessionInterval = interval;
      // generate first request and schedule it
      this->generateNewRequest(user, interval.getStart(), scheduler);
    }
  }
  BOOST_LOG_TRIVIAL(info) << "Total hours of scheduled viewing for the current day: "
          << totalHours << "; avg. hours per user: "
          << totalHours/(topo->getNumCustomers())
          << " (requested: " << avgHoursPerUser << ")";
}

void IPTVTopologyOracle::populateCatalog() {
  // normal distribution to generate content length in minutes
  boost::random::normal_distribution<> normDist(this->avgContentLength,
          this->devContentLength);
  Capacity size = 0;
  // generate content for the first day and the previous 6
  for (int day = -6; day < 1; day++) {
    for (uint i = 0; i < contentNum; i++) {
      size = std::ceil(normDist(gen) * 60 * this->bitrate);
      ContentElement* content = new ContentElement(
        boost::lexical_cast<string > ((day + 6) * contentNum + i), day, size,
              chunkSize);
      dailyCatalog[std::abs(day)].at(i) = content;
      // insert the content (NOTE: not the chunks) in the daily ranking
      dailyRanking.at(std::abs(day)).insert(content);
      this->addContent(content, 0);
      // BOOST_LOG_TRIVIAL(trace) << "generated content " << content->getName();
    }
  }
}

void IPTVTopologyOracle::updateCatalog(uint currentRound) {
  // Update catalogue
  dailyRanking.at(6).clear();
  BOOST_FOREACH(ContentElement* content, dailyCatalog[6]) {
    this->removeContent(content, currentRound+1);
    delete content;
  }
  for (uint day = 6; day > 0; day--) {
    dailyCatalog[day] = dailyCatalog[day - 1];
    dailyRanking.at(day) = dailyRanking.at(day-1);
    dailyRanking.at(day).resetRoundHits();
  }
  dailyRanking.at(0).clear();
  // normal distribution to generate content length in minutes
  boost::random::normal_distribution<> normDist(this->avgContentLength,
          this->devContentLength);
  Capacity size = 0;
  for (uint i = 0; i < contentNum; i++) {
    size = std::ceil(normDist(gen) * 60 * this->bitrate);
    ContentElement* content = new ContentElement(
            boost::lexical_cast<string > ((currentRound + 7) * contentNum + i),
            currentRound + 1, size, chunkSize);
    dailyCatalog[0].at(i) = content;
    dailyRanking.at(0).insert(content);
    this->addContent(content, currentRound+1);
  }
}

/* selects a movie to be requested according to the popularity distributions,
 * determines how long the user is going to be watching it (because of zapping)
 * and stores the required information in the userWatchMap, then creates a new
 * Flow and schedules it.
 */
void IPTVTopologyOracle::generateNewRequest(PonUser user, SimTime time,
        Scheduler* scheduler) {
  SimTime sessionEnd = userWatchMap.at(user).dailySessionInterval.getEnd();
  if (time < sessionEnd) {
    boost::random::uniform_int_distribution<> indexDist(0, 17);
    uint i = (*relDayDist)(gen);
    ContentElement* content = dailyCatalog[i].at((*rankDist)(gen));
    double watchingPortion = sessionLength[indexDist(gen)];
    BOOST_LOG_TRIVIAL(debug) << "At time " << time
            << " generated request from user " << user.first
            << "," << user.second << " for content " << content->getName()
            << ", watchingPortion = " << watchingPortion;
    /*
    Capacity reqLength =  watchingPortion * content->getSize();
    // calculate the time at which the user will be done with this content
    SimTime endTime = time + std::ceil(reqLength / this->bitrate);
    // check that the new request would not go past the desired session
    // length; this will make all requests past midnight end at midnight :(
    if (endTime > sessionEnd) {
      endTime = sessionEnd;
    }
    */
    // determine the number of chunks to be watched for this content
    // TODO: reduce the number of chunks to be watched if we're going past the sessionEnd deadline
    uint watchingChunks = std::ceil(watchingPortion * content->getTotalChunks());
    if (watchingChunks > content->getTotalChunks())
      watchingChunks = content->getTotalChunks();
    UserWatchingMap::iterator wIt = userWatchMap.find(user);
    assert (wIt != userWatchMap.end());
    wIt->second.content = content;
    wIt->second.chunksToBeWatched = watchingChunks;
    wIt->second.currentChunk = 0;
    // we are waiting for the first chunk to be downloaded
    wIt->second.waiting = true;
    /* Request enough chunks to fill the buffer.
     */
    for (uint i = 0; i < bufferSize; i++) {
      // stop if we fetched all the chunks of this content
      if (i >= content->getTotalChunks())
        break;
      else {
        Flow* request = new Flow(content, user, time, i);
        scheduler->schedule(request);
        wIt->second.highestChunkFetched = i;
        BOOST_LOG_TRIVIAL(debug) << "Fetching chunk " << i << " of content "
                << content->getName() << ", highestChunkFetched = "
                << userWatchMap.at(user).highestChunkFetched;
      }
    }
  }
}

void IPTVTopologyOracle::notifyEndRound(uint endingRound) {
  delete this->rankDist;
  this->rankDist = new boost::random::zipf_distribution<>(contentNum,
          (*shiftDist)(gen), (*expDist)(gen));
  delete this->relDayDist;
  this->relDayDist = new boost::random::zipf_distribution<>(7, 0, (*expDist)(gen));
}

void IPTVTopologyOracle::preCache() {
  bool isFull = false;
  uint i(0), day(0), index(0);
  ContentElement* content = nullptr;
  while (!isFull) {
    day = std::floor(i / dailyCatalog.at(0).size()); // all the same size
    if (day >= dailyCatalog.size())
      break;
    index = i % dailyCatalog.at(day).size();
    content = dailyCatalog.at(day).at(index);
    for (uint as = 0; as < topo->getNumASes(); as++) {
      if (localCacheMap->at(as).fitsInCache(content->getSize())) {
        auto chunks = content->getChunks();
        std::pair<bool, std::set<ChunkPtr> > result;
        for (auto j = chunks.begin(); j != chunks.end(); j++) {
          result = localCacheMap->at(as).addToCache(*j, (*j)->getSize(), 0);
          assert(result.first == true);
          assert(result.second.empty() == true);
          assert(localCacheMap->at(as).isCached(*j));
        }
      }
      else {
        isFull = true;
        break;
      }
    }
    i++;
  }
  BOOST_LOG_TRIVIAL(trace) << "cached " << i << " contents in AS caches";
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#include <gtsam/linear/Errors.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph-inl.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/base/DSFVector.h>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <list>

using namespace std;

namespace gtsam {

/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters &parameters)
  : parameters_(parameters)
{
  initialize(gfg);
}

/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters &parameters)
  : parameters_(parameters)
{
  initialize(*jfg);
}

/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters)
  : parameters_(parameters) {

  GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(Ab1)->eliminate(&EliminateQR);
  initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
}

/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1,
    const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters)
  : parameters_(parameters) {

  GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR);
  initialize(Rc1, Ab2);
}

/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2,
    const Parameters &parameters) : parameters_(parameters)
{
  initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
}

/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
    const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters) : parameters_(parameters)
{
  initialize(Rc1, Ab2);
}

VectorValues SubgraphSolver::optimize() {
  VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>(*pc_, pc_->zero(), parameters_);
  return pc_->x(ybar);
}

void SubgraphSolver::initialize(const GaussianFactorGraph &jfg)
{
  GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>(),
                                  Ab2 = boost::make_shared<GaussianFactorGraph>();

  boost::tie(Ab1, Ab2) = splitGraph(jfg) ;
  if (parameters_.verbosity())
    cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl;

  GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR);
  VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
  pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}

void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2)
{
  VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
  pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}

boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>
SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {

  const VariableIndex index(jfg);
  const size_t n = index.size();
  DSFVector D(n);

  GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph());
  GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph());

  size_t t = 0;
  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) {

    if ( gf->keys().size() > 2 ) {
      throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
    }

    bool augment = false ;

    /* check whether this factor should be augmented to the "tree" graph */
    if ( gf->keys().size() == 1 ) augment = true;
    else {
      const Index u = gf->keys()[0], v = gf->keys()[1],
                  u_root = D.findSet(u), v_root = D.findSet(v);
      if ( u_root != v_root ) {
        t++; augment = true ;
        D.makeUnionInPlace(u_root, v_root);
      }
    }
    if ( augment ) At->push_back(gf);
    else Ac->push_back(gf);
  }

  return boost::tie(At, Ac);
}

} // \namespace gtsam


#include "../../Positive_Matrix_With_Prefactor.hxx"

#include <El.hpp>
#include <boost/interprocess/file_mapping.hpp>
#include <boost/interprocess/mapped_region.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/filesystem.hpp>

std::vector<char>::const_iterator
parse_SDP(const char *begin, const char *end,
          std::vector<El::BigFloat> &objectives,
          std::vector<El::BigFloat> &normalization,
          std::vector<Positive_Matrix_With_Prefactor> &matrices,
          size_t &num_processed);

void read_mathematica(const boost::filesystem::path &input_path,
                      std::vector<El::BigFloat> &objectives,
                      std::vector<El::BigFloat> &normalization,
                      std::vector<Positive_Matrix_With_Prefactor> &matrices,
                      size_t &num_processed)

{
  boost::filesystem::ifstream input_stream(input_path);
  if(!input_stream.good())
    {
      throw std::runtime_error("Unable to open input: " + input_path.string());
    }

  boost::interprocess::file_mapping mapped_file(
    input_path.c_str(), boost::interprocess::read_only);
  boost::interprocess::mapped_region mapped_region(
    mapped_file, boost::interprocess::read_only);

  try
    {
      const char *begin(static_cast<const char *>(mapped_region.get_address())),
        *end(begin + mapped_region.get_size());
      parse_SDP(begin, end, objectives, normalization, matrices, num_processed);
    }
  catch(std::exception &e)
    {
      throw std::runtime_error("Error when parsing " + input_path.string()
                               + ": " + e.what());
    }
}


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center.

All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include <mitkBaseData.h>
#include <mitkImageCast.h>
#include <mitkImageToItk.h>
#include <metaCommand.h>
#include <mitkDiffusionCommandLineParser.h>
#include <usAny.h>
#include <mitkIOUtil.h>
#include <mitkLexicalCast.h>
#include <itksys/SystemTools.hxx>
#include <itkDirectory.h>
#include <mitkFiberBundle.h>
#include <vtkTransformPolyDataFilter.h>
#include <fstream>
#include <chrono>
#include <boost/progress.hpp>
#include <itkTractsToFiberEndingsImageFilter.h>
#include <itkTractDensityImageFilter.h>
#include <itkImageRegionConstIterator.h>

typedef itksys::SystemTools ist;
typedef itk::Image<unsigned char, 3>    ItkFloatImgType;
typedef itk::Image<unsigned int, 3>    ItkUIntImgType;

/*!
\brief
*/
int main(int argc, char* argv[])
{
  mitkDiffusionCommandLineParser parser;

  parser.setTitle("Calculate Overlap");
  parser.setCategory("Fiber Tracking Evaluation");
  parser.setDescription("");
  parser.setContributor("MIC");

  parser.setArgumentPrefix("--", "-");
  parser.addArgument("tractogram", "", mitkDiffusionCommandLineParser::String, "", "", us::Any(), false, false, false, mitkDiffusionCommandLineParser::Input);
  parser.addArgument("mask", "", mitkDiffusionCommandLineParser::String, "", "", us::Any(), false, false, false, mitkDiffusionCommandLineParser::Input);
  parser.addArgument("peaks", "", mitkDiffusionCommandLineParser::String, "", "", us::Any(), true, false, false, mitkDiffusionCommandLineParser::Input);

  std::map<std::string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
  if (parsedArgs.size()==0)
    return EXIT_FAILURE;

  std::string input_tractogram = us::any_cast<std::string>(parsedArgs["tractogram"]);
  std::string mask_file = us::any_cast<std::string>(parsedArgs["mask"]);

  try
  {
    mitk::FiberBundle::Pointer fib = mitk::IOUtil::Load<mitk::FiberBundle>(input_tractogram);
    mitk::Image::Pointer mask = mitk::IOUtil::Load<mitk::Image>(mask_file);

    ItkFloatImgType::Pointer itk_mask;
    mitk::CastToItkImage(mask, itk_mask);

    mitk::PeakImage::ItkPeakImageType::Pointer peaks = nullptr;
    if (parsedArgs.count("peaks"))
    {
      mitk::Image::Pointer mitk_peaks = mitk::IOUtil::Load<mitk::Image>(us::any_cast<std::string>(parsedArgs["peaks"]));

      typedef mitk::ImageToItk< mitk::PeakImage::ItkPeakImageType > CasterType;
      CasterType::Pointer caster = CasterType::New();
      caster->SetInput(mitk_peaks);
      caster->Update();
      peaks = caster->GetOutput();

      float overlap = 0;
      float directional_overlap = 0;
      std::tie(directional_overlap, overlap) = fib->GetDirectionalOverlap(itk_mask, peaks);
      MITK_INFO << "Overlap<<" << overlap;
      MITK_INFO << "DirectionalOverlap<<" << directional_overlap;
    }
    else
      MITK_INFO << "Overlap<<" << fib->GetOverlap(itk_mask);
  }
  catch (const itk::ExceptionObject& e)
  {
    std::cout << e.what();
    return EXIT_FAILURE;
  }
  catch (std::exception& e)
  {
    std::cout << e.what();
    return EXIT_FAILURE;
  }
  catch (...)
  {
    std::cout << "ERROR!?!";
    return EXIT_FAILURE;
  }
  return EXIT_SUCCESS;
}


// Author(s): Maurice Laveaux
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//

#define BOOST_TEST_MODULE symbolic_parity_game_test
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE(dummy_test)
{
  // This is an empty test since at least one test is required.
}

#ifdef MCRL2_ENABLE_SYLVAN

#include "mcrl2/data/variable.h"
#include "mcrl2/pbes/symbolic_parity_game.h"
#include "mcrl2/pbes/symbolic_pbessolve.h"
#include "mcrl2/pbes/pbes_summand_group.h"
#include "mcrl2/symbolic/data_index.h"
#include "mcrl2/symbolic/test_utility.h"

#include <sylvan_ldd.hpp>

#include <vector>
#include <random>

using namespace mcrl2::pbes_system;
using namespace mcrl2::symbolic;
using sylvan::ldds::ldd;

/// A single randomly generated transition group for parameter i
summand_group single_transition_group(const mcrl2::data::variable_list& parameters, std::size_t i, std::size_t N, std::size_t D)
{
  boost::dynamic_bitset<> dependencies(parameters.size()*2);
  dependencies[i] = 1;
  dependencies[i+1] = 1;

  summand_group group(parameters, dependencies, false);
  group.L = random_set(N, 2, D);

  return group;
}

/// \brief Generates a random parity game with N vertices and at most P priorities.
std::tuple<ldd, ldd, std::vector<ldd>, std::vector<summand_group>, std::vector<data_expression_index>> compute_random_game(std::size_t N, std::size_t P)
{
  std::size_t D = 10; // Size of the state space domain.
  std::size_t length = 4; // Length of the state vector.

  ldd V = random_set(N, length, D);
  ldd Veven = random_subset(V, N / 2);

  ldd remaining = V;
  std::vector<ldd> prio(P);
  for (std::size_t i = 0; i < P; ++i)
  {
    prio[i] = random_subset(remaining, N/P);
    remaining = minus(remaining, prio[i]);
  }

  // Define some list of parameters.
  mcrl2::data::variable_list parameters;
  for (std::size_t j = 0; j < length; ++j)
  {
    parameters.emplace_front(mcrl2::data::variable());
  }

  // Generate random transition groups.
  std::vector<summand_group> groups;
  groups.emplace_back(single_transition_group(parameters, 0, 3*D, D));
  groups.emplace_back(single_transition_group(parameters, 1, 3*D, D));
  groups.emplace_back(single_transition_group(parameters, 2, 3*D, D));
  groups.emplace_back(single_transition_group(parameters, 3, 3*D, D));

  std::vector<data_expression_index> index;

  return std::make_tuple(V, Veven, prio, groups, index);
}

/// \returns Minimal fixpoint of the given (monotone) predicate transformer A -> A.
template<typename F>
ldd least_fixpoint(F pred)
{
  ldd prev, current;
  do
  {
    prev = current;
    current = pred(current);
  }
  while (current != prev);

  return current;
}

/// \returns Maximal fixpoint of the given (monotone) predicate transformer A -> A.
template<typename F>
ldd greatest_fixpoint(F pred, ldd V)
{
  ldd prev;
  ldd current = V;
  do
  {
    prev = current;
    current = pred(current);
  }
  while (current != prev);

  return current;
}

/// \returns The predecessors of U.
ldd pre(const symbolic_parity_game& G, const ldd& V, const ldd& U)
{
  return G.predecessors(V, U);
}

/// \returns The control predecessors of U.
ldd cpre(const symbolic_parity_game& G, const ldd& V, std::size_t alpha, const ldd& U)
{
  std::array<const ldd, 2> Vplayer = G.players(V);

  return union_(
    intersect(Vplayer[alpha],        pre(G, V, U)),
    minus(Vplayer[1 - alpha], union_(pre(G, V, minus(V, U)), G.sinks(V, V)))
  );
}

/// \returns Standard attractor set computation.
ldd attr(const symbolic_parity_game& G, const ldd& V, std::size_t alpha, const ldd& U)
{
  return least_fixpoint(
    [&](const ldd& Z) -> ldd {
      return union_(U, cpre(G, V, alpha, Z));
    });
}

/// \returns The safe control predecessors of U.
ldd spre(const symbolic_parity_game& G, const ldd& V, std::size_t alpha, const ldd& U, const ldd& I)
{
  std::array<const ldd, 2> Vplayer = G.players(V);

  return union_(
    intersect(Vplayer[alpha], G.predecessors(V, U)),
    minus(Vplayer[1 - alpha], union_(union_(G.predecessors(V, minus(V, U)), G.sinks(V, V)), I))
  );
}

/// \returns Safe attractor set computation.
ldd sattr(const symbolic_parity_game& G, const ldd& V, std::size_t alpha, const ldd& U, const ldd& I)
{
  return least_fixpoint(
    [&](const ldd& Z) -> ldd {
      return union_(U, spre(G, V, alpha, Z, I));
    });
}

/// \brief Computes the monotone control predecessor.
ldd Mcpre(const symbolic_parity_game& G, const ldd& V, std::size_t alpha, const ldd& Z, const ldd& U, std::size_t c)
{
  return intersect(G.prio_above(V, c), cpre(G, V, alpha, union_(Z, U)));
}

/// \brief Computes monotone attractor for priority c.
ldd Mattr(const symbolic_parity_game& G, const ldd& V, std::size_t alpha, const ldd& U, std::size_t c)
{
  return least_fixpoint(
    [&](const ldd& Z) -> ldd
    {
      return Mcpre(G, V, alpha, Z, U, c);
    }
  );
}

/// \returns Safe vertices computation.
ldd safe(const symbolic_parity_game& G, const ldd& V, std::size_t alpha, const ldd& I)
{
  std::array<const ldd, 2> Vplayer = G.players(V);
  return minus(V, attr(G, V, 1-alpha, intersect(Vplayer[1-alpha], I)));
}

/// \brief Computes fatal attractor for priority c.
ldd fatal_attractor(const symbolic_parity_game& G, const ldd& V, std::size_t alpha, std::size_t c, const ldd& I)
{
  ldd Pequal = intersect(V, G.ranks().at(c));
  const ldd Vsafe = safe(G, V, alpha, I);

  return greatest_fixpoint(
    [&](const ldd& Z) -> ldd
    {
      // G \cap safe(G) is here represented by restricting V to Vsafe.
      return intersect(intersect(Pequal, Vsafe), Mattr(G, Vsafe, alpha, Z, c));
    },
    Vsafe
  );
}

/// \brief Computes the safe monotone control predecessor.
ldd sMcpre(const symbolic_parity_game& G, const ldd& V, std::size_t alpha, const ldd& Z, const ldd& U, std::size_t c, const ldd& I)
{
  return intersect(G.prio_above(V, c), spre(G, V, alpha, union_(Z, U), I));
}

/// \brief Computes monotone attractor for priority c.
ldd sMattr(const symbolic_parity_game& G, const ldd& V, std::size_t alpha, const ldd& U, std::size_t c, const ldd& I)
{
  return least_fixpoint(
    [&](const ldd& Z) -> ldd
    {
      return sMcpre(G, V, alpha, Z, U, c, I);
    }
  );
}

/// \brief Computes fatal attractor for priority c.
ldd safe_fatal_attractor(const symbolic_parity_game& G, const ldd& V, std::size_t alpha, std::size_t c, const ldd& I)
{
  ldd Pequal = intersect(V, G.ranks().at(c));

  return greatest_fixpoint(
    [&](const ldd& Z) -> ldd
    {
      return intersect(Pequal, sMattr(G, V, alpha, Z, c, I));
    },
    V
  );
}

/// \brief Returns the solitair cycles.
ldd solitair_cycles(const symbolic_parity_game& G, const ldd& V, std::size_t alpha)
{
  std::array<const ldd, 2> Vplayer = G.players(V);
  std::array<const ldd, 2> parity = G.parity(V);

  return greatest_fixpoint(
    [&](const ldd& Z) -> ldd {
      return intersect(intersect(Vplayer[alpha], parity[alpha]), pre(G, V, Z));
    },
    V
  );
}

/// \brief Returns the forced cycles.
ldd forced_cycles(const symbolic_parity_game& G, const ldd& V, std::size_t alpha, const ldd& I)
{
  std::array<const ldd, 2> parity = G.parity(V);
  const ldd Vsafe = safe(G, V, alpha, I);

  return greatest_fixpoint(
    [&](const ldd& Z) -> ldd {
      return intersect(intersect(parity[alpha], Vsafe), cpre(G, V, alpha, Z));
    },
    V
  );
}

BOOST_AUTO_TEST_CASE(random_test_attractor)
{
  initialise_sylvan();

  for (std::size_t i = 0; i < 100; ++i)
  {
    auto [V, Veven, prio, groups, index] = compute_random_game(5000, 2);
    symbolic_parity_game G(groups, index, V, Veven, prio, false, false);

    ldd U = random_subset(V, 250);

    std::array<const ldd, 2> Vplayer = G.players(V);
    ldd result = G.safe_attractor(U, 0, V, Vplayer);
    ldd expected = attr(G, V, 0, U);
    std::cerr << satcount(result) << std::endl;

    BOOST_CHECK_EQUAL(result, expected);
  }

  quit_sylvan();
}

BOOST_AUTO_TEST_CASE(random_test_safe_attractor)
{
  initialise_sylvan();

  for (std::size_t i = 0; i < 100; ++i)
  {
    auto [V, Veven, prio, groups, index] = compute_random_game(5000, 2);
    symbolic_parity_game G(groups, index, V, Veven, prio, false, false);

    ldd U = random_subset(V, 250);
    ldd I = random_subset(minus(V, U), 250);

    std::array<const ldd, 2> Vplayer = G.players(V);
    ldd result = G.safe_attractor(U, 0, V, Vplayer, I);
    ldd expected = sattr(G, V, 0, U, I);
    std::cerr << satcount(result) << std::endl;

    BOOST_CHECK_EQUAL(result, expected);
  }

  quit_sylvan();
}

BOOST_AUTO_TEST_CASE(random_test_chaining_safe_attractor)
{
  initialise_sylvan();

  for (std::size_t i = 0; i < 100; ++i)
  {
    auto [V, Veven, prio, groups, index] = compute_random_game(5000, 2);
    symbolic_parity_game G(groups, index, V, Veven, prio, false, true);

    ldd U = random_subset(V, 250);
    ldd I = random_subset(minus(V, U), 250);

    std::array<const ldd, 2> Vplayer = G.players(V);
    ldd result = G.safe_attractor(U, 0, V, Vplayer, I);
    ldd expected = sattr(G, V, 0, U, I);
    std::cerr << satcount(result) << std::endl;

    BOOST_CHECK_EQUAL(result, expected);
  }

  quit_sylvan();
}

BOOST_AUTO_TEST_CASE(random_test_monotone_attractor)
{
  initialise_sylvan();

  for (std::size_t i = 0; i < 100; ++i)
  {
    auto [V, Veven, prio, groups, index] = compute_random_game(5000, 2);
    symbolic_parity_game G(groups, index, V, Veven, prio, false, false);

    ldd U = random_subset(V, 250);

    std::array<const ldd, 2> Vplayer = G.players(V);
    ldd result = G.safe_monotone_attractor(U, 0, 0, V, Vplayer);
    ldd expected = Mattr(G, V, 0, U, 0);
    std::cerr << satcount(result) << std::endl;

    BOOST_CHECK_EQUAL(result, expected);
  }

  quit_sylvan();
}

BOOST_AUTO_TEST_CASE(random_test_safe_monotone_attractor)
{
  initialise_sylvan();

  for (std::size_t i = 0; i < 100; ++i)
  {
    auto [V, Veven, prio, groups, index] = compute_random_game(5000, 2);
    symbolic_parity_game G(groups, index, V, Veven, prio, false, false);

    ldd U = random_subset(V, 250);
    ldd I = random_subset(minus(V, U), 250);

    std::array<const ldd, 2> Vplayer = G.players(V);
    ldd result = G.safe_monotone_attractor(U, 0, 0, V, Vplayer, I);
    ldd expected = sMattr(G, V, 0, U, 0, I);
    std::cerr << satcount(result) << std::endl;

    BOOST_CHECK_EQUAL(result, expected);
  }

  quit_sylvan();
}

BOOST_AUTO_TEST_CASE(random_test_chaining_monotone_attractor)
{
  initialise_sylvan();

  for (std::size_t i = 0; i < 100; ++i)
  {
    auto [V, Veven, prio, groups, index] = compute_random_game(5000, 2);
    symbolic_parity_game G(groups, index, V, Veven, prio, false, true);

    ldd U = random_subset(V, 250);
    ldd I = random_subset(minus(V, U), 250);

    std::array<const ldd, 2> Vplayer = G.players(V);
    ldd result = G.safe_monotone_attractor(U, 0, 0, V, Vplayer, I);
    ldd expected = sMattr(G, V, 0, U, 0, I);
    std::cerr << satcount(result) << std::endl;

    BOOST_CHECK_EQUAL(result, expected);
  }

  quit_sylvan();
}

/*
BOOST_AUTO_TEST_CASE(random_test_solitair_cycles)
{
  initialise_sylvan();

  for (std::size_t i = 0; i < 1; ++i)
  {
    auto [V, Veven, prio, groups, index] = compute_random_game(100, 2);
    symbolic_parity_game G(groups, index, V, Veven, prio, false, false);
    mcrl2::pbes_system::symbolic_pbessolve_algorithm solver(G);

    ldd initial = random_subset(V, 1);
    ldd I;

    // Use V as initial vertex so it does not terminate early.
    std::pair<ldd, ldd> Vresult = solver.detect_solitair_cycles(V, V, I, false, G.sinks(V, V));

    std::array<ldd, 2> winning;
    ldd Vtotal = G.compute_total_graph(V, sylvan::ldds::empty_set(), G.sinks(V, V), winning);

    std::pair<ldd, ldd> Vexpected = { union_(winning[0], attr(G, V, 0, solitair_cycles(G, V, 0))),
                                      union_(winning[1], attr(G, V, 1, solitair_cycles(G, V, 1))) };
    BOOST_CHECK_EQUAL(Vresult.first, Vexpected.first);
    BOOST_CHECK_EQUAL(Vresult.second, Vexpected.second);
  }

  quit_sylvan();
}

BOOST_AUTO_TEST_CASE(random_test_solitair_cycles)
{
  initialise_sylvan();

  for (std::size_t i = 0; i < 1; ++i)
  {
    auto [V, Veven, prio, groups, index] = compute_random_game(500, 2);
    symbolic_parity_game G(groups, index, V, Veven, prio, false, false);
    mcrl2::pbes_system::symbolic_pbessolve_algorithm solver(G);

    ldd initial = random_subset(V, 1);
    ldd I;

    // Use V as initial vertex so it does not terminate early.
    std::pair<ldd, ldd> Vresult = solver.detect_forced_cycles(V, V, I, false, G.sinks(V, V));

    std::array<ldd, 2> winning;
    ldd Vtotal = G.compute_total_graph(V, sylvan::ldds::empty_set(), G.sinks(V, V), winning);

    std::pair<ldd, ldd> Vexpected = { union_(winning[0], attr(G, V, 0, forced_cycles(G, V, 0, I))),
                                      union_(winning[1], attr(G, V, 1, forced_cycles(G, V, 1, I))) };
    BOOST_CHECK_EQUAL(Vresult.first, Vexpected.first);
    BOOST_CHECK_EQUAL(Vresult.second, Vexpected.second);
  }

  quit_sylvan();
}*/

#endif // PLATFORM_WINDOWS


/*-------------------------------------------------------------
Copyright 2019 Wenxin Liu, Kartik Mohta, Giuseppe Loianno

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
--------------------------------------------------------------*/

#include "sdd_vio/vo_stereo.h"

#include <iostream>
#include "sdd_vio/utils/math_utils.h"
#include <algorithm>
#include <math.h>
#include <ros/ros.h>
#include <cmath>
#include <assert.h>
#include <boost/thread.hpp>
#include "sdd_vio/utils/ros_params_helper.h"
#include "sdd_vio/utils/timer.hpp"
#include <ctime>
#include <opencv2/core/eigen.hpp>
#include "sdd_vio/utils/calib_utils.h"

namespace sdd_vio {

    VoStereo::VoStereo(sdd_vio::PinholeCameraStereo *cam, const std::string& ns, const ros::NodeHandle &nh) :
        cam_(cam)
    {
        ROS_DEBUG_STREAM("init VoStereo object...");

        /* general settings */
        use_opt_vis_gb_ = getParam<bool>(ns+"/vo/use_opt_vis_gb");
        pub_image_debug_ = getParam<bool>(ns+"/vo/pub_image_debug");
        use_thread_ = getParam<bool>(ns+"/vo/use_thread");
        use_ica_ = getParam<bool>(ns+"/vo/use_ica");
        use_lma_ = getParam<bool>(ns+"/vo/use_lma");
        use_huber_ = getParam<bool>(ns+"/vo/use_huber");
        use_weights_ = getParam<bool>(ns+"/vo/use_weights");
        c_= getParam<float>(ns+"/vo/c");
        /* layer settings */
        full_layer_ = getParam<int>(ns+"/vo/full_layer");
        base_layer_ = getParam<int>(ns+"/vo/base_layer");
        /* feature point selection settings */
        disp_range_ = getParam<float>(ns+"/vo/disp_range");
        gd_size_ = getParam<int>(ns+"/vo/gd_size");
        use_gd_ = getParam<bool>(ns+"/vo/use_gd");
        /* keyframe switching settings */
        switch_count_ = getParam<int>(ns+"/vo/switch_count");
        switch_perc_ = getParam<float>(ns+"/vo/switch_perc");
        switch_depth_ = getParam<float>(ns+"/vo/switch_depth");
        switch_angle_ = getParam<float>(ns+"/vo/switch_angle");
        /* optimization settings */
        max_num_iter_ = getParam<int>(ns+"/vo/max_num_iter");
        verbose_ = getParam<bool>(ns+"/vo/verbose");
        lambda_ = getParam<float>(ns+"/vo/lambda");
        up_factor_ = getParam<float>(ns+"/vo/up_factor");
        down_factor_ = getParam<float>(ns+"/vo/down_factor");
        error_scale_factor_ = getParam<float>(ns+"/vo/error_scale_factor");
        target_derr_ = getParam<float>(ns+"/vo/target_derr");
        /* feature extraction settings */
        adapt_size_ = getParam<int>(ns+"/vo/adapt_size");
        adapt_thresh_ = getParam<int>(ns+"/vo/adapt_thresh");
        feat_margin_ = getParam<int>(ns+"/vo/feat_margin");
        /* visualization of points layers */
        visual_pts_layer_ = getParam<int>(ns+"/vo/visual_pts_layer");
        visual_image_layer_ = getParam<int>(ns+"/vo/visual_image_layer");
        /* imu coupling */
        alpha_ = getParam<float>(ns+"/vo/alpha");
        beta_ = getParam<float>(ns+"/vo/beta");
        float bias_a1 = getParam<float>(ns+"/vo/ba1");
        float bias_a2 = getParam<float>(ns+"/vo/ba2");
        float bias_a3 = getParam<float>(ns+"/vo/ba3");

        /* IMU Biases */
        b_w_ << 0,0,0;
        b_a_ << bias_a1, bias_a2, bias_a3;

        /* gravity - in IMU frame */
        g_ << 0,0,-9.8;

        /* set initial poses */
        T_ = Eigen::Isometry3f::Identity();
        T_kf_ = Eigen::Isometry3f::Identity();
        T_curr_ = Eigen::Isometry3f::Identity();

        // find transformation from camera to imu
        cv::Mat T_bc_inv_cv = utils::getTransformCV(nh, "cam0/T_cam_imu");
        cv::cv2eigen(T_bc_inv_cv, T_bc_inv_.matrix());
        T_bc_ = T_bc_inv_.inverse();

        /* initialize first state */
        T_wb_.setIdentity();
        T_wb_last_.setIdentity();
        v_last_.setZero();
        v_.setZero();

        /* set camera related stuff */
        frameSize_x_ = cam_->getWidth();
        frameSize_y_ = cam_->getHeight();
        focal_ = float(cam_->getf());

        /* set initial condition for member variables */
        stage_ = STAGE_PAUSED;
        set_reset_ = false;
        set_start_ = false;
        paused_ = true;
        vis_cam_avail_ = false;
        vis_pts_avail_ = false;
        frame_count_ = 0;
        perc_ = 100;
        avg_depth_ = 0;
        tracking_lost_ = false;
        layer_chop_ = false;
        num_feat_pts_.clear();

        nlays_ = full_layer_-base_layer_+1;
        assert(base_layer_ <= full_layer_);

        /* set member variables size */
        feat_pixels_.resize(nlays_);
        feat_3D_.resize(nlays_);
        intensities_.resize(nlays_);
        Gx_.resize(nlays_);
        Gy_.resize(nlays_);
        num_feat_pts_.resize(nlays_);
        error_.resize(nlays_);
        variance_.resize(nlays_);
        mask_.resize(nlays_);
        J_.resize(nlays_);

        disp_range_pyr_.resize(nlays_);
        frameSize_x_pyr_.resize(nlays_);
        frameSize_y_pyr_.resize(nlays_);
        focal_pyr_.resize(nlays_);

        /* set pyramid and grid */
        gd_.resize(nlays_);
        cam_->initPyramid(nlays_, base_layer_);
        cam_->configPyramid();
        setPyrScale();
        setGD();

        // !!NOTE:
        // number ilay starts from zero, the same size as nlays_, make sure ilay=0 corresponds to 400 pixel level pyramid.
        // Otherwise modify thread functions with the absolute ilay number passing in.

    }

    VoStereo::~VoStereo()
    {
        std::cout << "Destroying VoStereo object..." << std::endl;
        threadPrepImg0_.join();
        threadPrepImg1_.join();
        thread_pyr2_.join();
        thread_pyr3_.join();
        thread_pyr4_.join();
        thread_pyr1_disp_.join();
        thread_pyr1_feat_.join();
        thread_tracking_.join();
        std::cout << "All threads joined in vo." << std::endl;

        for (int i=0; i<nlays_; ++i) {
            if (gd_[i]!= NULL)
                delete gd_[i];
        }
    }

 /*
  * When sdd_vio_nodelet is stopped, vo_->reset() is called.
  * This will call adImagePair one more time to obtain the statistics.
  */
    void VoStereo::reset()
    {
        set_reset_ = true;
        set_start_ = false;

        threadPrepImg0_.join();
        threadPrepImg1_.join();
        thread_pyr2_.join();
        thread_pyr3_.join();
        thread_pyr4_.join();
        thread_pyr1_disp_.join();
        thread_pyr1_feat_.join();
        thread_tracking_.join();
        std::cout << "All threads joined in vo from reset." << std::endl;

        /* calling function with empty params to show statistics */
        cv::Mat img0, img1;
        double timestamp = 0;
        double seq = 0;
        addImagePair(img0, img1, timestamp, seq);

        std::cout << "vo reset finished!" << std::endl;

    }

    void VoStereo::addImagePair(const cv::Mat& img0, const cv::Mat& img1, double timestamp, double seq)
    {
        ROS_DEBUG_STREAM("addImagePair entered.");

        /* for statistics collection */
        static int keyframe_count = 0;      // total number of keyframes
        static int nextframe_count = 0;     // total number of normal frames
        static int totalframe_count = 0;    // total number of all frame
        static int num_points_largest_layer = 0;  // total number of points tracked

        if (set_reset_==true) {
            ROS_INFO_STREAM("*** RESET! -> STAGE_PAUSED");
            stage_ = STAGE_PAUSED;
            paused_ = true;
            set_reset_ = false;
        }

        vis_cam_avail_ = false;
        vis_pts_avail_ = false;

        if (stage_==STAGE_PAUSED) {
            if (set_start_==true)
            {
                ROS_INFO_STREAM("*** START! first frame received. STAGE_PAUSED");
                ROS_DEBUG_STREAM("add-ImagePair: getting image layers...");
                std::vector<cv::Mat> img0_ready(full_layer_), img1_ready(full_layer_);
                if (use_thread_ == false)
                {
                    cv::GaussianBlur(img0, img0_ready[0],cv::Size(3,3),0,0);
                    cv::GaussianBlur(img1, img1_ready[0],cv::Size(3,3),0,0);

                    for (int i=1; i<full_layer_; ++i) {
                        cv::pyrDown(img0_ready[i-1], img0_ready[i], cv::Size(int(img0_ready[i-1].cols/2), int(img0_ready[i-1].rows/2)));
                        cv::pyrDown(img1_ready[i-1], img1_ready[i], cv::Size(int(img1_ready[i-1].cols/2), int(img1_ready[i-1].rows/2)));
                    }
                }
                else
                {
                    threadPrepImg0_ = boost::thread(&VoStereo::threadCreatePyr, this, boost::ref(img0), boost::ref(img0_ready));
                    threadPrepImg1_ = boost::thread(&VoStereo::threadCreatePyr, this, boost::ref(img1), boost::ref(img1_ready));
                    threadPrepImg0_.join();
                    threadPrepImg1_.join();
                }

                /* ====== two image pipeline ====== */
                ROS_DEBUG_STREAM("addImagePair: calling twoImagePipeline...");
                twoImagePipeline(img0_ready, img1_ready);

                /* for statistics */
                keyframe_count++;
                totalframe_count++;
                num_points_largest_layer+=num_feat_pts_[0];

                /* for state machine */
                stage_ = STAGE_NEXT_FRAME;  // in the first-frame stage
                vis_cam_avail_ = true;
                vis_pts_avail_ = true;
                set_start_ = false;
                frame_count_ = 1;
                lost_frames_ = 0;
            }
            else
            {
                ROS_INFO_STREAM("*** STAGE_PAUSED ");
                std::cout << "$$$$$$$$$ -------- STATISTICS -------- $$$$$$$$$" << std::endl;
                std::cout << "Total number of keyframes: " << keyframe_count << std::endl;
                std::cout << "Total number of frames: " << totalframe_count << std::endl;
                std::cout << "Average number of points tracked in largest pyramid level: " <<
                             num_points_largest_layer/keyframe_count << std::endl;
                return;
            }

        }
        else if (stage_==STAGE_NEXT_FRAME) {
            ROS_INFO_STREAM("*** new frame received. STAGE_NEXT_FRAME");
            static kr::Timer<kr::us> timer_nextframe("nextframe_timer");
            timer_nextframe.Start();

            /* ====== get layers of images ====== */
            ROS_DEBUG_STREAM("add-ImagePair: getting image layers...");
            std::vector<cv::Mat> img0_ready(full_layer_);
            cv::GaussianBlur(img0, img0_ready[0],cv::Size(3,3),0,0);
            for (int i=1; i<full_layer_; ++i) {
                cv::pyrDown(img0_ready[i-1], img0_ready[i], cv::Size(int(img0_ready[i-1].cols/2), int(img0_ready[i-1].rows/2)));
            }

            ROS_DEBUG_STREAM("add-ImagePair: setting visualization image...");
            if (use_opt_vis_gb_)
                cv::cvtColor(img0_ready[base_layer_-1+visual_image_layer_],vis_curr,CV_GRAY2RGB);

            /* ====== one image pipeline ====== */
            ROS_DEBUG_STREAM("add-ImagePair: calling one-ImagePipeline...");
            oneImagePipeline(img0_ready);

            /* for statistics */
            nextframe_count++;
            totalframe_count++;

            /* for state machine */
            frame_count_++;
            // std::cout << "norm of translation since last keyframe: " << t_curr_.norm() << std::endl;
            const float rot_curr = log_SO3(T_curr_.rotation()).norm();  // norm of angle
            const Eigen::Vector3f t_curr = T_curr_.translation();
            if (perc_ < switch_perc_ || frame_count_ > switch_count_ || rot_curr > switch_angle_ ||
                    t_curr.norm() > switch_depth_*avg_depth_ || tracking_lost_ == true) {
                if (perc_ < switch_perc_)
                    ROS_INFO_STREAM("New KF for percentage of points visible falls below "<<switch_perc_<<"%");
                if (frame_count_ > switch_count_)
                    ROS_INFO_STREAM("New KF for number of frames exceeds "<<switch_count_);
                if (rot_curr > switch_angle_)
                    ROS_INFO_STREAM("New KF for rotation greater than threshold "<<switch_angle_);
                if (t_curr.norm() > 0.1*avg_depth_)
                    ROS_INFO_STREAM("New KF for translation over 0.1* average scene depth: "<<0.1*avg_depth_);
                if (tracking_lost_ == true)
                    ROS_INFO_STREAM("New KF for tracking lost");

                stage_ = STAGE_NEW_KEYFRAME;
                tracking_lost_ = false;
            }
            vis_cam_avail_ = true;

            timer_nextframe.Stop();
            timer_nextframe.Report();

        }
        else if (stage_ == STAGE_NEW_KEYFRAME) {
            ROS_INFO_STREAM("*** new keyframe received. STAGE_NEXT_FRAME -> STAGE_NEW_KEYFRAME");
            static kr::Timer<kr::us> timer_keyframe("keyframe_timer");
            timer_keyframe.Start();

            ROS_DEBUG_STREAM("add-ImagePair: getting image layers...");
            std::vector<cv::Mat> img0_ready(full_layer_), img1_ready(full_layer_);
            if (use_thread_ == false)
            {
                cv::GaussianBlur(img0, img0_ready[0],cv::Size(3,3),0,0);
                cv::GaussianBlur(img1, img1_ready[0],cv::Size(3,3),0,0);
                for (int i=1; i<full_layer_; ++i) {
                    cv::pyrDown(img0_ready[i-1], img0_ready[i], cv::Size(int(img0_ready[i-1].cols/2), int(img0_ready[i-1].rows/2)));
                    cv::pyrDown(img1_ready[i-1], img1_ready[i], cv::Size(int(img1_ready[i-1].cols/2), int(img1_ready[i-1].rows/2)));
                }
            }
            else
            {
                threadPrepImg0_ = boost::thread(&VoStereo::threadCreatePyr, this, boost::ref(img0), boost::ref(img0_ready));
                threadPrepImg1_ = boost::thread(&VoStereo::threadCreatePyr, this, boost::ref(img1), boost::ref(img1_ready));
                threadPrepImg0_.join();
                threadPrepImg1_.join();
            }

            ROS_DEBUG_STREAM("add-ImagePair: setting visualization image...");
            if (use_opt_vis_gb_)
                cv::cvtColor(img0_ready[base_layer_-1+visual_image_layer_],vis_curr,CV_GRAY2RGB);

            /* ====== two image pipeline ====== */
            ROS_DEBUG_STREAM("add-ImagePair: calling two-ImagePipeline...");
            twoImagePipeline(img0_ready, img1_ready);

            /* for statistics */
            keyframe_count++;
            totalframe_count++;
            num_points_largest_layer+=num_feat_pts_[0];

            /* for state machine */
            stage_ = STAGE_NEXT_FRAME;  // in the first-frame stage
            vis_cam_avail_ = true;
            vis_pts_avail_ = true;
            frame_count_ = 1;
            lost_frames_ = 0;

            timer_keyframe.Stop();
            timer_keyframe.Report();
        }

    }  // void VoStereo::addImagePair

    /* transformation of 3D points from one frame to another */
    // Input: points_f1, T_21, npts
    // Output: points_f2
    // points_f2 = T_21 * points_f1;
    void VoStereo::transform3D(const vector_aligned<Eigen::Vector3f>& points_f1, vector_aligned<Eigen::Vector3f>& points_f2,
            const Eigen::Isometry3f& T_21, const int npts)
    {
        points_f2.resize(npts);
        for (int i=0; i<npts; ++i)
        {
            Eigen::Vector4f p1 = sdd_vio::unproject3d(points_f1[i]);
            Eigen::Vector4f p2 = T_21 * p1;
            points_f2[i] = sdd_vio::project3d(p2);
        }
    }

    void VoStereo::updatePose()
    {
        /* update previous state */
        T_wb_last_ = T_bc_ * T_ * T_bc_inv_;
        /* update current pose (camera track) */
        T_ = T_kf_ * (T_curr_.inverse());
        /* update current state */
        T_wb_ = T_bc_ * T_ * T_bc_inv_;
    }

    void VoStereo::setKfPose()
    {
        /* copy current pose to keyframe pose */
        T_kf_ = T_;

        /* reinitialize current to keyframe pose */
        T_curr_ = Eigen::Isometry3f::Identity();

    }

    vector_aligned<Eigen::Vector3f> VoStereo::getVisPoints()
    {
        vector_aligned<Eigen::Vector3f> feat_3D_world;  // the points expressed in the first camera frame
        Eigen::Isometry3f T;
        T = T_bc_ * T_;

        /* for visualization only use the bottom layer of 3D points */
        transform3D(feat_3D_[visual_pts_layer_], feat_3D_world, T, feat_3D_[visual_pts_layer_].size());  // transform to camera frame

        return feat_3D_world;
    }

    void VoStereo::setPyrScale()
    {
        float scale;
        for (int i=0; i<nlays_; ++i) {
            scale = pow(0.5,(base_layer_-1+i));
            frameSize_x_pyr_[i] = scale * frameSize_x_;
            frameSize_y_pyr_[i] = scale * frameSize_y_;
            focal_pyr_[i] = scale * focal_;
            disp_range_pyr_[i] = scale * disp_range_;
        }
    }

    void VoStereo::setGD()
    {
        for (int i=0; i<nlays_; ++i) {
            gd_[i] = new Grid(frameSize_y_pyr_[i], frameSize_x_pyr_[i], gd_size_, gd_size_);
        }
    }

    void VoStereo::importImuMeas(const Eigen::Matrix3f& R_meas, const Eigen::Vector3f& v_meas, const Eigen::Vector3f& p_meas, const float t_meas, const Eigen::Matrix<float,9,9>& Cov_meas)
    {
        R_meas_ = R_meas;
        v_meas_ = v_meas;
        p_meas_ = p_meas;
        t_meas_ = t_meas;
        Cov_meas_ = Cov_meas;
    }

    void VoStereo::setImuBiasGyro(const Eigen::Vector3f& w_b)
    {
        b_w_ = w_b;
    }

    void VoStereo::setImuBiasAccel(const Eigen::Vector3f& a_b)
    {
        b_a_ = a_b;
    }

}  // namespace sdd_vio


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2014 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2013-2014 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2014, 2016, 2017.
// Modifications copyright (c) 2014-2017, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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)

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP

#include <algorithm>

#include <boost/geometry/core/exception.hpp>

#include <boost/geometry/geometries/concepts/point_concept.hpp>
#include <boost/geometry/geometries/concepts/segment_concept.hpp>

#include <boost/geometry/arithmetic/determinant.hpp>
#include <boost/geometry/algorithms/detail/assign_values.hpp>
#include <boost/geometry/algorithms/detail/assign_indexed_point.hpp>
#include <boost/geometry/algorithms/detail/equals/point_point.hpp>
#include <boost/geometry/algorithms/detail/recalculate.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_integral.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/strategies/cartesian/area_surveyor.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/strategies/cartesian/envelope_segment.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_winding.hpp>
#include <boost/geometry/strategies/cartesian/side_by_triangle.hpp>
#include <boost/geometry/strategies/covered_by.hpp>
#include <boost/geometry/strategies/intersection.hpp>
#include <boost/geometry/strategies/intersection_result.hpp>
#include <boost/geometry/strategies/side.hpp>
#include <boost/geometry/strategies/side_info.hpp>
#include <boost/geometry/strategies/within.hpp>

#include <boost/geometry/policies/robustness/robust_point_type.hpp>
#include <boost/geometry/policies/robustness/segment_ratio_type.hpp>

#if defined(BOOST_GEOMETRY_DEBUG_ROBUSTNESS)
#  include <boost/geometry/io/wkt/write.hpp>
#endif

namespace boost { namespace geometry
{

namespace strategy { namespace intersection
{

/*!
    \see http://mathworld.wolfram.com/Line-LineIntersection.html
 */
template
<
    typename CalculationType = void
>
struct cartesian_segments
{
    typedef side::side_by_triangle<CalculationType> side_strategy_type;

    static inline side_strategy_type get_side_strategy()
    {
        return side_strategy_type();
    }

    template <typename Geometry1, typename Geometry2>
    struct point_in_geometry_strategy
    {
        typedef strategy::within::cartesian_winding
            <
                typename point_type<Geometry1>::type,
                typename point_type<Geometry2>::type,
                CalculationType
            > type;
    };

    template <typename Geometry1, typename Geometry2>
    static inline typename point_in_geometry_strategy<Geometry1, Geometry2>::type
        get_point_in_geometry_strategy()
    {
        typedef typename point_in_geometry_strategy
            <
                Geometry1, Geometry2
            >::type strategy_type;
        return strategy_type();
    }

    template <typename Geometry>
    struct area_strategy
    {
        typedef area::surveyor
            <
                typename point_type<Geometry>::type,
                CalculationType
            > type;
    };

    template <typename Geometry>
    static inline typename area_strategy<Geometry>::type get_area_strategy()
    {
        typedef typename area_strategy<Geometry>::type strategy_type;
        return strategy_type();
    }

    template <typename Geometry>
    struct distance_strategy
    {
        typedef distance::pythagoras
            <
                CalculationType
            > type;
    };

    template <typename Geometry>
    static inline typename distance_strategy<Geometry>::type get_distance_strategy()
    {
        typedef typename distance_strategy<Geometry>::type strategy_type;
        return strategy_type();
    }

    typedef envelope::cartesian_segment<CalculationType>
        envelope_strategy_type;

    static inline envelope_strategy_type get_envelope_strategy()
    {
        return envelope_strategy_type();
    }

    template <typename CoordinateType, typename SegmentRatio>
    struct segment_intersection_info
    {
        typedef typename select_most_precise
            <
                CoordinateType, double
            >::type promoted_type;

        promoted_type comparable_length_a() const
        {
            return dx_a * dx_a + dy_a * dy_a;
        }

        promoted_type comparable_length_b() const
        {
            return dx_b * dx_b + dy_b * dy_b;
        }

        template <typename Point, typename Segment1, typename Segment2>
        void assign_a(Point& point, Segment1 const& a, Segment2 const& ) const
        {
            assign(point, a, dx_a, dy_a, robust_ra);
        }
        template <typename Point, typename Segment1, typename Segment2>
        void assign_b(Point& point, Segment1 const& , Segment2 const& b) const
        {
            assign(point, b, dx_b, dy_b, robust_rb);
        }

        template <typename Point, typename Segment>
        void assign(Point& point, Segment const& segment, CoordinateType const& dx, CoordinateType const& dy, SegmentRatio const& ratio) const
        {
            // Calculate the intersection point based on segment_ratio
            // Up to now, division was postponed. Here we divide using numerator/
            // denominator. In case of integer this results in an integer
            // division.
            BOOST_GEOMETRY_ASSERT(ratio.denominator() != 0);

            typedef typename promote_integral<CoordinateType>::type promoted_type;

            promoted_type const numerator
                = boost::numeric_cast<promoted_type>(ratio.numerator());
            promoted_type const denominator
                = boost::numeric_cast<promoted_type>(ratio.denominator());
            promoted_type const dx_promoted = boost::numeric_cast<promoted_type>(dx);
            promoted_type const dy_promoted = boost::numeric_cast<promoted_type>(dy);

            set<0>(point, get<0, 0>(segment) + boost::numeric_cast
                <
                    CoordinateType
                >(numerator * dx_promoted / denominator));
            set<1>(point, get<0, 1>(segment) + boost::numeric_cast
                <
                    CoordinateType
                >(numerator * dy_promoted / denominator));
        }

        CoordinateType dx_a, dy_a;
        CoordinateType dx_b, dy_b;
        SegmentRatio robust_ra;
        SegmentRatio robust_rb;
    };

    template <typename D, typename W, typename ResultType>
    static inline void cramers_rule(D const& dx_a, D const& dy_a,
        D const& dx_b, D const& dy_b, W const& wx, W const& wy,
        // out:
        ResultType& d, ResultType& da)
    {
        // Cramers rule
        d = geometry::detail::determinant<ResultType>(dx_a, dy_a, dx_b, dy_b);
        da = geometry::detail::determinant<ResultType>(dx_b, dy_b, wx, wy);
        // Ratio is da/d , collinear if d == 0, intersecting if 0 <= r <= 1
        // IntersectionPoint = (x1 + r * dx_a, y1 + r * dy_a)
    }

    // Relate segments a and b
    template
    <
        typename Segment1,
        typename Segment2,
        typename Policy,
        typename RobustPolicy
    >
    static inline typename Policy::return_type
        apply(Segment1 const& a, Segment2 const& b,
              Policy const& policy, RobustPolicy const& robust_policy)
    {
        // type them all as in Segment1 - TODO reconsider this, most precise?
        typedef typename geometry::point_type<Segment1>::type point_type;

        typedef typename geometry::robust_point_type
            <
                point_type, RobustPolicy
            >::type robust_point_type;

        point_type a0, a1, b0, b1;
        robust_point_type a0_rob, a1_rob, b0_rob, b1_rob;

        detail::assign_point_from_index<0>(a, a0);
        detail::assign_point_from_index<1>(a, a1);
        detail::assign_point_from_index<0>(b, b0);
        detail::assign_point_from_index<1>(b, b1);

        geometry::recalculate(a0_rob, a0, robust_policy);
        geometry::recalculate(a1_rob, a1, robust_policy);
        geometry::recalculate(b0_rob, b0, robust_policy);
        geometry::recalculate(b1_rob, b1, robust_policy);

        return apply(a, b, policy, robust_policy, a0_rob, a1_rob, b0_rob, b1_rob);
    }

    // The main entry-routine, calculating intersections of segments a / b
    // NOTE: Robust* types may be the same as Segments' point types
    template
    <
        typename Segment1,
        typename Segment2,
        typename Policy,
        typename RobustPolicy,
        typename RobustPoint1,
        typename RobustPoint2
    >
    static inline typename Policy::return_type
        apply(Segment1 const& a, Segment2 const& b,
              Policy const&, RobustPolicy const& /*robust_policy*/,
              RobustPoint1 const& robust_a1, RobustPoint1 const& robust_a2,
              RobustPoint2 const& robust_b1, RobustPoint2 const& robust_b2)
    {
        BOOST_CONCEPT_ASSERT( (concepts::ConstSegment<Segment1>) );
        BOOST_CONCEPT_ASSERT( (concepts::ConstSegment<Segment2>) );

        using geometry::detail::equals::equals_point_point;
        bool const a_is_point = equals_point_point(robust_a1, robust_a2);
        bool const b_is_point = equals_point_point(robust_b1, robust_b2);

        if(a_is_point && b_is_point)
        {
            return equals_point_point(robust_a1, robust_b2)
                ? Policy::degenerate(a, true)
                : Policy::disjoint()
                ;
        }

        side_info sides;
        sides.set<0>(side_strategy_type::apply(robust_b1, robust_b2, robust_a1),
                     side_strategy_type::apply(robust_b1, robust_b2, robust_a2));

        if (sides.same<0>())
        {
            // Both points are at same side of other segment, we can leave
            return Policy::disjoint();
        }

        sides.set<1>(side_strategy_type::apply(robust_a1, robust_a2, robust_b1),
                     side_strategy_type::apply(robust_a1, robust_a2, robust_b2));

        if (sides.same<1>())
        {
            // Both points are at same side of other segment, we can leave
            return Policy::disjoint();
        }

        bool collinear = sides.collinear();

        typedef typename select_most_precise
            <
                typename geometry::coordinate_type<RobustPoint1>::type,
                typename geometry::coordinate_type<RobustPoint2>::type
            >::type robust_coordinate_type;

        typedef typename segment_ratio_type
            <
                typename geometry::point_type<Segment1>::type, // TODO: most precise point?
                RobustPolicy
            >::type ratio_type;

        segment_intersection_info
            <
                typename select_calculation_type<Segment1, Segment2, CalculationType>::type,
                ratio_type
            > sinfo;

        sinfo.dx_a = get<1, 0>(a) - get<0, 0>(a); // distance in x-dir
        sinfo.dx_b = get<1, 0>(b) - get<0, 0>(b);
        sinfo.dy_a = get<1, 1>(a) - get<0, 1>(a); // distance in y-dir
        sinfo.dy_b = get<1, 1>(b) - get<0, 1>(b);

        robust_coordinate_type const robust_dx_a = get<0>(robust_a2) - get<0>(robust_a1);
        robust_coordinate_type const robust_dx_b = get<0>(robust_b2) - get<0>(robust_b1);
        robust_coordinate_type const robust_dy_a = get<1>(robust_a2) - get<1>(robust_a1);
        robust_coordinate_type const robust_dy_b = get<1>(robust_b2) - get<1>(robust_b1);

        // r: ratio 0-1 where intersection divides A/B
        // (only calculated for non-collinear segments)
        if (! collinear)
        {
            robust_coordinate_type robust_da0, robust_da;
            robust_coordinate_type robust_db0, robust_db;

            cramers_rule(robust_dx_a, robust_dy_a, robust_dx_b, robust_dy_b,
                get<0>(robust_a1) - get<0>(robust_b1),
                get<1>(robust_a1) - get<1>(robust_b1),
                robust_da0, robust_da);

            cramers_rule(robust_dx_b, robust_dy_b, robust_dx_a, robust_dy_a,
                get<0>(robust_b1) - get<0>(robust_a1),
                get<1>(robust_b1) - get<1>(robust_a1),
                robust_db0, robust_db);

            math::detail::equals_factor_policy<robust_coordinate_type>
                policy(robust_dx_a, robust_dy_a, robust_dx_b, robust_dy_b);
            robust_coordinate_type const zero = 0;
            if (math::detail::equals_by_policy(robust_da0, zero, policy)
             || math::detail::equals_by_policy(robust_db0, zero, policy))
            {
                // If this is the case, no rescaling is done for FP precision.
                // We set it to collinear, but it indicates a robustness issue.
                sides.set<0>(0,0);
                sides.set<1>(0,0);
                collinear = true;
            }
            else
            {
                sinfo.robust_ra.assign(robust_da, robust_da0);
                sinfo.robust_rb.assign(robust_db, robust_db0);
            }
        }

        if (collinear)
        {
            std::pair<bool, bool> const collinear_use_first
                    = is_x_more_significant(geometry::math::abs(robust_dx_a),
                                            geometry::math::abs(robust_dy_a),
                                            geometry::math::abs(robust_dx_b),
                                            geometry::math::abs(robust_dy_b),
                                            a_is_point, b_is_point);

            if (collinear_use_first.second)
            {
                // Degenerate cases: segments of single point, lying on other segment, are not disjoint
                // This situation is collinear too

                if (collinear_use_first.first)
                {
                    return relate_collinear<0, Policy, ratio_type>(a, b,
                            robust_a1, robust_a2, robust_b1, robust_b2,
                            a_is_point, b_is_point);
                }
                else
                {
                    // Y direction contains larger segments (maybe dx is zero)
                    return relate_collinear<1, Policy, ratio_type>(a, b,
                            robust_a1, robust_a2, robust_b1, robust_b2,
                            a_is_point, b_is_point);
                }
            }
        }

        return Policy::segments_crosses(sides, sinfo, a, b);
    }

private:
    // first is true if x is more significant
    // second is true if the more significant difference is not 0
    template <typename RobustCoordinateType>
    static inline std::pair<bool, bool>
        is_x_more_significant(RobustCoordinateType const& abs_robust_dx_a,
                              RobustCoordinateType const& abs_robust_dy_a,
                              RobustCoordinateType const& abs_robust_dx_b,
                              RobustCoordinateType const& abs_robust_dy_b,
                              bool const a_is_point,
                              bool const b_is_point)
    {
        //BOOST_GEOMETRY_ASSERT_MSG(!(a_is_point && b_is_point), "both segments shouldn't be degenerated");

        // for degenerated segments the second is always true because this function
        // shouldn't be called if both segments were degenerated

        if (a_is_point)
        {
            return std::make_pair(abs_robust_dx_b >= abs_robust_dy_b, true);
        }
        else if (b_is_point)
        {
            return std::make_pair(abs_robust_dx_a >= abs_robust_dy_a, true);
        }
        else
        {
            RobustCoordinateType const min_dx = (std::min)(abs_robust_dx_a, abs_robust_dx_b);
            RobustCoordinateType const min_dy = (std::min)(abs_robust_dy_a, abs_robust_dy_b);
            return min_dx == min_dy ?
                    std::make_pair(true, min_dx > RobustCoordinateType(0)) :
                    std::make_pair(min_dx > min_dy, true);
        }
    }

    template
    <
        std::size_t Dimension,
        typename Policy,
        typename RatioType,
        typename Segment1,
        typename Segment2,
        typename RobustPoint1,
        typename RobustPoint2
    >
    static inline typename Policy::return_type
        relate_collinear(Segment1 const& a,
                         Segment2 const& b,
                         RobustPoint1 const& robust_a1, RobustPoint1 const& robust_a2,
                         RobustPoint2 const& robust_b1, RobustPoint2 const& robust_b2,
                         bool a_is_point, bool b_is_point)
    {
        if (a_is_point)
        {
            return relate_one_degenerate<Policy, RatioType>(a,
                get<Dimension>(robust_a1),
                get<Dimension>(robust_b1), get<Dimension>(robust_b2),
                true);
        }
        if (b_is_point)
        {
            return relate_one_degenerate<Policy, RatioType>(b,
                get<Dimension>(robust_b1),
                get<Dimension>(robust_a1), get<Dimension>(robust_a2),
                false);
        }
        return relate_collinear<Policy, RatioType>(a, b,
                                get<Dimension>(robust_a1),
                                get<Dimension>(robust_a2),
                                get<Dimension>(robust_b1),
                                get<Dimension>(robust_b2));
    }

    /// Relate segments known collinear
    template
    <
        typename Policy,
        typename RatioType,
        typename Segment1,
        typename Segment2,
        typename RobustType1,
        typename RobustType2
    >
    static inline typename Policy::return_type
        relate_collinear(Segment1 const& a, Segment2 const& b,
                         RobustType1 oa_1, RobustType1 oa_2,
                         RobustType2 ob_1, RobustType2 ob_2)
    {
        // Calculate the ratios where a starts in b, b starts in a
        //         a1--------->a2         (2..7)
        //                b1----->b2      (5..8)
        // length_a: 7-2=5
        // length_b: 8-5=3
        // b1 is located w.r.t. a at ratio: (5-2)/5=3/5 (on a)
        // b2 is located w.r.t. a at ratio: (8-2)/5=6/5 (right of a)
        // a1 is located w.r.t. b at ratio: (2-5)/3=-3/3 (left of b)
        // a2 is located w.r.t. b at ratio: (7-5)/3=2/3 (on b)
        // A arrives (a2 on b), B departs (b1 on a)

        // If both are reversed:
        //         a2<---------a1         (7..2)
        //                b2<-----b1      (8..5)
        // length_a: 2-7=-5
        // length_b: 5-8=-3
        // b1 is located w.r.t. a at ratio: (8-7)/-5=-1/5 (before a starts)
        // b2 is located w.r.t. a at ratio: (5-7)/-5=2/5 (on a)
        // a1 is located w.r.t. b at ratio: (7-8)/-3=1/3 (on b)
        // a2 is located w.r.t. b at ratio: (2-8)/-3=6/3 (after b ends)

        // If both one is reversed:
        //         a1--------->a2         (2..7)
        //                b2<-----b1      (8..5)
        // length_a: 7-2=+5
        // length_b: 5-8=-3
        // b1 is located w.r.t. a at ratio: (8-2)/5=6/5 (after a ends)
        // b2 is located w.r.t. a at ratio: (5-2)/5=3/5 (on a)
        // a1 is located w.r.t. b at ratio: (2-8)/-3=6/3 (after b ends)
        // a2 is located w.r.t. b at ratio: (7-8)/-3=1/3 (on b)
        RobustType1 const length_a = oa_2 - oa_1; // no abs, see above
        RobustType2 const length_b = ob_2 - ob_1;

        RatioType ra_from(oa_1 - ob_1, length_b);
        RatioType ra_to(oa_2 - ob_1, length_b);
        RatioType rb_from(ob_1 - oa_1, length_a);
        RatioType rb_to(ob_2 - oa_1, length_a);

        // use absolute measure to detect endpoints intersection
        // NOTE: it'd be possible to calculate bx_wrt_a using ax_wrt_b values
        int const a1_wrt_b = position_value(oa_1, ob_1, ob_2);
        int const a2_wrt_b = position_value(oa_2, ob_1, ob_2);
        int const b1_wrt_a = position_value(ob_1, oa_1, oa_2);
        int const b2_wrt_a = position_value(ob_2, oa_1, oa_2);

        // fix the ratios if necessary
        // CONSIDER: fixing ratios also in other cases, if they're inconsistent
        // e.g. if ratio == 1 or 0 (so IP at the endpoint)
        // but position value indicates that the IP is in the middle of the segment
        // because one of the segments is very long
        // In such case the ratios could be moved into the middle direction
        // by some small value (e.g. EPS+1ULP)
        if (a1_wrt_b == 1)
        {
            ra_from.assign(0, 1);
            rb_from.assign(0, 1);
        }
        else if (a1_wrt_b == 3)
        {
            ra_from.assign(1, 1);
            rb_to.assign(0, 1);
        }

        if (a2_wrt_b == 1)
        {
            ra_to.assign(0, 1);
            rb_from.assign(1, 1);
        }
        else if (a2_wrt_b == 3)
        {
            ra_to.assign(1, 1);
            rb_to.assign(1, 1);
        }

        if ((a1_wrt_b < 1 && a2_wrt_b < 1) || (a1_wrt_b > 3 && a2_wrt_b > 3))
        //if ((ra_from.left() && ra_to.left()) || (ra_from.right() && ra_to.right()))
        {
            return Policy::disjoint();
        }

        bool const opposite = math::sign(length_a) != math::sign(length_b);

        return Policy::segments_collinear(a, b, opposite,
                                          a1_wrt_b, a2_wrt_b, b1_wrt_a, b2_wrt_a,
                                          ra_from, ra_to, rb_from, rb_to);
    }

    /// Relate segments where one is degenerate
    template
    <
        typename Policy,
        typename RatioType,
        typename DegenerateSegment,
        typename RobustType1,
        typename RobustType2
    >
    static inline typename Policy::return_type
        relate_one_degenerate(DegenerateSegment const& degenerate_segment,
                              RobustType1 d, RobustType2 s1, RobustType2 s2,
                              bool a_degenerate)
    {
        // Calculate the ratios where ds starts in s
        //         a1--------->a2         (2..6)
        //              b1/b2      (4..4)
        // Ratio: (4-2)/(6-2)
        RatioType const ratio(d - s1, s2 - s1);

        if (!ratio.on_segment())
        {
            return Policy::disjoint();
        }

        return Policy::one_degenerate(degenerate_segment, ratio, a_degenerate);
    }

    template <typename ProjCoord1, typename ProjCoord2>
    static inline int position_value(ProjCoord1 const& ca1,
                                     ProjCoord2 const& cb1,
                                     ProjCoord2 const& cb2)
    {
        // S1x  0   1    2     3   4
        // S2       |---------->
        return math::equals(ca1, cb1) ? 1
             : math::equals(ca1, cb2) ? 3
             : cb1 < cb2 ?
                ( ca1 < cb1 ? 0
                : ca1 > cb2 ? 4
                : 2 )
              : ( ca1 > cb1 ? 0
                : ca1 < cb2 ? 4
                : 2 );
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType>
struct default_strategy<cartesian_tag, CalculationType>
{
    typedef cartesian_segments<CalculationType> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::intersection

namespace strategy
{

namespace within { namespace services
{

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, linear_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, polygonal_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, linear_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, polygonal_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

}} // within::services

namespace covered_by { namespace services
{

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, linear_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, polygonal_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, linear_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, polygonal_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

}} // within::services

} // strategy

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP