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