00001
00020 #ifndef SyncFIFO_h
00021 #define SyncFIFO_h
00022
00023 #include <iostream>
00024 #include <vector>
00025 #include <algorithm>
00026
00027 #include <rtm/BufferBase.h>
00028
00029 namespace RTC
00030 {
00031 template <class DataType>
00032 class SyncFIFO
00033 : public BufferBase<DataType>
00034 {
00035 public:
00036 SyncFIFO(int size = 3)
00037 : m_buffer(NULL), m_size(size),
00038 m_buffer_org(NULL), m_size_org(0),
00039 m_wpos(0), m_wnext(1),
00040 m_rpos(0), m_rnext(1),
00041 m_wbegin(m_wnext), m_wend(m_rpos),
00042 m_rbegin(m_rnext), m_rend(m_wpos)
00043 {
00044 if (m_buffer == NULL)
00045 {
00046 m_buffer = new DataType[m_size];
00047 }
00048 }
00049
00050 virtual ~SyncFIFO()
00051 {
00052 if (m_buffer_org == NULL)
00053 {
00054 delete [] m_buffer;
00055 }
00056 else
00057 {
00058 delete [] m_buffer_org;
00059 }
00060 };
00061
00062 virtual void init(const DataType& data)
00063 {
00064 if (m_buffer)
00065 {
00066 for (int i(0); i < m_size; ++i)
00067 {
00068 write(data);
00069 }
00070 }
00071 }
00072
00073
00074 virtual void clear()
00075 {
00076 m_wpos = 0;
00077 m_wnext = 1;
00078 m_rpos = 0;
00079 m_rnext = 1;
00080 }
00081
00082 virtual bool setBuffer(DataType* buffer, int size)
00083 {
00084 if (!buffer) return false;
00085 if (size < 0) return false;
00086
00087 m_size_org = m_size;
00088 m_buffer_org = m_buffer;
00089 m_size = size;
00090 m_buffer = buffer;
00091 clear();
00092 return true;
00093 }
00094
00095 virtual DataType* unsetBuffer()
00096 {
00097 DataType* tmp(m_buffer);
00098 m_size = m_size_org;
00099 m_buffer = m_buffer_org;
00100 return tmp;
00101 }
00102
00103 virtual DataType* getBuffer() const
00104 {
00105 return m_buffer;
00106 }
00107
00108 virtual long int length() const
00109 {
00110 return m_size;
00111 }
00112
00113 virtual bool write(const DataType& data)
00114 {
00115 if (isFull())
00116 {
00117 overflow(data);
00118 return false;
00119 }
00120 put(data);
00121 pbump(1);
00122 return true;
00123 }
00124
00125 virtual int writable()
00126 {
00127 return (m_size + m_wend - m_wbegin) % m_size;
00128 }
00129
00130 virtual void put(const DataType& data)
00131 {
00132 m_buffer[m_wpos] = data;
00133 }
00134
00135 virtual int putn(DataType* data, int size)
00136 {
00137 if (!data) return -1;
00138 if (size < 0) return -1;
00139
00140 int len(size);
00141 if (size > this->writable()) len = this->writable();
00142
00143 for (int i(0); i < len; ++i)
00144 {
00145 m_buffer[m_wpos + i] = data[i];
00146 }
00147 pbump(len);
00148
00149 return len;
00150 }
00151
00152 virtual void putNext(const DataType& data)
00153 {
00154 this->pbump(1);
00155 this->put(data);
00156 }
00157
00158 virtual bool isFull() const
00159 {
00160 return m_wnext == m_rpos;
00161 }
00162
00163 virtual void pbump(int n)
00164 {
00165
00166 m_wpos = (m_wpos + n) % m_size;
00167 m_wnext = (m_wnext + n) % m_size;
00168 }
00169
00170 virtual void overflow(const DataType& data)
00171 {
00172 }
00173
00174
00175
00176
00177 virtual bool read(DataType& data)
00178 {
00179 if (isEmpty())
00180 {
00181 this->underflow();
00182 return false;
00183 }
00184
00185 data = get();
00186 gbump(1);
00187 return true;
00188 }
00189
00190 virtual int readable() const
00191 {
00192 return (m_size + m_rend - m_rbegin + 1) % m_size;
00193 }
00194
00195 virtual const DataType& get()
00196 {
00197 return m_buffer[m_rpos];
00198 }
00199
00200 virtual void gbump(int n)
00201 {
00202 m_rpos = (m_rpos + n) % m_size;
00203 m_rnext = (m_rnext + n) % m_size;
00204 }
00205
00206 virtual const DataType& getNext()
00207 {
00208 this->gbump(1);
00209 return this->get();
00210 }
00211
00212 virtual int getn(DataType* data, int size)
00213 {
00214 if (!data) return -1;
00215 if (size < 0) return -1;
00216
00217 int len(size);
00218 if (size > this->readable()) len = this->readable();
00219
00220 for (int i(0); i < len; ++i)
00221 {
00222 data[i] = m_buffer[m_rpos + i];
00223 }
00224 gbump(len);
00225
00226 return len;
00227 }
00228
00229 virtual bool isEmpty() const
00230 {
00231 return m_rpos == m_wpos;
00232 }
00233
00234 virtual void underflow()
00235 {
00236 ;
00237 }
00238
00239
00240
00241
00242 protected:
00243 virtual DataType* wptr()
00244 {
00245 return &m_buffer[m_wpos];
00246 }
00247
00248 virtual DataType* wnext()
00249 {
00250 return &m_buffer[m_wnext];
00251 }
00252
00253 virtual DataType* wbegin()
00254 {
00255 return &m_buffer[m_wbegin];
00256 }
00257
00258 virtual DataType* wend()
00259 {
00260 return &m_buffer[m_wend];
00261 }
00262
00263 virtual DataType* rptr()
00264 {
00265 return &m_buffer[m_rpos];
00266 }
00267
00268 virtual DataType* rnext()
00269 {
00270 return &m_buffer[m_rnext];
00271 }
00272
00273 virtual DataType* rbegin()
00274 {
00275 return &m_buffer[m_rbegin];
00276 }
00277
00278 virtual DataType* rend()
00279 {
00280 return &m_buffer[m_rend];
00281 }
00282
00283
00284 DataType* m_buffer;
00285 int m_size;
00286
00287 DataType* m_buffer_org;
00288 int m_size_org;
00289
00290
00291 int m_wpos;
00292
00293 int m_wnext;
00294 int m_rpos, m_rnext;
00295 int &m_wbegin, &m_wend;
00296 int &m_rbegin, &m_rend;
00297
00298 };
00299 };
00300
00301 #endif // SyncFIFO_h