refactoring

6 files changed, 218 insertions, 152 deletions

diff --git a/src/calibration.cpp b/src/calibration.cpp
index ff37e73..7fcdbcc 100644
--- a/src/calibration.cpp
+++ b/src/calibration.cpp
@@ -13,7 +13,8 @@
 
 #include "imagealgos.h"
 
-bool openCalibrationTable(const QString& filename, CalibrationTablePtr& table)
+bool openCalibrationTable(
+    const QString& filename, CalibrationTablePtr& table)
 {
     QFile f(filename);
 
@@ -46,7 +47,8 @@ bool openCalibrationTable(const QString& filename, CalibrationTablePtr& table)
     return true;
 }
 
-bool dump(const CalibrationTablePtr& table, const QString& filename)
+bool dump(
+    const CalibrationTablePtr& table, const QString& filename)
 {
     qDebug() << Q_FUNC_INFO << "size is" << sizeof(CalibrationTable);
 
@@ -71,7 +73,8 @@ bool dump(const CalibrationTablePtr& table, const QString& filename)
     return true;
 }
 
-void interpolate(CalibrationTablePtr& table)
+void interpolate(
+    CalibrationTablePtr& table)
 {
     std::for_each(std::execution::par,
                   table->begin(),
@@ -105,7 +108,8 @@ void interpolate(CalibrationTablePtr& table)
     // }
 }
 
-void interpolate(CalibrationColumn& column)
+void interpolate(
+    CalibrationColumn& column)
 {
     size_t start{0};
     auto& c = column;
@@ -147,7 +151,8 @@ void interpolate(CalibrationColumn& column)
     }
 }
 
-QImage calibrationTableToImage(const CalibrationTablePtr& calibrationTable)
+QImage calibrationTableToImage(
+    const CalibrationTablePtr& calibrationTable)
 {
     QImage result(QSize(calibrationTable->size(),
                         calibrationTable->at(0).size()),
@@ -185,28 +190,27 @@ QImage calibrationTableToImage(const CalibrationTablePtr& calibrationTable)
     }
 
     qDebug() << "not null count" << notNull << "of"
-             << sizeof(CalibrationTable)
-                    / sizeof(calibrationTable->at(0).at(0));
+             << sizeof(CalibrationTable) /
+                    sizeof(calibrationTable->at(0).at(0));
 
     return result;
 }
 
-QList<Pixels> filter(const QList<Pixels>& rawProfiles)
+QList<Pixels> filter(
+    const QList<Pixels>& rawProfiles)
 {
     QList<Pixels> result;
 
     QList<Pixels>::const_iterator it = rawProfiles.constBegin();
 
-    while (it != rawProfiles.constEnd())
-    {
+    while (it != rawProfiles.constEnd()) {
         Pixels sum = *it;
 
         size_t count{1};
         ++it;
 
         while (it != rawProfiles.constEnd() &&
-               it->counters.encoderPosition == sum.counters.encoderPosition)
-        {
+               it->counters.encoderPosition == sum.counters.encoderPosition) {
             sum += *it;
             ++count;
             ++it;
@@ -219,127 +223,79 @@ QList<Pixels> filter(const QList<Pixels>& rawProfiles)
     return result;
 }
 
-CalibrationTablePtr calibrateZ(const QList<Pixels>& rawProfiles,
-                               const uint32_t& stepsPerMm)
+CalibrationTablePtr calibrateZ(
+    const QList<Pixels>& rawProfiles, const uint32_t& stepsPerMm)
 {
     CalibrationTablePtr result{new CalibrationTable{{0}}};
 
     for (const auto& rawProfile : rawProfiles) {
-        // std::cout << "calibration: pos is "
-        //           << rawProfile.counters.encoderPosition << std::endl;
-        const float positionMm{float(rawProfile.counters.encoderPosition)
-                               / float(stepsPerMm)};
-
-        if (positionMm >= 200) {
-            qWarning() << "got invalid calibration distance:" << positionMm;
-            qWarning() << "encoder position:"
-                       << rawProfile.counters.encoderPosition;
-            exit(EXIT_FAILURE);
-        }
+        const float positionMm{float(rawProfile.counters.encoderPosition) /
+                               float(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 = 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) {
             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());
+            // TODO: move this validation to some better place
+            if (Q_UNLIKELY(discretePixelValue >= calibrationColumnHeight)) {
+                std::cerr << __func__
+                          << ":/tinvalid discrete value. col: " << columnIdx
+                          << ", val: " << pixelValue << std::endl;
 
-            // auto& calibrationColumn = result[columnIdx];
-            auto& calibrationColumn = (*result)[columnIdx];
+                return {};
+            }
 
+            auto& calibrationColumn = (*result)[columnIdx];
             calibrationColumn[discretePixelValue] = positionMm;
-
-            // if (columnIdx >= ((img_width - 10) / 2) &&
-            //     columnIdx < img_width - ((img_width - 10) / 2))
-            // {
-            //     std::cout << discretePixelValue << ";";
-            // }
         }
-
-        // std::cout << std::endl << std::flush;
     }
 
     return result;
-    // TODO: try something interesting
-    // return {};
 }
 
-CalibrationTablePtr calibrateX(const QList<Pixels>& rawProfiles)
+CalibrationTablePtr calibrateX(
+    const QList<Pixels>& rawProfiles)
 {
+    // TODO: move to settings
+    constexpr double triangleBaseMm{8.};
     CalibrationTablePtr result{new CalibrationTable{{0}}};
 
-    static size_t tmp_counter = 0;
-
     for (const auto& rawProfile : rawProfiles) {
         const auto& pixels = rawProfile.pixels;
         auto lines = pixelsToLines(rawProfile);
 
-        tmp_counter++;
-
-        Q_ASSERT_X(lines.count() > 2, Q_FUNC_INFO, "no lines");
-
-        if (tmp_counter == 9471) {
-            for (const auto& l : lines) {
-                qDebug() << "line:" << l;
-            }
+        if (lines.count() < 2) {
+            continue;
         }
 
+        // x coords of line endings - [l1.p1.x; lN.p2.x]
         QList<double> xAnchors(lines.size() + 1);
-
         std::transform(std::execution::par_unseq,
                        lines.constBegin(),
                        lines.constEnd(),
                        xAnchors.begin(),
                        [](const auto& l) { return l.x1(); });
-
         xAnchors.last() = lines.last().x2();
 
-        constexpr double triangleHeightMm{5.};
-        constexpr double triangleBaseMm{8.};
-
-        auto nearestAnchorToX0 = std::min_element(std::execution::par_unseq,
-                                                  xAnchors.constBegin(),
-                                                  xAnchors.constEnd(),
-                                                  [](const auto& a,
-                                                     const auto& b) {
-                                                      return std::abs(a)
-                                                             < std::abs(b);
-                                                  });
+        auto centralAnchorIt = std::min_element(std::execution::par_unseq,
+                                                xAnchors.constBegin(),
+                                                xAnchors.constEnd(),
+                                                [](const auto& a,
+                                                   const auto& b) {
+                                                    return std::abs(a) <
+                                                           std::abs(b);
+                                                });
 
-        int nearestAnchorToX0Idx = nearestAnchorToX0 - xAnchors.constBegin();
+        int centralAnchorIdx = centralAnchorIt - xAnchors.constBegin();
 
+        // convert line image coords to mm coords
         QList<double> xAnchorsMm(xAnchors.count());
 
         for (int i = 0; i < xAnchors.size(); ++i) {
-            xAnchorsMm[i] = (i - nearestAnchorToX0Idx) * triangleBaseMm / 2.;
+            xAnchorsMm[i] = (i - centralAnchorIdx) * triangleBaseMm / 2.;
         }
 
         auto xAnchorIt = xAnchors.constBegin() + 1;
@@ -349,17 +305,23 @@ CalibrationTablePtr calibrateX(const QList<Pixels>& rawProfiles)
             // skip points with to the left from left line and to the right from
             // right line
             const auto columnX = int(columnIdx) - int(img_width / 2);
+
             if (columnX < xAnchors.first() || columnX > xAnchors.last()) {
                 continue;
             }
 
-            if (columnX > *xAnchorIt) {
+            // if [...(anchor-1)...(anchor)...(column)...]
+            // then use next anchor to have [...(anchor-1)...(column)...anchor...]
+            if (*xAnchorIt < columnX) {
                 ++xAnchorIt;
                 ++xAnchorMmIt;
             }
             const auto xLeft = *(xAnchorIt - 1);
             const auto xRight = *(xAnchorIt);
 
+            // there could be points which don't belong to any lines, because
+            // some real lines can be too short and will be ignored by
+            // `pixelsToLines`. e.g. the most left/right lines. skip such points
             if (columnX < xLeft || columnX > xRight) {
                 if (rawProfile.counters.encoderPosition >= 0) {
                     qWarning()
@@ -372,37 +334,29 @@ CalibrationTablePtr calibrateX(const QList<Pixels>& rawProfiles)
             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());
+            // TODO: move this validation to some better place
+            if (Q_UNLIKELY(discretePixelValue >= calibrationColumnHeight)) {
+                std::cerr << __func__
+                          << ":/tinvalid discrete value. col: " << columnIdx
+                          << ", val: " << pixelValue << std::endl;
+                return {};
+            }
 
+            // use value interpolated between line endings (anchors)
             const auto xLineLen = xRight - xLeft;
             const auto xLeftMm = *(xAnchorMmIt - 1);
             const auto xRelative = float(columnX - xLeft) / xLineLen;
             const auto xMmValue = xLeftMm + xRelative * (triangleBaseMm / 2.);
 
-            auto& calibrationColumn = (*result)[columnIdx];
-
-            if (tmp_counter == 9471) {
-                qDebug() << "calibration value" << columnIdx
-                         << discretePixelValue << xMmValue;
-            }
-            calibrationColumn[discretePixelValue] = xMmValue;
+            (*result)[columnIdx][discretePixelValue] = xMmValue;
         }
     }
 
     return result;
 }
 
-void dumpCalibrationPixels(std::vector<Pixels>&& calibrationPixels)
+void dumpCalibrationPixels(
+    std::vector<Pixels>&& calibrationPixels)
 {
     std::vector<Pixels> rawProfiles;
 
diff --git a/src/camera/ov9281.cpp b/src/camera/ov9281.cpp
index bdb9f89..4d393a0 100644
--- a/src/camera/ov9281.cpp
+++ b/src/camera/ov9281.cpp
@@ -56,14 +56,6 @@ bool OV9281::init()
 
     m_config->orientation = libcamera::Orientation::Rotate90;
 
-    if (m_config->empty())
-    {
-        std::cerr << __func__ << ": " << m_camera->id() << ": config is empty"
-                  << std::endl;
-
-        return false;
-    }
-
     libcamera::StreamConfiguration &streamConfig = m_config->at(0);
 
     streamConfig.pixelFormat = OV9281::pixelFormat;
@@ -71,9 +63,10 @@ bool OV9281::init()
 
     if (!validateConfig())
     {
-        std::cerr << __func__ << ": " << m_camera->id()
-                  << ": cannot apply default config" << std::endl;
+        return false;
+    }
 
+    if (!applyConfig()) {
         return false;
     }
 
@@ -135,6 +128,11 @@ bool OV9281::applyConfig()
     return true;
 }
 
+/*
+ * Signals operate in the libcamera CameraManager thread context, so it is
+ * 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 OV9281::onRequestCompleted(libcamera::Request *completed_request)
 {
     using namespace libcamera;
@@ -184,6 +182,9 @@ void OV9281::onRequestCompleted(libcamera::Request *completed_request)
             auto pixels = img->pixels();
 #ifdef emit
 #undef emit
+            if (!pixels) {
+                std::cerr << "emit empty pixels" << std::endl;
+            }
             newPixels.emit(pixels);
 #define emit
 #endif
@@ -225,7 +226,9 @@ std::vector<std::shared_ptr<OV9281>> OV9281::search(
 
     for (const auto &camera : manager->cameras())
     {
-        auto ov9281 = std::shared_ptr<OV9281>(new OV9281(camera));
+        auto id = camera->id();
+        auto c = manager->get(id);
+        auto ov9281 = std::shared_ptr<OV9281>(new OV9281(c));
 
         if (!ov9281->init())
         {
diff --git a/src/image.cpp b/src/image.cpp
index a9280a4..7d1c824 100644
--- a/src/image.cpp
+++ b/src/image.cpp
@@ -104,6 +104,11 @@ std::shared_ptr<Pixels> Image::pixels() const
         result->pixels[i] = process_column(rotated_cw[i]);
     }
 
+    // for (size_t i = 640 - 5; i < 640 + 5; ++i) {
+    //     std::cout << result->pixels[i] << ' ';
+    // }
+    // std::cout << std::endl;
+
     stop_timer(process_columns);
 
     return result;
diff --git a/src/laser.cpp b/src/laser.cpp
new file mode 100644
index 0000000..f23faab
--- /dev/null
+++ b/src/laser.cpp
@@ -0,0 +1,70 @@
+#include "laser.h"
+
+#include <filesystem>
+#include <fstream>
+#include <iostream>
+
+#include "macro.h"
+
+PwmLaser::PwmLaser(const std::string &pwmChip, const std::string &pwm)
+    : INIT_FIELD(pwmChip)
+    , INIT_FIELD(pwm)
+{}
+
+bool PwmLaser::init()
+{
+    if (m_pwmChip.empty() || m_pwm.empty()) {
+        std::cerr << __func__ << ":\tinvalid pwm config: pwmChip == '"
+                  << m_pwmChip << "', pwm == '" << m_pwm << "'" << std::endl;
+        return false;
+    }
+
+    auto writeToFile = [](const auto &path, const auto &value) -> bool {
+        if (!std::filesystem::exists(path)) {
+            std::cerr << __func__ << "\tno such file: " << path << std::endl;
+
+            return false;
+        }
+
+        std::ofstream ofs(path, std::ios::out | std::ios::trunc);
+
+        if (!ofs) {
+            std::cerr << __func__ << "\tcannot open" << path << "for writing"
+                      << std::endl;
+
+            return false;
+        }
+
+        ofs << value;
+
+        return true;
+    };
+
+    const std::filesystem::path pwmSystemRoot{"/sys/class/pwm"};
+    const auto pwmChipRoot = pwmSystemRoot / m_pwmChip;
+    const auto pwmExportFile = pwmChipRoot / "export";
+
+    if (!writeToFile(pwmExportFile, m_pwm))
+        return false;
+
+    const auto pwmRoot = pwmChipRoot / m_pwm;
+    const auto periodFilename = pwmRoot / "period";
+    constexpr unsigned periodHz{50'000};
+
+    if (!writeToFile(periodFilename, periodHz))
+        return false;
+
+    const auto dutyCycleFilename = pwmRoot / "duty_cycle";
+    const unsigned dutyCycle{3'000};
+
+    if (!writeToFile(dutyCycleFilename, dutyCycle))
+        return false;
+
+    const auto enableFilename = pwmRoot / "enable";
+    const int enable{1};
+
+    if (!writeToFile(enableFilename, enable))
+        return false;
+
+    return true;
+}
diff --git a/src/laser.h b/src/laser.h
new file mode 100644
index 0000000..31fe7e3
--- /dev/null
+++ b/src/laser.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include <string>
+
+class ILaser
+{
+public:
+    virtual ~ILaser() = default;
+
+public:
+    virtual bool init() = 0;
+    virtual bool setEnabled(bool enabled) = 0;
+    virtual bool setLaserLevel(std::size_t level) = 0;
+
+public:
+    bool enable() { return setEnabled(true); }
+    bool disable() { return setEnabled(false); }
+};
+
+class PwmLaser : public ILaser
+{
+public:
+    explicit PwmLaser(const std::string& pwmChip, const std::string& pwm);
+    ~PwmLaser() override = default;
+
+    // ILaser
+public:
+    bool init() override;
+
+private:
+    std::string m_pwmChip;
+    std::string m_pwm;
+};
diff --git a/src/pixels.cpp b/src/pixels.cpp
index 5aac4a6..fcdfac7 100644
--- a/src/pixels.cpp
+++ b/src/pixels.cpp
@@ -5,39 +5,37 @@
 #include <fstream>
 #include <iostream>
 
-Pixels& Pixels::operator+=(const Pixels& other)
+Pixels& Pixels::operator+=(
+    const Pixels& other)
 {
-    std::transform(
-        std::execution::par,
-        pixels.begin(),
-        pixels.end(),
-        other.pixels.begin(),
-        pixels.begin(),
-        // [](auto& toAdd) { return dst += src; });
-        std::plus<>()
-    );
+    std::transform(std::execution::par,
+                   pixels.begin(),
+                   pixels.end(),
+                   other.pixels.begin(),
+                   pixels.begin(),
+                   // [](auto& toAdd) { return dst += src; });
+                   std::plus<>());
 
     return *this;
 }
 
-Pixels& Pixels::operator/=(const float divider)
+Pixels& Pixels::operator/=(
+    const float divider)
 {
-    std::for_each(
-        std::execution::par_unseq,
-        pixels.begin(),
-        pixels.end(),
-        [divider](auto& pixel) { pixel /= divider; }
-    );
+    std::for_each(std::execution::par_unseq,
+                  pixels.begin(),
+                  pixels.end(),
+                  [divider](auto& pixel) { pixel /= divider; });
 
     return *this;
 }
 
-std::optional<Pixels> Pixels::load(const QString& filename)
+std::optional<Pixels> Pixels::load(
+    const QString& filename)
 {
     const std::filesystem::path filepath{filename.toStdString()};
 
-    if (!std::filesystem::exists(filepath))
-    {
+    if (!std::filesystem::exists(filepath)) {
         std::cerr << "no such file: " << filepath << std::endl;
 
         return {};
@@ -52,8 +50,7 @@ std::optional<Pixels> Pixels::load(const QString& filename)
     ifs.read(reinterpret_cast<char*>(&result), sizeof(Pixels));
     ifs.close();
 
-    if (!ifs)
-    {
+    if (!ifs) {
         std::cerr << "cannot read " << filepath << std::endl;
 
         return {};
@@ -62,14 +59,14 @@ std::optional<Pixels> Pixels::load(const QString& filename)
     return result;
 }
 
-bool Pixels::save(const QString& filename)
+bool Pixels::save(
+    const QString& filename)
 {
     const std::filesystem::path filepath{filename.toStdString()};
     const auto parent_path = filepath.parent_path();
 
-    if (!std::filesystem::exists(parent_path) &&
-        !std::filesystem::create_directories(parent_path))
-    {
+    if (!std::filesystem::exists(parent_path)
+        && !std::filesystem::create_directories(parent_path)) {
         std::cerr << "cannot create parent directory for file " << filepath
                   << std::endl;
 
@@ -84,8 +81,7 @@ bool Pixels::save(const QString& filename)
     ofs.write(reinterpret_cast<const char*>(this), sizeof(Pixels));
     ofs.close();
 
-    if (!ofs)
-    {
+    if (!ofs) {
         std::cerr << "cannot write " << filepath << std::endl;
 
         return false;
@@ -96,7 +92,12 @@ bool Pixels::save(const QString& filename)
 
 Pixels::operator bool() const
 {
-    return std::find_if(pixels.cbegin(), pixels.cend(), [](const auto& p) {
-               return !qFuzzyIsNull(p) && !std::isnan(p);
-           }) != pixels.cend();
+    bool result = std::find_if(pixels.cbegin(),
+                               pixels.cend(),
+                               [](const auto& p) {
+                                   return !qFuzzyIsNull(p) && !std::isnan(p);
+                               })
+                  != pixels.cend();
+    // std::cout << __func__ << ":\t" << (result ? "true" : "false") << std::endl;
+    return result;
 }