///////////////////////////////////////////////////////////////
//  Copyright Jens Maurer 2006-1011
//  Copyright Steven Watanabe 2011
//  Copyright 2012 John Maddock. 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_

#ifndef BOOST_MP_RANDOM_HPP
#define BOOST_MP_RANDOM_HPP

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4127)
#endif

#include <boost/multiprecision/number.hpp>

namespace boost{ namespace random{ namespace detail{
//
// This is a horrible hack: this declaration has to appear before the definition of
// uniform_int_distribution, otherwise it won't be used...
// Need to find a better solution, like make Boost.Random safe to use with
// UDT's and depricate/remove this header altogether.
//
template<class Engine, class Backend, boost::multiprecision::expression_template_option ExpressionTemplates>
boost::multiprecision::number<Backend, ExpressionTemplates>
   generate_uniform_int(Engine& eng, const boost::multiprecision::number<Backend, ExpressionTemplates>& min_value, const boost::multiprecision::number<Backend, ExpressionTemplates>& max_value);

}}}

#include <boost/random.hpp>
#include <boost/mpl/eval_if.hpp>

namespace boost{
namespace random{
namespace detail{

template<class Backend, boost::multiprecision::expression_template_option ExpressionTemplates>
struct subtract<boost::multiprecision::number<Backend, ExpressionTemplates>, true>
{
  typedef boost::multiprecision::number<Backend, ExpressionTemplates> result_type;
  result_type operator()(result_type const& x, result_type const& y) { return x - y; }
};

}

template<class Engine, std::size_t w, class Backend, boost::multiprecision::expression_template_option ExpressionTemplates>
class independent_bits_engine<Engine, w, boost::multiprecision::number<Backend, ExpressionTemplates> >
{
public:
    typedef Engine base_type;
    typedef boost::multiprecision::number<Backend, ExpressionTemplates> result_type;

    static result_type min BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return 0; }
    // This is the only function we modify compared to the primary template:
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
    {
       // This expression allows for the possibility that w == std::numeric_limits<result_type>::digits:
       return (((result_type(1) << (w - 1)) - 1) << 1) + 1;
    }

    independent_bits_engine() { }

    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(independent_bits_engine,
        result_type, seed_arg)
    {
        _base.seed(seed_arg);
    }

    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(independent_bits_engine,
        SeedSeq, seq)
    { _base.seed(seq); }

    independent_bits_engine(const base_type& base_arg) : _base(base_arg) {}

    template<class It>
    independent_bits_engine(It& first, It last) : _base(first, last) { }

    void seed() { _base.seed(); }

    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(independent_bits_engine,
        result_type, seed_arg)
    { _base.seed(seed_arg); }

    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(independent_bits_engine,
        SeedSeq, seq)
    { _base.seed(seq); }

    template<class It> void seed(It& first, It last)
    { _base.seed(first, last); }

    result_type operator()()
    {
        // While it may seem wasteful to recalculate this
        // every time, both msvc and gcc can propagate
        // constants, resolving this at compile time.
        base_unsigned range =
            detail::subtract<base_result>()((_base.max)(), (_base.min)());
        std::size_t m =
            (range == (std::numeric_limits<base_unsigned>::max)()) ?
                std::numeric_limits<base_unsigned>::digits :
                detail::integer_log2(range + 1);
        std::size_t n = (w + m - 1) / m;
        std::size_t w0, n0;
        base_unsigned y0, y1;
        base_unsigned y0_mask, y1_mask;
        calc_params(n, range, w0, n0, y0, y1, y0_mask, y1_mask);
        if(base_unsigned(range - y0 + 1) > y0 / n) {
            // increment n and try again.
            ++n;
            calc_params(n, range, w0, n0, y0, y1, y0_mask, y1_mask);
        }

        BOOST_ASSERT(n0*w0 + (n - n0)*(w0 + 1) == w);

        result_type S = 0;
        for(std::size_t k = 0; k < n0; ++k) {
            base_unsigned u;
            do {
                u = detail::subtract<base_result>()(_base(), (_base.min)());
            } while(u > base_unsigned(y0 - 1));
            S = (S << w0) + (u & y0_mask);
        }
        for(std::size_t k = 0; k < (n - n0); ++k) {
            base_unsigned u;
            do {
                u = detail::subtract<base_result>()(_base(), (_base.min)());
            } while(u > base_unsigned(y1 - 1));
            S = (S << (w0 + 1)) + (u & y1_mask);
        }
        return S;
    }

    /** Fills a range with random values */
    template<class Iter>
    void generate(Iter first, Iter last)
    { detail::generate_from_int(*this, first, last); }

    /** Advances the state of the generator by @c z. */
    void discard(boost::uintmax_t z)
    {
        for(boost::uintmax_t i = 0; i < z; ++i) {
            (*this)();
        }
    }

    const base_type& base() const { return _base; }

    /**
     * Writes the textual representation if the generator to a @c std::ostream.
     * The textual representation of the engine is the textual representation
     * of the base engine.
     */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, independent_bits_engine, r)
    {
        os << r._base;
        return os;
    }

    /**
     * Reads the state of an @c independent_bits_engine from a
     * @c std::istream.
     */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, independent_bits_engine, r)
    {
        is >> r._base;
        return is;
    }

    /**
     * Returns: true iff the two @c independent_bits_engines will
     * produce the same sequence of values.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(independent_bits_engine, x, y)
    { return x._base == y._base; }
    /**
     * Returns: true iff the two @c independent_bits_engines will
     * produce different sequences of values.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(independent_bits_engine)

private:

    /// \cond show_private
    typedef typename base_type::result_type base_result;
    typedef typename make_unsigned<base_result>::type base_unsigned;

    void calc_params(
        std::size_t n, base_unsigned range,
        std::size_t& w0, std::size_t& n0,
        base_unsigned& y0, base_unsigned& y1,
        base_unsigned& y0_mask, base_unsigned& y1_mask)
    {
        BOOST_ASSERT(w >= n);
        w0 = w/n;
        n0 = n - w % n;
        y0_mask = (base_unsigned(2) << (w0 - 1)) - 1;
        y1_mask = (y0_mask << 1) | 1;
        y0 = (range + 1) & ~y0_mask;
        y1 = (range + 1) & ~y1_mask;
        BOOST_ASSERT(y0 != 0 || base_unsigned(range + 1) == 0);
    }
    /// \endcond

    Engine _base;
};

template<class Backend, boost::multiprecision::expression_template_option ExpressionTemplates>
class uniform_smallint<boost::multiprecision::number<Backend, ExpressionTemplates> >
{
public:
    typedef boost::multiprecision::number<Backend, ExpressionTemplates> input_type;
    typedef boost::multiprecision::number<Backend, ExpressionTemplates> result_type;

    class param_type
    {
    public:

        typedef uniform_smallint distribution_type;

        /** constructs the parameters of a @c uniform_smallint distribution. */
        param_type(result_type const& min_arg = 0, result_type const& max_arg = 9)
          : _min(min_arg), _max(max_arg)
        {
            BOOST_ASSERT(_min <= _max);
        }

        /** Returns the minimum value. */
        result_type a() const { return _min; }
        /** Returns the maximum value. */
        result_type b() const { return _max; }

        /** Writes the parameters to a @c std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        {
            os << parm._min << " " << parm._max;
            return os;
        }

        /** Reads the parameters from a @c std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        {
            is >> parm._min >> std::ws >> parm._max;
            return is;
        }

        /** Returns true if the two sets of parameters are equal. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._min == rhs._min && lhs._max == rhs._max; }

        /** Returns true if the two sets of parameters are different. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:
        result_type _min;
        result_type _max;
    };

    /**
     * Constructs a @c uniform_smallint. @c min and @c max are the
     * lower and upper bounds of the output range, respectively.
     */
    explicit uniform_smallint(result_type const& min_arg = 0, result_type const& max_arg = 9)
      : _min(min_arg), _max(max_arg) {}

    /**
     * Constructs a @c uniform_smallint from its parameters.
     */
    explicit uniform_smallint(const param_type& parm)
      : _min(parm.a()), _max(parm.b()) {}

    /** Returns the minimum value of the distribution. */
    result_type a() const { return _min; }
    /** Returns the maximum value of the distribution. */
    result_type b() const { return _max; }
    /** Returns the minimum value of the distribution. */
    result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return _min; }
    /** Returns the maximum value of the distribution. */
    result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return _max; }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(_min, _max); }
    /** Sets the parameters of the distribution. */
    void param(const param_type& parm)
    {
        _min = parm.a();
        _max = parm.b();
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() { }

    /** Returns a value uniformly distributed in the range [min(), max()]. */
    template<class Engine>
    result_type operator()(Engine& eng) const
    {
        typedef typename Engine::result_type base_result;
        return generate(eng, boost::is_integral<base_result>());
    }

    /** Returns a value uniformly distributed in the range [param.a(), param.b()]. */
    template<class Engine>
    result_type operator()(Engine& eng, const param_type& parm) const
    { return uniform_smallint(parm)(eng); }

    /** Writes the distribution to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, uniform_smallint, ud)
    {
        os << ud._min << " " << ud._max;
        return os;
    }

    /** Reads the distribution from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, uniform_smallint, ud)
    {
        is >> ud._min >> std::ws >> ud._max;
        return is;
    }

    /**
     * Returns true if the two distributions will produce identical
     * sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(uniform_smallint, lhs, rhs)
    { return lhs._min == rhs._min && lhs._max == rhs._max; }

    /**
     * Returns true if the two distributions may produce different
     * sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(uniform_smallint)

private:

    // \cond show_private
    template<class Engine>
    result_type generate(Engine& eng, boost::mpl::true_) const
    {
        // equivalent to (eng() - eng.min()) % (_max - _min + 1) + _min,
        // but guarantees no overflow.
        typedef typename Engine::result_type base_result;
        typedef typename boost::make_unsigned<base_result>::type base_unsigned;
        typedef result_type  range_type;
        range_type range = random::detail::subtract<result_type>()(_max, _min);
        base_unsigned base_range =
            random::detail::subtract<result_type>()((eng.max)(), (eng.min)());
        base_unsigned val =
            random::detail::subtract<base_result>()(eng(), (eng.min)());
        if(range >= base_range) {
            return boost::random::detail::add<range_type, result_type>()(
                static_cast<range_type>(val), _min);
        } else {
            base_unsigned modulus = static_cast<base_unsigned>(range) + 1;
            return boost::random::detail::add<range_type, result_type>()(
                static_cast<range_type>(val % modulus), _min);
        }
    }

    template<class Engine>
    result_type generate(Engine& eng, boost::mpl::false_) const
    {
        typedef typename Engine::result_type base_result;
        typedef result_type                  range_type;
        range_type range = random::detail::subtract<result_type>()(_max, _min);
        base_result val = boost::uniform_01<base_result>()(eng);
        // what is the worst that can possibly happen here?
        // base_result may not be able to represent all the values in [0, range]
        // exactly.  If this happens, it will cause round off error and we
        // won't be able to produce all the values in the range.  We don't
        // care about this because the user has already told us not to by
        // using uniform_smallint.  However, we do need to be careful
        // to clamp the result, or floating point rounding can produce
        // an out of range result.
        range_type offset = static_cast<range_type>(val * (range + 1));
        if(offset > range) return _max;
        return boost::random::detail::add<range_type, result_type>()(offset , _min);
    }
    // \endcond

    result_type _min;
    result_type _max;
};

namespace detail{

template<class Backend, boost::multiprecision::expression_template_option ExpressionTemplates>
struct select_uniform_01<boost::multiprecision::number<Backend, ExpressionTemplates> >
{
  template<class RealType>
  struct apply
  {
    typedef new_uniform_01<boost::multiprecision::number<Backend, ExpressionTemplates> > type;
  };
};

template<class Engine, class Backend, boost::multiprecision::expression_template_option ExpressionTemplates>
boost::multiprecision::number<Backend, ExpressionTemplates>
   generate_uniform_int(
    Engine& eng, const boost::multiprecision::number<Backend, ExpressionTemplates>& min_value, const boost::multiprecision::number<Backend, ExpressionTemplates>& max_value,
    boost::mpl::true_ /** is_integral<Engine::result_type> */)
{
    typedef boost::multiprecision::number<Backend, ExpressionTemplates> result_type;
    // Since we're using big-numbers, use the result type for all internal calculations:
    typedef result_type range_type;
    typedef result_type base_result;
    typedef result_type base_unsigned;
    const range_type range = random::detail::subtract<result_type>()(max_value, min_value);
    const base_result bmin = (eng.min)();
    const base_unsigned brange =
      random::detail::subtract<base_result>()((eng.max)(), (eng.min)());

    if(range == 0) {
      return min_value;
    } else if(brange == range) {
      // this will probably never happen in real life
      // basically nothing to do; just take care we don't overflow / underflow
      base_unsigned v = random::detail::subtract<base_result>()(eng(), bmin);
      return random::detail::add<base_unsigned, result_type>()(v, min_value);
    } else if(brange < range) {
      // use rejection method to handle things like 0..3 --> 0..4
      for(;;) {
        // concatenate several invocations of the base RNG
        // take extra care to avoid overflows

        //  limit == floor((range+1)/(brange+1))
        //  Therefore limit*(brange+1) <= range+1
        range_type limit;
        if(std::numeric_limits<range_type>::is_bounded && (range == (std::numeric_limits<range_type>::max)())) {
          limit = range/(range_type(brange)+1);
          if(range % (range_type(brange)+1) == range_type(brange))
            ++limit;
        } else {
          limit = (range+1)/(range_type(brange)+1);
        }

        // We consider "result" as expressed to base (brange+1):
        // For every power of (brange+1), we determine a random factor
        range_type result = range_type(0);
        range_type mult = range_type(1);

        // loop invariants:
        //  result < mult
        //  mult <= range
        while(mult <= limit) {
          // Postcondition: result <= range, thus no overflow
          //
          // limit*(brange+1)<=range+1                   def. of limit       (1)
          // eng()-bmin<=brange                          eng() post.         (2)
          // and mult<=limit.                            loop condition      (3)
          // Therefore mult*(eng()-bmin+1)<=range+1      by (1),(2),(3)      (4)
          // Therefore mult*(eng()-bmin)+mult<=range+1   rearranging (4)     (5)
          // result<mult                                 loop invariant      (6)
          // Therefore result+mult*(eng()-bmin)<range+1  by (5), (6)         (7)
          //
          // Postcondition: result < mult*(brange+1)
          //
          // result<mult                                 loop invariant      (1)
          // eng()-bmin<=brange                          eng() post.         (2)
          // Therefore result+mult*(eng()-bmin) <
          //           mult+mult*(eng()-bmin)            by (1)              (3)
          // Therefore result+(eng()-bmin)*mult <
          //           mult+mult*brange                  by (2), (3)         (4)
          // Therefore result+(eng()-bmin)*mult <
          //           mult*(brange+1)                   by (4)
          result += static_cast<range_type>(random::detail::subtract<base_result>()(eng(), bmin) * mult);

          // equivalent to (mult * (brange+1)) == range+1, but avoids overflow.
          if(mult * range_type(brange) == range - mult + 1) {
              // The destination range is an integer power of
              // the generator's range.
              return(result);
          }

          // Postcondition: mult <= range
          //
          // limit*(brange+1)<=range+1                   def. of limit       (1)
          // mult<=limit                                 loop condition      (2)
          // Therefore mult*(brange+1)<=range+1          by (1), (2)         (3)
          // mult*(brange+1)!=range+1                    preceding if        (4)
          // Therefore mult*(brange+1)<range+1           by (3), (4)         (5)
          //
          // Postcondition: result < mult
          //
          // See the second postcondition on the change to result.
          mult *= range_type(brange)+range_type(1);
        }
        // loop postcondition: range/mult < brange+1
        //
        // mult > limit                                  loop condition      (1)
        // Suppose range/mult >= brange+1                Assumption          (2)
        // range >= mult*(brange+1)                      by (2)              (3)
        // range+1 > mult*(brange+1)                     by (3)              (4)
        // range+1 > (limit+1)*(brange+1)                by (1), (4)         (5)
        // (range+1)/(brange+1) > limit+1                by (5)              (6)
        // limit < floor((range+1)/(brange+1))           by (6)              (7)
        // limit==floor((range+1)/(brange+1))            def. of limit       (8)
        // not (2)                                       reductio            (9)
        //
        // loop postcondition: (range/mult)*mult+(mult-1) >= range
        //
        // (range/mult)*mult + range%mult == range       identity            (1)
        // range%mult < mult                             def. of %           (2)
        // (range/mult)*mult+mult > range                by (1), (2)         (3)
        // (range/mult)*mult+(mult-1) >= range           by (3)              (4)
        //
        // Note that the maximum value of result at this point is (mult-1),
        // so after this final step, we generate numbers that can be
        // at least as large as range.  We have to really careful to avoid
        // overflow in this final addition and in the rejection.  Anything
        // that overflows is larger than range and can thus be rejected.

        // range/mult < brange+1  -> no endless loop
        range_type result_increment =
            generate_uniform_int(
                eng,
                static_cast<range_type>(0),
                static_cast<range_type>(range/mult),
                boost::mpl::true_());
        if(std::numeric_limits<range_type>::is_bounded && ((std::numeric_limits<range_type>::max)() / mult < result_increment)) {
          // The multiplication would overflow.  Reject immediately.
          continue;
        }
        result_increment *= mult;
        // unsigned integers are guaranteed to wrap on overflow.
        result += result_increment;
        if(result < result_increment) {
          // The addition overflowed.  Reject.
          continue;
        }
        if(result > range) {
          // Too big.  Reject.
          continue;
        }
        return random::detail::add<range_type, result_type>()(result, min_value);
      }
    } else {                   // brange > range
      range_type bucket_size;
      // it's safe to add 1 to range, as long as we cast it first,
      // because we know that it is less than brange.  However,
      // we do need to be careful not to cause overflow by adding 1
      // to brange.
      if(std::numeric_limits<base_unsigned>::is_bounded && (brange == (std::numeric_limits<base_unsigned>::max)())) {
        bucket_size = brange / (range+1);
        if(brange % (range+1) == range) {
          ++bucket_size;
        }
      } else {
        bucket_size = (brange+1) / (range+1);
      }
      for(;;) {
        range_type result =
          random::detail::subtract<base_result>()(eng(), bmin);
        result /= bucket_size;
        // result and range are non-negative, and result is possibly larger
        // than range, so the cast is safe
        if(result <= range)
          return result + min_value;
      }
    }
}

template<class Engine, class Backend, boost::multiprecision::expression_template_option ExpressionTemplates>
inline boost::multiprecision::number<Backend, ExpressionTemplates>
   generate_uniform_int(Engine& eng, const boost::multiprecision::number<Backend, ExpressionTemplates>& min_value, const boost::multiprecision::number<Backend, ExpressionTemplates>& max_value)
{
    typedef typename Engine::result_type base_result;
    typedef typename mpl::or_<boost::is_integral<base_result>, mpl::bool_<boost::multiprecision::number_category<Backend>::value == boost::multiprecision::number_kind_integer> >::type tag_type;
    return generate_uniform_int(eng, min_value, max_value,
        tag_type());
}

} // detail

}} // namespaces

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#endif


// ----------------- BEGIN LICENSE BLOCK ---------------------------------
//
// Copyright (C) 2018-2019 Intel Corporation
//
// SPDX-License-Identifier: MIT
//
// ----------------- END LICENSE BLOCK -----------------------------------

#include "ad/map/maker/map_data/Lane.hpp"
#include <ad/map/maker/geometry/Point2d.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <unordered_set>
#include "ad/map/maker/map_data/MapDataStore.hpp"
#include "ad/map/maker/map_data/PolyLineConversion.hpp"

namespace ad {
namespace map {
namespace maker {
namespace map_data {

typedef ::boost::geometry::model::d2::point_xy<double> Boost2dPoint;
typedef ::boost::geometry::model::polygon<Boost2dPoint> BoostPolygon;

BoostPolygon fromLane(MapDataStore const &store, Lane const &lane)
{
  auto const &leftBorder = store.polyLine(lane.leftBorder).mNodes;
  auto const &rightBorder = store.polyLine(lane.rightBorder).mNodes;
  // at first shot, we assume that both borders have the same order of points, e.g. from left to right
  std::vector<Boost2dPoint> points;
  for (auto const id : leftBorder)
  {
    auto const &p = store.point(id);
    points.push_back({p.x, p.y});
  }
  for (auto it = rightBorder.rbegin(); it != rightBorder.rend(); ++it)
  {
    auto const &p = store.point(*it);
    points.push_back({p.x, p.y});
  }
  // close polygon
  points.push_back(points.front());
  BoostPolygon result;
  ::boost::geometry::assign_points(result, points);
  // ensure that all polygons have the same orientation
  ::boost::geometry::correct(result);
  return result;
}

bool Lane::overlapsWithLane(MapDataStore const &store, Lane const &other) const
{
  if (&other == this)
  {
    return true;
  }
  auto thisP = fromLane(store, *this);
  auto thatP = fromLane(store, other);
  std::deque<BoostPolygon> output;
  (void)::boost::geometry::intersection(thisP, thatP, output);
  return (!output.empty());
}

Contact Lane::checkLeftRightContact(MapDataStore const &store, Lane const &other) const
{
  if (&other == this)
  {
    return Contact::Overlap;
  }

  // check forward contact
  auto const &thisLeftBorder = store.polyLine(leftBorder).mNodes;
  auto const &thisRightBorder = store.polyLine(rightBorder).mNodes;

  auto const &otherLeftBorder = store.polyLine(other.leftBorder).mNodes;
  auto const &otherRightBorder = store.polyLine(other.rightBorder).mNodes;

  auto const thisLeftStart = thisLeftBorder.front();
  auto const thisRightStart = thisRightBorder.front();
  auto const otherLeftStart = otherLeftBorder.front();
  auto const otherRightStart = otherRightBorder.front();

  auto const thisLeftEnd = thisLeftBorder.back();
  auto const thisRightEnd = thisRightBorder.back();
  auto const otherLeftEnd = otherLeftBorder.back();
  auto const otherRightEnd = otherRightBorder.back();

  std::unordered_set<MapDataId> startPoints;
  std::unordered_set<MapDataId> endPoints;

  startPoints.insert(thisLeftStart);
  startPoints.insert(thisRightStart);
  startPoints.insert(otherLeftStart);
  startPoints.insert(otherRightStart);

  endPoints.insert(thisLeftEnd);
  endPoints.insert(thisRightEnd);
  endPoints.insert(otherLeftEnd);
  endPoints.insert(otherRightEnd);

  // No contact means that all id points are unique and therefore we would have 4 points on each set.
  if ((startPoints.size() < 4) && (endPoints.size() < 4))
  {
    if ((thisLeftStart == otherRightStart) || (thisLeftEnd == otherRightEnd))
    {
      return Contact::Left;
    }

    if ((thisRightStart == otherLeftStart) || (thisRightEnd == otherLeftEnd))
    {
      return Contact::Right;
    }
  }

  // @TODO handle reverse contact. i.e. when there is contact with the inverted(other lane).

  return Contact::None;
}

bool Lane::hasLeftContactWithLane(MapDataStore const &store, Lane const &other) const
{
  if (checkLeftRightContact(store, other) == Contact::Left)
  {
    return true;
  }
  return false;
}

bool Lane::hasRightContactWithLane(MapDataStore const &store, Lane const &other) const
{
  if (checkLeftRightContact(store, other) == Contact::Right)
  {
    return true;
  }
  return false;
}

void Lane::invertLaneDirection(MapDataStore &store, bool invertBorderPolylines)
{
  if (invertBorderPolylines)
  {
    auto &leftBorderPolyLine = store.polyLine(leftBorder);
    auto &rightBorderPolyLine = store.polyLine(rightBorder);
    std::reverse(leftBorderPolyLine.mNodes.begin(), leftBorderPolyLine.mNodes.end());
    std::reverse(rightBorderPolyLine.mNodes.begin(), rightBorderPolyLine.mNodes.end());
  }

  if (drivingDirection == DrivingDirection::Forward)
  {
    drivingDirection = DrivingDirection::Backward;
  }
  else if (drivingDirection == DrivingDirection::Backward)
  {
    drivingDirection = DrivingDirection::Forward;
  }

  std::swap(leftBorder, rightBorder);
  std::swap(leftNeighbor, rightNeighbor);
  std::swap(predecessors, successors);
}

geometry::Point2d Lane::findNearestPoint(MapDataStore const &store, geometry::Point2d const &point) const
{
  if (leftBorder == InvalidId || rightBorder == InvalidId)
  {
    return geometry::Point2d{0.0, 0.0};
  }
  auto leftPoint = findNearestPointOnLeftBorder(store, point);
  auto rightPoint = findNearestPointOnRightBorder(store, point);
  double leftDistance = leftPoint.squaredDistance(point);
  double rightDistance = rightPoint.squaredDistance(point);

  if (leftDistance < rightDistance)
  {
    return leftPoint;
  }
  else
  {
    return rightPoint;
  }
}

geometry::Point2d Lane::findNearestPointOnLeftBorder(MapDataStore const &store, geometry::Point2d const &point) const
{
  if (!store.hasPolyLine(leftBorder))
  {
    return geometry::Point2d{0.0, 0.0};
  }
  auto borderPolyLine = store.polyLine(leftBorder);
  auto geometry = polylineToGeometry(store, borderPolyLine);
  return geometry.findNearestPoint(point);
}

geometry::Point2d Lane::findNearestPointOnRightBorder(MapDataStore const &store, geometry::Point2d const &point) const
{
  if (!store.hasPolyLine(rightBorder))
  {
    return geometry::Point2d{0.0, 0.0};
  }
  auto borderPolyLine = store.polyLine(rightBorder);
  auto geometry = polylineToGeometry(store, borderPolyLine);
  return geometry.findNearestPoint(point);
}

} // namespace map_data
} // namespace maker
} // namespace map
} // namespace ad


#include <Eigen/Dense>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/ForceTorqueSensor.h>
#include <SimDynamics/SimDynamics.h>

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

#include <unordered_map>

#include "kajita/FootForceController.h"
#include "kajita/FootTorqueController.h"
#include "kajita/ZMPTrackingController.h"

#include "stabilizer/kajita/ZMPDistributor.h"
#include "stabilizer/KajitaStabilizer.h"

#include "controller/PostureController.h"

namespace Bipedal
{

/*
 * These controller needs a trajectory with the following control features:
 *
 * Poses:
 *  - chest pose
 *  - pelvis pose
 *  - left foot pose
 *  - right foot pose
 *
 * Values:
 *  - support phase
 *
 * Points:
 *  - Reference ZMP
 */
KajitaStabilizer::KajitaStabilizer(const VirtualRobot::RobotPtr& robot,
                                   const VirtualRobot::RobotNodePtr& chest,
                                   const VirtualRobot::RobotNodePtr& leftFoot,
                                   const VirtualRobot::RobotNodePtr& rightFoot,
                                   const VirtualRobot::RobotNodePtr& leftFootBody,
                                   const VirtualRobot::RobotNodePtr& rightFootBody,
                                   const VirtualRobot::RobotNodePtr& leftAnkleBody,
                                   const VirtualRobot::RobotNodePtr& rightAnkleBody,
                                   const VirtualRobot::RobotNodePtr& pelvis,
                                   const VirtualRobot::ForceTorqueSensorPtr& leftAnkleSensorX,
                                   const VirtualRobot::ForceTorqueSensorPtr& rightAnkleSensorX,
                                   const VirtualRobot::ForceTorqueSensorPtr& leftAnkleSensorY,
                                   const VirtualRobot::ForceTorqueSensorPtr& rightAnkleSensorY)
/* Nodes */
: chest(chest)
, leftFoot(leftFoot)
, rightFoot(rightFoot)
, leftFootBody(leftFootBody)
, rightFootBody(rightFootBody)
, pelvis(pelvis)
, leftAnkleSensorX(leftAnkleSensorX)
, rightAnkleSensorX(rightAnkleSensorX)
, leftAnkleSensorY(leftAnkleSensorY)
, rightAnkleSensorY(rightAnkleSensorY)
/* Controllers */
, chestPostureController(new TwoDOFPostureController(40, 5, 80, 5))
, forceDistributor(new ZMPDistributor(robot->getMass(),
                                        Eigen::Vector3f(0.0, 0.0, -9.81),
                                        leftFootBody, rightFootBody, leftFoot, rightFoot))
, footTorqueController(new FootTorqueController())
// gains calculated with utils/zmp_gains.ipynote
, zmpTrackingController(new ZMPTrackingController(Eigen::Vector3f(-0.09150757, -0.02709391, -0.33112892)))
/* Adapted frames */
, chestPose(Eigen::Matrix4f::Identity())
, pelvisPose(Eigen::Matrix4f::Identity())
, leftFootPose(Eigen::Matrix4f::Identity())
, rightFootPose(Eigen::Matrix4f::Identity())
, zmpPosition(Eigen::Vector3f::Zero())
, comPosition(Eigen::Vector3f::Zero())
, comPositionRef(Eigen::Vector3f::Zero())
, zmpPositionRef(Eigen::Vector3f::Zero())
, ft({Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()})
{
    BOOST_ASSERT(robot);
    BOOST_ASSERT(chest);
    BOOST_ASSERT(leftFoot);
    BOOST_ASSERT(rightFoot);
    BOOST_ASSERT(leftFootBody);
    BOOST_ASSERT(rightFootBody);
    BOOST_ASSERT(leftAnkleBody);
    BOOST_ASSERT(rightAnkleBody);
    BOOST_ASSERT(pelvis);
    BOOST_ASSERT(leftAnkleSensorX);
    BOOST_ASSERT(rightAnkleSensorX);
    BOOST_ASSERT(leftAnkleSensorY);
    BOOST_ASSERT(rightAnkleSensorY);

    // FIXME Make node name configurable
    Eigen::Vector3f leftHipPos  = robot->getRobotNode("LeftLeg_Joint2")->getGlobalPose().block(0, 3, 3, 1);
    Eigen::Vector3f rightHipPos = robot->getRobotNode("RightLeg_Joint2")->getGlobalPose().block(0, 3, 3, 1);
    double hipJointDistance = (leftHipPos - rightHipPos).norm();
    footForceController = FootForceControllerPtr(new FootForceController(hipJointDistance));

    leftAnkleOffset = (leftFoot->getGlobalPose().inverse() * leftAnkleBody->getGlobalPose()).block(0,3,3,1);
    // project to foot sole
    leftAnkleOffset.z() = 0;
    rightAnkleOffset = (rightFoot->getGlobalPose().inverse() * rightAnkleBody->getGlobalPose()).block(0,3,3,1);
    // project to foot sole
    rightAnkleOffset.z() = 0;
}

void KajitaStabilizer::update(float dt,
                              const Eigen::Vector3f& comActualWorld,
                              const Eigen::Vector3f& comVelocityActualWorld,
                              const Eigen::Vector3f& zmpActualWorld,
                              Bipedal::SupportPhase phase,
                              const Eigen::Matrix4f& chestPoseRefWorld,
                              const Eigen::Matrix4f& pelvisPoseRefWorld,
                              const Eigen::Matrix4f& leftFootPoseRefWorld,
                              const Eigen::Matrix4f& rightFootPoseRefWorld,
                              const Eigen::Vector3f& comRefGroundFrame,
                              const Eigen::Vector3f& comVelocityRefGroundFrame,
                              const Eigen::Vector3f& zmpRefGroundFrame)
{
    Eigen::Matrix4f leftToWorld  = leftFoot->getGlobalPose();
    Eigen::Matrix4f rightToWorld = rightFoot->getGlobalPose();
    auto groundFrame = Bipedal::computeGroundFrame(leftToWorld, rightToWorld, phase);
    auto worldToGroundFrame = groundFrame.inverse();

    // Apply step adation frame to reference values
    chestPoseRef     = chestPoseRefWorld;
    pelvisPoseRef    = pelvisPoseRefWorld;
    leftFootPoseRef  = leftFootPoseRefWorld;
    rightFootPoseRef = rightFootPoseRefWorld;
    comPositionRef   = comRefGroundFrame;
    zmpPositionRef   = zmpRefGroundFrame;

    // We never change the reference
    comPosition = comPositionRef;

    // convert actual values to ground frame
    Eigen::Vector3f zmpActualGroundFrame = VirtualRobot::MathTools::transformPosition(zmpActualWorld, worldToGroundFrame);
    Eigen::Vector3f comActualGroundFrame = VirtualRobot::MathTools::transformPosition(comActualWorld, worldToGroundFrame);
    Eigen::Vector3f comVelocityActualGroundFrame = worldToGroundFrame.block(0,0,3,3) * comVelocityActualWorld;

    // Adapt the reference ZMP to stabilize
    zmpPosition = Eigen::Vector3f::Zero();
    zmpPosition.head(2) = zmpTrackingController->computeAdaptedZMP(zmpRefGroundFrame.head(2),
                                                                   comRefGroundFrame.head(2),
                                                                   comVelocityRefGroundFrame.head(2),
                                                                   zmpActualGroundFrame.head(2),
                                                                   comActualGroundFrame.head(2),
                                                                   comVelocityActualGroundFrame.head(2));

    // Distribute adapted ZMP to yielf reference force/torques
    ft = forceDistributor->distributeZMP(
        leftAnkleOffset,
        rightAnkleOffset,
        leftToWorld,
        rightToWorld,
        zmpPosition,
        phase
    );

    Eigen::Matrix3f worldToLeft  = leftToWorld.block(0, 0, 3, 3).inverse();
    Eigen::Matrix3f worldToRight = rightToWorld.block(0, 0, 3, 3).inverse();
    Eigen::Vector3f leftTorqueLocal  = worldToLeft * (leftAnkleSensorX->getAxisTorque()  + leftAnkleSensorY->getAxisTorque());
    Eigen::Vector3f rightTorqueLocal = worldToRight * (rightAnkleSensorX->getAxisTorque() + rightAnkleSensorY->getAxisTorque());

    leftFootPose  = Eigen::Matrix4f::Identity();
    rightFootPose = Eigen::Matrix4f::Identity();
    footTorqueController->correctFootOrientation(
        leftFootPoseRef,
        rightFootPoseRef,
        ft.leftTorque,
        ft.rightTorque,
        leftTorqueLocal,
        rightTorqueLocal,
        leftFootPose,
        rightFootPose
    );

/* FIXME RobotConfig needs a flag for deciding what to use
    pelvisPose = footForceController->correctPelvisOrientation(
        pelvisPoseRef,
        ft.leftForce,
        ft.rightForce,
        -leftAnkleSensorX->getForce(),
        rightAnkleSensorX->getForce()
    );
*/
    pelvisPose = pelvisPoseRef;
    footForceController->correctFootHeight(
        ft.leftForce,
        ft.rightForce,
        -leftAnkleSensorX->getForce(),
        rightAnkleSensorX->getForce(),
        leftFootPose,
        rightFootPose
    );

    chestPose = chestPoseRef * chestPostureController->correctPosture(
        chestPoseRef,
        chest->getGlobalPose()
    );
}

std::unordered_map<std::string, DampeningController*> KajitaStabilizer::getControllers()
{
    std::unordered_map<std::string, DampeningController*> controllers;

    controllers["LeftAnkle_TorqueX"]  = &footTorqueController->leftPhiDC;
    controllers["LeftAnkle_TorqueY"]  = &footTorqueController->leftThetaDC;
    controllers["RightAnkle_TorqueX"] = &footTorqueController->rightPhiDC;
    controllers["RightAnkle_TorqueY"] = &footTorqueController->rightThetaDC;
    controllers["Chest_Roll"]         = &chestPostureController->phiDC;
    controllers["Chest_Pitch"]        = &chestPostureController->thetaDC;
    controllers["Pelvis_Pitch"]       = &footForceController->zCtrlDC;

    return controllers;
}

}


/**
 * @project identt
 * @file src/store/StoreBase.cc
 * @author  S Roychowdhury <sroycode AT gmail DOT com>
 * @version 1.0.0
 *
 * @section LICENSE
 *
 * Copyright (c) 2017 S Roychowdhury.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 * @section DESCRIPTION
 *
 *  StoreBase.cc : Store Base Implementation
 *
 */
#include <iomanip>
#include <sstream>
#include <cmath>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#include <store/StoreBase.hpp>

#define IDENTT_MACRO_HEX(A)       std::setbase(16) << std::setfill ('0') << std::setw ( A )
#define IDENTT_MACRO_STR(A)       std::setfill (' ') << std::setw ( A )

#define IDENTT_FMT_ID_KEY(A)      IDENTT_MACRO_HEX(IDENTT_KEYID_LEN) << A

#ifdef IDENTT_USE_BASE80_KEYS
#define IDENTT_FMT_NODE_ONLY(A)   IDENTT_MACRO_STR(IDENTT_NODE_LEN) << Base80String<uint64_t,IDENTT_NODE_LEN>(A)
#else
#define IDENTT_FMT_NODE_ONLY(A)   IDENTT_MACRO_HEX(IDENTT_NODE_LEN) << A
#endif

#define IDENTT_FMT_NODE_KEY(A,B)  IDENTT_FMT_ID_KEY(A) << IDENTT_FMT_NODE_ONLY(B)

// #define IDENTT_FMT_SEP "\x1E"
#define IDENTT_FMT_SEP ":"
/* UINT is defined here so can be changed independent of node if needed */
#define IDENTT_FMT_UINT_ONLY(A)    IDENTT_FMT_NODE_ONLY(A)
/* STR is defined here so can be changed to hash in future if needed */
#define IDENTT_FMT_STRN_ONLY(A)    A
/* SINT is defined here so can be changed independent of node if needed */
#define IDENTT_FMT_SINT_ONLY(A)   IDENTT_MACRO_HEX(IDENTT_NODE_LEN) << A

#define IDENTT_FMT_UINT_KEY(A,B)   IDENTT_FMT_ID_KEY(A) << IDENTT_FMT_UINT_ONLY(B)
#define IDENTT_FMT_STRN_KEY(A,B)   IDENTT_FMT_ID_KEY(A) << IDENTT_FMT_STRN_ONLY(B)

/**
* Base80String : Get limited char string
*  : someone needs to get a faster way for this to be usable
*  refer http://www.hackersdelight.org/divcMore.pdf
*
*/
template<typename T, size_t N>
inline std::string Base80String(T dividend)
{
	const static char usechars[] = "()+,-./0123456789:;<=>?@ABCDEFGHIJKLMNOPQRSTUVWXYZ[]^_abcdefghijklmnopqrstuvwxyz";
	T modulo=0;
	std::string out(N,' '); // N  (char 32 space)
	size_t c=N;
	while (dividend > 0 && c>0) {
		modulo = (dividend - 1) % 80;
		dividend = (T)((dividend - modulo) / 80);
		out[--c] = usechars[modulo];
	}
	return out;
}

/**
* Str2Uint64 : Convert the first 8 characters of a string to 64 bit
*
* @param str
*   std::string& string to change
*
* @return
*   uint64_t hash
*/
inline uint64_t Str2Uint64 (std::string str)
{
	boost::algorithm::to_upper(str);
	uint64_t t=0;
	size_t counter=8;
	for (char& i : str) {
		t+= std::pow(i,counter);
		if (--counter==0) break;
	}
	return t;
}

/**
* EncodeKeyType : make prefix key with keytype
*
*/
std::string identt::store::StoreBase::EncodeKeyType(identt::store::KeyTypeE keytype)
{
	std::stringstream ss;
	ss << IDENTT_FMT_ID_KEY( (unsigned short)keytype );
	return ss.str();
}

/**
* EncodePrimaryKey : make primary key with id only
*
*/
std::string identt::store::StoreBase::EncodePrimaryKey(identt::store::KeyTypeE keytype, ::google::protobuf::uint64 id)
{
	std::stringstream ss;
	ss << IDENTT_FMT_NODE_KEY( (unsigned short)keytype, id );
	return ss.str();
}

/**
* DecodePrimaryKey : get keytype and id from primary key
*
*/
std::pair<identt::store::KeyTypeE,uint64_t> identt::store::StoreBase::DecodePrimaryKey (std::string& key)
{

	if (key.length() < ( IDENTT_KEYID_LEN + IDENTT_NODE_LEN ))
		throw identt::BadDataException("Invalid Key probed");
	identt::store::KeyTypeE x = (identt::store::KeyTypeE) std::stoull(key.substr(0,IDENTT_KEYID_LEN),0,16);
	uint64_t y = std::stoull(key.substr(IDENTT_KEYID_LEN,IDENTT_NODE_LEN),0,16);
	return std::make_pair(x,y);
}

namespace identt {
namespace store {

/** 1 value */

/**
* EncodeSecondaryKey : make secondary key with uint64_t
*  : always unique
*
*/
template<> std::string StoreBase::EncodeSecondaryKey (KeyTypeE keytype, uint64_t key)
{
	std::stringstream ss;
	ss << IDENTT_FMT_UINT_KEY( (unsigned short)keytype, key );
	return ss.str();
}

/**
* EncodeSecondaryKey : make secondary key with std::string
*  : always unique
*
*/
template<> std::string StoreBase::EncodeSecondaryKey (KeyTypeE keytype, std::string key)
{
	std::stringstream ss;
	ss << IDENTT_FMT_STRN_KEY( (unsigned short)keytype, key );
	return ss.str();
}

/** 2 value */

/**
* EncodeSecondaryKey : make secondary key with two keys uint64_t , uint64_t
*  : u-u unique , u non unique
*
*/
template<> std::string StoreBase::EncodeSecondaryKey (KeyTypeE keytype, uint64_t key1, uint64_t key2)
{
	std::stringstream ss;
	ss << IDENTT_FMT_UINT_KEY( (unsigned short)keytype, key1)
	   << IDENTT_FMT_SEP << IDENTT_FMT_UINT_ONLY(key2);
	return ss.str();
}

/**
* EncodeSecondaryKey : make secondary key with two keys uint64_t , std::string
*  : always unique
*
*/
template<> std::string StoreBase::EncodeSecondaryKey (KeyTypeE keytype, uint64_t key1, std::string key2)
{
	std::stringstream ss;
	ss << IDENTT_FMT_UINT_KEY( (unsigned short)keytype, key1)
	   << IDENTT_FMT_SEP << IDENTT_FMT_STRN_ONLY(key2);
	return ss.str();
}

/**
* EncodeSecondaryKey : make secondary key with two keys std::string , uint64_t
*  : always unique
*  : s-u unique , s non unique
*
*/
template<> std::string StoreBase::EncodeSecondaryKey (KeyTypeE keytype, std::string key1, uint64_t key2)
{
	std::stringstream ss;
	ss << IDENTT_FMT_STRN_KEY( (unsigned short)keytype, key1)
	   << IDENTT_FMT_SEP << IDENTT_FMT_UINT_ONLY(key2);
	return ss.str();
}

/**
* EncodeSecondaryKey : make secondary key with two keys std::string , std::string
*  : always unique
*
*/
template<> std::string StoreBase::EncodeSecondaryKey (KeyTypeE keytype, std::string key1, std::string key2)
{
	std::stringstream ss;
	ss << IDENTT_FMT_STRN_KEY( (unsigned short)keytype, key1)
	   << IDENTT_FMT_SEP << IDENTT_FMT_STRN_ONLY(key2);
	return ss.str();
}

/**
* EncodeSecondaryKey : make secondary key with two keys uint64_t, int32_t
*  : always unique
*
*/
template<> std::string StoreBase::EncodeSecondaryKey (KeyTypeE keytype, uint64_t key1, int32_t key2)
{
	std::stringstream ss;
	ss << IDENTT_FMT_UINT_KEY( (unsigned short)keytype, key1)
	   << IDENTT_FMT_SEP << IDENTT_FMT_SINT_ONLY(key2);
	return ss.str();
}

/** 3 value */

/**
* EncodeSecondaryKey : make secondary key with three keys uint64_t , uint64_t , uint64_t
*  : u-u-u unique , u-u non unique
*
*/
template<> std::string StoreBase::EncodeSecondaryKey (KeyTypeE keytype, uint64_t key1, uint64_t key2, uint64_t key3)
{
	std::stringstream ss;
	ss << IDENTT_FMT_UINT_KEY( (unsigned short)keytype, key1)
	   << IDENTT_FMT_SEP << IDENTT_FMT_UINT_ONLY(key2)
	   << IDENTT_FMT_SEP << IDENTT_FMT_UINT_ONLY(key3);
	return ss.str();
}

/**
* EncodeSecondaryKey : make secondary key with three keys uint64_t , uint64_t , std::string
*  : always unique
*
*/
template<> std::string StoreBase::EncodeSecondaryKey (KeyTypeE keytype, uint64_t key1, uint64_t key2, std::string key3)
{
	std::stringstream ss;
	ss << IDENTT_FMT_UINT_KEY( (unsigned short)keytype, key1)
	   << IDENTT_FMT_SEP << IDENTT_FMT_UINT_ONLY(key2)
	   << IDENTT_FMT_SEP << IDENTT_FMT_STRN_ONLY(key3);
	return ss.str();
}

/**
* EncodeSecondaryKey : make secondary key with three keys uint64_t , std::string , uint64_t
*  : always unique
*  : u-s-u unique , u-s non unique
*
*/
template<> std::string StoreBase::EncodeSecondaryKey (KeyTypeE keytype, uint64_t key1, std::string key2, uint64_t key3)
{
	std::stringstream ss;
	ss << IDENTT_FMT_UINT_KEY( (unsigned short)keytype, key1)
	   << IDENTT_FMT_SEP << IDENTT_FMT_STRN_ONLY(key2)
	   << IDENTT_FMT_SEP << IDENTT_FMT_UINT_ONLY(key3);
	return ss.str();
}

/**
* EncodeSecondaryKey : make secondary key with three keys uint64_t , std::string , std::string
*  : always unique
*
*/
template<> std::string StoreBase::EncodeSecondaryKey (KeyTypeE keytype, uint64_t key1, std::string key2, std::string key3)
{
	std::stringstream ss;
	ss << IDENTT_FMT_UINT_KEY( (unsigned short)keytype, key1)
	   << IDENTT_FMT_SEP << IDENTT_FMT_STRN_ONLY(key2)
	   << IDENTT_FMT_SEP << IDENTT_FMT_STRN_ONLY(key3);
	return ss.str();
}

/**
* EncodeSecondaryKey : make secondary key with three keys std::string , uint64_t , uint64_t
*  : s-u-u unique , s-u non unique
*
*/
template<> std::string StoreBase::EncodeSecondaryKey (KeyTypeE keytype, std::string key1, uint64_t key2, uint64_t key3)
{
	std::stringstream ss;
	ss << IDENTT_FMT_STRN_KEY( (unsigned short)keytype, key1)
	   << IDENTT_FMT_SEP << IDENTT_FMT_UINT_ONLY(key2)
	   << IDENTT_FMT_SEP << IDENTT_FMT_UINT_ONLY(key3);
	return ss.str();
}

/**
* EncodeSecondaryKey : make secondary key with three keys std::string , uint64_t , std::string
*  : always unique
*
*/
template<> std::string StoreBase::EncodeSecondaryKey (KeyTypeE keytype, std::string key1, uint64_t key2, std::string key3)
{
	std::stringstream ss;
	ss << IDENTT_FMT_STRN_KEY( (unsigned short)keytype, key1)
	   << IDENTT_FMT_SEP << IDENTT_FMT_UINT_ONLY(key2)
	   << IDENTT_FMT_SEP << IDENTT_FMT_STRN_ONLY(key3);
	return ss.str();
}

/**
* EncodeSecondaryKey : make secondary key with three keys std::string , std::string , uint64_t
*  : s-s-u unique , s-s non unique
*
*/
template<> std::string StoreBase::EncodeSecondaryKey (KeyTypeE keytype, std::string key1, std::string key2, uint64_t key3)
{
	std::stringstream ss;
	ss << IDENTT_FMT_STRN_KEY( (unsigned short)keytype, key1)
	   << IDENTT_FMT_SEP << IDENTT_FMT_STRN_ONLY(key2)
	   << IDENTT_FMT_SEP << IDENTT_FMT_UINT_ONLY(key3);
	return ss.str();
}

/**
* EncodeSecondaryKey : make secondary key with three keys std::string , std::string , std::string
*  : always unique
*
*/
template<> std::string StoreBase::EncodeSecondaryKey (KeyTypeE keytype, std::string key1, std::string key2, std::string key3)
{
	std::stringstream ss;
	ss << IDENTT_FMT_STRN_KEY( (unsigned short)keytype, key1)
	   << IDENTT_FMT_SEP << IDENTT_FMT_STRN_ONLY(key2)
	   << IDENTT_FMT_SEP << IDENTT_FMT_STRN_ONLY(key3);
	return ss.str();
}

} // namespace store
} // namespace identt


/**
 * Copyright Soramitsu Co., Ltd. All Rights Reserved.
 * SPDX-License-Identifier: Apache-2.0
 */

#ifndef KAGOME_RUNTIME_BINARYEN_WASM_MEMORY_IMPL_HPP
#define KAGOME_RUNTIME_BINARYEN_WASM_MEMORY_IMPL_HPP

#include <binaryen/shell-interface.h>

#include <array>
#include <cstring>  // for std::memset in gcc
#include <memory>
#include <unordered_map>

#include <boost/optional.hpp>

#include "common/literals.hpp"
#include "log/logger.hpp"
#include "primitives/math.hpp"
#include "runtime/wasm_memory.hpp"

namespace kagome::runtime::binaryen {

  using namespace kagome::common::literals;

  // Alignment for pointers, same with substrate:
  // https://github.com/paritytech/substrate/blob/743981a083f244a090b40ccfb5ce902199b55334/primitives/allocator/src/freeing_bump.rs#L56
  inline const uint8_t kAlignment = sizeof(size_t);
  inline const size_t kInitialMemorySize = 2_MB;  // 2Mb
  inline const size_t kDefaultHeapBase = 1_MB;    // 1Mb

  /**
   * Obtain closest multiple of kAllignment that is greater or equal to given
   * number
   * @tparam T T type of number
   * @param t given number
   * @return closest multiple
   */
  template <typename T>
  inline constexpr T roundUpAlign(T t) {
    return math::roundUp<kAlignment>(t);
  }

  static_assert(roundUpAlign(kDefaultHeapBase) == kDefaultHeapBase,
                "Heap base must be aligned");
  static_assert(kDefaultHeapBase < kInitialMemorySize,
                "Heap base must be in border of memory");

  /**
   * Memory implementation for wasm environment
   * Most code is taken from Binaryen's implementation here:
   * https://github.com/WebAssembly/binaryen/blob/master/src/shell-interface.h#L37
   * @note Memory size of this implementation is at least of the size of one
   * wasm page (4096 bytes)
   */
  class WasmMemoryImpl final : public WasmMemory {
   public:
    explicit WasmMemoryImpl(wasm::ShellExternalInterface::Memory *memory);
    WasmMemoryImpl(const WasmMemoryImpl &copy) = delete;
    WasmMemoryImpl &operator=(const WasmMemoryImpl &copy) = delete;
    WasmMemoryImpl(WasmMemoryImpl &&move) = delete;
    WasmMemoryImpl &operator=(WasmMemoryImpl &&move) = delete;
    ~WasmMemoryImpl() override = default;

    void setHeapBase(WasmSize initial_offset) override;

    void reset() override;

    WasmSize size() const override;
    void resize(WasmSize newSize) override;

    WasmPointer allocate(WasmSize size) override;
    boost::optional<WasmSize> deallocate(WasmPointer ptr) override;

    int8_t load8s(WasmPointer addr) const override;
    uint8_t load8u(WasmPointer addr) const override;
    int16_t load16s(WasmPointer addr) const override;
    uint16_t load16u(WasmPointer addr) const override;
    int32_t load32s(WasmPointer addr) const override;
    uint32_t load32u(WasmPointer addr) const override;
    int64_t load64s(WasmPointer addr) const override;
    uint64_t load64u(WasmPointer addr) const override;
    std::array<uint8_t, 16> load128(WasmPointer addr) const override;
    common::Buffer loadN(kagome::runtime::WasmPointer addr,
                         kagome::runtime::WasmSize n) const override;
    std::string loadStr(kagome::runtime::WasmPointer addr,
                        kagome::runtime::WasmSize length) const override;

    void store8(WasmPointer addr, int8_t value) override;
    void store16(WasmPointer addr, int16_t value) override;
    void store32(WasmPointer addr, int32_t value) override;
    void store64(WasmPointer addr, int64_t value) override;
    void store128(WasmPointer addr,
                  const std::array<uint8_t, 16> &value) override;
    void storeBuffer(kagome::runtime::WasmPointer addr,
                     gsl::span<const uint8_t> value) override;

    WasmSpan storeBuffer(gsl::span<const uint8_t> value) override;

    /// following methods are needed mostly for testing purposes
    boost::optional<WasmSize> getDeallocatedChunkSize(WasmPointer ptr) const;
    boost::optional<WasmSize> getAllocatedChunkSize(WasmPointer ptr) const;
    size_t getAllocatedChunksNum() const;
    size_t getDeallocatedChunksNum() const;

   private:
    wasm::ShellExternalInterface::Memory *memory_;
    WasmSize size_;

    // Heap base. Offset is reset to it on reset()
    WasmPointer heap_base_;

    // Offset on the tail of the last allocated MemoryImpl chunk
    WasmPointer offset_;

    log::Logger logger_;

    // map containing addresses of allocated MemoryImpl chunks
    std::unordered_map<WasmPointer, WasmSize> allocated_;

    // map containing addresses to the deallocated MemoryImpl chunks
    std::map<WasmPointer, WasmSize> deallocated_;

    template <typename T>
    static bool aligned(const char *address) {
      static_assert(!(sizeof(T) & (sizeof(T) - 1)), "must be a power of 2");
      // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
      return 0 == (reinterpret_cast<uintptr_t>(address) & (sizeof(T) - 1));
    }

    /**
     * Finds memory segment of given size among deallocated pieces of memory
     * and allocates a memory there
     * @param size of target memory
     * @return address of memory of given size, or -1 if it is impossible to
     * allocate this amount of memory
     */
    WasmPointer freealloc(WasmSize size);

    /**
     * Resize memory and allocate memory segment of given size
     * @param size memory size to be allocated
     * @return pointer to the allocated memory @or 0 if it is impossible to
     * allocate this amount of memory
     */
    WasmPointer growAlloc(WasmSize size);
  };
}  // namespace kagome::runtime::binaryen

#endif  // KAGOME_RUNTIME_BINARYEN_WASM_MEMORY_IMPL_HPP


// Boost.Geometry Index
//
// R-tree inserting visitor implementation
//
// Copyright (c) 2011-2015 Adam Wulkiewicz, Lodz, Poland.
//
// This file was modified by Oracle on 2019-2020.
// Modifications copyright (c) 2019-2020 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_INSERT_HPP
#define BOOST_GEOMETRY_INDEX_DETAIL_RTREE_VISITORS_INSERT_HPP

#ifdef BOOST_GEOMETRY_INDEX_EXPERIMENTAL_ENLARGE_BY_EPSILON
#include <type_traits>
#endif

#include <boost/geometry/algorithms/detail/expand_by_epsilon.hpp>
#include <boost/geometry/core/static_assert.hpp>
#include <boost/geometry/util/condition.hpp>

#include <boost/geometry/index/detail/algorithms/bounds.hpp>
#include <boost/geometry/index/detail/algorithms/content.hpp>

#include <boost/geometry/index/detail/rtree/node/subtree_destroyer.hpp>

namespace boost { namespace geometry { namespace index {

namespace detail { namespace rtree {

// Default choose_next_node
template
<
    typename MembersHolder,
    typename ChooseNextNodeTag = typename MembersHolder::options_type::choose_next_node_tag
>
class choose_next_node;

template <typename MembersHolder>
class choose_next_node<MembersHolder, choose_by_content_diff_tag>
{
public:
    typedef typename MembersHolder::box_type box_type;
    typedef typename MembersHolder::parameters_type parameters_type;

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

    typedef typename rtree::elements_type<internal_node>::type children_type;

    typedef typename index::detail::default_content_result<box_type>::type content_type;

    template <typename Indexable>
    static inline size_t apply(internal_node & n,
                               Indexable const& indexable,
                               parameters_type const& parameters,
                               size_t /*node_relative_level*/)
    {
        children_type & children = rtree::elements(n);

        BOOST_GEOMETRY_INDEX_ASSERT(!children.empty(), "can't choose the next node if children are empty");

        size_t children_count = children.size();

        // choose index with smallest content change or smallest content
        size_t choosen_index = 0;
        content_type smallest_content_diff = (std::numeric_limits<content_type>::max)();
        content_type smallest_content = (std::numeric_limits<content_type>::max)();

        // caculate areas and areas of all nodes' boxes
        for ( size_t i = 0 ; i < children_count ; ++i )
        {
            typedef typename children_type::value_type child_type;
            child_type const& ch_i = children[i];

            // expanded child node's box
            box_type box_exp(ch_i.first);
            index::detail::expand(box_exp, indexable,
                                  index::detail::get_strategy(parameters));

            // areas difference
            content_type content = index::detail::content(box_exp);
            content_type content_diff = content - index::detail::content(ch_i.first);

            // update the result
            if ( content_diff < smallest_content_diff ||
                ( content_diff == smallest_content_diff && content < smallest_content ) )
            {
                smallest_content_diff = content_diff;
                smallest_content = content;
                choosen_index = i;
            }
        }

        return choosen_index;
    }
};

// ----------------------------------------------------------------------- //

// Not implemented here
template
<
    typename MembersHolder,
    typename RedistributeTag = typename MembersHolder::options_type::redistribute_tag
>
struct redistribute_elements
{
    BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
        "Not implemented for this RedistributeTag type.",
        MembersHolder, RedistributeTag);
};

// ----------------------------------------------------------------------- //

// Split algorithm
template
<
    typename MembersHolder,
    typename SplitTag = typename MembersHolder::options_type::split_tag
>
class split
{
    BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
        "Not implemented for this SplitTag type.",
        MembersHolder, SplitTag);
};

// Default split algorithm
template <typename MembersHolder>
class split<MembersHolder, split_default_tag>
{
protected:
    typedef typename MembersHolder::parameters_type parameters_type;
    typedef typename MembersHolder::box_type box_type;
    typedef typename MembersHolder::translator_type translator_type;
    typedef typename MembersHolder::allocators_type allocators_type;
    typedef typename MembersHolder::size_type size_type;

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

    typedef typename MembersHolder::node_pointer node_pointer;

public:
    typedef index::detail::varray<
        typename rtree::elements_type<internal_node>::type::value_type,
        1
    > nodes_container_type;

    template <typename Node>
    static inline void apply(nodes_container_type & additional_nodes,
                             Node & n,
                             box_type & n_box,
                             parameters_type const& parameters,
                             translator_type const& translator,
                             allocators_type & allocators)
    {
        // TODO - consider creating nodes always with sufficient memory allocated

        // create additional node, use auto destroyer for automatic destruction on exception
        node_pointer n2_ptr = rtree::create_node<allocators_type, Node>::apply(allocators);                  // MAY THROW, STRONG (N: alloc)
        // create reference to the newly created node
        Node & n2 = rtree::get<Node>(*n2_ptr);

        BOOST_TRY
        {
            // NOTE: thread-safety
            // After throwing an exception by redistribute_elements the original node may be not changed or
            // both nodes may be empty. In both cases the tree won't be valid r-tree.
            // The alternative is to create 2 (or more) additional nodes here and store backup info
            // in the original node, then, if exception was thrown, the node would always have more than max
            // elements.
            // The alternative is to use moving semantics in the implementations of redistribute_elements,
            // it will be possible to throw from boost::move() in the case of e.g. static size nodes.

            // redistribute elements
            box_type box2;
            redistribute_elements<MembersHolder>
                ::apply(n, n2, n_box, box2, parameters, translator, allocators);                                   // MAY THROW (V, E: alloc, copy, copy)

            // check numbers of elements
            BOOST_GEOMETRY_INDEX_ASSERT(parameters.get_min_elements() <= rtree::elements(n).size() &&
                rtree::elements(n).size() <= parameters.get_max_elements(),
                "unexpected number of elements");
            BOOST_GEOMETRY_INDEX_ASSERT(parameters.get_min_elements() <= rtree::elements(n2).size() &&
                rtree::elements(n2).size() <= parameters.get_max_elements(),
                "unexpected number of elements");

            // return the list of newly created nodes (this algorithm returns one)
            additional_nodes.push_back(rtree::make_ptr_pair(box2, n2_ptr));                                  // MAY THROW, STRONG (alloc, copy)
        }
        BOOST_CATCH(...)
        {
            // NOTE: This code is here to prevent leaving the rtree in a state
            //  after an exception is thrown in which pushing new element could
            //  result in assert or putting it outside the memory of node elements.
            typename rtree::elements_type<Node>::type & elements = rtree::elements(n);
            size_type const max_size = parameters.get_max_elements();
            if (elements.size() > max_size)
            {
                rtree::destroy_element<MembersHolder>::apply(elements[max_size], allocators);
                elements.pop_back();
            }

            rtree::visitors::destroy<MembersHolder>::apply(n2_ptr, allocators);

            BOOST_RETHROW
        }
        BOOST_CATCH_END
    }
};

// ----------------------------------------------------------------------- //

namespace visitors { namespace detail {

template <typename InternalNode, typename InternalNodePtr, typename SizeType>
struct insert_traverse_data
{
    typedef typename rtree::elements_type<InternalNode>::type elements_type;
    typedef typename elements_type::value_type element_type;
    typedef typename elements_type::size_type elements_size_type;
    typedef SizeType size_type;

    insert_traverse_data()
        : parent(0), current_child_index(0), current_level(0)
    {}

    void move_to_next_level(InternalNodePtr new_parent,
                            elements_size_type new_child_index)
    {
        parent = new_parent;
        current_child_index = new_child_index;
        ++current_level;
    }

    bool current_is_root() const
    {
        return 0 == parent;
    }

    elements_type & parent_elements() const
    {
        BOOST_GEOMETRY_INDEX_ASSERT(parent, "null pointer");
        return rtree::elements(*parent);
    }

    element_type & current_element() const
    {
        BOOST_GEOMETRY_INDEX_ASSERT(parent, "null pointer");
        return rtree::elements(*parent)[current_child_index];
    }

    InternalNodePtr parent;
    elements_size_type current_child_index;
    size_type current_level;
};

// Default insert visitor
template <typename Element, typename MembersHolder>
class insert
    : public MembersHolder::visitor
{
protected:
    typedef typename MembersHolder::box_type box_type;
    typedef typename MembersHolder::value_type value_type;
    typedef typename MembersHolder::parameters_type parameters_type;
    typedef typename MembersHolder::translator_type translator_type;
    typedef typename MembersHolder::allocators_type allocators_type;

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

    typedef rtree::subtree_destroyer<MembersHolder> subtree_destroyer;
    typedef typename allocators_type::node_pointer node_pointer;
    typedef typename allocators_type::size_type size_type;

    //typedef typename allocators_type::internal_node_pointer internal_node_pointer;
    typedef internal_node * internal_node_pointer;

    inline insert(node_pointer & root,
                  size_type & leafs_level,
                  Element const& element,
                  parameters_type const& parameters,
                  translator_type const& translator,
                  allocators_type & allocators,
                  size_type relative_level = 0
    )
        : m_element(element)
        , m_parameters(parameters)
        , m_translator(translator)
        , m_relative_level(relative_level)
        , m_level(leafs_level - relative_level)
        , m_root_node(root)
        , m_leafs_level(leafs_level)
        , m_traverse_data()
        , m_allocators(allocators)
    {
        BOOST_GEOMETRY_INDEX_ASSERT(m_relative_level <= leafs_level, "unexpected level value");
        BOOST_GEOMETRY_INDEX_ASSERT(m_level <= m_leafs_level, "unexpected level value");
        BOOST_GEOMETRY_INDEX_ASSERT(0 != m_root_node, "there is no root node");
        // TODO
        // assert - check if Box is correct

        // When a value is inserted, during the tree traversal bounds of nodes
        // on a path from the root to a leaf must be expanded. So prepare
        // a bounding object at the beginning to not do it later for each node.
        // NOTE: This is actually only needed because conditionally the bounding
        //       object may be expanded below. Otherwise the indexable could be
        //       directly used instead
        index::detail::bounds(rtree::element_indexable(m_element, m_translator),
                              m_element_bounds,
                              index::detail::get_strategy(m_parameters));

#ifdef BOOST_GEOMETRY_INDEX_EXPERIMENTAL_ENLARGE_BY_EPSILON
        // Enlarge it in case if it's not bounding geometry type.
        // It's because Points and Segments are compared WRT machine epsilon
        // This ensures that leafs bounds correspond to the stored elements
        if (BOOST_GEOMETRY_CONDITION((
                std::is_same<Element, value_type>::value
             && ! index::detail::is_bounding_geometry
                    <
                        typename indexable_type<translator_type>::type
                    >::value )) )
        {
            geometry::detail::expand_by_epsilon(m_element_bounds);
        }
#endif
    }

    template <typename Visitor>
    inline void traverse(Visitor & visitor, internal_node & n)
    {
        // choose next node
        size_t choosen_node_index = rtree::choose_next_node<MembersHolder>
            ::apply(n, rtree::element_indexable(m_element, m_translator),
                    m_parameters,
                    m_leafs_level - m_traverse_data.current_level);

        // expand the node to contain value
        index::detail::expand(
            rtree::elements(n)[choosen_node_index].first,
            m_element_bounds,
            index::detail::get_strategy(m_parameters));

        // next traversing step
        traverse_apply_visitor(visitor, n, choosen_node_index);                                                 // MAY THROW (V, E: alloc, copy, N:alloc)
    }

    // TODO: awulkiew - change post_traverse name to handle_overflow or overflow_treatment?

    template <typename Node>
    inline void post_traverse(Node &n)
    {
        BOOST_GEOMETRY_INDEX_ASSERT(m_traverse_data.current_is_root() ||
                                    &n == &rtree::get<Node>(*m_traverse_data.current_element().second),
                                    "if node isn't the root current_child_index should be valid");

        // handle overflow
        if ( m_parameters.get_max_elements() < rtree::elements(n).size() )
        {
            // NOTE: If the exception is thrown current node may contain more than MAX elements or be empty.
            // Furthermore it may be empty root - internal node.
            split(n);                                                                                // MAY THROW (V, E: alloc, copy, N:alloc)
        }
    }

    template <typename Visitor>
    inline void traverse_apply_visitor(Visitor & visitor, internal_node &n, size_t choosen_node_index)
    {
        // save previous traverse inputs and set new ones
        insert_traverse_data<internal_node, internal_node_pointer, size_type>
            backup_traverse_data = m_traverse_data;

        // calculate new traverse inputs
        m_traverse_data.move_to_next_level(&n, choosen_node_index);

        // next traversing step
        rtree::apply_visitor(visitor, *rtree::elements(n)[choosen_node_index].second);                          // MAY THROW (V, E: alloc, copy, N:alloc)

        // restore previous traverse inputs
        m_traverse_data = backup_traverse_data;
    }

    // TODO: consider - split result returned as OutIter is faster than reference to the container. Why?

    template <typename Node>
    inline void split(Node & n) const
    {
        typedef rtree::split<MembersHolder> split_algo;

        typename split_algo::nodes_container_type additional_nodes;
        box_type n_box;

        split_algo::apply(additional_nodes, n, n_box, m_parameters, m_translator, m_allocators);                // MAY THROW (V, E: alloc, copy, N:alloc)

        BOOST_GEOMETRY_INDEX_ASSERT(additional_nodes.size() == 1, "unexpected number of additional nodes");

        // TODO add all additional nodes
        // For kmeans algorithm:
        // elements number may be greater than node max elements count
        // split and reinsert must take node with some elements count
        // and container of additional elements (std::pair<Box, node*>s or Values)
        // and translator + allocators
        // where node_elements_count + additional_elements > node_max_elements_count
        // What with elements other than std::pair<Box, node*> ?
        // Implement template <node_tag> struct node_element_type or something like that

        // for exception safety
        subtree_destroyer additional_node_ptr(additional_nodes[0].second, m_allocators);

#ifdef BOOST_GEOMETRY_INDEX_EXPERIMENTAL_ENLARGE_BY_EPSILON
        // Enlarge bounds of a leaf node.
        // It's because Points and Segments are compared WRT machine epsilon
        // This ensures that leafs' bounds correspond to the stored elements.
        if (BOOST_GEOMETRY_CONDITION((
                std::is_same<Node, leaf>::value
             && ! index::detail::is_bounding_geometry
                    <
                        typename indexable_type<translator_type>::type
                    >::value )))
        {
            geometry::detail::expand_by_epsilon(n_box);
            geometry::detail::expand_by_epsilon(additional_nodes[0].first);
        }
#endif

        // node is not the root - just add the new node
        if ( !m_traverse_data.current_is_root() )
        {
            // update old node's box
            m_traverse_data.current_element().first = n_box;
            // add new node to parent's children
            m_traverse_data.parent_elements().push_back(additional_nodes[0]);                                     // MAY THROW, STRONG (V, E: alloc, copy)
        }
        // node is the root - add level
        else
        {
            BOOST_GEOMETRY_INDEX_ASSERT(&n == &rtree::get<Node>(*m_root_node), "node should be the root");

            // create new root and add nodes
            subtree_destroyer new_root(rtree::create_node<allocators_type, internal_node>::apply(m_allocators), m_allocators); // MAY THROW, STRONG (N:alloc)

            BOOST_TRY
            {
                rtree::elements(rtree::get<internal_node>(*new_root)).push_back(rtree::make_ptr_pair(n_box, m_root_node));  // MAY THROW, STRONG (E:alloc, copy)
                rtree::elements(rtree::get<internal_node>(*new_root)).push_back(additional_nodes[0]);                 // MAY THROW, STRONG (E:alloc, copy)
            }
            BOOST_CATCH(...)
            {
                // clear new root to not delete in the ~subtree_destroyer() potentially stored old root node
                rtree::elements(rtree::get<internal_node>(*new_root)).clear();
                BOOST_RETHROW                                                                                // RETHROW
            }
            BOOST_CATCH_END

            m_root_node = new_root.get();
            ++m_leafs_level;

            new_root.release();
        }

        additional_node_ptr.release();
    }

    // TODO: awulkiew - implement dispatchable split::apply to enable additional nodes creation

    Element const& m_element;
    box_type m_element_bounds;
    parameters_type const& m_parameters;
    translator_type const& m_translator;
    size_type const m_relative_level;
    size_type const m_level;

    node_pointer & m_root_node;
    size_type & m_leafs_level;

    // traversing input parameters
    insert_traverse_data<internal_node, internal_node_pointer, size_type> m_traverse_data;

    allocators_type & m_allocators;
};

} // namespace detail

// Insert visitor forward declaration
template
<
    typename Element,
    typename MembersHolder,
    typename InsertTag = typename MembersHolder::options_type::insert_tag
>
class insert;

// Default insert visitor used for nodes elements
// After passing the Element to insert visitor the Element is managed by the tree
// I.e. one should not delete the node passed to the insert visitor after exception is thrown
// because this visitor may delete it
template <typename Element, typename MembersHolder>
class insert<Element, MembersHolder, insert_default_tag>
    : public detail::insert<Element, MembersHolder>
{
public:
    typedef detail::insert<Element, MembersHolder> base;

    typedef typename base::parameters_type parameters_type;
    typedef typename base::translator_type translator_type;
    typedef typename base::allocators_type allocators_type;

    typedef typename base::node node;
    typedef typename base::internal_node internal_node;
    typedef typename base::leaf leaf;

    typedef typename base::node_pointer node_pointer;
    typedef typename base::size_type size_type;

    inline insert(node_pointer & root,
                  size_type & leafs_level,
                  Element const& element,
                  parameters_type const& parameters,
                  translator_type const& translator,
                  allocators_type & allocators,
                  size_type relative_level = 0
    )
        : base(root, leafs_level, element, parameters, translator, allocators, relative_level)
    {}

    inline void operator()(internal_node & n)
    {
        BOOST_GEOMETRY_INDEX_ASSERT(base::m_traverse_data.current_level < base::m_leafs_level, "unexpected level");

        if ( base::m_traverse_data.current_level < base::m_level )
        {
            // next traversing step
            base::traverse(*this, n);                                                                           // MAY THROW (E: alloc, copy, N: alloc)
        }
        else
        {
            BOOST_GEOMETRY_INDEX_ASSERT(base::m_level == base::m_traverse_data.current_level, "unexpected level");

            BOOST_TRY
            {
                // push new child node
                rtree::elements(n).push_back(base::m_element);                                                  // MAY THROW, STRONG (E: alloc, copy)
            }
            BOOST_CATCH(...)
            {
                // if the insert fails above, the element won't be stored in the tree

                rtree::visitors::destroy<MembersHolder>::apply(base::m_element.second, base::m_allocators);

                BOOST_RETHROW                                                                                // RETHROW
            }
            BOOST_CATCH_END
        }

        base::post_traverse(n);                                                                                // MAY THROW (E: alloc, copy, N: alloc)
    }

    inline void operator()(leaf &)
    {
        BOOST_GEOMETRY_INDEX_ASSERT(false, "this visitor can't be used for a leaf");
    }
};

// Default insert visitor specialized for Values elements
template <typename MembersHolder>
class insert<typename MembersHolder::value_type, MembersHolder, insert_default_tag>
    : public detail::insert<typename MembersHolder::value_type, MembersHolder>
{
public:
    typedef detail::insert<typename MembersHolder::value_type, MembersHolder> base;

    typedef typename base::value_type value_type;
    typedef typename base::parameters_type parameters_type;
    typedef typename base::translator_type translator_type;
    typedef typename base::allocators_type allocators_type;

    typedef typename base::node node;
    typedef typename base::internal_node internal_node;
    typedef typename base::leaf leaf;

    typedef typename base::node_pointer node_pointer;
    typedef typename base::size_type size_type;

    inline insert(node_pointer & root,
                  size_type & leafs_level,
                  value_type const& value,
                  parameters_type const& parameters,
                  translator_type const& translator,
                  allocators_type & allocators,
                  size_type relative_level = 0
    )
        : base(root, leafs_level, value, parameters, translator, allocators, relative_level)
    {}

    inline void operator()(internal_node & n)
    {
        BOOST_GEOMETRY_INDEX_ASSERT(base::m_traverse_data.current_level < base::m_leafs_level, "unexpected level");
        BOOST_GEOMETRY_INDEX_ASSERT(base::m_traverse_data.current_level < base::m_level, "unexpected level");

        // next traversing step
        base::traverse(*this, n);                                                                                // MAY THROW (V, E: alloc, copy, N: alloc)

        base::post_traverse(n);                                                                                // MAY THROW (E: alloc, copy, N: alloc)
    }

    inline void operator()(leaf & n)
    {
        BOOST_GEOMETRY_INDEX_ASSERT(base::m_traverse_data.current_level == base::m_leafs_level, "unexpected level");
        BOOST_GEOMETRY_INDEX_ASSERT(base::m_level == base::m_traverse_data.current_level ||
                                    base::m_level == (std::numeric_limits<size_t>::max)(), "unexpected level");

        rtree::elements(n).push_back(base::m_element);                                                              // MAY THROW, STRONG (V: alloc, copy)

        base::post_traverse(n);                                                                                // MAY THROW (V: alloc, copy, N: alloc)
    }
};

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

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

#endif // BOOST_GEOMETRY_INDEX_DETAIL_RTREE_VISITORS_INSERT_HPP


/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * 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 "modules/perception/camera/common/undistortion_handler.h"

#include <npp.h>

#include <Eigen/Dense>
#include <vector>

#include "cyber/common/log.h"
#include "modules/perception/common/sensor_manager/sensor_manager.h"

namespace apollo {
namespace perception {
namespace camera {

bool UndistortionHandler::set_device(int device) {
  device_ = device;
  auto code = cudaSetDevice(device_);
  if (code != cudaSuccess) {
    AERROR << "cudaSetDevice failed: " << cudaGetErrorString(code);
    return false;
  }
  return true;
}
/* Initialization of the GPU routines for camera data preprocessing
 *
 * Return: 0 - success; other - failure
 * Params: device - dev number of the GPU device
 * Note: returns OK if already been inited.
 */
bool UndistortionHandler::Init(const std::string &sensor_name, int device) {
  if (inited_) {
    return true;
  }

  std::vector<double> D;
  std::vector<double> K;

  common::SensorManager *sensor_manager = common::SensorManager::Instance();
  if (!sensor_manager->IsSensorExist(sensor_name)) {
    AERROR << "Sensor '" << sensor_name << "' not exists!";
    return false;
  }

  if (!set_device(device)) {
    return false;
  }

  base::BrownCameraDistortionModelPtr distort_model =
      std::dynamic_pointer_cast<base::BrownCameraDistortionModel>(
          sensor_manager->GetDistortCameraModel(sensor_name));

  height_ = static_cast<int>(distort_model->get_height());
  width_ = static_cast<int>(distort_model->get_width());
  d_mapx_.Reshape({height_, width_});
  d_mapy_.Reshape({height_, width_});

  Eigen::Matrix3f I;
  I << 1.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 1.f;

  InitUndistortRectifyMap(distort_model->get_intrinsic_params(),
                          distort_model->get_distort_params(), I,
                          distort_model->get_intrinsic_params(), width_,
                          height_, &d_mapx_, &d_mapy_);

  inited_ = true;
  return true;
}

bool UndistortionHandler::Handle(const base::Image8U &src_img,
                                 base::Image8U *dst_img) {
  if (!inited_) {
    return false;
  }

  if (!set_device(device_)) {
    return false;
  }

  NppiInterpolationMode remap_mode = NPPI_INTER_LINEAR;
  NppiSize image_size;
  image_size.width = width_;
  image_size.height = height_;
  NppiRect remap_roi = {0, 0, width_, height_};

  NppStatus status;
  int d_map_step = static_cast<int>(d_mapx_.shape(1) * sizeof(float));
  switch (src_img.channels()) {
    case 1:
      status = nppiRemap_8u_C1R(
          src_img.gpu_data(), image_size, src_img.width_step(), remap_roi,
          d_mapx_.gpu_data(), d_map_step, d_mapy_.gpu_data(), d_map_step,
          dst_img->mutable_gpu_data(), dst_img->width_step(), image_size,
          remap_mode);
      break;
    case 3:
      status = nppiRemap_8u_C3R(
          src_img.gpu_data(), image_size, src_img.width_step(), remap_roi,
          d_mapx_.gpu_data(), d_map_step, d_mapy_.gpu_data(), d_map_step,
          dst_img->mutable_gpu_data(), dst_img->width_step(), image_size,
          remap_mode);
      break;
    default:
      AERROR << "Invalid number of channels: " << src_img.channels();
      return false;
  }

  if (status != NPP_SUCCESS) {
    AERROR << "NPP_CHECK_NPP - status = " << status;
    return false;
  }

  return true;
}

bool UndistortionHandler::Release(void) {
  inited_ = false;
  return true;
}

void UndistortionHandler::InitUndistortRectifyMap(
    const Eigen::Matrix3f &camera_model,
    const Eigen::Matrix<float, 5, 1> distortion, const Eigen::Matrix3f &R,
    const Eigen::Matrix3f &new_camera_model, int width, int height,
    base::Blob<float> *d_mapx, base::Blob<float> *d_mapy) {
  float fx = camera_model(0, 0);
  float fy = camera_model(1, 1);
  float cx = camera_model(0, 2);
  float cy = camera_model(1, 2);
  float nfx = new_camera_model(0, 0);
  float nfy = new_camera_model(1, 1);
  float ncx = new_camera_model(0, 2);
  float ncy = new_camera_model(1, 2);
  float k1 = distortion(0, 0);
  float k2 = distortion(1, 0);
  float p1 = distortion(2, 0);
  float p2 = distortion(3, 0);
  float k3 = distortion(4, 0);
  Eigen::Matrix3f Rinv = R.inverse();

  for (int v = 0; v < height_; ++v) {
    float *x_ptr = d_mapx->mutable_cpu_data() + d_mapx->offset(v);
    float *y_ptr = d_mapy->mutable_cpu_data() + d_mapy->offset(v);
    for (int u = 0; u < width_; ++u) {
      Eigen::Matrix<float, 3, 1> xy1;
      xy1 << (static_cast<float>(u) - ncx) / nfx,
          (static_cast<float>(v) - ncy) / nfy, 1;
      auto XYW = Rinv * xy1;
      double nx = XYW(0, 0) / XYW(2, 0);
      double ny = XYW(1, 0) / XYW(2, 0);
      double r_square = nx * nx + ny * ny;
      double scale = (1 + r_square * (k1 + r_square * (k2 + r_square * k3)));
      double nnx =
          nx * scale + 2 * p1 * nx * ny + p2 * (r_square + 2 * nx * nx);
      double nny =
          ny * scale + p1 * (r_square + 2 * ny * ny) + 2 * p2 * nx * ny;
      x_ptr[u] = static_cast<float>(nnx * fx + cx);
      y_ptr[u] = static_cast<float>(nny * fy + cy);
    }
  }
}

}  // namespace camera
}  // namespace perception
}  // namespace apollo


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

   BOOST_DISPATCH_OVERLOAD( touint_
                          , (typename A0)
                          , bs::vmx_
                          , bs::saturated_tag
                          , bs::pack_< bd::single_<A0>, bs::vmx_>
                          )
   {
      BOOST_FORCEINLINE  bd::as_integer_t<A0, unsigned>
      operator()(bs::saturated_tag const&, const A0& a0) const BOOST_NOEXCEPT
      {
        return vec_ctu(a0.storage(),0);
      }
   };
} } }

#endif