line search

1 files changed, 362 insertions, 68 deletions

diff --git a/main.cpp b/main.cpp
index 33f94ee..06b7700 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,13 +1,21 @@
+#include <chrono>
+#include <errno.h>
+#include <iostream>
+#include <iterator>
+#include <fstream>
+#include <string.h>
+#include <thread>
+#include <sys/mman.h>
 
+#include "LibCamera.h"
+#include "printerclient.h"
 #include "httpservice.h"
 #include "genetic_algos.h"
-#include "imagealgos.h"
-#include "LibCamera.h"
 #include "pigpio.h"
-#include "printerclient.h"
 #include "rotaryencoder.h"
+#include "imagealgos.h"
+#include "fuck_intel.h"
 
-#define QT_NO_KEYWORDS
 #include <QCoreApplication>
 #include <QDebug>
 #include <QDir>
@@ -20,14 +28,6 @@
 #include <QTextStream>
 #include <QtConcurrent/QtConcurrentRun>
 #include <QTimer>
-#undef QT_NO_KEYWORDS
-
-#include <chrono>
-#include <errno.h>
-#include <iostream>
-#include <fstream>
-#include <string.h>
-#include <thread>
 
 #define try_apply_config() \
 if(!applyConfig(config)) \
@@ -76,21 +76,83 @@ const QString dumpsRoot { QStringLiteral("/home/user/dumps") };
 
 using namespace std::chrono_literals;
 
-static std::shared_ptr<Camera> camera;
-std::unique_ptr<CameraConfiguration> config;
+static std::shared_ptr<libcamera::Camera> camera;
+std::unique_ptr<libcamera::CameraConfiguration> config;
 static std::map<int, std::pair<void *, unsigned int>> mappedBuffers_;
-std::vector<std::unique_ptr<Request>> requests;
-ControlList lastControls;
+std::vector<std::unique_ptr<libcamera::Request>> requests;
+libcamera::ControlList lastControls;
 
-static bool applyConfig(const std::unique_ptr<CameraConfiguration> & config);
-static void onRequestCompleted(Request *completed_request);
+static QList<Pixels> openDump(const QString& dumpPath = "");
+static std::optional<Pixels> openRawProfile(const QString& filePath);
+constexpr float hardcodedZRangeMm { 175.f };
+constexpr size_t calibrationTableHeight { 0x4000 }; // 16384
+// img_width * calibrationTableHeight
+using CalibrationColumn = std::array<float, calibrationTableHeight>;
+using CalibrationTable = std::array<CalibrationColumn, img_width>;
+constexpr auto calibrationColumnHeight = std::tuple_size<CalibrationColumn>();
+// CalibrationTable* calibrationTable { new CalibrationTable {{ 0 }} };
+
+static bool applyConfig(const std::unique_ptr<libcamera::CameraConfiguration> & config);
+static void onRequestCompleted(libcamera::Request *completed_request);
 static void printControls();
 static void dumpCalibrationPixels();
-static std::vector<Pixels> openDump(const QString dumpPath = "");
+static QList<Pixels> filter(const QList<Pixels>& rawProfiles);
+static QSharedPointer<CalibrationTable> calibrate(const QList<Pixels>& rawProfiles);
+
+auto printPixels = [](const auto& pixels) {
+    for (size_t i = (img_width - 10) / 2; i < img_width - ((img_width - 10) / 2); ++i)
+    {
+        std::cout << pixels[i] << " ";
+    }
+    std::cout << std::endl;
+};
+
+bool initLaser();
+
+class A
+{
+public:
+    A(const int var) : m_var{ var } { std::cout << m_var << std::endl; }
+
+private:
+    int m_var { 0 };
+};
+
+class B : public A
+{
+public:
+    B(const int var) : A{var} {}
+};
 
 int main(int argc, char *argv[]) {
     QCoreApplication app(argc, argv);
 
+    // if (!initLaser())
+    // {
+    //     return EXIT_FAILURE;
+    // }
+
+    if (false)
+    {
+        // auto rawProfiles = openDump("/home/user/dumps/2024.11.24_19.17.32");
+        // auto rawProfiles = openDump("/home/user/dumps/2024.11.26_21.53.55");
+        auto rawProfiles = openDump("/home/user/dumps/2024.11.26_21.53.55_small");
+        qDebug() << "raw profiles count is" << rawProfiles.size();
+        // qDebug() << "height" << calibrationColumnHeight;
+
+        auto filteredRawProfiles = filter(rawProfiles);
+        qDebug() << "filtered profiles count is" << filteredRawProfiles.count();
+
+        auto calibrationTable = calibrate(filteredRawProfiles);
+        // auto calibrationTable = calibrate(rawProfiles);
+        // CalibrationTable* aaa { new CalibrationTable {{ 0 }} };
+        // (*aaa)[1][0] = 123;
+        // CalibrationTable bbb {{ 0 }};
+        // bbb[1][0] = 123;
+
+        // float* values = new float[calibrationColumnHeight];
+    }
+
     QElapsedTimer t;
     t.start();
 
@@ -102,7 +164,7 @@ int main(int argc, char *argv[]) {
     qDebug() << "msecs before camera:" << t.elapsed();
     // FIXME: don't use one var for everything
     int ret;
-    std::unique_ptr<CameraManager> cm = std::make_unique<CameraManager>();
+    std::unique_ptr<libcamera::CameraManager> cm = std::make_unique<libcamera::CameraManager>();
     cm->start();
 
     const auto cameras = cm->cameras();
@@ -139,7 +201,7 @@ int main(int argc, char *argv[]) {
 
     // FIXME: nullptr
     // std::unique_ptr<CameraConfiguration> config = camera->generateConfiguration( { StreamRole::Viewfinder } );
-    /*std::unique_ptr<CameraConfiguration> */config = camera->generateConfiguration( { StreamRole::Raw } );
+    /*std::unique_ptr<CameraConfiguration> */config = camera->generateConfiguration( { libcamera::StreamRole::Raw } );
 
     if (config->empty())
     {
@@ -162,7 +224,7 @@ int main(int argc, char *argv[]) {
     // }
 
     // FIXME: nullptr
-    StreamConfiguration &streamConfig = config->at(0);
+    libcamera::StreamConfiguration &streamConfig = config->at(0);
     std::cout << "Default viewfinder configuration is: " << streamConfig.toString() << std::endl;
     std::cout << "Pixel format is: " << streamConfig.pixelFormat.toString() << std::endl;
     std::cout << "Buffer count is: " << streamConfig.bufferCount << std::endl;
@@ -179,7 +241,7 @@ int main(int argc, char *argv[]) {
     // streamConfig.pixelFormat = PixelFormat::fromString("Y8_1X8");
 
     // streamConfig.pixelFormat = formats::R8;
-    streamConfig.pixelFormat = formats::R16;
+    streamConfig.pixelFormat = libcamera::formats::R16;
     streamConfig.bufferCount = 2;
     // what is default R10_CSI2P? MONO_PISP_COMP1?
     // MONO_PISP_COMP1 - check rpicam-apps sources for decoding algos
@@ -215,7 +277,7 @@ int main(int argc, char *argv[]) {
 
     // TODO: try custom FrameBufferAllocator and compare performance
 
-    auto allocator = std::make_shared<FrameBufferAllocator>(camera);
+    auto allocator = std::make_shared<libcamera::FrameBufferAllocator>(camera);
 
     auto stream = streamConfig.stream();
 
@@ -232,13 +294,13 @@ int main(int argc, char *argv[]) {
     size_t allocated = size_t(ret);
     std::cout << "Allocated " << allocated << " buffers for stream" << std::endl;
 
-    const std::vector<std::unique_ptr<FrameBuffer>> &buffers = allocator->buffers(stream);
+    const std::vector<std::unique_ptr<libcamera::FrameBuffer>> &buffers = allocator->buffers(stream);
 
     // for (size_t i = 0; i < buffers.size(); ++i)
     static int expOffset = 0;
     for (const auto & buffer : buffers)
     {
-        std::unique_ptr<Request> request = camera->createRequest();
+        std::unique_ptr<libcamera::Request> request = camera->createRequest();
 
         if (!request)
         {
@@ -328,7 +390,7 @@ int main(int argc, char *argv[]) {
 
         for (size_t i = 0; i < img_width; ++i) {
             // pixels << img_height - img.pixels[i];
-            pixels << img_height - ::pixels.pixels[i];
+            pixels << ::pixels.pixels[i];
         }
 
         QJsonObject json;
@@ -337,6 +399,22 @@ int main(int argc, char *argv[]) {
         json["measurementCounter"] = qint64 { img.counters.measurementCounter };
         json["timestampUs"] = qint64(img.counters.timestampUs);
 
+        const auto lines = pixelsToLines(::pixels);
+
+        qDebug() << "lines count is " << lines.count();
+
+        QJsonArray jsonLines;
+
+        for (const auto& l : lines)
+        {
+            jsonLines << QJsonArray {
+                QJsonArray { l.p1().x(), l.p1().y() },
+                QJsonArray { l.p2().x(), l.p2().y() }
+            };
+        }
+
+        json["lines"] = jsonLines;
+
         return QHttpServerResponse(QJsonDocument(json).toJson());
     });
 
@@ -403,14 +481,14 @@ int main(int argc, char *argv[]) {
             std::lock_guard<std::mutex> lg(pgm_image_mtx);
             QJsonObject json;
 
-            const ControlIdMap & ctrlIdMap = camera->controls().idmap();
+            const libcamera::ControlIdMap & ctrlIdMap = camera->controls().idmap();
 
             qDebug() << "readParams:" << lastControls.size();
             qDebug() << request.method();
 
             for (const auto & [id, value]: lastControls)
             {
-                const ControlId * controlId = ctrlIdMap.at(id);
+                const libcamera::ControlId * controlId = ctrlIdMap.at(id);
                 auto name = QString::fromStdString(controlId->name());
                 const auto valueStr = QString::fromStdString(value.toString());
                 qDebug() << "\t param:"
@@ -611,8 +689,10 @@ int main(int argc, char *argv[]) {
  * important not to block the thread for a long time, as this blocks internal
  * processing of the camera pipelines, and can affect realtime performance.
 */
-void onRequestCompleted(Request *completed_request)
+void onRequestCompleted(libcamera::Request *completed_request)
 {
+    using namespace libcamera;
+
     static std::chrono::steady_clock::time_point fpsTimstamp = std::chrono::steady_clock::now();
 
     QElapsedTimer t;
@@ -683,8 +763,7 @@ void onRequestCompleted(Request *completed_request)
             // qDebug() << "calibration mode" << scanningModeFlags;
 
             if (scanningModeFlags == ScanningModeFlags::Calibration) {
-                constexpr int32_t hardcodedZRangeMm { 175 };
-                const int32_t maxEncoderPosition = hardcodedZRangeMm * requested_params.stepsPerMm;
+                const int32_t maxEncoderPosition = int32_t(hardcodedZRangeMm) * requested_params.stepsPerMm;
                 // qDebug() << "calibration max range" << maxEncoderPosition;
                 // qDebug() << "calibration encoder pos" << pixels.counters.encoderPosition;
                 if (pixels.counters.encoderPosition >= 0 &&
@@ -789,12 +868,14 @@ void onRequestCompleted(Request *completed_request)
 
 }
 
-static bool applyConfig(const std::unique_ptr<CameraConfiguration> & config)
+static bool applyConfig(const std::unique_ptr<libcamera::CameraConfiguration> & config)
 {
+    using namespace libcamera;
+
     auto status = config->validate();
 
     // WARNING: unsafe
-    StreamConfiguration &streamConfig = config->at(0);
+    libcamera::StreamConfiguration &streamConfig = config->at(0);
 
     switch (status) {
     case CameraConfiguration::Status::Valid:
@@ -819,7 +900,8 @@ static bool applyConfig(const std::unique_ptr<CameraConfiguration> & config)
 
 static void printControls()
 {
-    const ControlInfoMap & control_map = camera->controls();
+    using namespace libcamera;
+    const libcamera::ControlInfoMap & control_map = camera->controls();
 
     // for (const auto & [id, info]: control_map)
     for (const std::pair<const ControlId *, ControlInfo> & pair : control_map)
@@ -888,10 +970,10 @@ static void dumpCalibrationPixels()
         }
 
         QJsonObject jsonCounters {
-            { "timestampUs", qint64(rawProfile.counters.timestampUs) },
-            { "measurementCounter", qint64(rawProfile.counters.measurementCounter) },
-            { "encoderPosition", qint64(rawProfile.counters.encoderPosition) },
-        };
+                                 { "timestampUs", qint64(rawProfile.counters.timestampUs) },
+                                 { "measurementCounter", qint64(rawProfile.counters.measurementCounter) },
+                                 { "encoderPosition", qint64(rawProfile.counters.encoderPosition) },
+                                 };
 
         QJsonObject json;
 
@@ -920,11 +1002,10 @@ static void dumpCalibrationPixels()
     qDebug() << "dump finished";
 }
 
-static std::vector<Pixels> openDump(const QString dumpPath)
+static QList<Pixels> openDump(const QString &dumpPath)
 {
-    std::vector<Pixels> result;
-
     QString dirToRead { dumpPath };
+    qDebug() << "trying to open dump path:" << dirToRead;
 
     if (dirToRead.isEmpty())
     {
@@ -945,10 +1026,10 @@ static std::vector<Pixels> openDump(const QString dumpPath)
             return {};
         }
 
-        dirToRead = entries.last();
+        dirToRead = dumpsRoot + "/" + entries.last();
     }
 
-    QDir dumpDir { dumpsRoot + "/" + dirToRead };
+    QDir dumpDir { dirToRead };
 
     const auto filter = QDir::Files;
     const auto sort = QDir::Name;
@@ -958,47 +1039,260 @@ static std::vector<Pixels> openDump(const QString dumpPath)
     if (filenames.isEmpty())
     {
         qDebug() << "no filenames found in" << dumpDir.path();
+        return {};
     }
 
-    for (const auto& filename : filenames)
+    QList<std::optional<Pixels>> resultOptionals { filenames.size() };
+
+    QElapsedTimer t;
+    t.start();
+
+    std::transform(
+        std::execution::par,
+        filenames.begin(), filenames.end(),
+        resultOptionals.begin(),
+        [dirToRead](const auto& filename) {
+            std::cout << '.';
+            return openRawProfile(dirToRead + "/" + filename);
+        });
+
+    std::cout << std::endl;
+
+    std::remove_if(
+        std::execution::par,
+        resultOptionals.begin(),
+        resultOptionals.end(),
+        [](auto& a) { return !a.has_value(); });
+
+    QList<Pixels> result { resultOptionals.size() };
+
+    std::transform(
+        std::execution::par,
+        std::make_move_iterator(resultOptionals.begin()),
+        std::make_move_iterator(resultOptionals.end()),
+        result.begin(),
+        [](auto& p) { return p.value(); });
+
+    qDebug() << Q_FUNC_INFO << "elapsed (ms)" << t.elapsed();
+
+    return result;
+}
+
+static std::optional<Pixels> openRawProfile(const QString& filePath)
+{
+    QFile f { filePath };
+
+    if (!f.open(QFile::ReadOnly))
     {
-        qDebug() << "raw profile:" << filename;
+        qWarning() << "cannot open file for reading:" << f.fileName();
+        qWarning() << "error string:" << f.errorString();
+
+        return {};
+    }
 
-        QFile f { dumpDir.path() + "/" + filename };
+    // TODO: rewrite to remove manual serialization/deserialization
+    const auto json = QJsonDocument::fromJson(f.readAll()).object();
+    const auto jsonCounters = json["counters"].toObject();
 
-        if (!f.open(QFile::ReadOnly))
+    Pixels result;
+    result.counters.timestampUs = jsonCounters["timestampUs"].toInteger();
+    result.counters.measurementCounter = jsonCounters["measurementCounter"].toInteger();
+    result.counters.encoderPosition = jsonCounters["encoderPosition"].toInteger();
+
+    const auto jsonPixels = json["pixels"].toArray();
+
+    for (size_t i = 0; i < jsonPixels.count() && i < result.pixels.size(); ++i)
+    {
+        result.pixels[i] = jsonPixels[i].toDouble();
+    }
+
+    return result;
+}
+
+static QList<Pixels> filter(const QList<Pixels>& rawProfiles)
+{
+    QList<Pixels> result;
+
+    QList<Pixels>::const_iterator it = rawProfiles.constBegin();
+
+    // for (size_t i = 0; i < 10; ++i)
+    // {
+    //     std::cout << "pos - " << rawProfiles.at(i).counters.encoderPosition << std::endl;
+    // }
+
+    // ++it;
+    while (it != rawProfiles.constEnd())
+    {
+        Pixels sum = *it;
+        std::cout << "current pos is " << sum.counters.encoderPosition << std::endl;
+
+        printPixels(sum.pixels);
+
+        size_t count { 1 };
+        ++it;
+
+        std::cout << "here\n";
+        while (it != rawProfiles.constEnd() && it->counters.encoderPosition == sum.counters.encoderPosition)
         {
-            qWarning() << "cannot open file for reading:" << f.fileName();
-            qWarning() << "error string:" << f.errorString();
+            std::cout << "here2\n";
+            std::cout << "\tadd to pos " << it->counters.encoderPosition << std::endl;
+            sum += *it;
+            std::cout << "\tadded" << std::endl;
+            ++count;
+            ++it;
 
-            return {};
         }
 
-        // TODO: rewrite to remove manual serialization/deserialization
-        const auto json = QJsonDocument::fromJson(f.readAll()).object();
 
-        const auto jsonCounters = json["counters"].toObject();
-        qDebug() << jsonCounters;
+        std::cout << "here3\n";
+        sum /= float(count);
+        printPixels(sum.pixels);
 
-        Pixels rawProfile;
+        result << sum;
 
-        rawProfile.counters.timestampUs = jsonCounters["timestampUs"].toInteger();
-        rawProfile.counters.measurementCounter = jsonCounters["measurementCounter"].toInteger();
-        rawProfile.counters.encoderPosition = jsonCounters["encoderPosition"].toInteger();
+        // return result;
+    }
 
-        const auto jsonPixels = json["pixels"].toArray();
-        qDebug() << jsonPixels.count() << rawProfile.pixels.size();
+    return result;
+}
 
-        for (size_t i = 0; i < jsonPixels.count() && i < rawProfile.pixels.size(); ++i)
+static QSharedPointer<CalibrationTable> calibrate(const QList<Pixels>& rawProfiles)
+{
+    QSharedPointer<CalibrationTable> result { new CalibrationTable {{ 0 }} };
+
+    constexpr uint16_t discretesInRage { 16384 };
+
+    for (const auto& rawProfile : rawProfiles)
+    {
+        std::cout << "calibration: pos is " << rawProfile.counters.encoderPosition << std::endl;
+        const float positionMm {
+            float(rawProfile.counters.encoderPosition) /
+            float(requested_params.stepsPerMm)
+        };
+
+        const auto& pixels = rawProfile.pixels;
+        printPixels(pixels);
+        // static size_t counter { 0 };
+        // qDebug() << "calibrated" << counter++;
+        const float pos = rawProfile.counters.encoderPosition;
+        const float divider = requested_params.stepsPerMm;
+        std::cout << pos << " " << divider << " " << pos / divider << std::endl;
+        // std::cout << std::setw(5) << rawProfile.counters.encoderPosition
+        //           << std::setw(5) << requested_params.stepsPerMm
+        //           << std::setw(8) << positionMm
+        //           << std::setw(8) <<             float(rawProfile.counters.encoderPosition) /
+        //                                  float(requested_params.stepsPerMm)
+        //           << ": ";
+
+        for (size_t columnIdx = 0; columnIdx < pixels.size(); ++columnIdx)
         {
-            rawProfile.pixels[i] = jsonPixels[i].toDouble();
+            const auto& pixelValue = pixels.at(columnIdx);
+
+            // float[0, img_height] -> uint16_t[0, calibrationColumnHeight]
+            const uint16_t discretePixelValue {
+                uint16_t(pixelValue * discretesInRage / img_height)
+            };
+
+            Q_ASSERT_X(discretePixelValue > 0 && discretePixelValue < calibrationColumnHeight,
+                       Q_FUNC_INFO,
+                       ("got ivalid discrete value " +
+                        QString::number(discretePixelValue) +
+                        ". pixelValues is " +
+                        QString::number(pixelValue) +
+                        ". calc result is " +
+                        QString::number(pixelValue * discretesInRage / img_height)
+                        ).toStdString().c_str());
+
+            // auto& calibrationColumn = result[columnIdx];
+            auto& calibrationColumn = (*result)[columnIdx];
+            calibrationColumn[discretePixelValue] = positionMm;
+
+            if (columnIdx >= ((img_width - 10) / 2) && columnIdx < img_width - ((img_width - 10) / 2))
+            {
+                std::cout << discretePixelValue << ";";
+            }
         }
-    }
 
-    // {
-    //     QMutexLocker l(&calibrationPixelsMutex);
-    //     std::swap(result, ::calibrationPixels);
-    // }
+        std::cout << std::endl << std::flush;
+    }
 
     return result;
+    // TODO: try something interesting
+    // return {};
+}
+
+bool initLaser()
+{
+    const QLatin1String pwmChip { "pwmchip2" };
+    const uint16_t pwmChannel { 1 };
+    const QLatin1String pwmSystemRoot { "/sys/class/pwm" };
+    const QString pwmChipRoot { pwmSystemRoot + "/" + pwmChip };
+
+    const QString pwmExportFile { pwmChipRoot + "/export" };
+
+    QFile f { pwmExportFile };
+
+    if (!f.open(QFile::WriteOnly))
+    {
+        qWarning() << "cannot open" << f.fileName() << "for writing";
+        qWarning() << "error:" << f.errorString();
+
+        return false;
+    }
+
+    QTextStream s { &f };
+    s << pwmChannel;
+
+    const QString pwm { QLatin1String("pwm%1").arg(QString::number(pwmChannel)) };
+    const QString pwmRoot { pwmChipRoot + "/" + pwm };
+
+    const QString periodFilename { pwmRoot + "/period" };
+    f.close();
+    f.setFileName(periodFilename);
+
+    if (!f.open(QFile::WriteOnly))
+    {
+        qWarning() << "cannot open" << f.fileName() << "for writing";
+        qWarning() << "error:" << f.errorString();
+
+        return false;
+    }
+
+    const unsigned periodHz { 50'000 };
+
+    s << periodHz;
+
+    const QString dutyCycleFilename { pwmRoot + "/duty_cycle" };
+    f.close();
+    f.setFileName(dutyCycleFilename);
+
+    if (!f.open(QFile::WriteOnly))
+    {
+        qWarning() << "cannot open" << f.fileName() << "for writing";
+        qWarning() << "error:" << f.errorString();
+
+        return false;
+    }
+
+    const unsigned dutyCycle { 3'000 };
+
+    s << dutyCycle;
+
+    const QString enableFilename { pwmRoot + "/enable" };
+    f.close();
+    f.setFileName(enableFilename);
+
+    if (!f.open(QFile::WriteOnly))
+    {
+        qWarning() << "cannot open" << f.fileName() << "for writing";
+        qWarning() << "error:" << f.errorString();
+
+        return false;
+    }
+
+    const int enable { 1 };
+
+    s << enable;
+
+    return true;
 }