From f447739488ca41bedff68e7174d55650a886a027 Mon Sep 17 00:00:00 2001
From: John Zhao <johnzhao@slightech.com>
Date: Thu, 31 May 2018 20:36:17 +0800
Subject: [PATCH] Add async callback for motion datas

---
 src/api/api.cc                     |  5 ++---
 src/device/device.cc               |  2 +-
 src/internal/async_callback.h      |  6 ++++--
 src/internal/async_callback_impl.h | 26 +++++++++++++++++++-------
 4 files changed, 26 insertions(+), 13 deletions(-)

diff --git a/src/api/api.cc b/src/api/api.cc
index 9da5d30..bafd3ba 100644
--- a/src/api/api.cc
+++ b/src/api/api.cc
@@ -139,9 +139,8 @@ void API::SetStreamCallback(const Stream &stream, stream_callback_t callback) {
 void API::SetMotionCallback(motion_callback_t callback) {
   static auto callback_ = callback;
   if (callback_) {
-    device_->SetMotionCallback([](const device::MotionData &data) {
-      callback_({data.imu});
-    } /*, true*/);
+    device_->SetMotionCallback(
+        [](const device::MotionData &data) { callback_({data.imu}); }, true);
   } else {
     device_->SetMotionCallback(nullptr);
   }
diff --git a/src/device/device.cc b/src/device/device.cc
index f5de75c..e94c256 100644
--- a/src/device/device.cc
+++ b/src/device/device.cc
@@ -295,7 +295,7 @@ void Device::SetMotionCallback(motion_callback_t callback, bool async) {
   if (callback) {
     if (async)
       motion_async_callback_ =
-          std::make_shared<motion_async_callback_t>("motion", callback);
+          std::make_shared<motion_async_callback_t>("motion", callback, true);
   } else {
     motion_async_callback_ = nullptr;
   }
diff --git a/src/internal/async_callback.h b/src/internal/async_callback.h
index ffcc7e7..43944d2 100644
--- a/src/internal/async_callback.h
+++ b/src/internal/async_callback.h
@@ -20,6 +20,7 @@
 #include <mutex>
 #include <string>
 #include <thread>
+#include <vector>
 
 #include "mynteye/mynteye.h"
 
@@ -30,7 +31,7 @@ class AsyncCallback {
  public:
   using callback_t = std::function<void(Data data)>;
 
-  AsyncCallback(std::string name, callback_t callback);
+  AsyncCallback(std::string name, callback_t callback, bool concat = false);
   ~AsyncCallback();
 
   void PushData(Data data);
@@ -48,8 +49,9 @@ class AsyncCallback {
   bool running_;
   std::thread thread_;
 
-  Data data_;
   std::uint32_t count_;
+  bool concat_;
+  std::vector<Data> datas_;
 };
 
 MYNTEYE_END_NAMESPACE
diff --git a/src/internal/async_callback_impl.h b/src/internal/async_callback_impl.h
index dfdc0bd..23f8ead 100644
--- a/src/internal/async_callback_impl.h
+++ b/src/internal/async_callback_impl.h
@@ -23,8 +23,12 @@
 MYNTEYE_BEGIN_NAMESPACE
 
 template <class Data>
-AsyncCallback<Data>::AsyncCallback(std::string name, callback_t callback)
-    : name_(std::move(name)), callback_(std::move(callback)), count_(0) {
+AsyncCallback<Data>::AsyncCallback(
+    std::string name, callback_t callback, bool concat)
+    : name_(std::move(name)),
+      callback_(std::move(callback)),
+      count_(0),
+      concat_(concat) {
   VLOG(2) << __func__;
   running_ = true;
   thread_ = std::thread(&AsyncCallback<Data>::Run, this);
@@ -47,7 +51,10 @@ AsyncCallback<Data>::~AsyncCallback() {
 template <class Data>
 void AsyncCallback<Data>::PushData(Data data) {
   std::lock_guard<std::mutex> _(mtx_);
-  data_ = data;
+  if (!concat_) {
+    datas_.clear();
+  }
+  datas_.push_back(data);
   ++count_;
   cv_.notify_one();
 }
@@ -62,13 +69,18 @@ void AsyncCallback<Data>::Run() {
     if (!running_)
       break;
 
-    if (callback_)
-      callback_(data_);
+    if (callback_) {
+      for (auto &&data : datas_) {
+        callback_(data);
+      }
+    }
 
-    if (VLOG_IS_ON(2) && count_ > 1) {
-      VLOG(2) << "AsyncCallback(" << name_ << ") dropped " << (count_ - 1);
+    if (VLOG_IS_ON(2) && count_ > datas_.size()) {
+      VLOG(2) << "AsyncCallback(" << name_ << ") dropped "
+              << (count_ - datas_.size());
     }
     count_ = 0;
+    datas_.clear();
   }
   VLOG(2) << "AsyncCallback(" << name_ << ") thread end";
 }