Move superframe alignment after ordered queue

4 files changed, 39 insertions, 33 deletions

diff --git a/src/AVTInput.cpp b/src/AVTInput.cpp
index 2610b5c..11dd4cc 100644
--- a/src/AVTInput.cpp
+++ b/src/AVTInput.cpp
@@ -36,8 +36,6 @@
 
 #define MAX_PAD_FRAME_QUEUE_SIZE  (6)
 
-//#define DISTURB_INPUT
-
 // ETSI EN 300 797 V1.2.1 ch 8.2.1.2
 uint8_t STI_FSync0[3] = { 0x1F, 0x90, 0xCA };
 uint8_t STI_FSync1[3] = { 0xE0, 0x6F, 0x35 };
@@ -114,8 +112,6 @@ int AVTInput::setDabPlusParameters(int bitrate, int channels, int sample_rate, b
 
     _currentFrame.clear();
     _currentFrame.resize(_subChannelIndex*8*5*3);
-    _currentFrameSize = 0;
-    _nbFrames = 0;
 
     _sendCtrlMessage();
 
@@ -384,21 +380,7 @@ bool AVTInput::_readFrame()
 
         if (dataPtr) {
             if (dataSize == _dab24msFrameSize) {
-                if (_frameAligned or frameNumber%5 == 0) {
-#if defined(DISTURB_INPUT)
-                    // Duplicate a frame
-                    if (frameNumber % 250 == 0) _ordered.push(frameNumber, dataPtr, dataSize);
-
-                    // Invert 2 frames (content inverted, audio distrubed by this test))
-                    if (frameNumber % 200 == 0) frameNumber += 10;
-                    else if ((frameNumber-10) % 200 == 0) frameNumber -= 10;
-
-                    // Remove a frame (audio distrubed, frame missing)
-                    if (frameNumber % 300 > 5)
-#endif
-                    _ordered.push(frameNumber, dataPtr, dataSize);
-                    _frameAligned = true;
-                }
+                _ordered.push(frameNumber, dataPtr, dataSize);
             }
             else ERROR("Wrong frame size from encoder %zu != %zu\n", dataSize, _dab24msFrameSize);
         }
@@ -422,15 +404,41 @@ ssize_t AVTInput::getNextFrame(std::vector<uint8_t> &buf)
 
     //printf("B: _padFrameQueue size=%zu\n", _padFrameQueue.size());
 
-    // Assemble next frame
-    std::vector<uint8_t> part;
-    while (_nbFrames < 5 and not (part = _ordered.pop()).empty())
-    {
-        while (_checkMessage());
+    // Assemble next frame, ensuring it is composed of five parts with
+    // indexes that are contiguous, and where index%5==0 for the first part.
+    int32_t returnedIndex = -1;
+
+    while (_nbFrames < 5) {
+        auto part = _ordered.pop(&returnedIndex);
+        if (part.empty()) {
+            break;
+        }
 
-        memcpy(_currentFrame.data() + _currentFrameSize, part.data(), part.size());
-        _currentFrameSize += part.size();
-        _nbFrames ++;
+        while (_checkMessage()) {};
+
+        if (not _frameAligned) {
+            if (returnedIndex % 5 == 0) {
+                _frameAligned = true;
+
+                memcpy(_currentFrame.data() + _currentFrameSize, part.data(), part.size());
+                _currentFrameSize += part.size();
+                _nbFrames++;
+            }
+        }
+        else {
+            if (returnedIndex % 5 == _nbFrames) {
+                memcpy(_currentFrame.data() + _currentFrameSize, part.data(), part.size());
+                _currentFrameSize += part.size();
+                _nbFrames++;
+            }
+            else {
+                _nbFrames = 0;
+                _currentFrameSize = 0;
+                _frameAligned = false;
+
+                fprintf(stderr, "Frame alignment reset\n");
+            }
+        }
     }
 
     if (_nbFrames == 5 && _currentFrameSize <= buf.size()) {
diff --git a/src/AVTInput.h b/src/AVTInput.h
index 2921160..e925a80 100644
--- a/src/AVTInput.h
+++ b/src/AVTInput.h
@@ -115,12 +115,10 @@ class AVTInput
         int32_t _monoMode = AVT_MonoMode_LR2;
         int32_t _dac = AVT_DAC_48;
         size_t _dab24msFrameSize = DEF_BR*3;
-        uint32_t _dummyFrameNumber = 0;
         bool _frameAligned = false;
         std::vector<uint8_t> _currentFrame;
-        size_t _currentFrameSize = 0;
         int32_t _nbFrames = 0;
-        uint8_t* _nextFrameIndex = 0;
+        size_t _currentFrameSize = 0;
 
         bool _parseURI(const char* uri, std::string& address, long& port);
         int _openSocketSrv(Socket::UDPSocket* socket, const char* uri);
diff --git a/src/OrderedQueue.cpp b/src/OrderedQueue.cpp
index 9e69d95..eb2cf97 100644
--- a/src/OrderedQueue.cpp
+++ b/src/OrderedQueue.cpp
@@ -73,7 +73,7 @@ bool OrderedQueue::availableData() const
     return _stock.size() > 0;
 }
 
-std::vector<uint8_t> OrderedQueue::pop(int32_t *retCount)
+std::vector<uint8_t> OrderedQueue::pop(int32_t *returnedIndex)
 {
     OrderedQueueData buf;
     uint32_t gap = 0;
@@ -86,7 +86,7 @@ std::vector<uint8_t> OrderedQueue::pop(int32_t *retCount)
                 buf = move(_stock.at(nextIndex));
                 _stock.erase(nextIndex);
                 _lastIndexPop = nextIndex;
-                if (retCount) *retCount = _lastIndexPop;
+                if (returnedIndex) *returnedIndex = _lastIndexPop;
                 found = true;
             }
             catch (const std::out_of_range&) {
diff --git a/src/OrderedQueue.h b/src/OrderedQueue.h
index 4652762..7cc59ee 100644
--- a/src/OrderedQueue.h
+++ b/src/OrderedQueue.h
@@ -40,7 +40,7 @@ class OrderedQueue
         bool availableData() const;
 
         /* Return the next buffer, or an empty buffer if none available */
-        std::vector<uint8_t> pop(int32_t *retCount=nullptr);
+        std::vector<uint8_t> pop(int32_t *returnedIndex=nullptr);
 
         using OrderedQueueData = std::vector<uint8_t>;