broken setting exposure. added rotary encoder

7 files changed, 346 insertions, 87 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index a1d1b47..ccbc957 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,8 +39,11 @@ set(LIBCAMERA_LIBRARIES "${LIBCAMERA_LIBRARY}" "${LIBCAMERA_BASE_LIBRARY}")
 project(orpheus LANGUAGES CXX)
 
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_STANDARD 23)
+set(CMAKE_CXX20_STANDARD_COMPILE_OPTION "-std:c++latest")
+set(CMAKE_CXX20_EXTENSION_COMPILE_OPTION "-std:c++latest")
 
-find_package(Qt6 6.4 REQUIRED COMPONENTS Quick HttpServer)
+find_package(Qt6 6.4 REQUIRED COMPONENTS Quick HttpServer SerialPort)
 
 qt_standard_project_setup(REQUIRES 6.4)
 
@@ -61,12 +64,16 @@ qt_add_executable(apporpheus
     LibCamera.cpp
     genetic_algos.h
     genetic_algos.cpp
+    rotaryencoder.h
+    rotaryencoder.cpp
 )
 target_link_libraries(app${PROJECT_NAME}
     PRIVATE
     "${LIBCAMERA_LIBRARIES}"
     PkgConfig::Pistache
     Qt6::HttpServer
+    Qt6::SerialPort
+    wiringPi
 )
 
 include(GNUInstallDirs)
@@ -81,3 +88,7 @@ if ("${CMAKE_BUILD_TYPE}" MATCHES "Release")
 else()
     #message("it's not Debug")
 endif()
+
+
+# add_compile_options(-Wall -Wextra -Wpedantic)
+add_compile_options(-Ofast -fno-unroll-loops)
diff --git a/imagealgos.cpp b/imagealgos.cpp
index 23902d1..e22f1bd 100644
--- a/imagealgos.cpp
+++ b/imagealgos.cpp
@@ -95,11 +95,11 @@ void rotate(Image &image)
 
     using namespace std;
 
-    for (size_t i = 0; i < image.height; ++i)
+    for (size_t i = 0; i < img_height; ++i)
     {
-        for (size_t j = 0; j < image.width; ++j)
+        for (size_t j = 0; j < img_width; ++j)
         {
-            image.rotated_cw[j][i] = image.data[image.height - i][j];
+            image.rotated_cw[j][i] = image.data[img_height - i][j];
         }
     }
 
@@ -109,12 +109,19 @@ void rotate(Image &image)
 template<class T, size_t N>
 constexpr size_t mysize(T (&)[N]) { return N; }
 
+void smooth_column(uint16_t (&column)[]) {
+    for (size_t i = 1; i < img_height - 1; ++i) {
+        column[i] = median3(column[i - 1], column[i], column[i + 1]);
+    }
+}
+
 float process_column(uint16_t (&column)[])
 {
+    start_timer(process_column);
+
     float result = std::numeric_limits<float>::quiet_NaN();
 
-    constexpr uint32_t patternSize = 16; // good
-    constexpr uint32_t signalThreshold = 450; // = SKO * sqrt(patternSize)
+    constexpr uint32_t signalThreshold = 900; // = SKO * sqrt(patternSize)
     static constexpr uint32_t patternOffset = patternSize - ((patternSize % 2 == 1) ? 1 : 0);
     const uint32_t correlationSize = img_height - patternSize +
                                      ((patternSize % 2 == 1) ? 1 : 0);
@@ -125,13 +132,28 @@ float process_column(uint16_t (&column)[])
     int32_t y1 = 0;
     int32_t y2 = 0;
 
-    memset(correlation, 0, img_height * sizeof(uint32_t));
+    memset(correlation, 0, img_height * sizeof(correlation[0]));
     integralSum[0] = 0;
 
     for(uint32_t i = 1; i < img_height; ++i) {
-        if (column[i] < 100) {
-            column[i] = 1;
-        }
+        // if (column[i + 0] < 100) { column[i + 0] = 0; } integralSum[i + 0] = column[i + 0] / 256 + integralSum[i + 0 - 1];
+        // if (column[i + 1] < 100) { column[i + 1] = 0; } integralSum[i + 1] = column[i + 1] / 256 + integralSum[i + 1 - 1];
+        // if (column[i + 2] < 100) { column[i + 2] = 0; } integralSum[i + 2] = column[i + 2] / 256 + integralSum[i + 2 - 1];
+        // if (column[i + 3] < 100) { column[i + 3] = 0; } integralSum[i + 3] = column[i + 3] / 256 + integralSum[i + 3 - 1];
+        // if (column[i + 4] < 100) { column[i + 4] = 0; } integralSum[i + 4] = column[i + 4] / 256 + integralSum[i + 4 - 1];
+        // if (column[i + 5] < 100) { column[i + 5] = 0; } integralSum[i + 5] = column[i + 5] / 256 + integralSum[i + 5 - 1];
+        // if (column[i + 6] < 100) { column[i + 6] = 0; } integralSum[i + 6] = column[i + 6] / 256 + integralSum[i + 6 - 1];
+        // if (column[i + 7] < 100) { column[i + 7] = 0; } integralSum[i + 7] = column[i + 7] / 256 + integralSum[i + 7 - 1];
+        // if (column[i + 8] < 100) { column[i + 8] = 0; } integralSum[i + 8] = column[i + 8] / 256 + integralSum[i + 8 - 1];
+        // if (column[i + 9] < 100) { column[i + 9] = 0; } integralSum[i + 9] = column[i + 9] / 256 + integralSum[i + 9 - 1];
+        // if (column[i + 10] < 100) { column[i + 10] = 0; } integralSum[i + 10] = column[i + 10] / 256 + integralSum[i + 10 - 1];
+        // if (column[i + 11] < 100) { column[i + 11] = 0; } integralSum[i + 11] = column[i + 11] / 256 + integralSum[i + 11 - 1];
+        // if (column[i + 12] < 100) { column[i + 12] = 0; } integralSum[i + 12] = column[i + 12] / 256 + integralSum[i + 12 - 1];
+        // if (column[i + 13] < 100) { column[i + 13] = 0; } integralSum[i + 13] = column[i + 13] / 256 + integralSum[i + 13 - 1];
+        // if (column[i + 14] < 100) { column[i + 14] = 0; } integralSum[i + 14] = column[i + 14] / 256 + integralSum[i + 14 - 1];
+        // if (column[i + 15] < 100) { column[i + 15] = 0; } integralSum[i + 15] = column[i + 15] / 256 + integralSum[i + 15 - 1];
+
+        if (column[i] < 100) { column[i] = 0; }
         integralSum[i] = column[i] / 256 + integralSum[i - 1];
     }
 
@@ -198,6 +220,8 @@ float process_column(uint16_t (&column)[])
     }
     // std::cout << "result is '" << result << "'\n";
 
+    // stop_timer(process_column);
+
     return result;
 
 // center of mass
@@ -227,11 +251,12 @@ float process_column(uint16_t (&column)[])
 
 void process_columns(Image &image)
 {
-    std::cout << "here\n";
+    // std::cout << "here\n";
     start_timer(process_columns);
 
     for (size_t i = 0; i < image.width; i++)
     {
+        // smooth_column(image.rotated_cw[i]);
         image.pixels[i] = process_column(image.rotated_cw[i]);
         // Algo genetic(image.rotated_cw[i]);
 
diff --git a/imagealgos.h b/imagealgos.h
index b1efc87..bf5611a 100644
--- a/imagealgos.h
+++ b/imagealgos.h
@@ -7,6 +7,7 @@
 
 constexpr size_t img_width = 1280;
 constexpr size_t img_height = 800;
+constexpr uint32_t patternSize = 16;
 
 struct Image
 {
@@ -18,12 +19,20 @@ struct Image
     unsigned int stride;
     libcamera::PixelFormat pixelFormat;
     float pixels[img_width];
+    unsigned int measurementCounter;
+    uint64_t timestampNs;
 };
 
-size_t pgm_save(Image *img, FILE *outfile, bool really_save = true);
+size_t pgm_save(Image *img, FILE *outfile, bool really_save = false);
 
 void unpack_10bit(uint8_t const *src, Image const &image, uint16_t *dest);
 void unpack_16bit(uint8_t const *src, Image const &image, uint16_t *dest);
 
+template <typename T>
+T median3(const T& a, const T& b, const T& c) {
+    using namespace std;
+    return max(min(a,b), min(max(a,b),c));
+}
+
 void rotate(Image & image);
 void process_columns(Image & image);
diff --git a/macro.h b/macro.h
index 80998e5..c92b836 100644
--- a/macro.h
+++ b/macro.h
@@ -1,11 +1,18 @@
 #pragma once
 
+#include <chrono>
+#include <iostream>
+
 #define start_timer(name) \
     std::chrono::steady_clock::time_point begin_ ## name = \
         std::chrono::steady_clock::now(); \
     \
 
 #define stop_timer(name) \
-std::chrono::steady_clock::time_point end_ ## name = \
+    std::chrono::steady_clock::time_point end_ ## name = \
     std::chrono::steady_clock::now(); \
+    // std::cout << #name << " (us): " \
+    // << std::chrono::duration_cast<std::chrono::microseconds>( \
+    //     end_ ## name - begin_ ## name) \
+    // << std::endl;
 
diff --git a/main.cpp b/main.cpp
index 0aff149..1a121a3 100644
--- a/main.cpp
+++ b/main.cpp
@@ -6,7 +6,10 @@
 #include <QJsonArray>
 #include <QJsonDocument>
 #include <QJsonObject>
+#include <QSerialPort>
+#include <QTextStream>
 #include <QtConcurrent/QtConcurrentRun>
+#include <QTimer>
 #undef QT_NO_KEYWORDS
 
 #include <chrono>
@@ -20,6 +23,8 @@
 #include "genetic_algos.h"
 #include "imagealgos.h"
 #include "LibCamera.h"
+#include "pigpio.h"
+#include "rotaryencoder.h"
 
 #define try_apply_config() \
 if(!applyConfig(config)) \
@@ -34,6 +39,8 @@ if(!applyConfig(config)) \
 const QString exposureTimeKey = "exposureTime";
 const QString laserLevelKey = "laserLevel";
 
+extern volatile int64_t positionSteps;
+
 struct requested_params_t {
     int32_t exposureTime = { 200 };
     int32_t laserLevel = { 7000 };
@@ -54,7 +61,53 @@ static void printControls();
 
 int main(int argc, char *argv[]) {
     QCoreApplication app(argc, argv);
-    qDebug() << "Hello qt";
+
+    QElapsedTimer t;
+    t.start();
+
+    // qDebug() << "msecs before serial:" << t.elapsed();
+
+    // QSerialPort *serialPort { new QSerialPort { "/dev/ttyUSB0", qApp } };
+
+    // if (!serialPort->setBaudRate(QSerialPort::Baud115200)) {
+    //     qDebug() << "serial port: cannot set baud rate:"
+    //              << serialPort->errorString();
+
+    //     return EXIT_FAILURE;
+    // }
+
+    // if (!serialPort->open(QFile::ReadWrite)) {
+    //     qDebug() << "cannot open serial port:" << serialPort->errorString();
+
+    //     return EXIT_FAILURE;
+    // }
+
+    // qDebug() << "serial port baud rate:" << serialPort->baudRate();
+
+    // qDebug() << "serial port data bits:" << serialPort->dataBits();
+    // qDebug() << "serial port parity:" << serialPort->parity();
+    // qDebug() << "serial port stop bits:" << serialPort->stopBits();
+
+    // QObject::connect(serialPort, &QSerialPort::readyRead,
+    //                  serialPort, [=](){
+    //                      qDebug() << "serialPort: " << serialPort->readAll();
+    //                  });
+
+    // QObject::connect(serialPort, &QSerialPort::errorOccurred,
+    //                  serialPort, [=](){
+    //                      qWarning() << "serial port error:" << serialPort->errorString();
+    //                  });
+
+    // qDebug() << "msecs before write and flush:" << t.elapsed();
+
+    // serialPort->write(QByteArray { "G91\n" });
+    // serialPort->flush();
+
+    qDebug() << "msecs before encoder:" << t.elapsed();
+
+    rotaryencoder encoder;
+
+    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>();
@@ -217,16 +270,16 @@ int main(int argc, char *argv[]) {
                 std::make_pair(memory, plane.length);
         }
 
-        // size_t desiredFPS = 200;
+        // size_t desiredFPS = 144;
 
         // std::int64_t lowerUS = 1 * 1000 * 1000 / desiredFPS;
         // std::int64_t higherUS = lowerUS;
         // std::int64_t value_pair[2] = { higherUS / 2, higherUS };
         request->controls().set(libcamera::controls::AnalogueGain, 1.0);
-        request->controls().set(libcamera::controls::ExposureTime, 4321 + expOffset++ * 100);
+        request->controls().set(libcamera::controls::ExposureTime, 100);
         // request->controls().set(
-        //     libcamera::controls::FrameDurationLimits,
-        //     libcamera::Span<const std::int64_t, 2>(value_pair));
+        // libcamera::controls::FrameDurationLimits,
+        // libcamera::Span<const std::int64_t, 2>(value_pair));
 
         requests.push_back(std::move(request));
     }
@@ -270,10 +323,10 @@ int main(int argc, char *argv[]) {
         // qDebug() << "image";
         return QByteArray((const char*)pgm_image, pgm_image_size);
     });
-    qHttpServer.route("/v1/sensor/exposureTimeUs", [&]() {
-        std::lock_guard<std::mutex> lg(pgm_image_mtx);
-        return "123";
-    });
+    // qHttpServer.route("/v1/sensor/exposureTimeUs", [&]() {
+    //     // std::lock_guard<std::mutex> lg(pgm_image_mtx);
+    //     return "123";
+    // });
     qHttpServer.route("/v1/pixels", [&]() {
         std::lock_guard<std::mutex> lg(pgm_image_mtx);
 
@@ -285,10 +338,26 @@ int main(int argc, char *argv[]) {
 
         QJsonObject json;
         json["pixels"] = pixels;
+        json["encoderPosition"] = qint64 { encoder.position() };
+        json["measurementCounter"] = qint64 { img.measurementCounter };
+        json["timestampUs"] = qint64 { img.timestampNs / 1000. } ;
 
         return QHttpServerResponse(QJsonDocument(json).toJson());
     });
 
+
+    qHttpServer.route("/v1/commands/resetEncoder", [&](const QHttpServerRequest &request) -> QHttpServerResponse {
+        if (request.method() != QHttpServerRequest::Method::Post) {
+            return QHttpServerResponse::StatusCode::NotFound;
+        }
+
+        qDebug() << "reset encoder";
+
+        positionSteps = 0;
+
+        return QHttpServerResponse::StatusCode::Ok;
+    });
+
     qHttpServer.route("/v1/sensor/params", [&](const QHttpServerRequest &request) -> QHttpServerResponse {
 
         switch (request.method()) {
@@ -366,6 +435,8 @@ int main(int argc, char *argv[]) {
                 s << value;
 
                 s >> requested_params.laserLevel;
+
+                qDebug() << "done with laser level";
             }
 
             return QHttpServerResponse(request.body());
@@ -505,6 +576,13 @@ int main(int argc, char *argv[]) {
 */
 void onRequestCompleted(Request *completed_request)
 {
+    static std::chrono::steady_clock::time_point fpsTimstamp = std::chrono::steady_clock::now();
+
+    QElapsedTimer t;
+    t.start();
+    static uint32_t performanceCounter { 0 };
+    static uint32_t elapsedSum { 0 };
+
     bool verbose = false;
 
     if (completed_request->status() == Request::RequestCancelled)
@@ -528,12 +606,12 @@ void onRequestCompleted(Request *completed_request)
 
         const FrameMetadata &metadata = buffer->metadata();
 
-        if (verbose)
-        {
-            std::cout << " seq: " << std::setw(6) << std::setfill('0')
-            << metadata.sequence
-            << " bytesused: ";
-        }
+        // if (verbose)
+        // {
+        //     std::cout << " seq: " << std::setw(6) << std::setfill('0')
+        //     << metadata.sequence
+        //     << " bytesused: ";
+        // }
 
         for (size_t i = 0; i < buffer->planes().size(); ++i)
         {
@@ -559,6 +637,8 @@ void onRequestCompleted(Request *completed_request)
                 img.dataSize = size;
                 img.stride = stride;
                 img.pixelFormat = pixelFormat;
+                img.measurementCounter = metadata.sequence;
+                img.timestampNs = metadata.timestamp;
 
                 // uint16_t unpacked[img.width * img.height] = { 0 };
                 // unpack_16bit((uint8_t*)img.data, img, (uint16_t*)&unpacked);
@@ -567,58 +647,60 @@ void onRequestCompleted(Request *completed_request)
                 rotate(img);
                 process_columns(img);
 
-                static bool done = false;
+                // static bool done = false;
                 // mark pixels and max region
-                for (size_t i = 0; i < img_width; ++i)
-                {
-                    // std::cout << "\t" << img.pixels[i] << std::endl;
-                    // uint
-                    // const auto & p = img.pixels[i];
-                    // const auto int_p = int(p);
-                    // const auto fract = p - int_p;
+                // for (size_t i = 0; i < img_width; ++i)
+                // {
+                //     // std::cout << "\t" << img.pixels[i] << std::endl;
+                //     // uint
+                //     // const auto & p = img.pixels[i];
+                //     // const auto int_p = int(p);
+                //     // const auto fract = p - int_p;
 
 
-                    // img.data[int_p][i] = 256 * 256 * fract;
-                    // img.data[int_p + 1][i] = 256 * 256 * (1.0 - fract);
+                //     // img.data[int_p][i] = 256 * 256 * fract;
+                //     // img.data[int_p + 1][i] = 256 * 256 * (1.0 - fract);
 
 
-                    // if (!done) {
-                    //     std::cout << fract << " ";
-                    // }
+                //     // if (!done) {
+                //     //     std::cout << fract << " ";
+                //     // }
 
-                    img.data[size_t(img.pixels[i])][i] = 0;
-                    img.data[size_t(img.pixels[i]) - 6][i] = 0xffff;
-                    img.data[size_t(img.pixels[i]) + 6][i] = 0xffff;
-                }
-                done = true;
+                //     img.data[size_t(img.pixels[i])][i] = 0;
+                //     // crash is possible
+                //     img.data[size_t(img.pixels[i]) - patternSize / 2][i] = 0xffff;
+                //     img.data[size_t(img.pixels[i]) + patternSize / 2][i] = 0xffff;
+                // }
+                // done = true;
 
                 // // FILE * f = fopen("/tmp/R16.pgm", "w");
-                FILE * f = fopen("/tmp/img.pgm", "w");
-                // // FILE * f = fopen("/tmp/MONO_PISP_COMP1.pgm", "w");
+                // FILE * f = fopen("/tmp/img.pgm", "w");
+                // // // FILE * f = fopen("/tmp/MONO_PISP_COMP1.pgm", "w");
 
-                if (f == NULL)
-                {
-                    std::cerr << "cannot open output file: "
-                              << strerror(errno)
-                              << std::endl;
-                }
-                else
-                {
-                    // pgm_save(&img, f);
-                    pgm_save(&img, f);
-                    fclose(f);
-                    // std::cout << "file written" << std::endl;
-                }
+                // if (f == NULL)
+                // {
+                //     std::cerr << "cannot open output file: "
+                //               << strerror(errno)
+                //               << std::endl;
+                // }
+                // else
+                // {
+                //     // pgm_save(&img, f);
+                //     // pgm_save(&img, f);
+                //     fclose(f);
+                //     // std::cout << "file written" << std::endl;
+                // }
+                pgm_save(&img, nullptr);
             }
         }
     }
 
     const libcamera::ControlList &metadata = completed_request->metadata();
     const ControlInfoMap & control_map = camera->controls();
-    const ControlIdMap & ctrlIdMap = control_map.idmap();
+    // const ControlIdMap & ctrlIdMap = control_map.idmap();
 
     auto frameDurationCtrl = control_map.find(&controls::FrameDurationLimits);
-    auto expTimeCtrl = control_map.find(&controls::ExposureTime);
+    // auto expTimeCtrl = control_map.find(&controls::ExposureTime);
     double fps = frameDurationCtrl == control_map.end() ?
                      std::numeric_limits<double>::quiet_NaN() :
                      (1e6 / frameDurationCtrl->second.min().get<int64_t>());
@@ -641,10 +723,10 @@ void onRequestCompleted(Request *completed_request)
                   << std::endl;
     }
 
-    for (const auto & [id, value] : metadata)
-    {
+    // for (const auto & [id, value] : metadata)
+    // {
 
-    }
+    // }
 
     // metadata.set(controls::ExposureTime, 300);
 
@@ -687,39 +769,45 @@ void onRequestCompleted(Request *completed_request)
     // completed_request->reuse(Request::ReuseBuffers);
     // camera->queueRequest(completed_request);
 
+    qDebug() << "set exposureTime to" << requested_params.exposureTime;
+    completed_request->controls().set(libcamera::controls::ExposureTime,
+                                      requested_params.exposureTime);
+
 
     completed_request->reuse(Request::ReuseBuffers);
 
     completed_request->controls().set(libcamera::controls::AeEnable, false);
     completed_request->controls().set(libcamera::controls::draft::NoiseReductionMode,
                                       libcamera::controls::draft::NoiseReductionModeEnum::NoiseReductionModeHighQuality);
-    // completed_request->controls().set(libcamera::controls::ExposureTime, rand() % 1000 + 100);
 
-    static bool done0 = false;
-    static bool done1 = false;
+    camera->queueRequest(completed_request);
+
+    ++performanceCounter;
+    elapsedSum += t.elapsed();
+
+    // if (performanceCounter == 20)
+    std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now();
 
-    if (!done0 || !done1)
+    if ((now - fpsTimstamp) > 1000ms)
     {
+        auto msPerFrame { float(elapsedSum / performanceCounter) };
 
-        if (completed_request->sequence() % 2 == 0)
-        {
-            // qDebug() << "set 0" << completed_request->sequence();
-            completed_request->controls().set(
-                libcamera::controls::ExposureTime,
-                requested_params.exposureTime);
-            done0 == true;
-        }
-        else
-        {
-            // qDebug() << "set 1" << completed_request->sequence();
-            completed_request->controls().set(
-                libcamera::controls::ExposureTime,
-                requested_params.exposureTime);
-            done1 = true;
-        }
+        double configFps = frameDurationCtrl == control_map.end() ?
+                               std::numeric_limits<double>::quiet_NaN() :
+                               (1e6 / frameDurationCtrl->second.min().get<int64_t>());
+
+        auto fps { 1000.f / msPerFrame };
+
+        qDebug() << "fps ideal/real is" << configFps << "/" << fps
+                 << "; ms per frame is" << msPerFrame << "counted fps" << performanceCounter;
+
+        elapsedSum = 0;
+        performanceCounter = 0;
+        fpsTimstamp = now;
     }
 
-    camera->queueRequest(completed_request);
+    // qDebug() << "-------------------------------------------";
+
 }
 
 static bool applyConfig(const std::unique_ptr<CameraConfiguration> & config)
diff --git a/rotaryencoder.cpp b/rotaryencoder.cpp
new file mode 100644
index 0000000..41d56e8
--- /dev/null
+++ b/rotaryencoder.cpp
@@ -0,0 +1,107 @@
+#include "rotaryencoder.h"
+
+#include <cstdint>
+#include <cstdio>
+#include <cstdlib>
+#include <vector>
+
+#include <QDebug>
+#include <QElapsedTimer>
+
+#include <wiringPi.h>
+
+// got from https://gist.github.com/ast/a19813fce9d34c7240091db11b8190dd
+// https://gist.github.com/ast
+// Inspired by Paul Stoffregen's excellent Arduino library Encoder:
+// https://github.com/PaulStoffregen/Encoder
+
+constexpr int gpioA = 17;
+constexpr int gpioB = 27;
+// constexpr int gpioA = 0;
+// constexpr int gpioB = 2;
+// constexpr int gpioA = 11;
+// constexpr int gpioB = 13;
+
+const std::vector<int> encoderTable = {
+    0, 1, -1, 0, -1, 0, 0, 1, 1, 0, 0, -1, 0, -1, 1, 0
+};
+
+volatile int64_t positionSteps;
+volatile uint8_t state;
+
+void pin_isr(void) {
+    uint8_t p1val = digitalRead(gpioA);
+    uint8_t p2val = digitalRead(gpioB);
+    uint8_t s = state & 3;
+
+    if (p1val) s |= 4;
+    if (p2val) s |= 8;
+
+    state = (s >> 2);
+
+    switch (s) {
+    case 1: case 7: case 8: case 14:
+        positionSteps = positionSteps + 1;
+        return;
+    case 2: case 4: case 11: case 13:
+        positionSteps = positionSteps - 1;
+        return;
+    case 3: case 12:
+        positionSteps = positionSteps + 2;
+        return;
+    case 6: case 9:
+        positionSteps = positionSteps - 2;
+        return;
+    }
+}
+
+rotaryencoder::rotaryencoder()
+{
+    QElapsedTimer t;
+    t.start();
+
+    if (wiringPiSetupGpio()) {
+        perror("wiringPiSetupGpio");
+        exit(EXIT_FAILURE);
+    }
+
+    qDebug() << "msecs to setup wiringPi:" << t.elapsed();
+
+    if ( wiringPiISR (gpioA, INT_EDGE_BOTH, &pin_isr) < 0 ) {
+        perror("wiringPiISR");
+        exit(EXIT_FAILURE);
+    }
+
+    qDebug() << "msecs to register interruption A:" << t.elapsed();
+
+    if ( wiringPiISR (gpioB, INT_EDGE_BOTH, &pin_isr) < 0 ) {
+        perror("wiringPiISR");
+        exit(EXIT_FAILURE);
+    }
+
+    qDebug() << "msecs to register interruption B:" << t.elapsed();
+
+    // pinMode (gpioA,  INPUT) ;
+    // pinMode (gpioB,  INPUT) ;
+    // pullUpDnControl(gpioA, PUD_UP);
+    // pullUpDnControl(gpioB, PUD_UP);
+
+    qDebug() << "encoder is ok";
+
+    // // Show position every second
+    // while ( 1 ) {
+    //     constexpr double stepsPerMm = 200;
+    //     const double positionMm = ::positionSteps / stepsPerMm;
+    //     qDebug() << ::positionSteps
+    //              << '-'
+    //              << QString::number(positionMm, 'f', 3)
+    //              << "(mm)";
+    //     // printf( "%ld\n", ::position);
+    //     usleep( 100 * 1000 ); // wait 1 second
+    // }
+}
+
+int64_t rotaryencoder::position() const
+{
+    return ::positionSteps;
+}
diff --git a/rotaryencoder.h b/rotaryencoder.h
new file mode 100644
index 0000000..f2f9b76
--- /dev/null
+++ b/rotaryencoder.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <cstdint>
+
+class rotaryencoder
+{
+public:
+    rotaryencoder();
+
+public:
+    int64_t position() const;
+};