#pragma once

/**
 * \brief Increments a 2D index such that it points to the next element.
 * \param index The current index into a matrix/array. It will be modified by the function.
 * \param extents The size of the matrix/array. Required for switching between rows.
 **/
void increment(Eigen::Vector2i& index, const Eigen::Vector2i& extents)
{
  ++index.x();
  if (index.x() >= extents.x())
  {
    index.x() = 0;
    ++index.y();
  }
}

/**
 * \brief Decrements a 2D index such that it points to the previous element.
 * \param index The current index into a matrix/array. It will be modified by the function.
 * \param extents The size of the matrix/array. Required for switching between rows.
 **/
void decrement(Eigen::Vector2i& index, const Eigen::Vector2i& extents)
{
  --index.x();
  if (index.x() < 0)
  {
    index.x() = extents.x()-1;
    --index.y();
  }
}

/**
 * \brief Advances (forward and backward) a 2D index.
 * \param index The current index into a matrix/array. It will be modified by the function.
 * \param extents The size of the matrix/array. Required for switching between rows.
 **/
template <typename difference_type>
void advance(difference_type offset, Eigen::Vector2i& index, const Eigen::Vector2i& extents)
{
  index.x() += offset;

  // This code has been evaluated for performance on MSVC 2012; switching temporarily to float
  // is faster than using pure integer arithmetic with modulo/remainder operations and also
  // faster than using std::div.
  difference_type delta = static_cast<difference_type>(std::floor(index.x() / static_cast<float>(extents.x())));
  index.x() -= delta*extents.x();
  index.y() += delta;
}