StreamAligner.hpp

Go to the documentation of this file.

#ifndef __AGGREGATOR_HPP__
#define __AGGREGATOR_HPP__

#include <base/Time.hpp>
#include <cmath>
#include <base-logging/Logging.hpp>
#include <vector>
#include <base/CircularBuffer.hpp>
#include <algorithm>
#include <boost/function.hpp>
#include <boost/tuple/tuple.hpp>
#include <stdexcept> 
#include <iostream>
#include <aggregator/StreamAlignerStatus.hpp>

namespace aggregator {

    class StreamAligner
    {
        class StreamBase
        {
            friend class StreamAligner;
            public:
                StreamBase() : active( true ) {}
                virtual ~StreamBase() {}
                virtual base::Time pop() = 0;
                virtual bool hasData() const = 0;
                virtual int getPriority() const = 0;
                virtual base::Time latestTimeStamp() const = 0;
                virtual base::Time latestDataTime() const = 0;
                virtual base::Time earliestDataTime() const = 0;
                virtual const StreamStatus &getBufferStatus() const = 0;
                virtual void copyState( const StreamBase& other ) = 0;
                virtual void clear() = 0;

                bool isActive() const { return active; }
                void setActive( bool active ) { this->active = active; }
                
                friend std::ostream &operator<<(std::ostream &stream, const aggregator::StreamAligner::StreamBase &base);
                
            protected:
                mutable StreamStatus status;
                bool active;
        };

        public:
        template <class T> class Stream : public StreamBase
        {
        public:
            typedef boost::function<void (const base::Time &ts, const T &value)> callback_t;

        protected:
            typedef std::pair<base::Time,T> item;
            boost::circular_buffer<item> buffer;
            size_t bufferSize;
            callback_t callback;
            base::Time period; 
            base::Time lastTime;
            int priority;

        public:
            Stream( callback_t callback, size_t bufferSize, base::Time period, int priority, const std::string &name )
                : bufferSize( bufferSize ), callback(callback), period(period), lastTime(base::Time::fromSeconds(0)), priority(priority)
            {
                status.name = name;
                status.priority = priority;

                if (bufferSize > 0)
                    buffer.set_capacity( bufferSize );
                else
                {
                    // initial size, will be reallocated at runtime
                    buffer.set_capacity( 20 );
                }
                status.buffer_size = buffer.capacity();
            }

            virtual ~Stream() {};

            bool getNextSample(item &sample) const
            {
                if(buffer.empty())
                    return false;
                
                sample = buffer.front();
                return true;
            }

            virtual int getPriority() const
            {
                return priority;
            }

            virtual const StreamStatus &getBufferStatus() const
            {
                status.buffer_fill = buffer.size();
                status.latest_data_time = latestDataTime();
                status.earliest_data_time = earliestDataTime();
                status.active = isActive();
                return status;
            }

            virtual void copyState( const StreamBase& other )
            {
                const Stream<T> &stream(dynamic_cast<const Stream<T>& >(other));
                
                lastTime = stream.lastTime;
                buffer = stream.buffer;
                bufferSize = stream.bufferSize;
                status = stream.status; 
            }

            void push(const base::Time &ts, const T &data ) 
            { 
                if(ts < lastTime)
                {
                    status.samples_backward_in_time++;
                    return;
                }
                
                lastTime = ts;

                if (buffer.full())
                {
                    if (bufferSize > 0)
                    {
                        // if the buffer is full, just use the behaviour of the circular
                        // buffer: discard old data.
                        status.samples_dropped_buffer_full++;
                    }
                    else
                    {
                        buffer.set_capacity(buffer.capacity() * 2);
                        status.buffer_size = buffer.capacity();
                    }
                }
                buffer.push_back( std::make_pair(ts, data) ); 
            }

            base::Time pop() 
            { 
                if( hasData() )
                {
                    status.samples_processed++;
                    base::Time ts = buffer.front().first;
                    if(callback)
                        callback( ts, buffer.front().second );
                    buffer.pop_front();
                    return ts;
                }

                throw std::runtime_error("pop() called on stream with no data.");
            }

            bool hasData() const
            { return !buffer.empty(); }

            base::Time latestTimeStamp() const
            {
                if( hasData() )
                    return buffer.front().first;
                else 
                    return lastTime + period;
            }
            
            virtual base::Time latestDataTime() const
            {
                return lastTime;
            }
            
            virtual base::Time earliestDataTime() const
            {
                if( hasData() )
                    return buffer.front().first;
                return base::Time();
            }
            
            virtual void clear()
            {   
                lastTime = base::Time();
                buffer.clear();
                
                status.latest_sample_time = base::Time();
                status.latest_data_time = base::Time();
                status.samples_dropped_buffer_full = 0;
                status.samples_dropped_late_arriving = 0;
                status.buffer_fill = 0;
                status.active = true;
            };
        };

        static bool compareStreams( const StreamBase* b1, const StreamBase* b2 )
        {
            if(!b1)
                return false;
            
            if(!b2)
                return true;
            
            const base::Time &ts1( b1->latestTimeStamp() );
            const base::Time &ts2( b2->latestTimeStamp() );

            if(ts1 == ts2)
            {
                if(b1->hasData() && !b2->hasData())
                    return true;
                
                if(!b1->hasData() && b2->hasData())
                    return false;
                
                return b1->getPriority() < b2->getPriority();
            }
            
            return ts1 < ts2;
        }

        typedef std::vector<StreamBase*> stream_vector;
        stream_vector streams;
        base::Time timeout;

        base::Time latest_ts;

        base::Time current_ts;

        double buffer_size_factor;

        mutable StreamAlignerStatus status;

    public:
        explicit StreamAligner(base::Time timeout = base::Time::fromSeconds(1))
            : timeout(timeout), buffer_size_factor(2.0) {}

        virtual ~StreamAligner()
        {
            for(stream_vector::iterator it=streams.begin();it != streams.end();it++)
                delete *it;
        }

        void copyState(const StreamAligner& other)
        {
            latest_ts = other.latest_ts;
            current_ts = other.current_ts;

            assert( streams.size() == other.streams.size() );
            for(size_t i=0;i<streams.size();i++)
            {
                bool weGotStream = streams[i];
                bool otherGotStream = other.streams[i];
                if(weGotStream != otherGotStream)
                    throw std::runtime_error("Stream setup of second stream aligner differs");
                    
                if(streams[i])
                {
                    streams[i]->copyState( *other.streams[i] );
                }
            }
        }

        void setTimeout(const base::Time &t )
        {
            timeout = t;
        }

        void disableStream( int idx )
        {
            if(!streams[idx])
                throw std::runtime_error("invalid stream index.");              

            streams[idx]->setActive( false );
        }

        void enableStream( int idx )
        {
            if(!streams[idx])
                throw std::runtime_error("invalid stream index.");              

            streams[idx]->setActive( true );
        }

        bool isStreamActive( int idx ) const 
        {
            if(!streams[idx])
                throw std::runtime_error("invalid stream index.");              

            return streams[idx]->isActive();
        }

        void unregisterStream(int idx)
        {
            if(!streams[idx])
            {
                throw std::runtime_error("invalid stream index.");              
            }
            
            delete streams[idx];
            
            streams[idx] = 0;
            
            status.streams[idx].active = false;
        }

        template <class T> int registerStream( typename Stream<T>::callback_t callback, int bufferSize, base::Time period, int priority  = -1, const std::string &name = std::string()) 
        {
            if( bufferSize < 0 )
            {
                if( period == base::Time() )
                {
                    throw std::runtime_error("No buffer size provided for stream with unknown period.");
                }
                else if( period < base::Time() )
                {
                    // for a negative period, just calculate the buffer size, but don't set any lookahead.
                    bufferSize = buffer_size_factor * std::ceil( timeout.toSeconds() / -period.toSeconds() );
                    period = base::Time();
                }
                else
                {
                    bufferSize = buffer_size_factor * std::ceil( timeout.toSeconds() / period.toSeconds() );
                }
            }

            if( bufferSize == 0 )
            {
                LOG_DEBUG_S << "dynamically allocating stream aligner buffer for stream: " << name;
            }

            StreamBase *newStream = new Stream<T>(callback, bufferSize, period, priority, name);
            
            //check if there is a free slot from a previous deleted stream
            for(size_t i = 0; i < streams.size(); i++)
            {
                if(!streams[i])
                {
                    streams[i] = newStream;
                    status.streams[i] = StreamStatus();
                    return i;
                }
            }
                
            streams.push_back( newStream );
            status.streams.push_back(StreamStatus());
            return streams.size() - 1;
        }
        
        template <class T> void push( int idx, const base::Time &ts, const T& data )
        {
            if( !streams.at(idx) )
                throw std::runtime_error("invalid stream index.");

            Stream<T>* stream = dynamic_cast<Stream<T>*>(streams[idx]);
            assert( stream );

            stream->status.samples_received++;
            stream->status.latest_sample_time = ts;

            // mark stream as active, since it is receiving data items will
            // have no effect on an already active stream, but enables
            // streams which have been marked passive before.
            stream->setActive( true );

            //any sample, that is older than the last replayed sample
            //will never be played back and gets dropped by default
            if(ts < current_ts) 
            {
                status.samples_dropped_late_arriving++;
                stream->status.samples_dropped_late_arriving++;
                return;
            }

            if( ts > latest_ts )
                latest_ts = ts;
            
            stream->push( ts, data );
        }

        template <class T> bool getNextSample( int idx, std::pair<base::Time,T> &sample) const
        {
            if( !streams.at(idx) )
                throw std::runtime_error("invalid stream index.");

            Stream<T>* stream = dynamic_cast<Stream<T>*>(streams[idx]);
            assert( stream );

            return stream->getNextSample(sample);
        }

        bool step()
        {
            if( streams.empty() )
                return false;

            // copy streams vector and sort it by next ts
            stream_vector items = streams;
            std::sort( items.begin(), items.end(), &compareStreams );

            for(stream_vector::iterator it=items.begin();it != items.end();it++)
            {
                //first stream is unregistered no data there
                if(!*it)
                    return false;
                
                if( (*it)->hasData() ) 
                {
                    // if stream has current data, pop that data
                    current_ts = (*it)->pop();
                    return true;
                }
                else if( (*it)->isActive() )
                {

                    base::Time latestDataTime;
                    base::Time firstDataTime;
                    
                    //initalization case
                    if(current_ts == base::Time())
                    {
                        //check if one stream timed out
                        for(stream_vector::iterator it2=items.begin();it2 != items.end();it2++)
                        {
                            
                            if(*it2 && (*it2)->hasData())
                            {
                                if(latestDataTime < (*it2)->latestDataTime())
                                    latestDataTime = (*it2)->latestDataTime();
                                
                                if(firstDataTime == base::Time() || firstDataTime > (*it2)->earliestDataTime())
                                    firstDataTime = (*it2)->earliestDataTime();
                            }
                        }                       
                    } else {
                        latestDataTime = latest_ts;
                        firstDataTime = current_ts;
                    }

                    if(latestDataTime - firstDataTime < timeout)
                    {
                        // if there is no data, but the expected data has
                        // not run out yet, wait for it.
                        return false;
                    }
                }
            }
            return false;
        }

        void clear()
        {
            for(size_t i = 0; i < streams.size(); i++)
            {
                if(streams[i])
                {
                    streams[i]->clear();
                }
            }
            
            latest_ts = base::Time();
            current_ts = base::Time();
            
            status.current_time = base::Time();
            status.latest_time = base::Time();
            status.samples_dropped_late_arriving = 0;
        }

        base::Time getTimeOut() const { return timeout; };
        
        base::Time getLatency() const { return latest_ts - current_ts; };

        base::Time getCurrentTime() const { return current_ts; };

        base::Time getLatestTime() const { return latest_ts; }

        int getStreamSize() const { return streams.size();  } 

        const StreamStatus &getBufferStatus(int idx) const
        {
            if( !streams.at(idx) )
                throw std::runtime_error("invalid stream index.");
            
            return streams[idx]->getBufferStatus();
        }

        const StreamAlignerStatus& getStatus() const 
        {
            status.time = base::Time::now();
            status.current_time = getCurrentTime();
            status.latest_time = getLatestTime();

            for(size_t i=0;i<streams.size();i++)
            {
                if(streams[i])
                    status.streams[i] = streams[i]->getBufferStatus();
            }

            return status;
        }

        friend std::ostream &operator<<(std::ostream &stream, const aggregator::StreamAligner::StreamBase &base);
        friend std::ostream &operator<<(std::ostream &stream, const aggregator::StreamAligner &re);
    };

    inline std::ostream &operator<<(std::ostream &stream, const aggregator::StreamAligner &re)
    {
        using ::operator <<;
        stream << "current time: " << re.getCurrentTime() << " latest time:" << re.getLatestTime() << " latency: " << re.getLatency() << std::endl;
        for(size_t i=0;i<re.streams.size();i++)
        {
            stream << i << ":" << *re.streams[i] << std::endl;
        }

        return stream;
    }

    inline std::ostream &operator<<(std::ostream &stream, const aggregator::StreamAligner::StreamBase &base)
    {
        using ::operator <<;
        stream << base.getBufferStatus();
        return stream;
    }
}

#endif