buffer.h

/*
 * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
 *               http://gearbox.sf.net/
 * Copyright (c) 2004-2010 Alex Brooks, Alexei Makarenko, Tobias Kaupp
 *
 * This distribution is licensed to you under the terms described in
 * the LICENSE file included in this distribution.
 *
 */

#ifndef GBXICEUTILACFR_BUFFER_H
#define GBXICEUTILACFR_BUFFER_H

#include <queue>
#include <gbxutilacfr/exceptions.h>

#include <IceUtil/Monitor.h>
#include <IceUtil/Mutex.h>
#include <IceUtil/Time.h>

namespace gbxiceutilacfr {

enum BufferType
{
    BufferTypeCircular,
    BufferTypeQueue
};

template<class Type>
class Buffer : public IceUtil::Monitor<IceUtil::Mutex>
{
public:

    Buffer( int depth = -1, BufferType type = BufferTypeQueue );

    virtual ~Buffer();

    void configure( int depth, BufferType type=BufferTypeCircular );

    int depth() const;

    BufferType type() const;

    bool isEmpty() const;

    int  size() const;

    void purge();

    void push( const Type & obj );

    void pop();

    void  get( Type & obj ) const;

    void  get( Type & obj, unsigned int n ) const;

    void  getAndPop( Type & obj );

    int  getWithTimeout( Type & obj, int timeoutMs=-1 );

    int  getAndPopWithTimeout( Type & obj, int timeoutMs=-1 );

protected:
    
    // The buffer itself
    std::deque<Type> queue_;

    // Reimplement this function for non-standard types.
    virtual void internalGet( Type & obj ) const ;

    // Reimplement this function for non-standard types.
    virtual void internalGet( Type & obj, unsigned int n ) const ;
    
    // Reimplement this function for non-standard types.
    virtual void internalPush( const Type & obj );

private:

    // buffer depth:
    //      positive numbers to specify finite depth,
    //      negative numbers for infinite depth (memory size),
    //      zero is undefined
    int depth_;

    // buffer type (see type definitions in BufferType enum)
    BufferType type_;

    // internal implementation of getWithTimeout( obj, -1 );
    void getWithInfiniteWait( Type & obj );
};




template<class Type>
Buffer<Type>::Buffer( int depth, BufferType type )
    : depth_(depth),
      type_(type)
{
    purge();
}

template<class Type>
Buffer<Type>::~Buffer()
{
}

template<class Type>
void Buffer<Type>::configure( int depth, BufferType type )
{
    // all data is lost!
    purge();

    IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
    depth_ = depth;
    type_ = type;
}

template<class Type>
int 
Buffer<Type>::depth() const
{
    IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
    return depth_;
}

template<class Type>
BufferType 
Buffer<Type>::type() const
{
    IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
    return type_;
}

template<class Type>
void Buffer<Type>::purge()
{
    IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
    queue_.resize(0);
}

template<class Type>
bool Buffer<Type>::isEmpty() const
{
    IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
    return queue_.empty();
}

template<class Type>
int Buffer<Type>::size() const
{
    IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
    return queue_.size();
}

template<class Type>
void Buffer<Type>::pop()
{
    IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
    if ( queue_.empty() ) {
        return;
    }
    // must check for empty queue above, otherwise get seg fault!
    queue_.pop_front();
}

template<class Type>
void Buffer<Type>::get( Type &obj ) const
{
    IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
    if ( !queue_.empty() )
    {
        internalGet( obj );
    }
    else
    {
        throw gbxutilacfr::Exception( ERROR_INFO, "trying to read from an empty buffer." );
    }
}

template<class Type>
void Buffer<Type>::get( Type &obj, unsigned int n ) const
{
    IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
    if ( queue_.empty() ){
        throw gbxutilacfr::Exception( ERROR_INFO, "trying to read from an empty buffer." );
    }
    else if( n >= queue_.size()){
        throw gbxutilacfr::Exception( ERROR_INFO, "index out of bounds while trying to read buffer." );
    }
    else{
        internalGet( obj ,n );
    }
}

template<class Type>
void Buffer<Type>::getAndPop( Type &obj )
{
    IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
    if ( !queue_.empty() )
    {
        internalGet( obj );
    }
    else
    {
        throw gbxutilacfr::Exception( ERROR_INFO, "trying to read from an empty buffer." );
    }
    queue_.pop_front();
}

template<class Type>
int Buffer<Type>::getWithTimeout( Type &obj, int timeoutMs )
{
    // special case: infinite wait time
    if ( timeoutMs == -1 ) 
    {
        getWithInfiniteWait( obj );
        return 0;
    }

    // finite wait time
    IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);

    // if already have data in the buffer, return it and get out
    if ( !queue_.empty() )
    {
        internalGet( obj );
        return 0;
    }

    // empty buffer: figure out when to wake up
    // notice that we are still holding the lock, so it's ok to call timedWait()
    if (  this->timedWait( IceUtil::Time::milliSeconds( timeoutMs ) ) )  
    {
        // someone woke us up, we are holding the lock again
        // check new data again (could be a spurious wakeup)
        if ( !queue_.empty() ) 
        {
            internalGet( obj );
            return 0;
        }
        else {
            // spurious wakup, don't wait again, just return
            return 1;
        }
    }
    else {
        // wait timedout, nobody woke us up
        return -1;
    }
}

template<class Type>
int Buffer<Type>::getAndPopWithTimeout( Type &obj, int timeoutMs )
{
    int ret = getWithTimeout( obj, timeoutMs );
    if ( ret==0 ) {
        pop();
    }
    return ret;
}

// internal utility function (waits for update infinitely)
template<class Type>
void Buffer<Type>::getWithInfiniteWait( Type &obj )
{
    IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
    
    // check the condition before and after waiting to deal with spurious wakeups
    // (see Ice manual sec. 28.9.2)
    while ( queue_.empty() ) 
    {
        this->wait();
    }
    
    internalGet( obj );
}

// NOTE: see notes on efficient notification in Ice sec. 28.9.3
template<class Type>
void Buffer<Type>::push( const Type & obj )
{
    IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);

    // buffer is not full, or buffer is configured to be of infinite size (at least for STL)
    if ( (int)queue_.size() < depth_ || depth_<0 )
    {
        internalPush( obj );
    }
    else if ( type_ == BufferTypeCircular )
    {
        // pop the oldest entry
        queue_.pop_front();
        // push the new enty
        internalPush( obj );
    }
    else // we have a full, non-circular buffer
    {
        // do nothing, the new object is lost
    }

    // wakeup someone who's waiting for an update
    this->notify();
}

template<class Type>
void Buffer<Type>::internalGet( Type & obj ) const
{
    obj = queue_.front();
}

template<class Type>
void Buffer<Type>::internalGet( Type & obj, unsigned int n ) const
{
    obj = queue_[n];
}

template<class Type>    
void Buffer<Type>::internalPush( const Type & obj )
{
    queue_.push_back( obj );
}

} // end namespace

#endif