/*******************************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2021, PickNik Robotics
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice, this
 *   list of conditions and the following disclaimer.
 *
 * * Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * * Neither the name of the copyright holder nor the names of its
 *   contributors may be used to endorse or promote products derived from
 *   this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *******************************************************************************/

/* Author: Jack Center, Wyatt Rees, Andy Zelenak */

#include <algorithm>
#include <cmath>
#include <Eigen/Geometry>
#include <limits>
#include <moveit/trajectory_processing/ruckig_traj_smoothing.h>
#include <rclcpp/rclcpp.hpp>
#include <vector>

namespace trajectory_processing
{
namespace
{
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.ruckig_traj_smoothing");
constexpr double DEFAULT_MAX_VELOCITY = 5;       // rad/s
constexpr double DEFAULT_MAX_ACCELERATION = 10;  // rad/s^2
constexpr double DEFAULT_MAX_JERK = 20;          // rad/s^3
}  // namespace

bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
                                     const double max_velocity_scaling_factor,
                                     const double max_acceleration_scaling_factor)
{
  const moveit::core::JointModelGroup* group = trajectory.getGroup();
  if (!group)
  {
    RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for");
    return false;
  }

  const size_t num_waypoints = trajectory.getWayPointCount();
  if (num_waypoints < 2)
  {
    RCLCPP_ERROR(LOGGER, "Trajectory does not have enough points to smooth with Ruckig");
    return false;
  }

  const size_t num_dof = group->getVariableCount();

  // This lib does not actually work properly when angles wrap around, so we need to unwind the path first
  trajectory.unwind();

  // Instantiate the smoother
  double timestep = trajectory.getAverageSegmentDuration();
  ruckig::Ruckig<0> ruckig{ num_dof, timestep };
  ruckig::InputParameter<0> ruckig_input{ num_dof };
  ruckig::OutputParameter<0> ruckig_output{ num_dof };

  // Initialize the smoother
  //  ruckig_input.synchronization = ruckig::Synchronization::Time;
  //  ruckig_input.interface = ruckig::Interface::Velocity;
  const std::vector<int>& idx = group->getVariableIndexList();
  std::vector<double> current_positions_vector(num_dof);
  std::vector<double> current_velocities_vector(num_dof);
  std::vector<double> current_accelerations_vector(num_dof);
  moveit::core::RobotStatePtr waypoint = trajectory.getFirstWayPointPtr();
  for (size_t i = 0; i < num_dof; ++i)
  {
    current_positions_vector.at(i) = waypoint->getVariablePosition(idx.at(i));
    current_velocities_vector.at(i) = waypoint->getVariableVelocity(idx.at(i));
    current_accelerations_vector.at(i) = waypoint->getVariableAcceleration(idx.at(i));
  }
  std::copy_n(current_positions_vector.begin(), num_dof, ruckig_input.current_position.begin());
  std::copy_n(current_velocities_vector.begin(), num_dof, ruckig_input.current_velocity.begin());
  std::copy_n(current_accelerations_vector.begin(), num_dof, ruckig_input.current_acceleration.begin());
  // Initialize output data struct
  ruckig_output.new_position = ruckig_input.current_position;
  ruckig_output.new_velocity = ruckig_input.current_velocity;
  ruckig_output.new_acceleration = ruckig_input.current_acceleration;

  // Kinematic limits (vel/accel/jerk)
  const std::vector<std::string>& vars = group->getVariableNames();
  const moveit::core::RobotModel& rmodel = group->getParentModel();
  for (size_t i = 0; i < num_dof; ++i)
  {
    // TODO(andyz): read this from the joint group if/when jerk limits are added to the JointModel
    ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK;

    const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars.at(i));

    // This assumes min/max bounds are symmetric
    if (bounds.velocity_bounded_)
    {
      ruckig_input.max_velocity.at(i) = max_velocity_scaling_factor * bounds.max_velocity_;
    }
    else
    {
      ruckig_input.max_velocity.at(i) = max_velocity_scaling_factor * DEFAULT_MAX_VELOCITY;
    }
    if (bounds.acceleration_bounded_)
    {
      ruckig_input.max_acceleration.at(i) = max_acceleration_scaling_factor * bounds.max_acceleration_;
    }
    else
    {
      ruckig_input.max_acceleration.at(i) = max_acceleration_scaling_factor * DEFAULT_MAX_ACCELERATION;
    }
  }

  ruckig::Result ruckig_result;
  for (size_t waypoint_idx = 0; waypoint_idx < num_waypoints - 1; ++waypoint_idx)
  {
    moveit::core::RobotStatePtr next_waypoint = trajectory.getWayPointPtr(waypoint_idx + 1);

    getNextCurrentTargetStates(ruckig_input, ruckig_output, next_waypoint, num_dof, idx);

    // Run Ruckig
    ruckig_result = ruckig.update(ruckig_input, ruckig_output);

    // If the requested velocity is too great, a joint can actually "move backward" to give itself more time to
    // accelerate to the target velocity. Iterate and decrease velocities until that behavior is gone.
    bool backward_motion_detected = checkForLaggingMotion(num_dof, ruckig_input, ruckig_output);

    double minimum_velocity_magnitude = 0.01;  // rad/s
    double velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, num_dof);
    while (backward_motion_detected && rclcpp::ok() && (velocity_magnitude > minimum_velocity_magnitude))
    {
      // decrease target velocity
      for (size_t joint = 0; joint < num_dof; ++joint)
      {
        ruckig_input.target_velocity.at(joint) *= 0.9;
        // Propogate the change in velocity to position and acceleration, too.
        // This derives from a first-order approximation, e.g. x_i+1 = x_i + v_i * delta_t
        // and 0.9 * x_i+1 = 0.9 * (x_i + v_i * delta_t)
        ruckig_input.target_position.at(joint) -= (0.9 * 0.9 - 1) * timestep * ruckig_input.target_velocity.at(joint);
        ruckig_input.target_acceleration.at(joint) =
            (ruckig_input.target_velocity.at(joint) - ruckig_output.new_velocity.at(joint)) / timestep;
      }
      velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, num_dof);
      // Run Ruckig
      ruckig_result = ruckig.update(ruckig_input, ruckig_output);
      // check for backward motion
      backward_motion_detected = checkForLaggingMotion(num_dof, ruckig_input, ruckig_output);
    }

    if (ruckig_result != ruckig::Result::Working)
    {
      RCLCPP_ERROR_STREAM(LOGGER, "Ruckig trajectory smoothing failed at waypoint " << waypoint_idx);
      RCLCPP_ERROR_STREAM(LOGGER, "Ruckig error: " << ruckig_result);
      return false;
    }

    // Overwrite pos/vel/accel of the target waypoint
    for (size_t joint = 0; joint < num_dof; ++joint)
    {
      next_waypoint->setVariablePosition(idx.at(joint), ruckig_output.new_position.at(joint));
      next_waypoint->setVariableVelocity(idx.at(joint), ruckig_output.new_velocity.at(joint));
      next_waypoint->setVariableAcceleration(idx.at(joint), ruckig_output.new_acceleration.at(joint));
    }
    next_waypoint->update();
  }

  return true;
}

double RuckigSmoothing::getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input, size_t num_dof)
{
  double vel_magnitude = 0;
  for (size_t joint = 0; joint < num_dof; ++joint)
  {
    vel_magnitude += ruckig_input.target_velocity.at(joint) * ruckig_input.target_velocity.at(joint);
  }
  return sqrt(vel_magnitude);
}

bool RuckigSmoothing::checkForLaggingMotion(const size_t num_dof, const ruckig::InputParameter<0>& ruckig_input,
                                            const ruckig::OutputParameter<0>& ruckig_output)
{
  // Check for backward motion of any joint
  for (size_t joint = 0; joint < num_dof; ++joint)
  {
    // This indicates the jerk-limited output lags the target output
    if ((ruckig_output.new_velocity.at(joint) / ruckig_input.target_velocity.at(joint)) < 1)
    {
      return true;
    }
  }
  return false;
}

void RuckigSmoothing::getNextCurrentTargetStates(ruckig::InputParameter<0>& ruckig_input,
                                                 ruckig::OutputParameter<0>& ruckig_output,
                                                 const moveit::core::RobotStatePtr& next_waypoint, size_t num_dof,
                                                 const std::vector<int>& idx)
{
  for (size_t joint = 0; joint < num_dof; ++joint)
  {
    // Feed output from the previous timestep back as input
    ruckig_input.current_position.at(joint) = ruckig_output.new_position.at(joint);
    ruckig_input.current_velocity.at(joint) = ruckig_output.new_velocity.at(joint);
    ruckig_input.current_acceleration.at(joint) = ruckig_output.new_acceleration.at(joint);

    // Target state is the next waypoint
    ruckig_input.target_position.at(joint) = next_waypoint->getVariablePosition(idx.at(joint));
    ruckig_input.target_velocity.at(joint) = next_waypoint->getVariableVelocity(idx.at(joint));
    ruckig_input.target_acceleration.at(joint) = next_waypoint->getVariableAcceleration(idx.at(joint));
  }
}
}  // namespace trajectory_processing


// Copyright 2018 Dr. Matthias Hölzl
// Licensed under the MIT License
//

#include <iostream>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/io.hpp>
#include <boost/units/systems/cgs.hpp>
#include <boost/units/systems/cgs/io.hpp>
#include <boost/units/operators.hpp>

using namespace boost::units;
using namespace boost::units::si;
using namespace boost::units::cgs;

bool printLengths()
{
    auto one_meter{ 1.0 * meter };
    auto one_hundred_centimeters{ 100.0 * centimeters};

    std::cout << "1 m = " << one_meter << " = " << one_hundred_centimeters << std::endl;
}

int main() {
    std::cout << "Hello, World!" << std::endl;
    printLengths();
    return 0;
}


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Ion Gaztanaga 2015-2016.
// 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)
//
// See http://www.boost.org/libs/move for documentation.
//
//////////////////////////////////////////////////////////////////////////////
//
// Stable sorting that works in O(N*log(N)) worst time
// and uses O(1) extra memory
//
//////////////////////////////////////////////////////////////////////////////
//
// The main idea of the adaptive_sort algorithm was developed by Andrey Astrelin
// and explained in the article from the russian collaborative blog
// Habrahabr (http://habrahabr.ru/post/205290/). The algorithm is based on
// ideas from B-C. Huang and M. A. Langston explained in their article
// "Fast Stable Merging and Sorting in Constant Extra Space (1989-1992)"
// (http://comjnl.oxfordjournals.org/content/35/6/643.full.pdf).
//
// This implementation by Ion Gaztanaga uses previous ideas with additional
// changes:
//
// - Use of GCD-based rotation.
// - Non power of two buffer-sizes.
// - Tries to find sqrt(len)*2 unique keys, so that the merge sort
//   phase can form up to sqrt(len)*4 segments if enough keys are found.
// - The merge-sort phase can take advantage of external memory to
//   save some additional combination steps.
// - Combination phase: Blocks are selection sorted and merged in parallel.
// - The combination phase is performed alternating merge to left and merge
//   to right phases minimizing swaps due to internal buffer repositioning.
// - When merging blocks special optimizations are made to avoid moving some
//   elements twice.
//
// The adaptive_merge algorithm was developed by Ion Gaztanaga reusing some
// parts from the sorting algorithm and implementing an additional block merge
// algorithm without moving elements to left or right.
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_MOVE_ADAPTIVE_SORT_MERGE_HPP
#define BOOST_MOVE_ADAPTIVE_SORT_MERGE_HPP

#include <boost/assert.hpp>
#include <boost/cstdint.hpp>
#include <boost/move/adl_move_swap.hpp>
#include <boost/move/algo/detail/heap_sort.hpp>
#include <boost/move/algo/detail/insertion_sort.hpp>
#include <boost/move/algo/detail/is_sorted.hpp>
#include <boost/move/algo/detail/merge.hpp>
#include <boost/move/algo/detail/merge_sort.hpp>
#include <boost/move/algo/move.hpp>
#include <boost/move/detail/config_begin.hpp>
#include <boost/move/detail/reverse_iterator.hpp>

#ifndef BOOST_MOVE_ADAPTIVE_SORT_STATS_LEVEL
#define BOOST_MOVE_ADAPTIVE_SORT_STATS_LEVEL 1
#endif

#ifdef BOOST_MOVE_ADAPTIVE_SORT_STATS
#if BOOST_MOVE_ADAPTIVE_SORT_STATS_LEVEL == 2
#define BOOST_MOVE_ADAPTIVE_SORT_PRINT_L1(STR, L) print_stats(STR, L) //

#define BOOST_MOVE_ADAPTIVE_SORT_PRINT_L2(STR, L) print_stats(STR, L) //
#else
#define BOOST_MOVE_ADAPTIVE_SORT_PRINT_L1(STR, L) print_stats(STR, L) //

#define BOOST_MOVE_ADAPTIVE_SORT_PRINT_L2(STR, L)
#endif
#else
#define BOOST_MOVE_ADAPTIVE_SORT_PRINT_L1(STR, L)
#define BOOST_MOVE_ADAPTIVE_SORT_PRINT_L2(STR, L)
#endif

#ifdef BOOST_MOVE_ADAPTIVE_SORT_INVARIANTS
#define BOOST_MOVE_ADAPTIVE_SORT_INVARIANT BOOST_ASSERT
#else
#define BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(L)
#endif

namespace boost {
namespace movelib {

#if defined(BOOST_MOVE_ADAPTIVE_SORT_INVARIANTS)

bool is_sorted(::order_perf_type *first, ::order_perf_type *last,
               ::order_type_less) {
  if (first != last) {
    const order_perf_type *next = first, *cur(first);
    while (++next != last) {
      if (!(cur->key < next->key ||
            (cur->key == next->key && cur->val < next->val)))
        return false;
      cur = next;
    }
  }
  return true;
}

#endif // BOOST_MOVE_ADAPTIVE_SORT_INVARIANTS

namespace detail_adaptive {

static const std::size_t AdaptiveSortInsertionSortThreshold = 16;
// static const std::size_t AdaptiveSortInsertionSortThreshold = 4;
BOOST_STATIC_ASSERT((AdaptiveSortInsertionSortThreshold &
                     (AdaptiveSortInsertionSortThreshold - 1)) == 0);

#if defined BOOST_HAS_INTPTR_T
typedef ::boost::uintptr_t uintptr_t;
#else
typedef std::size_t uintptr_t;
#endif

template <class T> const T &min_value(const T &a, const T &b) {
  return a < b ? a : b;
}

template <class T> const T &max_value(const T &a, const T &b) {
  return a > b ? a : b;
}

template <class ForwardIt, class Pred, class V>
typename iterator_traits<ForwardIt>::size_type
count_if_with(ForwardIt first, ForwardIt last, Pred pred, const V &v) {
  typedef typename iterator_traits<ForwardIt>::size_type size_type;
  size_type count = 0;
  while (first != last) {
    count += static_cast<size_type>(0 != pred(*first, v));
    ++first;
  }
  return count;
}

template <class RandIt, class Compare>
RandIt
skip_until_merge(RandIt first1, RandIt const last1,
                 const typename iterator_traits<RandIt>::value_type &next_key,
                 Compare comp) {
  while (first1 != last1 && !comp(next_key, *first1)) {
    ++first1;
  }
  return first1;
}

template <class RandItKeys, class RandIt>
void swap_and_update_key(RandItKeys const key_next, RandItKeys const key_range2,
                         RandItKeys &key_mid, RandIt const begin,
                         RandIt const end, RandIt const with) {
  if (begin != with) {
    ::boost::adl_move_swap_ranges(begin, end, with);
    ::boost::adl_move_swap(*key_next, *key_range2);
    if (key_next == key_mid) {
      key_mid = key_range2;
    } else if (key_mid == key_range2) {
      key_mid = key_next;
    }
  }
}

template <class RandItKeys>
void update_key(RandItKeys const key_next, RandItKeys const key_range2,
                RandItKeys &key_mid) {
  if (key_next != key_range2) {
    ::boost::adl_move_swap(*key_next, *key_range2);
    if (key_next == key_mid) {
      key_mid = key_range2;
    } else if (key_mid == key_range2) {
      key_mid = key_next;
    }
  }
}

template <class RandItKeys, class RandIt, class RandIt2, class Op>
RandIt2 buffer_and_update_key(RandItKeys const key_next,
                              RandItKeys const key_range2, RandItKeys &key_mid,
                              RandIt begin, RandIt end, RandIt with,
                              RandIt2 buffer, Op op) {
  if (begin != with) {
    while (begin != end) {
      op(three_way_t(), begin++, with++, buffer++);
    }
    ::boost::adl_move_swap(*key_next, *key_range2);
    if (key_next == key_mid) {
      key_mid = key_range2;
    } else if (key_mid == key_range2) {
      key_mid = key_next;
    }
  }
  return buffer;
}

///////////////////////////////////////////////////////////////////////////////
//
//                         MERGE BUFFERLESS
//
///////////////////////////////////////////////////////////////////////////////

// [first1, last1) merge [last1,last2) -> [first1,last2)
template <class RandIt, class Compare>
RandIt partial_merge_bufferless_impl(RandIt first1, RandIt last1,
                                     RandIt const last2,
                                     bool *const pis_range1_A, Compare comp) {
  if (last1 == last2) {
    return first1;
  }
  bool const is_range1_A = *pis_range1_A;
  if (first1 != last1 && comp(*last1, last1[-1])) {
    do {
      RandIt const old_last1 = last1;
      last1 = boost::movelib::lower_bound(last1, last2, *first1, comp);
      first1 =
          rotate_gcd(first1, old_last1, last1); // old_last1 == last1 supported
      if (last1 == last2) {
        return first1;
      }
      do {
        ++first1;
      } while (last1 != first1 && !comp(*last1, *first1));
    } while (first1 != last1);
  }
  *pis_range1_A = !is_range1_A;
  return last1;
}

// [first1, last1) merge [last1,last2) -> [first1,last2)
template <class RandIt, class Compare>
RandIt partial_merge_bufferless(RandIt first1, RandIt last1, RandIt const last2,
                                bool *const pis_range1_A, Compare comp) {
  return *pis_range1_A
             ? partial_merge_bufferless_impl(first1, last1, last2, pis_range1_A,
                                             comp)
             : partial_merge_bufferless_impl(first1, last1, last2, pis_range1_A,
                                             antistable<Compare>(comp));
}

template <class SizeType>
static SizeType needed_keys_count(SizeType n_block_a, SizeType n_block_b) {
  return n_block_a + n_block_b;
}

template <class RandItKeys, class KeyCompare, class RandIt, class Compare>
typename iterator_traits<RandIt>::size_type find_next_block(
    RandItKeys const key_first, KeyCompare key_comp, RandIt const first,
    typename iterator_traits<RandIt>::size_type const l_block,
    typename iterator_traits<RandIt>::size_type const ix_first_block,
    typename iterator_traits<RandIt>::size_type const ix_last_block,
    Compare comp) {
  typedef typename iterator_traits<RandIt>::size_type size_type;
  typedef typename iterator_traits<RandIt>::value_type value_type;
  typedef typename iterator_traits<RandItKeys>::value_type key_type;
  BOOST_ASSERT(ix_first_block <= ix_last_block);
  size_type ix_min_block = 0u;
  for (size_type szt_i = ix_first_block; szt_i < ix_last_block; ++szt_i) {
    const value_type &min_val = first[ix_min_block * l_block];
    const value_type &cur_val = first[szt_i * l_block];
    const key_type &min_key = key_first[ix_min_block];
    const key_type &cur_key = key_first[szt_i];

    bool const less_than_minimum =
        comp(cur_val, min_val) ||
        (!comp(min_val, cur_val) && key_comp(cur_key, min_key));

    if (less_than_minimum) {
      ix_min_block = szt_i;
    }
  }
  return ix_min_block;
}

template <class RandItKeys, class KeyCompare, class RandIt, class Compare>
void merge_blocks_bufferless(
    RandItKeys const key_first, KeyCompare key_comp, RandIt const first,
    typename iterator_traits<RandIt>::size_type const l_block,
    typename iterator_traits<RandIt>::size_type const l_irreg1,
    typename iterator_traits<RandIt>::size_type const n_block_a,
    typename iterator_traits<RandIt>::size_type const n_block_b,
    typename iterator_traits<RandIt>::size_type const l_irreg2, Compare comp) {
  typedef typename iterator_traits<RandIt>::size_type size_type;
  size_type const key_count = needed_keys_count(n_block_a, n_block_b);
  (void)key_count;
  // BOOST_ASSERT(n_block_a || n_block_b);
  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(boost::movelib::is_sorted_and_unique(
      key_first, key_first + key_count, key_comp));
  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
      !n_block_b || n_block_a == count_if_with(key_first, key_first + key_count,
                                               key_comp, key_first[n_block_a]));

  size_type n_bef_irreg2 = 0;
  bool l_irreg_pos_count = true;
  RandItKeys key_mid(key_first + n_block_a);
  RandIt const first_irr2 =
      first + l_irreg1 + (n_block_a + n_block_b) * l_block;
  RandIt const last_irr2 = first_irr2 + l_irreg2;

  { // Selection sort blocks
    size_type n_block_left = n_block_b + n_block_a;
    RandItKeys key_range2(key_first);

    size_type min_check = n_block_a == n_block_left ? 0u : n_block_a;
    size_type max_check = min_value<size_type>(min_check + 1, n_block_left);
    for (RandIt f = first + l_irreg1; n_block_left; --n_block_left,
                ++key_range2, f += l_block, min_check -= min_check != 0,
                max_check -= max_check != 0) {
      size_type const next_key_idx = find_next_block(
          key_range2, key_comp, f, l_block, min_check, max_check, comp);
      RandItKeys const key_next(key_range2 + next_key_idx);
      max_check = min_value<size_type>(
          max_value<size_type>(max_check, next_key_idx + size_type(2)),
          n_block_left);

      RandIt const first_min = f + next_key_idx * l_block;

      // Check if irregular b block should go here.
      // If so, break to the special code handling the irregular block
      if (l_irreg_pos_count && l_irreg2 && comp(*first_irr2, *first_min)) {
        l_irreg_pos_count = false;
      }
      n_bef_irreg2 += l_irreg_pos_count;

      swap_and_update_key(key_next, key_range2, key_mid, f, f + l_block,
                          first_min);
      BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
          boost::movelib::is_sorted(f, f + l_block, comp));
      BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
          boost::movelib::is_sorted(first_min, first_min + l_block, comp));
      BOOST_MOVE_ADAPTIVE_SORT_INVARIANT((f == (first + l_irreg1)) ||
                                         !comp(*f, *(f - l_block)));
    }
  }
  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(boost::movelib::is_sorted(
      first + l_irreg1 + n_bef_irreg2 * l_block, first_irr2, comp));

  RandIt first1 = first;
  RandIt last1 = first + l_irreg1;
  RandItKeys const key_end(key_first + n_bef_irreg2);
  bool is_range1_A = true;

  for (RandItKeys key_next = key_first; key_next != key_end; ++key_next) {
    bool is_range2_A =
        key_mid == (key_first + key_count) || key_comp(*key_next, *key_mid);
    first1 = is_range1_A == is_range2_A
                 ? last1
                 : partial_merge_bufferless(first1, last1, last1 + l_block,
                                            &is_range1_A, comp);
    last1 += l_block;
    BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
        boost::movelib::is_sorted(first, first1, comp));
  }

  merge_bufferless(is_range1_A ? first1 : last1, first_irr2, last_irr2, comp);
  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
      boost::movelib::is_sorted(first, last_irr2, comp));
}

// Complexity: 2*distance(first, last)+max_collected^2/2
//
// Tries to collect at most n_keys unique elements from [first, last),
// in the begining of the range, and ordered according to comp
//
// Returns the number of collected keys
template <class RandIt, class Compare, class XBuf>
typename iterator_traits<RandIt>::size_type
collect_unique(RandIt const first, RandIt const last,
               typename iterator_traits<RandIt>::size_type const max_collected,
               Compare comp, XBuf &xbuf) {
  typedef typename iterator_traits<RandIt>::size_type size_type;
  size_type h = 0;
  if (max_collected) {
    ++h; // first key is always here
    RandIt h0 = first;
    RandIt u = first;
    ++u;
    RandIt search_end = u;

    if (xbuf.capacity() >= max_collected) {
      typename XBuf::iterator const ph0 = xbuf.add(first);
      while (u != last && h < max_collected) {
        typename XBuf::iterator const r =
            boost::movelib::lower_bound(ph0, xbuf.end(), *u, comp);
        // If key not found add it to [h, h+h0)
        if (r == xbuf.end() || comp(*u, *r)) {
          RandIt const new_h0 = boost::move(search_end, u, h0);
          search_end = u;
          ++search_end;
          ++h;
          xbuf.insert(r, u);
          h0 = new_h0;
        }
        ++u;
      }
      boost::move_backward(first, h0, h0 + h);
      boost::move(xbuf.data(), xbuf.end(), first);
    } else {
      while (u != last && h < max_collected) {
        RandIt const r = boost::movelib::lower_bound(h0, search_end, *u, comp);
        // If key not found add it to [h, h+h0)
        if (r == search_end || comp(*u, *r)) {
          RandIt const new_h0 = rotate_gcd(h0, search_end, u);
          search_end = u;
          ++search_end;
          ++h;
          rotate_gcd(r + (new_h0 - h0), u, search_end);
          h0 = new_h0;
        }
        ++u;
      }
      rotate_gcd(first, h0, h0 + h);
    }
  }
  return h;
}

template <class Unsigned> Unsigned floor_sqrt(Unsigned const n) {
  Unsigned x = n;
  Unsigned y = x / 2 + (x & 1);
  while (y < x) {
    x = y;
    y = (x + n / x) / 2;
  }
  return x;
}

template <class Unsigned> Unsigned ceil_sqrt(Unsigned const n) {
  Unsigned r = floor_sqrt(n);
  return r + Unsigned((n % r) != 0);
}

template <class Unsigned>
Unsigned floor_merge_multiple(Unsigned const n, Unsigned &base, Unsigned &pow) {
  Unsigned s = n;
  Unsigned p = 0;
  while (s > AdaptiveSortInsertionSortThreshold) {
    s /= 2;
    ++p;
  }
  base = s;
  pow = p;
  return s << p;
}

template <class Unsigned>
Unsigned ceil_merge_multiple(Unsigned const n, Unsigned &base, Unsigned &pow) {
  Unsigned fm = floor_merge_multiple(n, base, pow);

  if (fm != n) {
    if (base < AdaptiveSortInsertionSortThreshold) {
      ++base;
    } else {
      base = AdaptiveSortInsertionSortThreshold / 2 + 1;
      ++pow;
    }
  }
  return base << pow;
}

template <class Unsigned>
Unsigned ceil_sqrt_multiple(Unsigned const n, Unsigned *pbase = 0) {
  Unsigned const r = ceil_sqrt(n);
  Unsigned pow = 0;
  Unsigned base = 0;
  Unsigned const res = ceil_merge_multiple(r, base, pow);
  if (pbase)
    *pbase = base;
  return res;
}

struct less {
  template <class T> bool operator()(const T &l, const T &r) { return l < r; }
};

///////////////////////////////////////////////////////////////////////////////
//
//                            MERGE BLOCKS
//
///////////////////////////////////////////////////////////////////////////////

//#define ADAPTIVE_SORT_MERGE_SLOW_STABLE_SORT_IS_NLOGN

#if defined ADAPTIVE_SORT_MERGE_SLOW_STABLE_SORT_IS_NLOGN
template <class RandIt, class Compare>
void slow_stable_sort(RandIt const first, RandIt const last, Compare comp) {
  boost::movelib::inplace_stable_sort(first, last, comp);
}

#else // ADAPTIVE_SORT_MERGE_SLOW_STABLE_SORT_IS_NLOGN

template <class RandIt, class Compare>
void slow_stable_sort(RandIt const first, RandIt const last, Compare comp) {
  typedef typename iterator_traits<RandIt>::size_type size_type;
  size_type L = size_type(last - first);
  { // Use insertion sort to merge first elements
    size_type m = 0;
    while ((L - m) > size_type(AdaptiveSortInsertionSortThreshold)) {
      insertion_sort(first + m,
                     first + m + size_type(AdaptiveSortInsertionSortThreshold),
                     comp);
      m += AdaptiveSortInsertionSortThreshold;
    }
    insertion_sort(first + m, last, comp);
  }

  size_type h = AdaptiveSortInsertionSortThreshold;
  for (bool do_merge = L > h; do_merge; h *= 2) {
    do_merge = (L - h) > h;
    size_type p0 = 0;
    if (do_merge) {
      size_type const h_2 = 2 * h;
      while ((L - p0) > h_2) {
        merge_bufferless(first + p0, first + p0 + h, first + p0 + h_2, comp);
        p0 += h_2;
      }
    }
    if ((L - p0) > h) {
      merge_bufferless(first + p0, first + p0 + h, last, comp);
    }
  }
}

#endif // ADAPTIVE_SORT_MERGE_SLOW_STABLE_SORT_IS_NLOGN

// Returns new l_block and updates use_buf
template <class Unsigned>
Unsigned lblock_for_combine(Unsigned const l_block, Unsigned const n_keys,
                            Unsigned const l_data, bool &use_buf) {
  BOOST_ASSERT(l_data > 1);

  // We need to guarantee lblock >= l_merged/(n_keys/2) keys for the
  // combination. We have at least 4 keys guaranteed (which are the minimum to
  // merge 2 ranges) If l_block != 0, then n_keys is already enough to merge all
  // blocks in all phases as we've found all needed keys for that buffer and
  // length before. If l_block == 0 then see if half keys can be used as buffer
  // and the rest as keys guaranteeing that n_keys >= (2*l_merged)/lblock =
  if (!l_block) {
    // If l_block == 0 then n_keys is power of two
    //(guaranteed by build_params(...))
    BOOST_ASSERT(n_keys >= 4);
    // BOOST_ASSERT(0 == (n_keys &(n_keys-1)));

    // See if half keys are at least 4 and if half keys fulfill
    Unsigned const new_buf = n_keys / 2;
    Unsigned const new_keys = n_keys - new_buf;
    use_buf = new_keys >= 4 && new_keys >= l_data / new_buf;
    if (use_buf) {
      return new_buf;
    } else {
      return l_data / n_keys;
    }
  } else {
    use_buf = true;
    return l_block;
  }
}

template <class RandIt, class Compare, class XBuf>
void stable_sort(RandIt first, RandIt last, Compare comp, XBuf &xbuf) {
  typedef typename iterator_traits<RandIt>::size_type size_type;
  size_type const len = size_type(last - first);
  size_type const half_len = len / 2 + (len & 1);
  if (std::size_t(xbuf.capacity() - xbuf.size()) >= half_len) {
    merge_sort(first, last, comp, xbuf.data() + xbuf.size());
  } else {
    slow_stable_sort(first, last, comp);
  }
}

template <class RandIt, class Comp, class XBuf>
void unstable_sort(RandIt first, RandIt last, Comp comp, XBuf &xbuf) {
  heap_sort(first, last, comp);
  (void)xbuf;
}

template <class RandIt, class Compare, class XBuf>
void stable_merge(RandIt first, RandIt const middle, RandIt last, Compare comp,
                  XBuf &xbuf) {
  BOOST_ASSERT(xbuf.empty());
  typedef typename iterator_traits<RandIt>::size_type size_type;
  size_type const len1 = size_type(middle - first);
  size_type const len2 = size_type(last - middle);
  size_type const l_min = min_value<size_type>(len1, len2);
  if (xbuf.capacity() >= l_min) {
    buffered_merge(first, middle, last, comp, xbuf);
    xbuf.clear();
  } else {
    // merge_bufferless(first, middle, last, comp);
    merge_adaptive_ONlogN(first, middle, last, comp, xbuf.begin(),
                          xbuf.capacity());
  }
  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(boost::movelib::is_sorted(
      first, last, boost::movelib::unantistable(comp)));
}

template <class RandIt, class Comp, class XBuf>
void initialize_keys(RandIt first, RandIt last, Comp comp, XBuf &xbuf) {
  unstable_sort(first, last, comp, xbuf);
  BOOST_ASSERT(boost::movelib::is_sorted_and_unique(first, last, comp));
}

template <class RandIt, class U>
void initialize_keys(RandIt first, RandIt last, less, U &) {
  typedef typename iterator_traits<RandIt>::value_type value_type;
  std::size_t count = std::size_t(last - first);
  for (std::size_t i = 0; i != count; ++i) {
    *first = static_cast<value_type>(i);
    ++first;
  }
}

template <class Unsigned>
Unsigned calculate_total_combined(Unsigned const len,
                                  Unsigned const l_prev_merged,
                                  Unsigned *pl_irreg_combined = 0) {
  typedef Unsigned size_type;

  size_type const l_combined = 2 * l_prev_merged;
  size_type l_irreg_combined = len % l_combined;
  size_type l_total_combined = len;
  if (l_irreg_combined <= l_prev_merged) {
    l_total_combined -= l_irreg_combined;
    l_irreg_combined = 0;
  }
  if (pl_irreg_combined)
    *pl_irreg_combined = l_irreg_combined;
  return l_total_combined;
}

template <class RandItKeys, class KeyCompare, class SizeType, class XBuf>
void combine_params(RandItKeys const keys, KeyCompare key_comp,
                    SizeType l_combined, SizeType const l_prev_merged,
                    SizeType const l_block,
                    XBuf &xbuf
                    // Output
                    ,
                    SizeType &n_block_a, SizeType &n_block_b,
                    SizeType &l_irreg1,
                    SizeType &l_irreg2
                    // Options
                    ,
                    bool do_initialize_keys = true) {
  typedef SizeType size_type;

  // Initial parameters for selection sort blocks
  l_irreg1 = l_prev_merged % l_block;
  l_irreg2 = (l_combined - l_irreg1) % l_block;
  BOOST_ASSERT(((l_combined - l_irreg1 - l_irreg2) % l_block) == 0);
  size_type const n_reg_block = (l_combined - l_irreg1 - l_irreg2) / l_block;
  n_block_a = l_prev_merged / l_block;
  n_block_b = n_reg_block - n_block_a;
  BOOST_ASSERT(n_reg_block >= n_block_a);

  // Key initialization
  if (do_initialize_keys) {
    initialize_keys(keys, keys + needed_keys_count(n_block_a, n_block_b),
                    key_comp, xbuf);
  }
}

//////////////////////////////////
//
//          partial_merge
//
//////////////////////////////////
template <class InputIt1, class InputIt2, class OutputIt, class Compare,
          class Op>
OutputIt op_partial_merge_impl(InputIt1 &r_first1, InputIt1 const last1,
                               InputIt2 &r_first2, InputIt2 const last2,
                               OutputIt d_first, Compare comp, Op op) {
  InputIt1 first1(r_first1);
  InputIt2 first2(r_first2);
  if (first2 != last2 && last1 != first1)
    while (1) {
      if (comp(*first2, *first1)) {
        op(first2++, d_first++);
        if (first2 == last2) {
          break;
        }
      } else {
        op(first1++, d_first++);
        if (first1 == last1) {
          break;
        }
      }
    }
  r_first1 = first1;
  r_first2 = first2;
  return d_first;
}

template <class InputIt1, class InputIt2, class OutputIt, class Compare,
          class Op>
OutputIt op_partial_merge(InputIt1 &r_first1, InputIt1 const last1,
                          InputIt2 &r_first2, InputIt2 const last2,
                          OutputIt d_first, Compare comp, Op op,
                          bool is_stable) {
  return is_stable
             ? op_partial_merge_impl(r_first1, last1, r_first2, last2, d_first,
                                     comp, op)
             : op_partial_merge_impl(r_first1, last1, r_first2, last2, d_first,
                                     antistable<Compare>(comp), op);
}

//////////////////////////////////
//////////////////////////////////
//////////////////////////////////
//
//    op_partial_merge_and_save
//
//////////////////////////////////
//////////////////////////////////
//////////////////////////////////
template <class InputIt1, class InputIt2, class OutputIt, class Compare,
          class Op>
OutputIt
op_partial_merge_and_swap_impl(InputIt1 &r_first1, InputIt1 const last1,
                               InputIt2 &r_first2, InputIt2 const last2,
                               InputIt2 &r_first_min, OutputIt d_first,
                               Compare comp, Op op) {
  InputIt1 first1(r_first1);
  InputIt2 first2(r_first2);

  if (first2 != last2 && last1 != first1) {
    InputIt2 first_min(r_first_min);
    bool non_empty_ranges = true;
    do {
      if (comp(*first_min, *first1)) {
        op(three_way_t(), first2++, first_min++, d_first++);
        non_empty_ranges = first2 != last2;
      } else {
        op(first1++, d_first++);
        non_empty_ranges = first1 != last1;
      }
    } while (non_empty_ranges);
    r_first_min = first_min;
    r_first1 = first1;
    r_first2 = first2;
  }
  return d_first;
}

template <class RandIt, class InputIt2, class OutputIt, class Compare, class Op>
OutputIt op_partial_merge_and_swap(RandIt &r_first1, RandIt const last1,
                                   InputIt2 &r_first2, InputIt2 const last2,
                                   InputIt2 &r_first_min, OutputIt d_first,
                                   Compare comp, Op op, bool is_stable) {
  return is_stable
             ? op_partial_merge_and_swap_impl(r_first1, last1, r_first2, last2,
                                              r_first_min, d_first, comp, op)
             : op_partial_merge_and_swap_impl(r_first1, last1, r_first2, last2,
                                              r_first_min, d_first,
                                              antistable<Compare>(comp), op);
}

template <class RandIt1, class RandIt2, class RandItB, class Compare, class Op>
RandItB op_buffered_partial_merge_and_swap_to_range1_and_buffer(
    RandIt1 first1, RandIt1 const last1, RandIt2 &rfirst2, RandIt2 const last2,
    RandIt2 &rfirst_min, RandItB &rfirstb, Compare comp, Op op) {
  RandItB firstb = rfirstb;
  RandItB lastb = firstb;
  RandIt2 first2 = rfirst2;

  // Move to buffer while merging
  // Three way moves need less moves when op is swap_op so use it
  // when merging elements from range2 to the destination occupied by range1
  if (first1 != last1 && first2 != last2) {
    RandIt2 first_min = rfirst_min;
    op(four_way_t(), first2++, first_min++, first1++, lastb++);

    while (first1 != last1) {
      if (first2 == last2) {
        lastb = op(forward_t(), first1, last1, firstb);
        break;
      }

      if (comp(*first_min, *firstb)) {
        op(four_way_t(), first2++, first_min++, first1++, lastb++);
      } else {
        op(three_way_t(), firstb++, first1++, lastb++);
      }
    }
    rfirst2 = first2;
    rfirstb = firstb;
    rfirst_min = first_min;
  }

  return lastb;
}

template <class RandIt1, class RandIt2, class RandItB, class Compare, class Op>
RandItB op_buffered_partial_merge_to_range1_and_buffer(
    RandIt1 first1, RandIt1 const last1, RandIt2 &rfirst2, RandIt2 const last2,
    RandItB &rfirstb, Compare comp, Op op) {
  RandItB firstb = rfirstb;
  RandItB lastb = firstb;
  RandIt2 first2 = rfirst2;

  // Move to buffer while merging
  // Three way moves need less moves when op is swap_op so use it
  // when merging elements from range2 to the destination occupied by range1
  if (first1 != last1 && first2 != last2) {
    op(three_way_t(), first2++, first1++, lastb++);

    while (true) {
      if (first1 == last1) {
        break;
      }
      if (first2 == last2) {
        lastb = op(forward_t(), first1, last1, firstb);
        break;
      }
      if (comp(*first2, *firstb)) {
        op(three_way_t(), first2++, first1++, lastb++);
      } else {
        op(three_way_t(), firstb++, first1++, lastb++);
      }
    }
    rfirst2 = first2;
    rfirstb = firstb;
  }

  return lastb;
}

template <class RandIt, class RandItBuf, class Compare, class Op>
RandIt op_partial_merge_and_save_impl(RandIt first1, RandIt const last1,
                                      RandIt &rfirst2, RandIt last2,
                                      RandIt first_min,
                                      RandItBuf &buf_first1_in_out,
                                      RandItBuf &buf_last1_in_out, Compare comp,
                                      Op op) {
  RandItBuf buf_first1 = buf_first1_in_out;
  RandItBuf buf_last1 = buf_last1_in_out;
  RandIt first2(rfirst2);

  bool const do_swap = first2 != first_min;
  if (buf_first1 == buf_last1) {
    // Skip any element that does not need to be moved
    RandIt new_first1 = skip_until_merge(first1, last1, *first_min, comp);
    buf_first1 += (new_first1 - first1);
    first1 = new_first1;
    buf_last1 =
        do_swap
            ? op_buffered_partial_merge_and_swap_to_range1_and_buffer(
                  first1, last1, first2, last2, first_min, buf_first1, comp, op)
            : op_buffered_partial_merge_to_range1_and_buffer(
                  first1, last1, first2, last2, buf_first1, comp, op);
    first1 = last1;
  } else {
    BOOST_ASSERT((last1 - first1) == (buf_last1 - buf_first1));
  }

  // Now merge from buffer
  first1 = do_swap ? op_partial_merge_and_swap_impl(buf_first1, buf_last1,
                                                    first2, last2, first_min,
                                                    first1, comp, op)
                   : op_partial_merge_impl(buf_first1, buf_last1, first2, last2,
                                           first1, comp, op);
  buf_first1_in_out = buf_first1;
  buf_last1_in_out = buf_last1;
  rfirst2 = first2;
  return first1;
}

template <class RandIt, class RandItBuf, class Compare, class Op>
RandIt op_partial_merge_and_save(RandIt first1, RandIt const last1,
                                 RandIt &rfirst2, RandIt last2,
                                 RandIt first_min, RandItBuf &buf_first1_in_out,
                                 RandItBuf &buf_last1_in_out, Compare comp,
                                 Op op, bool is_stable) {
  return is_stable
             ? op_partial_merge_and_save_impl(first1, last1, rfirst2, last2,
                                              first_min, buf_first1_in_out,
                                              buf_last1_in_out, comp, op)
             : op_partial_merge_and_save_impl(
                   first1, last1, rfirst2, last2, first_min, buf_first1_in_out,
                   buf_last1_in_out, antistable<Compare>(comp), op);
}

//////////////////////////////////
//////////////////////////////////
//////////////////////////////////
//
//    op_merge_blocks_with_irreg
//
//////////////////////////////////
//////////////////////////////////
//////////////////////////////////

template <class RandItKeys, class KeyCompare, class RandIt, class RandIt2,
          class OutputIt, class Compare, class Op>
OutputIt op_merge_blocks_with_irreg(
    RandItKeys key_first, RandItKeys key_mid, KeyCompare key_comp,
    RandIt first_reg, RandIt2 &first_irr, RandIt2 const last_irr, OutputIt dest,
    typename iterator_traits<RandIt>::size_type const l_block,
    typename iterator_traits<RandIt>::size_type n_block_left,
    typename iterator_traits<RandIt>::size_type min_check,
    typename iterator_traits<RandIt>::size_type max_check, Compare comp,
    bool const is_stable, Op op) {
  typedef typename iterator_traits<RandIt>::size_type size_type;

  for (; n_block_left; --n_block_left, ++key_first, min_check -= min_check != 0,
                       max_check -= max_check != 0) {
    size_type next_key_idx = find_next_block(
        key_first, key_comp, first_reg, l_block, min_check, max_check, comp);
    max_check = min_value<size_type>(
        max_value<size_type>(max_check, next_key_idx + size_type(2)),
        n_block_left);
    RandIt const last_reg = first_reg + l_block;
    RandIt first_min = first_reg + next_key_idx * l_block;
    RandIt const last_min = first_min + l_block;
    (void)last_min;

    BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
        boost::movelib::is_sorted(first_reg, last_reg, comp));
    BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
        !next_key_idx || boost::movelib::is_sorted(first_min, last_min, comp));
    BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
        (!next_key_idx || !comp(*first_reg, *first_min)));

    OutputIt orig_dest = dest;
    (void)orig_dest;
    dest = next_key_idx
               ? op_partial_merge_and_swap(first_irr, last_irr, first_reg,
                                           last_reg, first_min, dest, comp, op,
                                           is_stable)
               : op_partial_merge(first_irr, last_irr, first_reg, last_reg,
                                  dest, comp, op, is_stable);
    BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
        boost::movelib::is_sorted(orig_dest, dest, comp));

    if (first_reg == dest) {
      dest = next_key_idx
                 ? ::boost::adl_move_swap_ranges(first_min, last_min, first_reg)
                 : last_reg;
    } else {
      dest = next_key_idx ? op(three_way_forward_t(), first_reg, last_reg,
                               first_min, dest)
                          : op(forward_t(), first_reg, last_reg, dest);
    }

    RandItKeys const key_next(key_first + next_key_idx);
    swap_and_update_key(key_next, key_first, key_mid, last_reg, last_reg,
                        first_min);

    BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
        boost::movelib::is_sorted(orig_dest, dest, comp));
    first_reg = last_reg;
  }
  return dest;
}

//////////////////////////////////
//////////////////////////////////
//////////////////////////////////
//
//    op_merge_blocks_left/right
//
//////////////////////////////////
//////////////////////////////////
//////////////////////////////////

template <class RandItKeys, class KeyCompare, class RandIt, class Compare,
          class Op>
void op_merge_blocks_left(
    RandItKeys const key_first, KeyCompare key_comp, RandIt const first,
    typename iterator_traits<RandIt>::size_type const l_block,
    typename iterator_traits<RandIt>::size_type const l_irreg1,
    typename iterator_traits<RandIt>::size_type const n_block_a,
    typename iterator_traits<RandIt>::size_type const n_block_b,
    typename iterator_traits<RandIt>::size_type const l_irreg2, Compare comp,
    Op op) {
  typedef typename iterator_traits<RandIt>::size_type size_type;
  size_type const key_count = needed_keys_count(n_block_a, n_block_b);
  (void)key_count;
  //   BOOST_ASSERT(n_block_a || n_block_b);
  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(boost::movelib::is_sorted_and_unique(
      key_first, key_first + key_count, key_comp));
  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
      !n_block_b || n_block_a == count_if_with(key_first, key_first + key_count,
                                               key_comp, key_first[n_block_a]));

  size_type n_block_b_left = n_block_b;
  size_type n_block_a_left = n_block_a;
  size_type n_block_left = n_block_b + n_block_a;
  RandItKeys key_mid(key_first + n_block_a);

  RandIt buffer = first - l_block;
  RandIt first1 = first;
  RandIt last1 = first1 + l_irreg1;
  RandIt first2 = last1;
  RandIt const irreg2 = first2 + n_block_left * l_block;
  bool is_range1_A = true;

  RandItKeys key_range2(key_first);

  ////////////////////////////////////////////////////////////////////////////
  // Process all regular blocks before the irregular B block
  ////////////////////////////////////////////////////////////////////////////
  size_type min_check = n_block_a == n_block_left ? 0u : n_block_a;
  size_type max_check =
      min_value<size_type>(min_check + size_type(1), n_block_left);
  for (; n_block_left; --n_block_left, ++key_range2,
                       min_check -= min_check != 0,
                       max_check -= max_check != 0) {
    size_type const next_key_idx = find_next_block(
        key_range2, key_comp, first2, l_block, min_check, max_check, comp);
    max_check = min_value<size_type>(
        max_value<size_type>(max_check, next_key_idx + size_type(2)),
        n_block_left);
    RandIt const first_min = first2 + next_key_idx * l_block;
    RandIt const last_min = first_min + l_block;
    (void)last_min;
    RandIt const last2 = first2 + l_block;

    BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
        boost::movelib::is_sorted(first1, last1, comp));
    BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
        boost::movelib::is_sorted(first2, last2, comp));
    BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
        !n_block_left || boost::movelib::is_sorted(first_min, last_min, comp));

    // Check if irregular b block should go here.
    // If so, break to the special code handling the irregular block
    if (!n_block_b_left && ((l_irreg2 && comp(*irreg2, *first_min)) ||
                            (!l_irreg2 && is_range1_A))) {
      break;
    }

    RandItKeys const key_next(key_range2 + next_key_idx);
    bool const is_range2_A =
        key_mid == (key_first + key_count) || key_comp(*key_next, *key_mid);

    bool const is_buffer_middle = last1 == buffer;
    BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
        (is_buffer_middle && size_type(first2 - buffer) == l_block &&
         buffer == last1) ||
        (!is_buffer_middle && size_type(first1 - buffer) == l_block &&
         first2 == last1));

    if (is_range1_A == is_range2_A) {
      BOOST_ASSERT((first1 == last1) || !comp(*first_min, last1[-1]));
      if (!is_buffer_middle) {
        buffer = op(forward_t(), first1, last1, buffer);
      }
      swap_and_update_key(key_next, key_range2, key_mid, first2, last2,
                          first_min);
      first1 = first2;
      last1 = last2;
    } else {
      RandIt unmerged;
      RandIt buf_beg;
      RandIt buf_end;
      if (is_buffer_middle) {
        buf_end = buf_beg = first2 - (last1 - first1);
        unmerged =
            op_partial_merge_and_save(first1, last1, first2, last2, first_min,
                                      buf_beg, buf_end, comp, op, is_range1_A);
      } else {
        buf_beg = first1;
        buf_end = last1;
        unmerged = op_partial_merge_and_save(buffer, buffer + (last1 - first1),
                                             first2, last2, first_min, buf_beg,
                                             buf_end, comp, op, is_range1_A);
      }
      (void)unmerged;
      BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
          boost::movelib::is_sorted(first - l_block, unmerged, comp));

      swap_and_update_key(key_next, key_range2, key_mid, first2, last2,
                          last_min - size_type(last2 - first2));

      if (buf_beg != buf_end) { // range2 exhausted: is_buffer_middle for the
                                // next iteration
        first1 = buf_beg;
        last1 = buf_end;
        BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(buf_end == (last2 - l_block));
        buffer = last1;
      } else { // range1 exhausted: !is_buffer_middle for the next iteration
        first1 = first2;
        last1 = last2;
        buffer = first2 - l_block;
        is_range1_A = is_range2_A;
      }
    }
    BOOST_MOVE_ADAPTIVE_SORT_INVARIANT((is_range2_A && n_block_a_left) ||
                                       (!is_range2_A && n_block_b_left));
    is_range2_A ? --n_block_a_left : --n_block_b_left;
    first2 = last2;
  }

  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
      !n_block_b ||
      n_block_a == count_if_with(key_first, key_range2 + n_block_left, key_comp,
                                 *key_mid));
  BOOST_ASSERT(!n_block_b_left);

  ////////////////////////////////////////////////////////////////////////////
  // Process remaining range 1 left before the irregular B block
  ////////////////////////////////////////////////////////////////////////////
  bool const is_buffer_middle = last1 == buffer;
  RandIt first_irr2 = irreg2;
  RandIt const last_irr2 = first_irr2 + l_irreg2;
  if (l_irreg2 && is_range1_A) {
    if (is_buffer_middle) {
      first1 = skip_until_merge(first1, last1, *first_irr2, comp);
      // Even if we copy backward, no overlapping occurs so use forward copy
      // that can be faster specially with trivial types
      RandIt const new_first1 = first2 - (last1 - first1);
      op(forward_t(), first1, last1, new_first1);
      first1 = new_first1;
      last1 = first2;
      buffer = first1 - l_block;
    }
    buffer = op_partial_merge_impl(first1, last1, first_irr2, last_irr2, buffer,
                                   comp, op);
    buffer = op(forward_t(), first1, last1, buffer);
  } else if (!is_buffer_middle) {
    buffer = op(forward_t(), first1, last1, buffer);
  }
  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
      boost::movelib::is_sorted(first - l_block, buffer, comp));

  ////////////////////////////////////////////////////////////////////////////
  // Process irregular B block and remaining A blocks
  ////////////////////////////////////////////////////////////////////////////
  buffer = op_merge_blocks_with_irreg(
      key_range2, key_mid, key_comp, first2, first_irr2, last_irr2, buffer,
      l_block, n_block_left, min_check, max_check, comp, false, op);
  buffer = op(forward_t(), first_irr2, last_irr2, buffer);
  (void)buffer;
  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
      boost::movelib::is_sorted(first - l_block, buffer, comp));
}

// first - first element to merge.
// first[-l_block, 0) - buffer (if use_buf == true)
// l_block - length of regular blocks. First nblocks are stable sorted by 1st
// elements and key-coded keys - sequence of keys, in same order as blocks.
// key<midkey means stream A n_bef_irreg2/n_aft_irreg2 are regular blocks
// l_irreg2 is a irregular block, that is to be combined after n_bef_irreg2
// blocks and before n_aft_irreg2 blocks If l_irreg2==0 then n_aft_irreg2==0 (no
// irregular blocks).
template <class RandItKeys, class KeyCompare, class RandIt, class Compare>
void merge_blocks_left(
    RandItKeys const key_first, KeyCompare key_comp, RandIt const first,
    typename iterator_traits<RandIt>::size_type const l_block,
    typename iterator_traits<RandIt>::size_type const l_irreg1,
    typename iterator_traits<RandIt>::size_type const n_block_a,
    typename iterator_traits<RandIt>::size_type const n_block_b,
    typename iterator_traits<RandIt>::size_type const l_irreg2, Compare comp,
    bool const xbuf_used) {
  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
      !n_block_b ||
      n_block_a ==
          count_if_with(key_first,
                        key_first + needed_keys_count(n_block_a, n_block_b),
                        key_comp, key_first[n_block_a]));
  if (xbuf_used) {
    op_merge_blocks_left(key_first, key_comp, first, l_block, l_irreg1,
                         n_block_a, n_block_b, l_irreg2, comp, move_op());
  } else {
    op_merge_blocks_left(key_first, key_comp, first, l_block, l_irreg1,
                         n_block_a, n_block_b, l_irreg2, comp, swap_op());
  }
}

// first - first element to merge.
// [first+l_block*(n_bef_irreg2+n_aft_irreg2)+l_irreg2,
// first+l_block*(n_bef_irreg2+n_aft_irreg2+1)+l_irreg2) - buffer l_block -
// length of regular blocks. First nblocks are stable sorted by 1st elements and
// key-coded keys - sequence of keys, in same order as blocks. key<midkey means
// stream A n_bef_irreg2/n_aft_irreg2 are regular blocks l_irreg2 is a irregular
// block, that is to be combined after n_bef_irreg2 blocks and before
// n_aft_irreg2 blocks If l_irreg2==0 then n_aft_irreg2==0 (no irregular
// blocks).
template <class RandItKeys, class KeyCompare, class RandIt, class Compare>
void merge_blocks_right(
    RandItKeys const key_first, KeyCompare key_comp, RandIt const first,
    typename iterator_traits<RandIt>::size_type const l_block,
    typename iterator_traits<RandIt>::size_type const n_block_a,
    typename iterator_traits<RandIt>::size_type const n_block_b,
    typename iterator_traits<RandIt>::size_type const l_irreg2, Compare comp,
    bool const xbuf_used) {
  merge_blocks_left((make_reverse_iterator)(
                        key_first + needed_keys_count(n_block_a, n_block_b)),
                    inverse<KeyCompare>(key_comp),
                    (make_reverse_iterator)(
                        first + ((n_block_a + n_block_b) * l_block + l_irreg2)),
                    l_block, l_irreg2, n_block_b, n_block_a, 0,
                    inverse<Compare>(comp), xbuf_used);
}

//////////////////////////////////
//////////////////////////////////
//////////////////////////////////
//
//    op_merge_blocks_with_buf
//
//////////////////////////////////
//////////////////////////////////
//////////////////////////////////
template <class RandItKeys, class KeyCompare, class RandIt, class Compare,
          class Op, class RandItBuf>
void op_merge_blocks_with_buf(
    RandItKeys key_first, KeyCompare key_comp, RandIt const first,
    typename iterator_traits<RandIt>::size_type const l_block,
    typename iterator_traits<RandIt>::size_type const l_irreg1,
    typename iterator_traits<RandIt>::size_type const n_block_a,
    typename iterator_traits<RandIt>::size_type const n_block_b,
    typename iterator_traits<RandIt>::size_type const l_irreg2, Compare comp,
    Op op, RandItBuf const buf_first) {
  typedef typename iterator_traits<RandIt>::size_type size_type;
  size_type const key_count = needed_keys_count(n_block_a, n_block_b);
  (void)key_count;
  // BOOST_ASSERT(n_block_a || n_block_b);
  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(boost::movelib::is_sorted_and_unique(
      key_first, key_first + key_count, key_comp));
  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
      !n_block_b || n_block_a == count_if_with(key_first, key_first + key_count,
                                               key_comp, key_first[n_block_a]));

  size_type n_block_b_left = n_block_b;
  size_type n_block_a_left = n_block_a;
  size_type n_block_left = n_block_b + n_block_a;
  RandItKeys key_mid(key_first + n_block_a);

  RandItBuf buffer = buf_first;
  RandItBuf buffer_end = buffer;
  RandIt first1 = first;
  RandIt last1 = first1 + l_irreg1;
  RandIt first2 = last1;
  RandIt const first_irr2 = first2 + n_block_left * l_block;
  bool is_range1_A = true;
  const size_type len =
      l_block * n_block_a + l_block * n_block_b + l_irreg1 + l_irreg2;
  (void)len;

  RandItKeys key_range2(key_first);

  ////////////////////////////////////////////////////////////////////////////
  // Process all regular blocks before the irregular B block
  ////////////////////////////////////////////////////////////////////////////
  size_type min_check = n_block_a == n_block_left ? 0u : n_block_a;
  size_type max_check =
      min_value<size_type>(min_check + size_type(1), n_block_left);
  for (; n_block_left; --n_block_left, ++key_range2,
                       min_check -= min_check != 0,
                       max_check -= max_check != 0) {
    size_type const next_key_idx = find_next_block(
        key_range2, key_comp, first2, l_block, min_check, max_check, comp);
    max_check = min_value<size_type>(
        max_value<size_type>(max_check, next_key_idx + size_type(2)),
        n_block_left);
    RandIt first_min = first2 + next_key_idx * l_block;
    RandIt const last_min = first_min + l_block;
    (void)last_min;
    RandIt const last2 = first2 + l_block;

    bool const buffer_empty = buffer == buffer_end;
    (void)buffer_empty;
    BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
        buffer_empty ? boost::movelib::is_sorted(first1, last1, comp)
                     : boost::movelib::is_sorted(buffer, buffer_end, comp));
    BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
        boost::movelib::is_sorted(first2, last2, comp));
    BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
        !n_block_left || boost::movelib::is_sorted(first_min, last_min, comp));

    // Check if irregular b block should go here.
    // If so, break to the special code handling the irregular block
    if (!n_block_b_left && ((l_irreg2 && comp(*first_irr2, *first_min)) ||
                            (!l_irreg2 && is_range1_A))) {
      break;
    }

    RandItKeys const key_next(key_range2 + next_key_idx);
    bool const is_range2_A =
        key_mid == (key_first + key_count) || key_comp(*key_next, *key_mid);

    if (is_range1_A == is_range2_A) {
      BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
          (first1 == last1) ||
          (buffer_empty ? !comp(*first_min, last1[-1])
                        : !comp(*first_min, buffer_end[-1])));
      // If buffered, put those elements in place
      RandIt res = op(forward_t(), buffer, buffer_end, first1);
      BOOST_MOVE_ADAPTIVE_SORT_PRINT_L2("   merge_blocks_w_fwd: ", len);
      buffer = buffer_end = buf_first;
      BOOST_ASSERT(buffer_empty || res == last1);
      (void)res;
      // swap_and_update_key(key_next, key_range2, key_mid, first2, last2,
      // first_min);
      buffer_end =
          buffer_and_update_key(key_next, key_range2, key_mid, first2, last2,
                                first_min, buffer = buf_first, op);
      BOOST_MOVE_ADAPTIVE_SORT_PRINT_L2("   merge_blocks_w_swp: ", len);
      BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
          boost::movelib::is_sorted(first2, last2, comp));
      BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
          boost::movelib::is_sorted(first_min, last_min, comp));
      first1 = first2;
      BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
          boost::movelib::is_sorted(first, first1, comp));
    } else {
      RandIt const unmerged =
          op_partial_merge_and_save(first1, last1, first2, last2, first_min,
                                    buffer, buffer_end, comp, op, is_range1_A);
      BOOST_MOVE_ADAPTIVE_SORT_PRINT_L2("   merge_blocks_w_mrs: ", len);
      bool const is_range_1_empty = buffer == buffer_end;
      BOOST_ASSERT(is_range_1_empty ||
                   (buffer_end - buffer) == (last1 + l_block - unmerged));
      if (is_range_1_empty) {
        buffer = buffer_end = buf_first;
        first_min = last_min - (last2 - first2);
        // swap_and_update_key(key_next, key_range2, key_mid, first2, last2,
        // first_min);
        buffer_end =
            buffer_and_update_key(key_next, key_range2, key_mid, first2, last2,
                                  first_min, buf_first, op);
      } else {
        first_min = last_min;
        // swap_and_update_key(key_next, key_range2, key_mid, first2, last2,
        // first_min);
        update_key(key_next, key_range2, key_mid);
      }
      BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
          !is_range_1_empty || (last_min - first_min) == (last2 - unmerged));
      BOOST_MOVE_ADAPTIVE_SORT_PRINT_L2("   merge_blocks_w_swp: ", len);
      BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
          boost::movelib::is_sorted(first_min, last_min, comp));
      is_range1_A ^= is_range_1_empty;
      first1 = unmerged;
      BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
          boost::movelib::is_sorted(first, unmerged, comp));
    }
    BOOST_ASSERT((is_range2_A && n_block_a_left) ||
                 (!is_range2_A && n_block_b_left));
    is_range2_A ? --n_block_a_left : --n_block_b_left;
    last1 += l_block;
    first2 = last2;
  }
  RandIt res = op(forward_t(), buffer, buffer_end, first1);
  (void)res;
  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
      boost::movelib::is_sorted(first, res, comp));
  BOOST_MOVE_ADAPTIVE_SORT_PRINT_L2("   merge_blocks_w_fwd: ", len);

  ////////////////////////////////////////////////////////////////////////////
  // Process irregular B block and remaining A blocks
  ////////////////////////////////////////////////////////////////////////////
  RandIt const last_irr2 = first_irr2 + l_irreg2;
  op(forward_t(), first_irr2, first_irr2 + l_irreg2, buf_first);
  BOOST_MOVE_ADAPTIVE_SORT_PRINT_L2("   merge_blocks_w_fwir:", len);
  buffer = buf_first;
  buffer_end = buffer + l_irreg2;

  reverse_iterator<RandItBuf> rbuf_beg(buffer_end);
  RandIt dest =
      op_merge_blocks_with_irreg(
          (make_reverse_iterator)(key_first + n_block_b + n_block_a),
          (make_reverse_iterator)(key_mid), inverse<KeyCompare>(key_comp),
          (make_reverse_iterator)(first_irr2), rbuf_beg,
          (make_reverse_iterator)(buffer), (make_reverse_iterator)(last_irr2),
          l_block, n_block_left, 0, n_block_left, inverse<Compare>(comp), true,
          op)
          .base();
  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
      boost::movelib::is_sorted(dest, last_irr2, comp));
  BOOST_MOVE_ADAPTIVE_SORT_PRINT_L2("   merge_blocks_w_irg: ", len);

  buffer_end = rbuf_beg.base();
  BOOST_ASSERT((dest - last1) == (buffer_end - buffer));
  op_merge_with_left_placed(is_range1_A ? first1 : last1, last1, dest, buffer,
                            buffer_end, comp, op);
  BOOST_MOVE_ADAPTIVE_SORT_PRINT_L2("   merge_with_left_plc:", len);
  BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
      boost::movelib::is_sorted(first, last_irr2, comp));
}

//////////////////////////////////
//////////////////////////////////
//////////////////////////////////
//
//  op_insertion_sort_step_left/right
//
//////////////////////////////////
//////////////////////////////////
//////////////////////////////////

template <class RandIt, class Compare, class Op>
typename iterator_traits<RandIt>::size_type op_insertion_sort_step_left(
    RandIt const first,
    typename iterator_traits<RandIt>::size_type const length,
    typename iterator_traits<RandIt>::size_type const step, Compare comp,
    Op op) {
  typedef typename iterator_traits<RandIt>::size_type size_type;
  size_type const s =
      min_value<size_type>(step, AdaptiveSortInsertionSortThreshold);
  size_type m = 0;

  while ((length - m) > s) {
    insertion_sort_op(first + m, first + m + s, first + m - s, comp, op);
    m += s;
  }
  insertion_sort_op(first + m, first + length, first + m - s, comp, op);
  return s;
}

template <class RandIt, class Compare, class Op>
void op_merge_right_step_once(
    RandIt first_block,
    typename iterator_traits<RandIt>::size_type const elements_in_blocks,
    typename iterator_traits<RandIt>::size_type const l_build_buf, Compare comp,
    Op op) {
  typedef typename iterator_traits<RandIt>::size_type size_type;
  size_type restk = elements_in_blocks % (2 * l_build_buf);
  size_type p = elements_in_blocks - restk;
  BOOST_ASSERT(0 == (p % (2 * l_build_buf)));

  if (restk <= l_build_buf) {
    op(backward_t(), first_block + p, first_block + p + restk,
       first_block + p + restk + l_build_buf);
  } else {
    op_merge_right(first_block + p, first_block + p + l_build_buf,
                   first_block + p + restk,
                   first_block + p + restk + l_build_buf, comp, op);
  }
  while (p > 0) {
    p -= 2 * l_build_buf;
    op_merge_right(first_block + p, first_block + p + l_build_buf,
                   first_block + p + 2 * l_build_buf,
                   first_block + p + 3 * l_build_buf, comp, op);
  }
}

//////////////////////////////////
//////////////////////////////////
//////////////////////////////////
//
//    insertion_sort_step
//
//////////////////////////////////
//////////////////////////////////
//////////////////////////////////
template <class RandIt, class Compare>
typename iterator_traits<RandIt>::size_type
insertion_sort_step(RandIt const first,
                    typename iterator_traits<RandIt>::size_type const length,
                    typename iterator_traits<RandIt>::size_type const step,
                    Compare comp) {
  typedef typename iterator_traits<RandIt>::size_type size_type;
  size_type const s =
      min_value<size_type>(step, AdaptiveSortInsertionSortThreshold);
  size_type m = 0;

  while ((length - m) > s) {
    insertion_sort(first + m, first + m + s, comp);
    m += s;
  }
  insertion_sort(first + m, first + length, comp);
  return s;
}

//////////////////////////////////
//////////////////////////////////
//////////////////////////////////
//
//    op_merge_left_step_multiple
//
//////////////////////////////////
//////////////////////////////////
//////////////////////////////////
template <class RandIt, class Compare, class Op>
typename iterator_traits<RandIt>::size_type op_merge_left_step_multiple(
    RandIt first_block,
    typename iterator_traits<RandIt>::size_type const elements_in_blocks,
    typename iterator_traits<RandIt>::size_type l_merged,
    typename iterator_traits<RandIt>::size_type const l_build_buf,
    typename iterator_traits<RandIt>::size_type l_left_space, Compare comp,
    Op op) {
  typedef typename iterator_traits<RandIt>::size_type size_type;
  for (; l_merged < l_build_buf && l_left_space >= l_merged; l_merged *= 2) {
    size_type p0 = 0;
    RandIt pos = first_block;
    while ((elements_in_blocks - p0) > 2 * l_merged) {
      op_merge_left(pos - l_merged, pos, pos + l_merged, pos + 2 * l_merged,
                    comp, op);
      BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(
          boost::movelib::is_sorted(pos - l_merged, pos + l_merged, comp));
      p0 += 2 * l_merged;
      pos = first_block + p0;
    }
    if ((elements_in_blocks - p0) > l_merged) {
      op_merge_left(pos - l_merged, pos, pos + l_merged,
                    first_block + elements_in_blocks, comp, op);
      BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(boost::movelib::is_sorted(
          pos - l_merged,
          pos - l_merged + (first_block + elements_in_blocks - pos), comp));
    } else {
      op(forward_t(), pos, first_block + elements_in_blocks, pos - l_merged);
      BOOST_MOVE_ADAPTIVE_SORT_INVARIANT(boost::movelib::is_sorted(
          pos - l_merged, first_block + elements_in_blocks - l_merged, comp));
    }
    first_block -= l_merged;
    l_left_space -= l_merged;
  }
  return l_merged;
}

} // namespace detail_adaptive
} // namespace movelib
} // namespace boost

#include <boost/move/detail/config_end.hpp>

#endif //#define BOOST_MOVE_ADAPTIVE_SORT_MERGE_HPP


// Andrew Naplavkov

#ifndef BRIG_OSM_DETAIL_TILES_HPP
#define BRIG_OSM_DETAIL_TILES_HPP

#include <boost/geometry/geometry.hpp>
#include <brig/boost/geometry.hpp>
#include <brig/osm/detail/tile.hpp>

namespace brig { namespace osm { namespace detail {

class tiles {
  const int m_zoom;
  const boost::box m_env;
  tile m_next;
  bool find_in(const tile& tl);
public:
  tiles(int zoom, const boost::box& env);
  bool fetch(tile& tl);
}; // tiles

inline bool tiles::find_in(const tile& tl)
{
  if (!tl.is_valid() || tl.z > m_zoom || !::boost::geometry::intersects(m_env, tl.get_box()))
    return false;

  if (tl.z < m_zoom)
  {
    for (int i(0); i < 2; ++i)
      for (int j(0); j < 2; ++j)
      {
        tile child(2*tl.x + i, 2*tl.y + j, tl.z + 1);
        if (find_in(child))
          return true;
      }
    return false;
  }

  m_next = tl;
  return true;
}

inline tiles::tiles(int zoom, const boost::box& env)
  : m_zoom(zoom), m_env(env), m_next(0, 0, -1)
{
  find_in(tile(0, 0, 0));
}

inline bool tiles::fetch(tile& tl)
{
  if (!m_next.is_valid())
    return false;
  tl = m_next;

  while (m_next.is_valid())
  {
    const tile parent(m_next.x / 2, m_next.y /2, m_next.z - 1);
    if (parent.is_valid())
      for (int i(0); i < 2; ++i)
        for (int j(0); j < 2; ++j)
        {
          tile child(2*parent.x + i, 2*parent.y + j, parent.z + 1);
          if (((m_next.x < child.x) || (m_next.x == child.x && m_next.y < child.y)) && find_in(child))
            return true;
        }
    m_next = parent;
  }
  return true;
} // tiles::

} } } // brig::osm::detail

#endif // BRIG_OSM_DETAIL_TILES_HPP


//  (C) Copyright 2015 - 2018 Christopher Beck

//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef SPIRIT_PO_DEFAULT_PLURAL_FORMS_EXPRESSIONS_HPP_INCLUDED
#define SPIRIT_PO_DEFAULT_PLURAL_FORMS_EXPRESSIONS_HPP_INCLUDED

/***
 * The namespace default_plural_forms contains all the details to implement
 * the subset of the C grammar used by standard GNU gettext po headers.
 *
 * Boolean expressions return uint 0 or 1.
 *
 * The 'compiler' is a spirit grammar which parses a string into an expression
 * object. The expressions are evaluated by a simple stack machine.
 */

#ifndef BOOST_SPIRIT_USE_PHOENIX_V3
#define BOOST_SPIRIT_USE_PHOENIX_V3
#endif

#include <algorithm>
#include <vector>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/variant/variant.hpp>
#include <boost/variant/recursive_wrapper.hpp>

#ifdef SPIRIT_PO_DEBUG
#include <cassert>
#include <string>
#endif

namespace spirit_po {

namespace qi = boost::spirit::qi;
typedef unsigned int uint;

namespace default_plural_forms {

// X Macro for repetitive binary ops declarations

#define FOREACH_SPIRIT_PO_BINARY_OP(X_) \
 X_(eq_op, ==) X_(neq_op, !=) X_(ge_op, >=) X_(le_op, <=) X_(gt_op, >) X_(lt_op, <) X_(mod_op, %)

// && and || are treated slightly differently from other binary ops

#define FOREACH_SPIRIT_PO_CONJUNCTION(X_) \
 X_(and_op, &&) X_(or_op, ||)

/***
 * Declare / forward declare expr struct types
 */

struct constant { uint value; };
struct n_var { n_var() = default; explicit n_var(char) {}}; // work around a quirk in spirit
struct not_op;
struct ternary_op;

#define FWD_DECL_(name, op) \
struct name ; \

FOREACH_SPIRIT_PO_BINARY_OP(FWD_DECL_)
FOREACH_SPIRIT_PO_CONJUNCTION(FWD_DECL_)

#undef FWD_DECL_

/***
 * Define expr variant type
 */

#define WRAP_(name, op) boost::recursive_wrapper< name >, \

typedef boost::variant<constant, n_var, boost::recursive_wrapper<not_op>,
FOREACH_SPIRIT_PO_BINARY_OP(WRAP_)
FOREACH_SPIRIT_PO_CONJUNCTION(WRAP_)
boost::recursive_wrapper<ternary_op>> expr;

#undef WRAP_

/***
 * Define structs
 */

struct not_op { expr e1; };
struct ternary_op { expr e1, e2, e3; };

#define DECL_(name, op) \
struct name { expr e1, e2; }; \

FOREACH_SPIRIT_PO_BINARY_OP(DECL_)
FOREACH_SPIRIT_PO_CONJUNCTION(DECL_)

#undef DECL_

/***
 * Visitor that naively evaluates expressions
 */
struct evaluator : public boost::static_visitor<uint> {
  uint n_value_;
  explicit evaluator(uint n) : n_value_(n) {}

  uint operator()(const constant & c) const { return c.value; }
  uint operator()(n_var) const { return n_value_; }
  uint operator()(const not_op & op) const { return !boost::apply_visitor(*this, op.e1); }

#define EVAL_OP_(name, OPERATOR) \
  uint operator()(const name & op) const { return (boost::apply_visitor(*this, op.e1)) OPERATOR (boost::apply_visitor(*this, op.e2)); } \

FOREACH_SPIRIT_PO_BINARY_OP(EVAL_OP_)
FOREACH_SPIRIT_PO_CONJUNCTION(EVAL_OP_)
#undef EVAL_OP_

  uint operator()(const ternary_op & op) const { return boost::apply_visitor(*this, op.e1) ? boost::apply_visitor(*this, op.e2) : boost::apply_visitor(*this, op.e3); }
};

} // end namespace default_plural_forms

} // end namespace spirit_po

/***
 * Adapt structs for fusion / qi
 */

BOOST_FUSION_ADAPT_STRUCT(spirit_po::default_plural_forms::constant,
  (unsigned int, value))
BOOST_FUSION_ADAPT_STRUCT(spirit_po::default_plural_forms::not_op,
  (spirit_po::default_plural_forms::expr, e1))
BOOST_FUSION_ADAPT_STRUCT(spirit_po::default_plural_forms::ternary_op,
  (spirit_po::default_plural_forms::expr, e1)
  (spirit_po::default_plural_forms::expr, e2)
  (spirit_po::default_plural_forms::expr, e3))

#define ADAPT_STRUCT_(name, op) \
BOOST_FUSION_ADAPT_STRUCT(spirit_po::default_plural_forms:: name, \
  (spirit_po::default_plural_forms::expr, e1) \
  (spirit_po::default_plural_forms::expr, e2)) \

FOREACH_SPIRIT_PO_BINARY_OP(ADAPT_STRUCT_)
FOREACH_SPIRIT_PO_CONJUNCTION(ADAPT_STRUCT_)

#undef ADAPT_STRUCT_

namespace spirit_po {

namespace default_plural_forms {

/***
 * Pseudo-C Grammar
 *
 * Note that the grammar has been somewhat optimized by using local variables
 * and inherited attributes, in order to avoid exponential backtracking overhead.
 * This makes it a little harder to read than if we got rid of all local variables,
 * but then it is too slow to parse the expressions for certain languages.
 *
 * The main idea is that instead of parsing things like
 *
 *   BINARY_OP = LOWER_PRECENDENCE >> BINARY_OP_LITERAL >> CURRENT_PRECEDENCE
 *   CURRENT_PRECEDENCE = BINARY_OP | OTHER_OP | YET_ANOTHER_OP | LOWER_PRECEDENCE
 *
 * (which is bad because if the binary op literal is not there then we have to
 *  backtrack through an entire subexpression)
 *
 * we make BINARY_OP take the subexpression as a parameter, and in each
 * precedence level, we capture the subexpression first and store it in a local
 * variable, so that it does not get reparsed when we backtrack.
 *
 *   BINARY_OP = BINARY_OP_LITERAL >> qi::attr(parameter) >> CURRENT_PRECEDENCE
 *
 *   CURRENT_PRECEDENCE = LOWER_PRECEDENCE[local_var = result] >>
 *     (BINARY_OP(local_var) | OTHER_OP(local_var) | YET_ANOTHER_OP(local_var) | qi::attr(local_var)
 *
 */

template <typename Iterator>
struct op_grammar : qi::grammar<Iterator, expr(), qi::space_type> {
  qi::rule<Iterator, constant(), qi::space_type> constant_;
  qi::rule<Iterator, n_var(), qi::space_type> n_;
  qi::rule<Iterator, not_op(), qi::space_type> not_;
  qi::rule<Iterator, and_op(expr), qi::space_type> and_;
  qi::rule<Iterator, or_op(expr), qi::space_type> or_;
  qi::rule<Iterator, eq_op(expr), qi::space_type> eq_;
  qi::rule<Iterator, neq_op(expr), qi::space_type> neq_;
  qi::rule<Iterator, ge_op(expr), qi::space_type> ge_;
  qi::rule<Iterator, le_op(expr), qi::space_type> le_;
  qi::rule<Iterator, gt_op(expr), qi::space_type> gt_;
  qi::rule<Iterator, lt_op(expr), qi::space_type> lt_;
  qi::rule<Iterator, mod_op(expr), qi::space_type> mod_;
  qi::rule<Iterator, ternary_op(expr), qi::space_type> ternary_;
  qi::rule<Iterator, expr(), qi::space_type> paren_expr_;

  // expression precedence levels
  qi::rule<Iterator, expr(), qi::space_type, qi::locals<expr>> ternary_level_;
  qi::rule<Iterator, expr(), qi::space_type, qi::locals<expr>> or_level_;
  qi::rule<Iterator, expr(), qi::space_type, qi::locals<expr>> and_level_;
  qi::rule<Iterator, expr(), qi::space_type, qi::locals<expr>> eq_level_;
  qi::rule<Iterator, expr(), qi::space_type, qi::locals<expr>> rel_level_;
  qi::rule<Iterator, expr(), qi::space_type, qi::locals<expr>> mod_level_;
  qi::rule<Iterator, expr(), qi::space_type> atom_level_;
  qi::rule<Iterator, expr(), qi::space_type> expr_;

  // handle optional ';' at end
  qi::rule<Iterator, expr(), qi::space_type> main_;

  op_grammar() : op_grammar::base_type(main_) {
    using qi::attr;
    using qi::lit;

    constant_ = qi::uint_;
    n_ = qi::char_('n');
    paren_expr_ = lit('(') >> expr_ >> lit(')');
    not_ = lit('!') >> atom_level_;
    atom_level_ = paren_expr_ | not_ | n_ | constant_;

    mod_ = lit('%') >> attr(qi::_r1) >> atom_level_;
    mod_level_ = qi::omit[atom_level_[qi::_a = qi::_1]] >> (mod_(qi::_a) | attr(qi::_a));

    ge_ = lit(">=") >> attr(qi::_r1) >> mod_level_;
    le_ = lit("<=") >> attr(qi::_r1) >> mod_level_;
    gt_ = lit('>') >> attr(qi::_r1) >> mod_level_;
    lt_ = lit('<') >> attr(qi::_r1) >> mod_level_;
    rel_level_ = qi::omit[mod_level_[qi::_a = qi::_1]] >> (ge_(qi::_a) | le_(qi::_a) | gt_(qi::_a) | lt_(qi::_a) | attr(qi::_a));

    eq_ = lit("==") >> attr(qi::_r1) >> rel_level_;
    neq_ = lit("!=") >> attr(qi::_r1) >> rel_level_;
    eq_level_ = qi::omit[rel_level_[qi::_a = qi::_1]] >> (eq_(qi::_a) | neq_(qi::_a) | attr(qi::_a));

    and_ = lit("&&") >> attr(qi::_r1) >> and_level_;
    and_level_ = qi::omit[eq_level_[qi::_a = qi::_1]] >> (and_(qi::_a) | attr(qi::_a));

    or_ = lit("||") >> attr(qi::_r1) >> or_level_;
    or_level_ = qi::omit[and_level_[qi::_a = qi::_1]] >> (or_(qi::_a) | attr(qi::_a));

    ternary_ = lit('?') >> attr(qi::_r1) >> ternary_level_ >> lit(':') >> ternary_level_;
    ternary_level_ = qi::omit[or_level_[qi::_a = qi::_1]] >> (ternary_(qi::_a) | attr(qi::_a));

    expr_ = ternary_level_;

    main_ = expr_ >> -lit(';');
  }
};

/***
 * Now define a simple stack machine to evaluate the expressions efficiently.
 *
 * First define op_codes
 */

#define ENUMERATE(X, Y) X,

enum class op_code { n_var, FOREACH_SPIRIT_PO_BINARY_OP(ENUMERATE) not_op };

#undef ENUMERATE

/// Instruction that causes us to skip upcoming instructions
struct skip {
  uint distance;
};

/// Instructions that conditionally cause us to skip upcoming instructions
struct skip_if {
  uint distance;
};

struct skip_if_not {
  uint distance;
};

/***
 * Instruction is a variant type that represents either a push_constant, branch, jump, or arithmetic op.
 */
typedef boost::variant<constant, skip, skip_if, skip_if_not, op_code> instruction;

/***
 * Debug strings for instruction set
 */
#ifdef SPIRIT_PO_DEBUG
inline std::string op_code_string(op_code oc) {
  std::string result = "[  ";
  switch (oc) {
    case op_code::n_var: {
      result += "n ";
      break;
    }
    case op_code::not_op: {
      result += "! ";
      break;
    }
#define OP_CODE_STR_CASE_(X, Y)                  \
    case op_code::X: {                           \
      result += #Y;                              \
      break;                                     \
    }

FOREACH_SPIRIT_PO_BINARY_OP(OP_CODE_STR_CASE_)

#undef OP_CODE_STR_CASE_
  }

  if (result.size() < 5) { result += ' '; } \
  result += "  :   ]";
  return result;
}

struct instruction_debug_string_maker : boost::static_visitor<std::string> {
  std::string operator()(const constant & c) const {
    return "[ push : " + std::to_string(c.value) + " ]";
  }
  std::string operator()(const skip & s) const {
    return "[ skip : " + std::to_string(s.distance) + " ]";
  }
  std::string operator()(const skip_if & s) const {
    return "[ sif  : " + std::to_string(s.distance) + " ]";
  }
  std::string operator()(const skip_if_not & s) const {
    return "[ sifn : " + std::to_string(s.distance) + " ]";
  }
  std::string operator()(const op_code & oc) const {
    return op_code_string(oc);
  }
};

inline std::string debug_string(const instruction & i) {
  return boost::apply_visitor(instruction_debug_string_maker{}, i);
}

#endif // SPIRIT_PO_DEBUG

/***
 * Helper: Check if an expression obviously is zero-one valued
 */
struct is_boolean : public boost::static_visitor<bool> {
  bool operator()(const and_op &) const { return true; }
  bool operator()(const or_op &) const { return true; }
  bool operator()(const not_op &) const { return true; }
  bool operator()(const eq_op &) const { return true; }
  bool operator()(const neq_op &) const { return true; }
  bool operator()(const ge_op &) const { return true; }
  bool operator()(const le_op &) const { return true; }
  bool operator()(const gt_op &) const { return true; }
  bool operator()(const lt_op &) const { return true; }
  bool operator()(const n_var &) const { return false; }
  bool operator()(const constant & c) const { return (c.value == 0 || c.value == 1); }
  bool operator()(const mod_op & m) const { return boost::apply_visitor(*this, m.e1); }
  bool operator()(const ternary_op & t) const { return boost::apply_visitor(*this, t.e2) && boost::apply_visitor(*this, t.e3); }
};

/***
 * Visitor that maps expressions to instruction sequences
 */
struct emitter : public boost::static_visitor<std::vector<instruction>> {
  std::vector<instruction> operator()(const constant & c) const {
    return std::vector<instruction>{instruction{c}};
  }
  std::vector<instruction> operator()(const n_var &) const {
    return std::vector<instruction>{instruction{op_code::n_var}};
  }
  std::vector<instruction> operator()(const not_op & o) const {
    auto result = boost::apply_visitor(*this, o.e1);
    result.emplace_back(op_code::not_op);
    return result;
  }
#define EMIT_OP_(name, op) \
  std::vector<instruction> operator()(const name & o) const {        \
    auto result = boost::apply_visitor(*this, o.e1);                 \
    auto temp = boost::apply_visitor(*this, o.e2);                   \
    std::move(temp.begin(), temp.end(), std::back_inserter(result)); \
    result.emplace_back(op_code::name);                              \
    return result;                                                   \
  }

FOREACH_SPIRIT_PO_BINARY_OP(EMIT_OP_)

#undef EMIT_OP_

  /***
   * We make &&, ||, and ? shortcut
   */
  std::vector<instruction> operator()(const and_op & o) const {
    auto result = boost::apply_visitor(*this, o.e1);
    auto second = boost::apply_visitor(*this, o.e2);
    bool second_is_boolean = boost::apply_visitor(is_boolean(), o.e2);

    uint sec_size = static_cast<uint>(second.size());
    if (!second_is_boolean) { sec_size += 2; }

    result.emplace_back(skip_if{2});
    result.emplace_back(constant{0});
    result.emplace_back(skip{sec_size});

    std::move(second.begin(), second.end(), std::back_inserter(result));
    if (!second_is_boolean) {
      result.emplace_back(op_code::not_op);
      result.emplace_back(op_code::not_op);
    }

    return result;
  }

  std::vector<instruction> operator()(const or_op & o) const {
    auto result = boost::apply_visitor(*this, o.e1);
    auto second = boost::apply_visitor(*this, o.e2);
    bool second_is_boolean = boost::apply_visitor(is_boolean(), o.e2);

    uint sec_size = static_cast<uint>(second.size());
    if (!second_is_boolean) { sec_size += 2; }

    result.emplace_back(skip_if_not{2});
    result.emplace_back(constant{1});
    result.emplace_back(skip{sec_size});

    std::move(second.begin(), second.end(), std::back_inserter(result));
    if (!second_is_boolean) {
      result.emplace_back(op_code::not_op);
      result.emplace_back(op_code::not_op);
    }

    return result;
  }

  std::vector<instruction> operator()(const ternary_op & o) const {
    auto result = boost::apply_visitor(*this, o.e1);
    auto tbranch = boost::apply_visitor(*this, o.e2);
    auto fbranch = boost::apply_visitor(*this, o.e3);

    uint tsize = static_cast<uint>(tbranch.size());
    uint fsize = static_cast<uint>(fbranch.size());

    // We use jump if / jump if not in the way that will let us put the shorter branch first.
    if (tbranch.size() > fbranch.size()) {
       // + 1 to size because we have to put a jump at end of this branch also
      result.emplace_back(skip_if{fsize + 1});
      std::move(fbranch.begin(), fbranch.end(), std::back_inserter(result));
      result.emplace_back(skip{tsize});
      std::move(tbranch.begin(), tbranch.end(), std::back_inserter(result));
    } else {
      result.emplace_back(skip_if_not{tsize + 1});
      std::move(tbranch.begin(), tbranch.end(), std::back_inserter(result));
      result.emplace_back(skip{fsize});
      std::move(fbranch.begin(), fbranch.end(), std::back_inserter(result));
    }
    return result;
  }
};

/***
 * Actual stack machine
 */

class stack_machine : public boost::static_visitor<uint> {
  std::vector<instruction> instruction_seq_;
  std::vector<uint> stack_;
  uint n_value_;

#ifdef SPIRIT_PO_DEBUG
public:
  void debug_print_instructions() const {
    std::cerr << "Instruction sequence:\n";
    for (const auto & i : instruction_seq_) {
      std::cerr << debug_string(i) << std::endl;
    }
  }
private:

#define MACHINE_ASSERT(X)                       \
  do {                                          \
    if (!(X)) {                                 \
      std::cerr << "Stack machine failure:\n";  \
      debug_print_instructions();               \
      assert(false && #X);                      \
    }                                           \
  } while(0)

#else // SPIRIT_PO_DEBUG

#define MACHINE_ASSERT(...)  do {} while(0)

#endif // SPIRIT_PO_DEBUG

  uint pop_one() {
    MACHINE_ASSERT(stack_.size());

    uint result = stack_.back();
    stack_.resize(stack_.size() - 1);
    return result;
  }

public:
  explicit stack_machine(const expr & e)
    : instruction_seq_(boost::apply_visitor(emitter(), e))
    , stack_()
    , n_value_()
  {}

  /***
   * operator() takes the instruction that we should execute
   * It should perform the operation adjusting the stack
   * It returns the amount by which we should increment the
   * program counter.
   */
  uint operator()(const constant & c) {
    stack_.emplace_back(c.value);
    return 1;
  }

  uint operator()(const skip & s) {
    return 1 + s.distance;
  }

  uint operator()(const skip_if & s) {
   return 1 + (pop_one() ? s.distance : 0);
  }

  uint operator()(const skip_if_not & s) {
   return 1 + (pop_one() ? 0 : s.distance);
  }

  uint operator()(op_code oc) {
    switch (oc) {
      case op_code::n_var: {
        stack_.emplace_back(n_value_);
        return 1;
      }
      case op_code::not_op: {
        MACHINE_ASSERT(stack_.size());
        stack_.back() = !stack_.back();
        return 1;
      }
#define STACK_MACHINE_CASE_(name, op)                                                                  \
      case op_code::name: {                                                                            \
        MACHINE_ASSERT(stack_.size() >= 2);                                                            \
        uint parm2 = pop_one();                                                                        \
                                                                                \
        if (op_code::name == op_code::mod_op) {                                                        \
          MACHINE_ASSERT(parm2 && "Division by zero when evaluating gettext plural form expression");  \
        }                                                                                \
                                                                                \
        stack_.back() = (stack_.back() op parm2);                                                      \
        return 1;                                                                                \
      }

FOREACH_SPIRIT_PO_BINARY_OP(STACK_MACHINE_CASE_)

#undef STACK_MACHINE_CASE_
    }
    MACHINE_ASSERT(false);
    return 1;
  }

  uint compute(uint arg) {
    n_value_ = arg;
    stack_.resize(0);
    uint pc = 0;
    while (pc < instruction_seq_.size()) {
      pc += boost::apply_visitor(*this, instruction_seq_[pc]);
    }
    MACHINE_ASSERT(pc == instruction_seq_.size());
    MACHINE_ASSERT(stack_.size() == 1);
    return stack_[0];
  }
};

#undef MACHINE_ASSERT

// X macros not used anymore
#undef FOREACH_SPIRIT_PO_BINARY_OP
#undef FOREACH_SPIRIT_PO_CONJUNCTION

} // end namespace default_plural_forms

} // end namespace spirit_po

#endif // SPIRIT_PO_DEFAULT_PLURAL_FORMS_EXPRESSIONS_HPP_INCLUDED


#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/console/parse.h>
#include <pcl/common/time.h>
#include <pcl/common/centroid.h>

#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h>

#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>

#include <pcl/search/pcl_search.h>
#include <pcl/common/transforms.h>

#include <boost/format.hpp>

#include <pcl/tracking/tracking.h>
#include <pcl/tracking/particle_filter.h>
#include <pcl/tracking/kld_adaptive_particle_filter_omp.h>
#include <pcl/tracking/particle_filter_omp.h>
#include <pcl/tracking/coherence.h>
#include <pcl/tracking/distance_coherence.h>
#include <pcl/tracking/hsv_color_coherence.h>
#include <pcl/tracking/approx_nearest_pair_point_cloud_coherence.h>
#include <pcl/tracking/nearest_pair_point_cloud_coherence.h>

using namespace pcl::tracking;

typedef pcl::PointXYZRGBA RefPointType;
typedef ParticleXYZRPY ParticleT;
typedef pcl::PointCloud<pcl::PointXYZRGBA> Cloud;
typedef Cloud::Ptr CloudPtr;
typedef Cloud::ConstPtr CloudConstPtr;
typedef ParticleFilterTracker<RefPointType, ParticleT> ParticleFilter;

CloudPtr cloud_pass_;
CloudPtr cloud_pass_downsampled_;
CloudPtr target_cloud;

boost::mutex mtx_;
boost::shared_ptr<ParticleFilter> tracker_;
bool new_cloud_;
double downsampling_grid_size_;
int counter;

//Filter along a specified dimension
void filterPassThrough (const CloudConstPtr &cloud, Cloud &result)
{
  pcl::PassThrough<pcl::PointXYZRGBA> pass;
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0.0, 10.0);
  pass.setKeepOrganized (false);
  pass.setInputCloud (cloud);
  pass.filter (result);
}

void gridSampleApprox (const CloudConstPtr &cloud, Cloud &result, double leaf_size)
{
  pcl::ApproximateVoxelGrid<pcl::PointXYZRGBA> grid;
  grid.setLeafSize (static_cast<float> (leaf_size), static_cast<float> (leaf_size), static_cast<float> (leaf_size));
  grid.setInputCloud (cloud);
  grid.filter (result);
}

//Draw the current particles
bool
drawParticles (pcl::visualization::PCLVisualizer& viz)
{
  ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles ();
  if (particles && new_cloud_)
    {
      //Set pointCloud with particle's points
      pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
      for (size_t i = 0; i < particles->points.size (); i++)
	{
	  pcl::PointXYZ point;

	  point.x = particles->points[i].x;
	  point.y = particles->points[i].y;
	  point.z = particles->points[i].z;
	  particle_cloud->points.push_back (point);
	}

      //Draw red particles
      {
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red_color (particle_cloud, 250, 99, 71);

	if (!viz.updatePointCloud (particle_cloud, red_color, "particle cloud"))
	  viz.addPointCloud (particle_cloud, red_color, "particle cloud");
      }
      return true;
    }
  else
    {
      return false;
    }
}

//Draw model reference point cloud
void
drawResult (pcl::visualization::PCLVisualizer& viz)
{
  ParticleXYZRPY result = tracker_->getResult ();
  Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);

  //move close to camera a little for better visualization
  transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
  CloudPtr result_cloud (new Cloud ());
  pcl::transformPointCloud<RefPointType> (*(tracker_->getReferenceCloud ()), *result_cloud, transformation);

  //Draw blue model reference point cloud
  {
    pcl::visualization::PointCloudColorHandlerCustom<RefPointType> blue_color (result_cloud, 0, 0, 255);

    if (!viz.updatePointCloud (result_cloud, blue_color, "resultcloud"))
      viz.addPointCloud (result_cloud, blue_color, "resultcloud");
  }
}

//visualization's callback function
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
  boost::mutex::scoped_lock lock (mtx_);

  if (!cloud_pass_)
    {
      boost::this_thread::sleep (boost::posix_time::seconds (1));
      return;
   }

  //Draw downsampled point cloud from sensor
  if (new_cloud_ && cloud_pass_downsampled_)
    {
      CloudPtr cloud_pass;
      cloud_pass = cloud_pass_downsampled_;

      if (!viz.updatePointCloud (cloud_pass, "cloudpass"))
	{
	  viz.addPointCloud (cloud_pass, "cloudpass");
	  viz.resetCameraViewpoint ("cloudpass");
	}
      bool ret = drawParticles (viz);
      if (ret)
        drawResult (viz);
    }
  new_cloud_ = false;
}

//OpenNI Grabber's cloud Callback function
void
cloud_cb (const CloudConstPtr &cloud)
{
  boost::mutex::scoped_lock lock (mtx_);
  cloud_pass_.reset (new Cloud);
  cloud_pass_downsampled_.reset (new Cloud);
  filterPassThrough (cloud, *cloud_pass_);
  gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);

  if(counter < 10){
	counter++;
  }else{
  	//Track the object
	tracker_->setInputCloud (cloud_pass_downsampled_);
	tracker_->compute ();
	new_cloud_ = true;
  }
}

int
main (int argc, char** argv)
{
  if (argc < 3)
    {
      PCL_WARN("Please set device_id pcd_filename(e.g. $ %s '#1' sample.pcd)\n", argv[0]);
      exit (1);
    }

  //read pcd file
  target_cloud.reset(new Cloud());
  if(pcl::io::loadPCDFile (argv[2], *target_cloud) == -1){
    std::cout << "pcd file not found" << std::endl;
    exit(-1);
  }

  std::string device_id = std::string (argv[1]);

  counter = 0;

  //Set parameters
  new_cloud_  = false;
  downsampling_grid_size_ =  0.002;

  std::vector<double> default_step_covariance = std::vector<double> (6, 0.015 * 0.015);
  default_step_covariance[3] *= 40.0;
  default_step_covariance[4] *= 40.0;
  default_step_covariance[5] *= 40.0;

  std::vector<double> initial_noise_covariance = std::vector<double> (6, 0.00001);
  std::vector<double> default_initial_mean = std::vector<double> (6, 0.0);

  boost::shared_ptr<KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> > tracker
    (new KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> (8));

  ParticleT bin_size;
  bin_size.x = 0.1f;
  bin_size.y = 0.1f;
  bin_size.z = 0.1f;
  bin_size.roll = 0.1f;
  bin_size.pitch = 0.1f;
  bin_size.yaw = 0.1f;

  //Set all parameters for  KLDAdaptiveParticleFilterOMPTracker
  tracker->setMaximumParticleNum (1000);
  tracker->setDelta (0.99);
  tracker->setEpsilon (0.2);
  tracker->setBinSize (bin_size);

  //Set all parameters for  ParticleFilter
  tracker_ = tracker;
  tracker_->setTrans (Eigen::Affine3f::Identity ());
  tracker_->setStepNoiseCovariance (default_step_covariance);
  tracker_->setInitialNoiseCovariance (initial_noise_covariance);
  tracker_->setInitialNoiseMean (default_initial_mean);
  tracker_->setIterationNum (1);
  tracker_->setParticleNum (600);
  tracker_->setResampleLikelihoodThr(0.00);
  tracker_->setUseNormal (false);

  //Setup coherence object for tracking
  ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr coherence = ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr
    (new ApproxNearestPairPointCloudCoherence<RefPointType> ());

  boost::shared_ptr<DistanceCoherence<RefPointType> > distance_coherence
    = boost::shared_ptr<DistanceCoherence<RefPointType> > (new DistanceCoherence<RefPointType> ());
  coherence->addPointCoherence (distance_coherence);

  boost::shared_ptr<pcl::search::Octree<RefPointType> > search (new pcl::search::Octree<RefPointType> (0.01));
  coherence->setSearchMethod (search);
  coherence->setMaximumDistance (0.01);

  tracker_->setCloudCoherence (coherence);

  //prepare the model of tracker's target
  Eigen::Vector4f c;
  Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
  CloudPtr transed_ref (new Cloud);
  CloudPtr transed_ref_downsampled (new Cloud);

  pcl::compute3DCentroid<RefPointType> (*target_cloud, c);
  trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]);
  pcl::transformPointCloud<RefPointType> (*target_cloud, *transed_ref, trans.inverse());
  gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_);

  //set reference model and trans
  tracker_->setReferenceCloud (transed_ref_downsampled);
  tracker_->setTrans (trans);

  //Setup OpenNIGrabber and viewer
  pcl::visualization::CloudViewer* viewer_ = new pcl::visualization::CloudViewer("PCL OpenNI Tracking Viewer");
  pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
  boost::function<void (const CloudConstPtr&)> f =
    boost::bind (&cloud_cb, _1);
  interface->registerCallback (f);

  viewer_->runOnVisualizationThread (boost::bind(&viz_cb, _1), "viz_cb");

  //Start viewer and object tracking
  interface->start();
  while (!viewer_->wasStopped ())
    boost::this_thread::sleep(boost::posix_time::seconds(1));
  interface->stop();
}