/* #define DEBUGTRACE_ENABLED */
#include "lasp_timebuffer.h"
#include "debugtrace.hpp"
#include <algorithm>
#include <cassert>
#include <deque>
#include <memory>
#include <optional>
#include <stdexcept>

using rte = std::runtime_error;

class TimeBufferImp {
  /**
   * @brief Storage in a double-ended queue of armadillo row vectors.
   */
  std::deque<arma::rowvec> _storage;

public:
  void reset() {
    DEBUGTRACE_ENTER;
    _storage.clear();
  }
  void push(const dmat &mat) {
    DEBUGTRACE_ENTER;
#if LASP_DEBUG == 1
    if (!_storage.empty()) {
      if (mat.n_cols != _storage.front().n_cols) {
        throw rte("Invalid number of channels in mat");
      }
    }
#endif
    for (us i = 0; i < mat.n_rows; i++) {
      _storage.push_back(mat.row(i));
    }
  }

  dmat pop(const us nframes, const us keep) {

    DEBUGTRACE_ENTER;

    if (keep >= nframes) {
      throw rte("keep should be < nframes");
    }
    if (nframes > n_frames()) {
      throw rte("Requested more than currently in storage");
    }

    assert(!_storage.empty());

    dmat res(nframes, _storage.front().n_cols);

    us j = 0;
    for (us i = 0; i < nframes; i++) {
      if (i + keep < nframes) {
        // Just pop elements and copy over
        res.row(i) = _storage.front();
        _storage.pop_front();

      } else {

        // Suppose keep == 0, then we never arrive here
        // Suppose keep == 1, then storage[0] is copyied over.
        // Suppose keep == 2, then storage[0[ and storage[1] is copyied over.
        // Etc.
        res.row(i) = _storage[j];
        j++;
      }
    }

    return res;
  }

  /**
   * @brief Counts the number of available frames in the queue
   *
   * @return
   */
  us n_frames() const { return _storage.size(); }
};

TimeBuffer::TimeBuffer() : _imp(std::make_unique<TimeBufferImp>()) {}
dmat TimeBuffer::pop(const us n_rows, const us keep) {
  return _imp->pop(n_rows, keep);
}
TimeBuffer::~TimeBuffer() {}
void TimeBuffer::reset() { _imp->reset(); }
void TimeBuffer::push(const dmat &dat) { _imp->push(dat); }
us TimeBuffer::size() const { return _imp->n_frames(); }