xsens_fifoqueue.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#ifndef _FIFOQUEUE_H_2006_05_03
#define _FIFOQUEUE_H_2006_05_03

//#define _LOG_QUEUE
#if (defined(_DEBUG) || defined(_LOG_ALWAYS)) && defined(_LOG_QUEUE)
#include <typeinfo.h>
#else
#define QUEUELOG(...)
#endif

namespace xsens
{
//////////////////////////////////////////////////////////////////////////////////////////

/*! \brief A FIFO queue with limited length (cyclic).

    The class is based on the STL queue class, but has a limited size. If more
   items are
    inserted than would fit, the oldest item is overwritten. The class can only
   handle
    pointer types.
*/
template <class T, bool E = true>
class FifoQueue
{
   private:
#if !defined(QUEUELOG)
    // write to screen/debug-stream
    void QUEUELOG(const char* str, ...)
    {
#if defined(_LOG_TO_STDOUT)
        va_list ptr;
        va_start(ptr, str);
        vprintf(str, ptr);
#else
        //#ifdef _LOG_TO_DBVIEW
        char buf[2048];

        va_list ptr;
        va_start(ptr, str);
        vsprintf(buf, str, ptr);

        OutputDebugString(buf);
#endif
    }
#endif
   protected:
    size_t m_maxCount;
    size_t m_currentCount;
    size_t m_first;
    bool m_deleteOnOverwrite;

    T* m_list;

   public:
    /** The type of the value stored in this queue. */
    typedef T value_type;
    /** The type of a 'size' value. */
    typedef size_t size_type;

    //! Create an empty queue with capacity size.
    FifoQueue(size_type size = 16, bool delOnOverwrite = true)
    {
        QUEUELOG(
            "[%p]FifoQueue.new (base), T=%s, size=%d, del=%d\n", this,
            typeid(T).name(), size, delOnOverwrite);

        if (size > 0)
            m_maxCount = size;
        else
            m_maxCount = 1;
        m_list = new T[m_maxCount];
        m_currentCount = 0;
        m_first = 0;
        m_deleteOnOverwrite = delOnOverwrite;
    }

    //! The copy constructor.
    template <bool E2>
    FifoQueue(const FifoQueue<T, E2>& q)
    {
        QUEUELOG(
            "[%p]FifoQueue.new (copy), T=%s, size=%d, count=%d, del=%d\n", this,
            typeid(T).name(), q.m_maxCount, q.m_currentCount,
            q.m_deleteOnOverwrite);
        m_maxCount = q.m_maxCount;
        m_list = new T[m_maxCount];
        m_currentCount = q.m_currentCount;
        m_deleteOnOverwrite = q.m_deleteOnOverwrite;
        m_first = 0;
        for (size_t i = 0; i < m_currentCount; ++i)
            m_list[i] = q.m_list[(i + q.m_first) % m_maxCount];
    }

    void eraseAndClear(void)
    {
        QUEUELOG(
            "[%p]FifoQueue.eraseAndClear, T=%s, count=%d\n", this,
            typeid(T).name(), m_currentCount);
        for (size_t i = 0; i < m_currentCount; ++i)
            delete m_list[(i + m_first) % m_maxCount];
        m_currentCount = 0;
        m_first = 0;
    }

    //! The destructor.
    ~FifoQueue()
    {
        QUEUELOG(
            "[%p]FifoQueue.delete, T=%s, count=%d\n", this, typeid(T).name(),
            m_currentCount);
        if (E) eraseAndClear();
        m_maxCount = 0;
        delete[] m_list;
    }

    //! The assignment operator.
    template <bool E2>
    FifoQueue<T, E>& operator=(const FifoQueue<T, E2>& q)
    {
        QUEUELOG(
            "[%p]FifoQueue.operator =, T=%s, max=%d, count=%d, smax=%d, "
            "scount=%d\n",
            this, typeid(T).name(), m_maxCount, m_currentCount, q.m_maxCount,
            q.m_currentCount);
        if (m_maxCount != q.m_maxCount)
        {
            delete[] m_list;
            m_maxCount = q.m_maxCount;
            m_list = new T[m_maxCount];
        }
        m_currentCount = q.m_currentCount;
        m_first = 0;
        for (size_t i = 0; i < m_currentCount; ++i)
            m_list[i] = q.m_list[(i + q.m_first) % m_maxCount];

        return *this;
    }

    //! Resize the queue, note that this function clears the queue.
    void resize(const size_t size)
    {
        QUEUELOG(
            "[%p]FifoQueue.resize, T=%s, max=%d, count=%d, size=%d\n", this,
            typeid(T).name(), m_maxCount, m_currentCount, size);
        if (E) eraseAndClear();
        delete[] m_list;
        if (size > 0)
            m_maxCount = size;
        else
            m_maxCount = 1;
        m_list = new T[m_maxCount];
        m_currentCount = 0;
        m_first = 0;
    }

    //! Return true if the queue is empty.
    bool empty() const { return (m_currentCount == 0); }
    //! Return the maximum number of elements in the queue.
    size_type size() const { return m_maxCount; }
    //! Return the number of elements currnetly in the queue.
    size_type length() const { return m_currentCount; }
    //! Return the oldest element in the queue.
    value_type& front() { return m_list[m_first]; }
    //! Return the oldest element in the queue.
    const value_type& front() const { return m_list[m_first]; }
    //! Return the newest element in the queue.
    value_type& back()
    {
        return m_list[(m_first + m_currentCount - 1) % m_maxCount];
    }

    //! Return the newest element in the queue.
    const value_type& back() const
    {
        return m_list[(m_first + m_currentCount - 1) % m_maxCount];
    }

    //! Insert x at the back of the queue.
    void push(const value_type& x)
    {
        QUEUELOG(
            "[%p]FifoQueue.push, T=%s, max=%d, count=%d, x=%p\n", this,
            typeid(T).name(), m_maxCount, m_currentCount, x);
        if (m_currentCount == m_maxCount)
        {
            if (m_deleteOnOverwrite) delete (m_list[m_first]);

            m_list[m_first] = x;
            m_first = (m_first + 1) % m_maxCount;
        }
        else
        {
            m_list[(m_first + m_currentCount++) % m_maxCount] = x;
        }
    }

    //! Remove the element at the front of the queue.
    void pop(void)
    {
        QUEUELOG(
            "[%p]FifoQueue.pop, T=%s, max=%d, count=%d\n", this,
            typeid(T).name(), m_maxCount, m_currentCount);
        m_first = (m_first + 1) % m_maxCount;
        --m_currentCount;
    }

    //! Remove the element at the back of the queue.
    void popBack(void)
    {
        QUEUELOG(
            "[%p]FifoQueue.popBack, T=%s, max=%d, count=%d\n", this,
            typeid(T).name(), m_maxCount, m_currentCount);
        --m_currentCount;
    }

    //! Return the index'th oldest item from the queue
    const value_type& operator[](size_t index) const
    {
        if (index >= m_currentCount)
            return m_list[(m_first + m_currentCount - 1) % m_maxCount];
        else
            return m_list[(m_first + index) % m_maxCount];
    }

    //! Return the index'th oldest item from the queue
    value_type& operator[](size_t index)
    {
        if (index >= m_currentCount)
            return m_list[(m_first + m_currentCount - 1) % m_maxCount];
        else
            return m_list[(m_first + index) % m_maxCount];
    }

    void clear(void)
    {
        QUEUELOG(
            "[%p]FifoQueue.clear, T=%s, max=%d, count=%d\n", this,
            typeid(T).name(), m_maxCount, m_currentCount);
        m_currentCount = 0;
        m_first = 0;
    }

    void remove(size_t index)
    {
        QUEUELOG(
            "[%p]FifoQueue.remove, T=%s, max=%d, count=%d, index=%d\n", this,
            typeid(T).name(), m_maxCount, m_currentCount, index);
        if (index >= m_currentCount) return;
        if (index == 0)
            pop();
        else
        {
            --m_currentCount;
            for (size_t i = index; i < m_currentCount; ++i)
                m_list[(m_first + i) % m_maxCount] =
                    m_list[(1 + m_first + i) % m_maxCount];
        }
    }
};

//////////////////////////////////////////////////////////////////////////////////////////

/*! \brief A FIFO queue with limited length (cyclic).

    The class is based on the STL queue class, but has a limited size. If more
   items are
    inserted than would fit, the oldest item is overwritten. The class can only
   handle
    non-pointer types.
*/
template <class T>
class FifoQueueBasic
{
   private:
#if !defined(QUEUELOG)
    // write to screen/debug-stream
    void QUEUELOG(const char* str, ...)
    {
#if defined(_LOG_TO_STDOUT)
        va_list ptr;
        va_start(ptr, str);
        vprintf(str, ptr);
#else
        //#ifdef _LOG_TO_DBVIEW
        char buf[2048];

        va_list ptr;
        va_start(ptr, str);
        vsprintf(buf, str, ptr);

        OutputDebugString(buf);
#endif
    }
#endif
   protected:
    size_t m_maxCount;
    size_t m_currentCount;
    size_t m_first;

    T* m_list;

   public:
    /** The type of the value stored in this queue. */
    typedef T value_type;
    /** The type of a 'size' value. */
    typedef size_t size_type;

    //! Create an empty queue with capacity size.
    FifoQueueBasic(size_type size = 16)
    {
        QUEUELOG(
            "[%p]FifoQueueBase.new (base), T=%s, size=%d\n", this,
            typeid(T).name(), size);
        if (size > 0)
            m_maxCount = size;
        else
            m_maxCount = 1;
        m_list = new T[m_maxCount];
        m_currentCount = 0;
        m_first = 0;
    }

    //! The copy constructor.
    FifoQueueBasic(const FifoQueueBasic<T>& q)
    {
        QUEUELOG(
            "[%p]FifoQueueBase.new (copy), T=%s, size=%d\n", this,
            typeid(T).name(), q.m_maxCount);
        m_maxCount = q.m_maxCount;
        m_list = new T[m_maxCount];
        m_currentCount = q.m_currentCount;
        m_first = 0;
        for (size_t i = 0; i < m_currentCount; ++i)
            m_list[i] = q.m_list[(i + q.m_first) % m_maxCount];
    }

    void eraseAndClear(void)
    {
        QUEUELOG(
            "[%p]FifoQueueBase.eraseAndClear, T=%s, count=%d\n", this,
            typeid(T).name(), m_currentCount);
        for (size_t i = 0; i < m_currentCount; ++i)
            delete m_list[(i + m_first) % m_maxCount];
        m_currentCount = 0;
        m_first = 0;
    }

    //! The destructor.
    ~FifoQueueBasic()
    {
        QUEUELOG(
            "[%p]FifoQueueBase.delete, T=%s, count=%d\n", this,
            typeid(T).name(), m_currentCount);
        m_maxCount = 0;
        delete[] m_list;
    }

    //! The assignment operator.
    FifoQueueBasic<T>& operator=(const FifoQueueBasic<T>& q)
    {
        QUEUELOG(
            "[%p]FifoQueueBase.operator =, T=%s, max=%d, count=%d, smax=%d, "
            "scount=%d\n",
            this, typeid(T).name(), m_maxCount, m_currentCount, q.m_maxCount,
            q.m_currentCount);
        if (m_maxCount != q.m_maxCount)
        {
            delete[] m_list;
            m_maxCount = q.m_maxCount;
            m_list = new T[m_maxCount];
        }
        m_currentCount = q.m_currentCount;
        m_first = 0;
        for (size_t i = 0; i < m_currentCount; ++i)
            m_list[i] = q.m_list[(i + q.m_first) % m_maxCount];

        return *this;
    }

    //! Resize the queue, note that this function clears the queue.
    void resize(const size_t size)
    {
        QUEUELOG(
            "[%p]FifoQueueBase.resize, T=%s, max=%d, count=%d, size=%d\n", this,
            typeid(T).name(), m_maxCount, m_currentCount, size);
        delete[] m_list;
        if (size > 0)
            m_maxCount = size;
        else
            m_maxCount = 1;
        m_list = new T[m_maxCount];
        m_currentCount = 0;
        m_first = 0;
    }

    //! Return true if the queue is empty.
    bool empty() const { return (m_currentCount == 0); }
    //! Return the maximum number of elements in the queue.
    size_type size() const { return m_maxCount; }
    //! Return the number of elements currnetly in the queue.
    size_type length() const { return m_currentCount; }
    //! Return the oldest element in the queue.
    value_type& front() { return m_list[m_first]; }
    //! Return the oldest element in the queue.
    const value_type& front() const { return m_list[m_first]; }
    //! Return the newest element in the queue.
    value_type& back()
    {
        return m_list[(m_first + m_currentCount - 1) % m_maxCount];
    }

    //! Return the newest element in the queue.
    const value_type& back() const
    {
        return m_list[(m_first + m_currentCount - 1) % m_maxCount];
    }

    //! Insert x at the back of the queue.
    void push(const value_type& x)
    {
        QUEUELOG(
            "[%p]FifoQueueBase.push, T=%s, max=%d, count=%d, x=%p\n", this,
            typeid(T).name(), m_maxCount, m_currentCount, x);
        if (m_currentCount == m_maxCount)
        {
            m_list[m_first] = x;
            m_first = (m_first + 1) % m_maxCount;
        }
        else
        {
            m_list[(m_first + m_currentCount++) % m_maxCount] = x;
        }
    }

    //! Insert x at the front of the queue (LIFO operation).
    void push_front(const value_type& x)
    {
        QUEUELOG(
            "[%p]FifoQueueBase.push_front, T=%s, max=%d, count=%d, x=%p\n",
            this, typeid(T).name(), m_maxCount, m_currentCount, x);
        m_first = (m_first + m_maxCount - 1) % m_maxCount;
        if (m_currentCount == 0) m_first = 0;
        m_list[m_first] = x;
        if (m_currentCount < m_maxCount) ++m_currentCount;
    }

    //! Remove the element at the front of the queue.
    void pop(void)
    {
        QUEUELOG(
            "[%p]FifoQueueBase.pop, T=%s, max=%d, count=%d\n", this,
            typeid(T).name(), m_maxCount, m_currentCount);
        if (m_currentCount > 0)
        {
            m_first = (m_first + 1) % m_maxCount;
            --m_currentCount;
        }
    }

    //! Remove the element at the back of the queue.
    void popBack(void)
    {
        QUEUELOG(
            "[%p]FifoQueueBase.popBack, T=%s, max=%d, count=%d\n", this,
            typeid(T).name(), m_maxCount, m_currentCount);
        if (m_currentCount > 0) --m_currentCount;
    }

    //! Return the index'th oldest item from the queue
    const value_type& operator[](size_t index) const
    {
        if (index >= m_currentCount)
            return m_list[(m_first + m_currentCount - 1) % m_maxCount];
        else
            return m_list[(m_first + index) % m_maxCount];
    }

    //! Return the index'th oldest item from the queue
    value_type& operator[](size_t index)
    {
        if (index >= m_currentCount)
            return m_list[(m_first + m_currentCount - 1) % m_maxCount];
        else
            return m_list[(m_first + index) % m_maxCount];
    }

    void clear(void)
    {
        QUEUELOG(
            "[%p]FifoQueueBase.clear, T=%s, max=%d, count=%d\n", this,
            typeid(T).name(), m_maxCount, m_currentCount);
        m_currentCount = 0;
        m_first = 0;
    }

    void remove(size_t index)
    {
        QUEUELOG(
            "[%p]FifoQueueBase.remove, T=%s, max=%d, count=%d, index=%d\n",
            this, typeid(T).name(), m_maxCount, m_currentCount, index);
        if (index >= m_currentCount) return;
        if (index == 0)
            pop();
        else
        {
            --m_currentCount;
            for (size_t i = index; i < m_currentCount; ++i)
                m_list[(m_first + i) % m_maxCount] =
                    m_list[(1 + m_first + i) % m_maxCount];
        }
    }
};

}  // end of xsens namespace

#endif  // _FIFOQUEUE_H_2006_05_03