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                 :        617 :                                [](const Impl::CacheEntry& candidate, const Impl::CacheEntry& queued) {
     160         [ +  + ]:        617 :                                  if (candidate.data_timestamp_valid != queued.data_timestamp_valid) {
     161                 :          2 :                                    return !candidate.data_timestamp_valid;
     162                 :            :                                  }
     163                 :            : 
     164         [ +  + ]:        615 :                                  if (!candidate.data_timestamp_valid) {
     165                 :          4 :                                    return candidate.frame.timestamp < queued.frame.timestamp;
     166                 :            :                                  }
     167                 :            : 
     168                 :        611 :                                  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   [ +  +  -  + ]:        115 :     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                 :         76 : void BagProcessor::reset_timeline() {
     224                 :         76 :   impl_->last_data_timestamp = 0;
     225                 :         76 :   impl_->last_timestamp = 0;
     226                 :         76 :   impl_->data_timestamp_anchor = 0;
     227                 :         76 :   impl_->timestamp_anchor = 0;
     228                 :         76 :   impl_->last_output_timestamp = 0;
     229                 :         76 :   impl_->last_resolved_data_timestamp_valid = false;
     230                 :         76 :   impl_->timestamp_anchor_valid = false;
     231                 :         76 :   impl_->output_timestamp_valid = false;
     232                 :         76 : }
     233                 :            : 
     234                 :        179 : bool BagProcessor::on_check() {
     235         [ +  + ]:        179 :   if (impl_->data_queue.empty()) {
     236                 :            :     return false;  // LCOV_EXCL_LINE GCOVR_EXCL_LINE
     237                 :            :   }
     238                 :            : 
     239         [ +  + ]:         85 :   if (impl_->current_size >= impl_->config.max_cache_size) {
     240                 :          1 :     return true;
     241                 :            :   }
     242                 :            : 
     243                 :         84 :   const int64_t min_cache_time = impl_->config.min_cache_time * 1000;
     244                 :         84 :   const auto& oldest = impl_->data_queue.front();
     245                 :         84 :   const auto& newest = impl_->data_queue.back();
     246                 :            : 
     247         [ +  + ]:         84 :   if (oldest.data_timestamp_valid != newest.data_timestamp_valid) {
     248                 :          1 :     return true;
     249                 :            :   }
     250                 :            : 
     251         [ +  + ]:         83 :   const int64_t oldest_timestamp = oldest.data_timestamp_valid ? oldest.data_timestamp : oldest.frame.timestamp;
     252         [ +  + ]:         83 :   const int64_t newest_timestamp = newest.data_timestamp_valid ? newest.data_timestamp : newest.frame.timestamp;
     253                 :            : 
     254                 :         83 :   return newest_timestamp - oldest_timestamp >= min_cache_time;
     255                 :            : }
     256                 :            : 
     257                 :        113 : void BagProcessor::on_output(std::unique_lock<std::mutex>& lock, bool at_end) {
     258         [ +  + ]:        113 :   if (impl_->data_queue.empty()) {
     259                 :         53 :     return;
     260                 :            :   }
     261                 :            : 
     262                 :            :   do {
     263                 :        204 :     const int64_t min_cache_time = impl_->config.min_cache_time * 1000;
     264   [ +  +  +  + ]:        204 :     const bool flush_all = at_end || impl_->current_size >= impl_->config.max_cache_size;
     265                 :        204 :     bool should_output = flush_all;
     266                 :            : 
     267         [ +  + ]:        204 :     if (!should_output) {
     268                 :         12 :       const auto& oldest = impl_->data_queue.front();
     269                 :         12 :       const auto& newest = impl_->data_queue.back();
     270                 :            : 
     271         [ +  + ]:         12 :       if (oldest.data_timestamp_valid != newest.data_timestamp_valid) {
     272                 :          1 :         should_output = true;
     273                 :            :       } else {
     274         [ +  + ]:         11 :         const int64_t oldest_timestamp = oldest.data_timestamp_valid ? oldest.data_timestamp : oldest.frame.timestamp;
     275         [ +  + ]:         11 :         const int64_t newest_timestamp = newest.data_timestamp_valid ? newest.data_timestamp : newest.frame.timestamp;
     276                 :         11 :         const int64_t timestamp_span = newest_timestamp - oldest_timestamp;
     277                 :            : 
     278   [ +  -  +  - ]:         11 :         should_output = timestamp_span >= min_cache_time && oldest_timestamp <= newest_timestamp - min_cache_time;
     279                 :            :       }
     280                 :            : 
     281         [ -  + ]:         12 :       if (!should_output) {
     282                 :            :         return;  // LCOV_EXCL_LINE GCOVR_EXCL_LINE
     283                 :            :       }
     284                 :            :     }
     285                 :            : 
     286                 :        204 :     auto entry = std::move(impl_->data_queue.front());
     287                 :        204 :     impl_->data_queue.pop_front();
     288                 :            : 
     289                 :        204 :     impl_->current_size -= entry.frame.data.size();
     290                 :            : 
     291                 :        204 :     Frame frame = std::move(entry.frame);
     292                 :        204 :     int64_t output_timestamp = frame.timestamp;
     293                 :        204 :     bool update_timestamp_anchor = false;
     294                 :            : 
     295   [ +  +  +  +  :        204 :     if (entry.data_timestamp_valid && !impl_->timestamp_anchor_valid) {
                   +  + ]
     296                 :         45 :       impl_->data_timestamp_anchor = entry.data_timestamp;
     297                 :         45 :       update_timestamp_anchor = true;
     298         [ +  + ]:        159 :     } else if (entry.data_timestamp_valid) {
     299                 :        151 :       output_timestamp = impl_->timestamp_anchor + (entry.data_timestamp - impl_->data_timestamp_anchor);
     300                 :            :     }
     301                 :            : 
     302   [ +  +  +  +  :        204 :     if (impl_->output_timestamp_valid && output_timestamp <= impl_->last_output_timestamp) {
                   +  + ]
     303                 :          2 :       output_timestamp = impl_->last_output_timestamp + 1;
     304                 :            :     }
     305                 :            : 
     306         [ +  + ]:        204 :     if (update_timestamp_anchor) {
     307                 :         45 :       impl_->timestamp_anchor_valid = true;
     308                 :         45 :       impl_->timestamp_anchor = output_timestamp;
     309                 :            :     }
     310                 :            : 
     311                 :        204 :     impl_->output_timestamp_valid = true;
     312                 :        204 :     impl_->last_output_timestamp = output_timestamp;
     313                 :        204 :     frame.timestamp = output_timestamp;
     314                 :            : 
     315         [ +  - ]:        204 :     lock.unlock();
     316                 :            : 
     317         [ +  - ]:        204 :     impl_->output_callback(frame);
     318                 :            : 
     319         [ +  - ]:        204 :     lock.lock();
     320   [ +  +  +  +  :        204 :   } while (at_end && !impl_->data_queue.empty());
                   +  + ]
     321                 :            : }
     322                 :            : 
     323                 :         43 : void BagProcessor::on_run() {
     324         [ +  + ]:        175 :   while (!impl_->quit_flag.load(std::memory_order_acquire)) {
     325                 :        132 :     on_exec(false);
     326                 :            :   }
     327                 :            : 
     328                 :         43 :   on_exec(true);
     329                 :         43 : }
     330                 :            : 
     331                 :        175 : void BagProcessor::on_exec(bool at_end) {
     332         [ +  - ]:        175 :   std::unique_lock lock(impl_->mtx);
     333                 :            : 
     334         [ +  + ]:        175 :   if VLIKELY (!at_end) {
     335         [ +  - ]:        132 :     impl_->cv.wait(lock, [this]() -> bool {
     336   [ +  +  +  +  :        298 :       return impl_->quit_flag.load(std::memory_order_acquire) || impl_->request != Impl::Request::kNone || on_check();
                   +  + ]
     337                 :            :     });
     338                 :            : 
     339         [ +  + ]:        132 :     if VUNLIKELY (impl_->quit_flag.load(std::memory_order_acquire)) {
     340                 :         43 :       return;
     341                 :            :     }
     342                 :            : 
     343         [ +  + ]:         89 :     if VUNLIKELY (impl_->request != Impl::Request::kNone) {
     344         [ +  + ]:         76 :       if (impl_->request == Impl::Request::kFlush) {
     345         [ +  - ]:         57 :         on_output(lock, true);
     346                 :            :       } else {
     347                 :         19 :         impl_->data_queue.clear();
     348                 :         19 :         impl_->current_size = 0;
     349                 :            :       }
     350                 :            : 
     351         [ +  - ]:         76 :       reset_timeline();
     352                 :         76 :       impl_->request = Impl::Request::kNone;
     353                 :            : 
     354                 :         76 :       impl_->cv.notify_all();
     355                 :            : 
     356                 :         76 :       return;
     357                 :            :     }
     358                 :            :   }
     359                 :            : 
     360         [ +  - ]:         56 :   on_output(lock, at_end);
     361                 :            : 
     362         [ +  + ]:         56 :   if VLIKELY (!at_end) {
     363                 :         13 :     impl_->cv.notify_all();
     364                 :            :   }
     365         [ +  + ]:        175 : }
     366                 :            : 
     367                 :            : }  // namespace vlink