/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <stdexcept>
#include <vector>

namespace mrpt::containers
{
/** A circular buffer of fixed size (defined at construction-time), implemented
 * with a std::vector as the underlying storage.
 * \note Defined in #include <mrpt/containers/circular_buffer.h>
 * \ingroup mrpt_containers_grp
 */
template <typename T>
class circular_buffer
{
 private:
  std::vector<T> m_data;
  /** not "const" to allow copy/move = ops. */
  size_t m_size;
  size_t m_next_read, m_next_write;

 public:
  circular_buffer(size_t size) : m_data(size), m_size(size), m_next_read(0), m_next_write(0)
  {
    if (m_size <= 2) throw std::invalid_argument("size must be >2");
  }

  /** Insert a copy of the given element in the buffer.
   * \exception std::out_of_range If the buffer run out of space.
   */
  void push(T d)
  {
    auto new_idx = m_next_write + 1;
    if (new_idx == m_size) new_idx = 0;
    if (new_idx == m_next_read) throw std::out_of_range("push: circular_buffer is full");
    m_data[m_next_write] = d;
    m_next_write = new_idx;
  }

  /** Insert a reference of the given element in the buffer.
   * \exception std::out_of_range If the buffer run out of space.
   */
  void push_ref(const T& d)
  {
    m_data[m_next_write++] = d;
    if (m_next_write == m_size) m_next_write = 0;

    if (m_next_write == m_next_read) throw std::out_of_range("push: circular_buffer is full");
  }

  /** Insert an array of elements in the buffer.
   * \exception std::out_of_range If the buffer run out of space.
   */
  void push_many(T* array_elements, size_t count)
  {
    while (count--) push(*array_elements++);
  }

  /** Retrieve an element from the buffer.
   * \exception std::out_of_range If the buffer is empty.
   */
  T pop()
  {
    if (m_next_read == m_next_write) throw std::out_of_range("pop: circular_buffer is empty");

    const size_t i = m_next_read++;
    if (m_next_read == m_size) m_next_read = 0;
    return m_data[i];
  }

  /** Retrieve an element from the buffer.
   * \exception std::out_of_range If the buffer is empty.
   */
  void pop(T& out_val)
  {
    if (m_next_read == m_next_write) throw std::out_of_range("pop: circular_buffer is empty");

    out_val = m_data[m_next_read++];
    if (m_next_read == m_size) m_next_read = 0;
  }

  /** Pop a number of elements into a user-provided array.
   * \exception std::out_of_range If the buffer has less elements than
   * requested. */
  void pop_many(T* out_array, size_t count)
  {
    while (count--) pop(*out_array++);
  }

  /** Peek (see without modifying) what is to be read from the buffer if pop()
   * was to be called.
   * \exception std::out_of_range If the buffer is empty. */
  T peek() const
  {
    if (m_next_read == m_next_write) throw std::out_of_range("peek: circular_buffer is empty");
    return m_data[m_next_read];
  }
  /** Like peek(), but seeking ahead in the buffer (index=0 means the
   * immediate next element, index=1 the following one, etc.)
   * \exception std::out_of_range If trying to read passing the number of
   * available elements. */
  T peek(size_t index) const
  {
    if (index >= this->size()) throw std::out_of_range("peek: seek out of range");
    return m_data[(m_next_read + index) % m_size];
  }

  /** Like peek(), for multiple elements, storing a number of elements into a
   * user-provided array.
   * \exception std::out_of_range If the buffer has less elements than
   * requested. */
  void peek_many(T* out_array, size_t count) const
  {
    size_t peek_read = m_next_read;
    while (count--)
    {
      if (peek_read == m_next_write) throw std::out_of_range("peek: circular_buffer is empty");
      T val = m_data[peek_read++];
      if (peek_read == m_size) peek_read = 0;
      *out_array++ = val;
    }
  }

  /** Return the number of elements available for read ("pop") in the buffer
   * (this is NOT the maximum size of the internal buffer)
   * \sa capacity */
  size_t size() const
  {
    if (m_next_write >= m_next_read)
      return m_next_write - m_next_read;
    else
      return m_next_write + (m_size - m_next_read);
  }

  /** Return the maximum capacity of the buffer.
   * \sa size
   */
  size_t capacity() const { return m_size; }
  /** The maximum number of elements that can be written ("push") without
   * rising an overflow error.
   */
  size_t available() const { return (capacity() - size()) - 1; }
  /** Delete all the stored data, if any. */
  void clear() { m_next_write = m_next_read = 0; }
};  // end class circular_buffer

}  // namespace mrpt::containers
