Branch data     Line data    Source code

       1                 :            : /*
       2                 :            :  * Copyright (C) 2026 by Thun Lu. All rights reserved.
       3                 :            :  * Author: Thun Lu <thun.lu@zohomail.cn>
       4                 :            :  * Repo:   https://github.com/thun-res/vlink
       5                 :            :  *  _    __   __      _           __
       6                 :            :  * | |  / /  / /     (_) ____    / /__
       7                 :            :  * | | / /  / /     / / / __ \  / //_/
       8                 :            :  * | |/ /  / /___  / / / / / / / ,<
       9                 :            :  * |___/  /_____/ /_/ /_/ /_/ /_/|_|
      10                 :            :  *
      11                 :            :  * Licensed under the Apache License, Version 2.0 (the "License");
      12                 :            :  * you may not use this file except in compliance with the License.
      13                 :            :  * You may obtain a copy of the License at
      14                 :            :  *
      15                 :            :  *     http://www.apache.org/licenses/LICENSE-2.0
      16                 :            :  *
      17                 :            :  * Unless required by applicable law or agreed to in writing, software
      18                 :            :  * distributed under the License is distributed on an "AS IS" BASIS,
      19                 :            :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      20                 :            :  * See the License for the specific language governing permissions and
      21                 :            :  * limitations under the License.
      22                 :            :  */
      23                 :            : 
      24                 :            : #include "./extension/bag_processor.h"
      25                 :            : 
      26                 :            : #include <algorithm>
      27                 :            : #include <atomic>
      28                 :            : #include <deque>
      29                 :            : #include <memory>
      30                 :            : #include <mutex>
      31                 :            : #include <thread>
      32                 :            : #include <utility>
      33                 :            : 
      34                 :            : #include "./base/condition_variable.h"
      35                 :            : #include "./base/logger.h"
      36                 :            : 
      37                 :            : namespace vlink {
      38                 :            : 
      39                 :            : // BagProcessor::Impl
      40                 :            : struct BagProcessor::Impl final {
      41                 :            :   enum class Request : uint8_t {
      42                 :            :     kNone = 0,
      43                 :            :     kFlush = 1,
      44                 :            :     kReset = 2,
      45                 :            :   };
      46                 :            : 
      47                 :            :   struct CacheEntry final {
      48                 :            :     int64_t data_timestamp{0};
      49                 :            :     Frame frame;
      50                 :            :     bool data_timestamp_valid{false};
      51                 :            :   };
      52                 :            : 
      53                 :            :   BagProcessor::Config config;
      54                 :            :   BagProcessor::OutputCallback output_callback;
      55                 :            :   std::deque<CacheEntry> data_queue;
      56                 :            :   std::mutex mtx;
      57                 :            :   ConditionVariable cv;
      58                 :            :   std::thread thread;
      59                 :            : 
      60                 :            :   int64_t current_size{0};
      61                 :            :   int64_t last_data_timestamp{0};
      62                 :            :   int64_t last_timestamp{0};
      63                 :            :   int64_t data_timestamp_anchor{0};
      64                 :            :   int64_t timestamp_anchor{0};
      65                 :            :   int64_t last_output_timestamp{0};
      66                 :            : 
      67                 :            :   std::atomic_bool quit_flag{false};
      68                 :            :   Request request{Request::kNone};
      69                 :            :   bool last_resolved_data_timestamp_valid{false};
      70                 :            :   bool timestamp_anchor_valid{false};
      71                 :            :   bool output_timestamp_valid{false};
      72                 :            : };
      73                 :            : 
      74                 :            : // BagProcessor
      75                 :         48 : BagProcessor::BagProcessor(const Config& config) : impl_(std::make_unique<Impl>()) { impl_->config = config; }
      76                 :            : 
      77                 :         48 : BagProcessor::~BagProcessor() {
      78                 :            :   {
      79                 :         48 :     std::lock_guard lock(impl_->mtx);
      80                 :            : 
      81                 :         48 :     impl_->quit_flag.store(true, std::memory_order_release);
      82                 :         48 :   }
      83                 :            : 
      84                 :         48 :   impl_->cv.notify_all();
      85                 :            : 
      86         [ +  + ]:         48 :   if VLIKELY (impl_->thread.joinable()) {
      87                 :         43 :     impl_->thread.join();
      88                 :            :   }
      89                 :         48 : }
      90                 :            : 
      91                 :         46 : void BagProcessor::register_output_callback(OutputCallback&& output_callback) {
      92         [ +  - ]:         46 :   std::lock_guard lock(impl_->mtx);
      93                 :            : 
      94         [ +  + ]:         46 :   if VUNLIKELY (impl_->output_callback) {
      95   [ +  -  +  -  :          4 :     VLOG_W("BagProcessor output callback has already been registered.");
                   +  - ]
      96                 :          2 :     return;
      97                 :            :   }
      98                 :            : 
      99         [ +  + ]:         44 :   if VUNLIKELY (!output_callback) {
     100   [ +  -  +  -  :          2 :     VLOG_F("BagProcessor output callback is empty.");
                   -  + ]
     101                 :            :   }
     102                 :            : 
     103                 :         43 :   impl_->output_callback = std::move(output_callback);
     104         [ +  - ]:         43 :   impl_->thread = std::thread(&BagProcessor::on_run, this);
     105         [ +  + ]:         46 : }
     106                 :            : 
     107                 :        218 : void BagProcessor::push(int64_t data_timestamp, const Frame& frame) {
     108         [ +  - ]:        218 :   std::unique_lock lock(impl_->mtx);
     109                 :            : 
     110         [ +  + ]:        218 :   if VUNLIKELY (!impl_->output_callback) {
     111   [ +  -  +  -  :          2 :     VLOG_F("BagProcessor output callback has not been registered.");
                   -  + ]
     112                 :            :   }
     113                 :            : 
     114         [ -  + ]:        217 :   if VUNLIKELY (impl_->current_size >= impl_->config.max_cache_size) {
     115                 :            :     VLOG_W("BagProcessor: Cache size is full, waiting to consume.");  // LCOV_EXCL_LINE GCOVR_EXCL_LINE
     116                 :            :   }
     117                 :            : 
     118                 :        217 :   impl_->cv.wait(lock, [this]() -> bool {
     119         [ +  + ]:        217 :     if (impl_->data_queue.empty()) {
     120                 :         56 :       return true;
     121                 :            :     }
     122                 :            : 
     123   [ -  +  -  - ]:        161 :     return impl_->current_size < impl_->config.max_cache_size || impl_->quit_flag.load(std::memory_order_acquire);
     124                 :            :   });
     125                 :            : 
     126         [ -  + ]:        217 :   if VUNLIKELY (impl_->quit_flag.load(std::memory_order_acquire)) {
     127                 :            :     return;  // LCOV_EXCL_LINE GCOVR_EXCL_LINE
     128                 :            :   }
     129                 :            : 
     130                 :        217 :   bool data_timestamp_valid = true;
     131                 :            : 
     132         [ +  + ]:        217 :   if (data_timestamp < 0) {
     133         [ +  + ]:         10 :     if (!impl_->last_resolved_data_timestamp_valid) {
     134                 :          8 :       data_timestamp = -1;
     135                 :          8 :       data_timestamp_valid = false;
     136                 :            :     } else {
     137                 :          2 :       data_timestamp = impl_->last_data_timestamp + (frame.timestamp - impl_->last_timestamp);
     138                 :            :     }
     139         [ +  + ]:        207 :   } else if (impl_->last_resolved_data_timestamp_valid) {
     140                 :        153 :     const int64_t max_jump = impl_->config.max_jump_time * 1000;
     141                 :        153 :     const int64_t jump = data_timestamp - impl_->last_data_timestamp;
     142                 :            : 
     143   [ +  -  +  +  :        153 :     if (max_jump > 0 && (jump > max_jump || jump < -max_jump)) {
                   +  + ]
     144                 :          2 :       data_timestamp = impl_->last_data_timestamp + (frame.timestamp - impl_->last_timestamp);
     145                 :            :     }
     146                 :            :   }
     147                 :            : 
     148         [ +  + ]:        217 :   if (data_timestamp_valid) {
     149                 :        209 :     impl_->last_resolved_data_timestamp_valid = true;
     150                 :        209 :     impl_->last_data_timestamp = data_timestamp;
     151                 :        209 :     impl_->last_timestamp = frame.timestamp;
     152                 :            :   }
     153                 :            : 
     154                 :        217 :   impl_->current_size += frame.data.size();
     155                 :            : 
     156         [ +  - ]:        217 :   Impl::CacheEntry entry{data_timestamp, frame, data_timestamp_valid};
     157                 :            : 
     158                 :        217 :   auto iter = std::upper_bound(impl_->data_queue.begin(), impl_->data_queue.end(), entry,
     159                 :        613 :                                [](const Impl::CacheEntry& candidate, const Impl::CacheEntry& queued) {
     160         [ +  + ]:        613 :                                  if (candidate.data_timestamp_valid != queued.data_timestamp_valid) {
     161                 :          2 :                                    return !candidate.data_timestamp_valid;
     162                 :            :                                  }
     163                 :            : 
     164         [ +  + ]:        611 :                                  if (!candidate.data_timestamp_valid) {
     165                 :          4 :                                    return candidate.frame.timestamp < queued.frame.timestamp;
     166                 :            :                                  }
     167                 :            : 
     168                 :        607 :                                  return candidate.data_timestamp < queued.data_timestamp;
     169         [ +  - ]:        217 :                                });
     170         [ +  - ]:        217 :   impl_->data_queue.emplace(iter, std::move(entry));
     171                 :            : 
     172                 :        217 :   impl_->cv.notify_one();
     173         [ +  - ]:        218 : }
     174                 :            : 
     175                 :         59 : void BagProcessor::flush() {
     176         [ +  - ]:         59 :   std::unique_lock lock(impl_->mtx);
     177                 :            : 
     178   [ +  -  +  +  :         59 :   if VUNLIKELY (impl_->quit_flag.load(std::memory_order_acquire) || !impl_->thread.joinable()) {
                   +  + ]
     179                 :          2 :     return;
     180                 :            :   }
     181                 :            : 
     182                 :         57 :   impl_->cv.wait(lock, [this]() -> bool {
     183   [ -  +  -  - ]:         57 :     return impl_->request == Impl::Request::kNone || impl_->quit_flag.load(std::memory_order_acquire);
     184                 :            :   });
     185                 :            : 
     186         [ -  + ]:         57 :   if VUNLIKELY (impl_->quit_flag.load(std::memory_order_acquire)) {
     187                 :          0 :     return;
     188                 :            :   }
     189                 :            : 
     190                 :         57 :   impl_->request = Impl::Request::kFlush;
     191                 :            : 
     192                 :         57 :   impl_->cv.notify_all();
     193                 :            : 
     194                 :         57 :   impl_->cv.wait(lock, [this]() -> bool {
     195   [ +  +  -  + ]:        114 :     return impl_->request == Impl::Request::kNone || impl_->quit_flag.load(std::memory_order_acquire);
     196                 :            :   });  // LCOV_EXCL_LINE GCOVR_EXCL_LINE
     197         [ +  + ]:         59 : }
     198                 :            : 
     199                 :         20 : void BagProcessor::reset() {
     200         [ +  - ]:         20 :   std::unique_lock lock(impl_->mtx);
     201                 :            : 
     202   [ +  -  +  +  :         20 :   if VUNLIKELY (impl_->quit_flag.load(std::memory_order_acquire) || !impl_->thread.joinable()) {
                   +  + ]
     203                 :          1 :     return;
     204                 :            :   }
     205                 :            : 
     206                 :         19 :   impl_->cv.wait(lock, [this]() -> bool {
     207   [ -  +  -  - ]:         19 :     return impl_->request == Impl::Request::kNone || impl_->quit_flag.load(std::memory_order_acquire);
     208                 :            :   });
     209                 :            : 
     210         [ -  + ]:         19 :   if VUNLIKELY (impl_->quit_flag.load(std::memory_order_acquire)) {
     211                 :          0 :     return;
     212                 :            :   }
     213                 :            : 
     214                 :         19 :   impl_->request = Impl::Request::kReset;
     215                 :            : 
     216                 :         19 :   impl_->cv.notify_all();
     217                 :            : 
     218                 :         19 :   impl_->cv.wait(lock, [this]() -> bool {
     219   [ +  +  -  + ]:         38 :     return impl_->request == Impl::Request::kNone || impl_->quit_flag.load(std::memory_order_acquire);
     220                 :            :   });  // LCOV_EXCL_LINE GCOVR_EXCL_LINE
     221         [ +  + ]:         20 : }
     222                 :            : 
     223                 :        160 : bool BagProcessor::on_check() {
     224         [ +  + ]:        160 :   if (impl_->data_queue.empty()) {
     225                 :            :     return false;  // LCOV_EXCL_LINE GCOVR_EXCL_LINE
     226                 :            :   }
     227                 :            : 
     228         [ +  + ]:         68 :   if (impl_->current_size >= impl_->config.max_cache_size) {
     229                 :          1 :     return true;
     230                 :            :   }
     231                 :            : 
     232                 :         67 :   const int64_t min_cache_time = impl_->config.min_cache_time * 1000;
     233                 :         67 :   const auto& oldest = impl_->data_queue.front();
     234                 :         67 :   const auto& newest = impl_->data_queue.back();
     235                 :            : 
     236         [ +  + ]:         67 :   if (oldest.data_timestamp_valid != newest.data_timestamp_valid) {
     237                 :          1 :     return true;
     238                 :            :   }
     239                 :            : 
     240         [ +  + ]:         66 :   const int64_t oldest_timestamp = oldest.data_timestamp_valid ? oldest.data_timestamp : oldest.frame.timestamp;
     241         [ +  + ]:         66 :   const int64_t newest_timestamp = newest.data_timestamp_valid ? newest.data_timestamp : newest.frame.timestamp;
     242                 :            : 
     243                 :         66 :   return newest_timestamp - oldest_timestamp >= min_cache_time;
     244                 :            : }
     245                 :            : 
     246                 :        112 : void BagProcessor::on_output(std::unique_lock<std::mutex>& lock, bool at_end) {
     247         [ +  + ]:        112 :   if (impl_->data_queue.empty()) {
     248                 :         53 :     return;
     249                 :            :   }
     250                 :            : 
     251                 :            :   do {
     252                 :        204 :     const int64_t min_cache_time = impl_->config.min_cache_time * 1000;
     253   [ +  +  +  + ]:        204 :     const bool flush_all = at_end || impl_->current_size >= impl_->config.max_cache_size;
     254                 :        204 :     bool should_output = flush_all;
     255                 :            : 
     256         [ +  + ]:        204 :     if (!should_output) {
     257                 :         11 :       const auto& oldest = impl_->data_queue.front();
     258                 :         11 :       const auto& newest = impl_->data_queue.back();
     259                 :            : 
     260         [ +  + ]:         11 :       if (oldest.data_timestamp_valid != newest.data_timestamp_valid) {
     261                 :          1 :         should_output = true;
     262                 :            :       } else {
     263         [ +  + ]:         10 :         const int64_t oldest_timestamp = oldest.data_timestamp_valid ? oldest.data_timestamp : oldest.frame.timestamp;
     264         [ +  + ]:         10 :         const int64_t newest_timestamp = newest.data_timestamp_valid ? newest.data_timestamp : newest.frame.timestamp;
     265                 :         10 :         const int64_t timestamp_span = newest_timestamp - oldest_timestamp;
     266                 :            : 
     267   [ +  -  +  - ]:         10 :         should_output = timestamp_span >= min_cache_time && oldest_timestamp <= newest_timestamp - min_cache_time;
     268                 :            :       }
     269                 :            : 
     270         [ -  + ]:         11 :       if (!should_output) {
     271                 :            :         return;  // LCOV_EXCL_LINE GCOVR_EXCL_LINE
     272                 :            :       }
     273                 :            :     }
     274                 :            : 
     275                 :        204 :     auto entry = std::move(impl_->data_queue.front());
     276                 :        204 :     impl_->data_queue.pop_front();
     277                 :            : 
     278                 :        204 :     impl_->current_size -= entry.frame.data.size();
     279                 :            : 
     280                 :        204 :     Frame frame = std::move(entry.frame);
     281                 :        204 :     int64_t output_timestamp = frame.timestamp;
     282                 :        204 :     bool update_timestamp_anchor = false;
     283                 :            : 
     284   [ +  +  +  +  :        204 :     if (entry.data_timestamp_valid && !impl_->timestamp_anchor_valid) {
                   +  + ]
     285                 :         45 :       impl_->data_timestamp_anchor = entry.data_timestamp;
     286                 :         45 :       update_timestamp_anchor = true;
     287         [ +  + ]:        159 :     } else if (entry.data_timestamp_valid) {
     288                 :        151 :       output_timestamp = impl_->timestamp_anchor + (entry.data_timestamp - impl_->data_timestamp_anchor);
     289                 :            :     }
     290                 :            : 
     291   [ +  +  +  +  :        204 :     if (impl_->output_timestamp_valid && output_timestamp <= impl_->last_output_timestamp) {
                   +  + ]
     292                 :          2 :       output_timestamp = impl_->last_output_timestamp + 1;
     293                 :            :     }
     294                 :            : 
     295         [ +  + ]:        204 :     if (update_timestamp_anchor) {
     296                 :         45 :       impl_->timestamp_anchor_valid = true;
     297                 :         45 :       impl_->timestamp_anchor = output_timestamp;
     298                 :            :     }
     299                 :            : 
     300                 :        204 :     impl_->output_timestamp_valid = true;
     301                 :        204 :     impl_->last_output_timestamp = output_timestamp;
     302                 :        204 :     frame.timestamp = output_timestamp;
     303                 :            : 
     304         [ +  - ]:        204 :     lock.unlock();
     305                 :            : 
     306         [ +  - ]:        204 :     impl_->output_callback(frame);
     307                 :            : 
     308         [ +  - ]:        204 :     lock.lock();
     309   [ +  +  +  +  :        204 :   } while (at_end && !impl_->data_queue.empty());
                   +  + ]
     310                 :            : }
     311                 :            : 
     312                 :         43 : void BagProcessor::on_run() {
     313         [ +  + ]:        174 :   while (!impl_->quit_flag.load(std::memory_order_acquire)) {
     314                 :        131 :     on_exec(false);
     315                 :            :   }
     316                 :            : 
     317                 :         43 :   on_exec(true);
     318                 :         43 : }
     319                 :            : 
     320                 :        174 : void BagProcessor::on_exec(bool at_end) {
     321         [ +  - ]:        174 :   std::unique_lock lock(impl_->mtx);
     322                 :            : 
     323         [ +  + ]:        174 :   if VLIKELY (!at_end) {
     324         [ +  - ]:        131 :     impl_->cv.wait(lock, [this]() -> bool {
     325   [ +  +  +  +  :        279 :       return impl_->quit_flag.load(std::memory_order_acquire) || impl_->request != Impl::Request::kNone || on_check();
                   +  + ]
     326                 :            :     });
     327                 :            : 
     328         [ +  + ]:        131 :     if VUNLIKELY (impl_->quit_flag.load(std::memory_order_acquire)) {
     329                 :         43 :       return;
     330                 :            :     }
     331                 :            : 
     332         [ +  + ]:         88 :     if VUNLIKELY (impl_->request != Impl::Request::kNone) {
     333         [ +  + ]:         76 :       if (impl_->request == Impl::Request::kFlush) {
     334         [ +  - ]:         57 :         on_output(lock, true);
     335                 :            :       } else {
     336                 :         19 :         impl_->data_queue.clear();
     337                 :         19 :         impl_->current_size = 0;
     338                 :            :       }
     339                 :            : 
     340         [ +  - ]:         76 :       reset_timeline();
     341                 :         76 :       impl_->request = Impl::Request::kNone;
     342                 :            : 
     343                 :         76 :       impl_->cv.notify_all();
     344                 :            : 
     345                 :         76 :       return;
     346                 :            :     }
     347                 :            :   }
     348                 :            : 
     349         [ +  - ]:         55 :   on_output(lock, at_end);
     350                 :            : 
     351         [ +  + ]:         55 :   if VLIKELY (!at_end) {
     352                 :         12 :     impl_->cv.notify_all();
     353                 :            :   }
     354         [ +  + ]:        174 : }
     355                 :            : 
     356                 :         76 : void BagProcessor::reset_timeline() {
     357                 :         76 :   impl_->last_data_timestamp = 0;
     358                 :         76 :   impl_->last_timestamp = 0;
     359                 :         76 :   impl_->data_timestamp_anchor = 0;
     360                 :         76 :   impl_->timestamp_anchor = 0;
     361                 :         76 :   impl_->last_output_timestamp = 0;
     362                 :         76 :   impl_->last_resolved_data_timestamp_valid = false;
     363                 :         76 :   impl_->timestamp_anchor_valid = false;
     364                 :         76 :   impl_->output_timestamp_valid = false;
     365                 :         76 : }
     366                 :            : 
     367                 :            : }  // namespace vlink