get rid of video buffers; refactor; add garbage code

4 files changed, 147 insertions, 80 deletions

diff --git a/src/camera/icamera.h b/src/camera/icamera.h
index 6ceb520..545daf9 100644
--- a/src/camera/icamera.h
+++ b/src/camera/icamera.h
@@ -50,6 +50,7 @@ public:
     [[nodiscard]] virtual std::optional<float> get_gain() = 0;
 
     [[nodiscard]] virtual bool getImage(Image *image) = 0;
+    [[nodiscard]] virtual std::shared_ptr<Image> getImage() = 0;
 
 public:
     [[nodiscard]] virtual bool startStream() = 0;
diff --git a/src/camera/veyeimx287m.cpp b/src/camera/veyeimx287m.cpp
index 6491d9d..04ee7d2 100644
--- a/src/camera/veyeimx287m.cpp
+++ b/src/camera/veyeimx287m.cpp
@@ -16,6 +16,7 @@
 // orpheus
 #include "camera/veye_i2c.h"
 #include "constants.h"
+#include "pixels.h"
 #include "protocols/httpserver.h"
 #include "veyeimx287m_types.h"
 
@@ -196,7 +197,12 @@ VeyeIMX287m::VeyeIMX287m() {}
 
 VeyeIMX287m::~VeyeIMX287m()
 {
-    for (auto &t : m_calcThreads) {
+    for (auto &t : m_rotateThreads) {
+        t.request_stop();
+        t.join();
+    }
+
+    for (auto &t : m_getThreads) {
         t.request_stop();
         t.join();
     }
@@ -211,19 +217,11 @@ VeyeIMX287m::~VeyeIMX287m()
         std::cout << "cannot stop stream" << std::endl;
     }
 
-#ifdef RADXA_ZERO_3E
-    for (const auto buffer : m_buffers) {
+    for (const auto &buffer : m_buffers) {
         if (munmap(buffer.mem, radxa_raw_img_size) < 0) {
             DBG("Munmap failed!!.");
         }
     }
-#else
-    for (const auto buffer : m_videoBuffers) {
-        if (munmap(buffer, img_size) < 0) {
-            DBG("Munmap failed!!.");
-        }
-    }
-#endif // RADXA_ZERO_3E
 
     if (m_cam_fd >= 0) {
         if (close(m_cam_fd) == -1) {
@@ -273,8 +271,12 @@ bool VeyeIMX287m::startStream()
         return false;
     }
 
-    for (auto &t : m_calcThreads) {
-        t = std::jthread{&VeyeIMX287m::calcFrameLoop, this};
+    for (auto &t : m_getThreads) {
+        t = std::jthread{&VeyeIMX287m::getFrameLoop, this};
+    }
+
+    for (auto &t : m_rotateThreads) {
+        t = std::jthread{&VeyeIMX287m::rotateFrameLoop, this};
     }
 
     std::cout << __func__ << " - OK" << std::endl;
@@ -551,15 +553,8 @@ bool VeyeIMX287m::initCam()
             return false;
         }
 
-        m_videoBuffers[i] = (uint8_t *) m_buffers[i].mem;
-
-        auto p1 = (Image::radxa_data_t *) m_videoBuffers[i];
-        auto p2 = (Image::radxa_data_t *) m_buffers[i].mem;
-        if (p1 != p2) {
-            std::cout << "AAAAAAA: " << (void *) p1 << ' ' << (void *) p2 << std::endl;
-        }
-        m_buffers[i].size[i] = length;
-        m_buffers[i].padding[i] = 0;
+        m_buffers[i].size = length;
+        m_buffers[i].padding = 0;
 
         printf("Buffer mapped at address %p.\n", m_buffers[i].mem);
 
@@ -592,7 +587,7 @@ bool VeyeIMX287m::initI2C()
 //     return m_httpServer != nullptr;
 // }
 
-void VeyeIMX287m::calcFrameLoop(std::stop_token stopToken)
+void VeyeIMX287m::getFrameLoop(std::stop_token stopToken)
 {
     QElapsedTimer t;
 
@@ -603,38 +598,62 @@ void VeyeIMX287m::calcFrameLoop(std::stop_token stopToken)
             continue;
         }
 
+        // std::cout << __func__ << " acquire" << std::endl;
+        // std::cout << __func__ << " acquired" << std::endl;
         // ++processedCounter;
         // continue;
 
-        std::lock_guard lock{m_imageMutexes[bufferIdx]};
+        // std::lock_guard lock{m_imageMutexes[bufferIdx]};
 
-        auto &image = m_images[bufferIdx];
+        // auto &image = m_images[bufferIdx];
+        const auto &image = m_buffers[bufferIdx].image;
 
         {
             t.start();
 
-            auto p1 = (void *) m_videoBuffers[bufferIdx];
-            auto p2 = (void *) m_buffers[bufferIdx].mem;
-
-            // if (p1 != p2) {
-            //     std::cout << (void *) p1 << ' ' << (void *) p2 << std::endl;
-            // }
-
-            auto &src = *(Image::radxa_data_t *) m_videoBuffers[bufferIdx];
-            // auto &src = *(Image::radxa_data_t *) buffers[bufferIdx].mem[0];
-            auto &dst = image.data;
+            auto &src = *(Image::radxa_data_t *) m_buffers[bufferIdx].mem;
+            auto &dst = image->data;
             Image::copy(dst, src);
 
             get_elapsed_ns += t.nsecsElapsed();
         }
 
-        image.rotate();
-        const auto pixels = image.sharedPixels();
+        // m_receiverCalculatorSem.main2calc.release();
+        // std::cout << __func__ << ": main2calc.release" << std::endl;
+        // m_receiverCalculatorSem.bufferIdx = bufferIdx;
+        // m_receiverCalculatorSem.calc2main.acquire();
+        // std::cout << __func__ << ": calc2main.acquire" << std::endl;
+        // std::cout << __func__ << " released" << std::endl;
 
         ++processedCounter;
+        // return;
+
+        image->rotate();
+        const auto pixels = image->sharedPixels();
+
+    }
+}
+
+void VeyeIMX287m::rotateFrameLoop(std::stop_token stopToken)
+{
+    return;
+    while (!stopToken.stop_requested()) {
+        std::cout << __func__ << std::endl;
+
+        m_receiverCalculatorSem.main2calc.acquire();
+        std::cout << __func__ << ": main2calc.acquired" << std::endl;
+        const auto idx = m_receiverCalculatorSem.bufferIdx;
+        m_receiverCalculatorSem.calc2main.release();
+        std::cout << __func__ << ": calc2main.released" << std::endl;
+
+        const auto &image = m_buffers[idx].image;
+        image->rotate();
+        std::cout << "rotated" << std::endl;
+        const auto pixels = image->sharedPixels();
     }
 }
 
+// TODO: check if some of buffers are being overritten during processing
 bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
 // TODO: get Image from video_buffer_ptr
 {
@@ -716,20 +735,22 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
     }
 
     imageIndex = buf.index;
-    auto &image = m_images[buf.index];
-    image.height = img_height;
-    image.width = img_width;
+    // auto &image = m_images[buf.index];
+    const auto &image = m_buffers[buf.index].image;
+    image->height = img_height;
+    image->width = img_width;
     // TODO: fill
     // image.counters.encoderPosition = RotaryEncoder::instance()->position();
-    image.counters.measurementCounter = buf.sequence;
+    image->counters.measurementCounter = buf.sequence;
 
     // FIXME: git rid of static vars
-    static int64_t prevCounter = buf.sequence;
-    dropped_count += buf.sequence - prevCounter - 1;
-    prevCounter = buf.sequence;
+    // if (not m_previousFrameCounter.has_value()) {
+    //     m_previousFrameCounter = buf.sequence;
+    // }
+    dropped_count += buf.sequence - m_previousFrameCounter.value_or(buf.sequence) - 1;
+    m_previousFrameCounter = buf.sequence;
 
-    image.counters.timestampUs = buf.timestamp.tv_sec * 1000 * 1000
-                                 + buf.timestamp.tv_usec;
+    image->counters.timestampUs = buf.timestamp.tv_sec * 1000 * 1000 + buf.timestamp.tv_usec;
 
     {
         std::lock_guard<std::mutex> lock(m_camMtx);
@@ -761,12 +782,14 @@ bool VeyeIMX287m::getImage(Image *image)
 
     // TODO: remove this bullshit. return ptr to image or copy image metainfo
     // only, then copy data
-    *image = std::move(m_images[bufferIdx]);
+    // *image = std::move(m_images[bufferIdx]);
+    *image = std::move(*m_buffers[bufferIdx].image);
     {
         QElapsedTimer t;
         t.start();
         std::lock_guard lock{m_imageMutexes[bufferIdx]};
-        auto &src = *(Image::radxa_data_t *) m_videoBuffers[bufferIdx];
+
+        auto &src = *(Image::radxa_data_t *) m_buffers[bufferIdx].mem;
         auto &dst = image->data;
         Image::copy(dst, src);
         get_elapsed_ns += t.nsecsElapsed();
@@ -774,3 +797,31 @@ bool VeyeIMX287m::getImage(Image *image)
 
     return true;
 }
+
+std::shared_ptr<Image> VeyeIMX287m::getImage()
+{
+    std::shared_ptr<Image> result;
+
+    size_t bufferIdx{};
+
+    if (!dequeueImageBuffer(bufferIdx)) {
+        return {};
+    }
+
+    // TODO: remove this bullshit. return ptr to image or copy image metainfo
+    // only, then copy data
+    // *image = std::move(m_images[bufferIdx]);
+    result = m_buffers[bufferIdx].image;
+    {
+        QElapsedTimer t;
+        t.start();
+        std::lock_guard lock{m_imageMutexes[bufferIdx]};
+
+        auto &src = *(Image::radxa_data_t *) m_buffers[bufferIdx].mem;
+        auto &dst = result->data;
+        Image::copy(dst, src);
+        get_elapsed_ns += t.nsecsElapsed();
+    }
+
+    return result;
+}
diff --git a/src/camera/veyeimx287m.h b/src/camera/veyeimx287m.h
index 7946d2f..ab7e3e5 100644
--- a/src/camera/veyeimx287m.h
+++ b/src/camera/veyeimx287m.h
@@ -6,6 +6,7 @@
 #include <thread>
 
 #include <QReadWriteLock>
+#include <QSemaphore>
 
 #include "constants.h"
 #include "image.h"
@@ -40,6 +41,7 @@ public:
     bool dequeueImageBuffer(size_t &image);
     // bool getImage(Image &image);
     bool getImage(Image *image) override;
+    std::shared_ptr<Image> getImage() override;
 
     bool init();
 
@@ -71,7 +73,8 @@ private:
     bool initI2C();
     // bool initHttpServer();
 
-    void calcFrameLoop(std::stop_token stopToken);
+    void getFrameLoop(std::stop_token stopToken);
+    void rotateFrameLoop(std::stop_token stopToken);
 
 private:
     /*!
@@ -79,42 +82,47 @@ private:
      */
     int m_cam_fd{-1};
 
-    static constexpr uint8_t BUFFER_COUNT{16};
+    /*!
+     * \brief m_previousFrameCounter - used to detect dropped frames
+     */
+    std::optional<int64_t> m_previousFrameCounter{};
 
-    struct MA
-    {
-        QReadWriteLock rwLock;
-        Image image;
-    } bbbbb[BUFFER_COUNT];
+    static constexpr uint8_t BUFFER_COUNT{16};
 
-    std::array<Image, BUFFER_COUNT> m_images;
+    // std::array<Image, BUFFER_COUNT> m_images;
     /*!
      * \brief m_imageMutexes - lock while processing image from m_images
      */
     std::array<std::mutex, BUFFER_COUNT> m_imageMutexes;
-    /*!
-     * \todo copy image right after dequeue to avoid situation with ioctl writing
-     * to m_videoBuffers[i] which is being copied to m_images[i]. In theory, it
-     * should not overlap if BUFFER_COUNT > theads count
-     */
-    std::array<uint8_t *, BUFFER_COUNT> m_videoBuffers{0};
 
     struct buffer
     {
-        unsigned int padding[VIDEO_MAX_PLANES]{std::numeric_limits<unsigned int>::max()};
-        unsigned int size[VIDEO_MAX_PLANES]{std::numeric_limits<unsigned int>::max()};
-        // void *mem[VIDEO_MAX_PLANES]{nullptr};
+        unsigned int padding{0};
+        unsigned int size{0};
         void *mem{nullptr};
+        std::shared_ptr<Image> image{std::make_shared<Image>()};
     };
     std::vector<buffer> m_buffers;
 
+    struct Semaphore
+    {
+        const uint8_t maxSize{BUFFER_COUNT};
+        uint8_t bufferIdx{std::numeric_limits<decltype(bufferIdx)>::max()};
+        std::binary_semaphore main2calc{0};
+        std::binary_semaphore calc2main{0};
+    } m_receiverCalculatorSem;
+
     // std::mutex m_queueMtx;
     std::mutex m_camMtx;
-    std::queue<size_t> m_buffersQueue;
+    /*!
+     * \brief m_buffersQueue - queue of buffers which require extracting pixels
+     */
+    std::queue<std::remove_const<decltype(BUFFER_COUNT)>> m_buffersQueue;
 
     std::jthread m_streamThread;
-    // std::jthread m_calcThreads[1];
-    std::jthread m_calcThreads[4];
+    // std::jthread m_getThreads[1];
+    std::jthread m_getThreads[4];
+    std::jthread m_rotateThreads[1];
 
     std::shared_ptr<veye::imx287m::i2c> m_i2c;
     std::shared_ptr<HttpServer> m_httpServer;
diff --git a/src/protocols/httpserver.cpp b/src/protocols/httpserver.cpp
index d10518b..96b87db 100644
--- a/src/protocols/httpserver.cpp
+++ b/src/protocols/httpserver.cpp
@@ -76,14 +76,17 @@ void HttpServer::stop()
 
 QHttpServerResponse HttpServer::GET_image()
 {
-    Image img;
+    // Image img;
 
-    if (!m_camera->getImage(&img)) {
+    const auto image = m_camera->getImage();
+
+    // if (!m_camera->getImage(&img)) {
+    if (!image) {
         qCritical() << "cannot get image";
         return QHttpServerResponse::StatusCode::ServiceUnavailable;
     }
 
-    pgm_save(&img);
+    pgm_save(image.get());
     std::lock_guard<std::mutex> lg(pgm_image_mtx);
 
     return QHttpServerResponse{QByteArray{(const char *) (pgm_image),
@@ -108,7 +111,14 @@ QHttpServerResponse HttpServer::GET_pixels()
                        }};
     }
 
-    Image img;
+    // Image img;
+    if (!m_camera) {
+        qWarning() << "NO CAM";
+        return QHttpServerResponse::StatusCode::ServiceUnavailable;
+    }
+
+    const auto image = m_camera->getImage();
+
     {
         // const auto sharedCam = m_camera.lock();
         // FIME: don't cast anything, use interface
@@ -116,15 +126,12 @@ QHttpServerResponse HttpServer::GET_pixels()
         // auto cam = dynamic_cast<VeyeIMX287m *>(m_camera);
 
         // if (!cam) {
-        if (!m_camera) {
-            qWarning() << "NO CAM";
-            return QHttpServerResponse::StatusCode::ServiceUnavailable;
-        }
 
         // yeaah
         // ::img = &img;
         // if (!cam->getImage(img)) {
-        if (!m_camera->getImage(&img)) {
+        // if (!m_camera->getImage(&img)) {
+        if (!image) {
             qWarning() << "cannot get image";
             return QHttpServerResponse::StatusCode::ServiceUnavailable;
         }
@@ -136,7 +143,7 @@ QHttpServerResponse HttpServer::GET_pixels()
     }
 
     // FIXME: not thread-safe, don't use this static shared_ptr at all
-    const auto pixels = img.sharedPixels();
+    const auto pixels = image->sharedPixels();
     // const auto lines = pixelsToLines(::pixels);
     const auto lines = pixelsToLines(*pixels);
 
@@ -168,8 +175,8 @@ QHttpServerResponse HttpServer::GET_pixels()
 
     // jd.AddMember("encoderPosition", qint64{encoder.position()});
     // FIXME: get prom pixels struct
-    jd.AddMember("measurementCounter", img.counters.measurementCounter, al);
-    jd.AddMember("timestampUs", img.counters.timestampUs, al);
+    jd.AddMember("measurementCounter", image->counters.measurementCounter, al);
+    jd.AddMember("timestampUs", image->counters.timestampUs, al);
     jd.AddMember("pixels", rjPixels.Move(), al);
     jd.AddMember("lines", rjLines.Move(), al);
 
@@ -179,7 +186,7 @@ QHttpServerResponse HttpServer::GET_pixels()
     jd.Accept(writer);
     const QString res{(const char *) buffer.GetString()};
     // qDebug() << "size:" << res.size();
-    // qDebug().noquote() << "ret pix";
+    // qDebug().noquote().nospace() << "ret pix: " << res;
 
     return QHttpServerResponse{res};
 }