path: root/src/calibration.cpp

#include "calibration.h"

#include <execution>
#include <iostream>

#include <QDebug>
#include <QDir>
#include <QFile>
#include <QImage>
#include <QJsonArray>
#include <QJsonDocument>
#include <QJsonObject>

#include "imagealgos.h"

bool openCalibrationTable(const QString& filename, CalibrationTablePtr& table)
{
    QFile f(filename);

    if (!f.open(QFile::ReadOnly)) {
        qWarning() << Q_FUNC_INFO << "cannot open" << filename
                   << "for reading:" << f.errorString();

        return false;
    }

    table.reset(new CalibrationTable{{0}});
    auto bytes = f.read((char*) table.data(), sizeof(CalibrationTable));

    if (bytes != sizeof(CalibrationTable)) {
        qWarning() << "cannot read calibration table from" << filename << bytes;

        if (f.error()) {
            qWarning() << f.errorString() << f.error() << (void*) table.data();
        } else {
            qWarning() << "file size:" << f.size() << "; got:" << bytes;
        }

        return false;
    }

    // for (const auto& col : *table) {
    //     qDebug() << "calibration column mid:" << col[640];
    // }

    return true;
}

bool dump(const CalibrationTablePtr& table, const QString& filename)
{
    qDebug() << Q_FUNC_INFO << "size is" << sizeof(CalibrationTable);

    QFile f(filename);

    if (!f.open(QFile::WriteOnly)) {
        qWarning() << Q_FUNC_INFO << "cannot open" << filename
                   << "for writing:" << f.errorString();

        return false;
    }

    const auto written = f.write((const char*) table.data(),
                                 sizeof(CalibrationTable));

    if (written != sizeof(CalibrationTable) || !f.flush()) {
        qWarning() << Q_FUNC_INFO << "cannot write" << filename << ":"
                   << f.errorString();
        return false;
    }

    return true;
}

void interpolate(CalibrationTablePtr& table)
{
    std::for_each(std::execution::par,
                  table->begin(),
                  table->end(),
                  [](auto& column) { interpolate(column); });

    for (size_t i = 9471; i < 9472; i++) {
        std::cout << "row #" << i << ": ";

        for (size_t j = 0; j < 1280; ++j) {
            const auto& p = table->at(j).at(i);
            std::cout << p << ' ';
        }

        std::cout << std::endl;
    }

    // for (size_t i = 0; i < discretesInRage; i++) {
    //     std::cout << "row #" << i << ": ";

    //     for (size_t j = 640 - 5; j < 640 + 5; ++j) {
    //         const auto& p = table->at(j).at(i);
    //         std::cout << p << ' ';
    //     }

    //     std::cout << std::endl;
    // }

    // for (const auto& p : (*table)[table->size() / 2]) {
    //     qDebug() << "mid column pixel" << p;
    // }
}

void interpolate(CalibrationColumn& column)
{
    size_t start{0};
    auto& c = column;

#define FIND_IF(index, condition) \
    while (bool(c[index]) != condition && index < calibrationTableHeight) \
        ++index; \
\
    if (index == calibrationTableHeight) \
    return

    FIND_IF(start, true);

    while (true) {
        size_t left = start + 1;
        FIND_IF(left, false);
        --left;

        size_t right = left + 1;
        FIND_IF(right, true);

        auto delta = (c[right] - c[left]) / (right - left);

        for (auto i = left + 1; i < right; ++i) {
            c[i] = c[i - 1] + delta;

            if (c[i] > 190.) {
                qWarning() << "interpolate: got invalid value mm" << c[i];

                qWarning() << "left/i/right" << left << i << right;
                qWarning() << "delta" << delta;
                qWarning() << "c[left/i/right]" << c[left] << c[i] << c[right];

                exit(EXIT_FAILURE);
            }
        }

        start = right;
    }
}

QImage calibrationTableToImage(const CalibrationTablePtr& calibrationTable)
{
    QImage result(QSize(calibrationTable->size(),
                        calibrationTable->at(0).size()),
                  QImage::Format::Format_Indexed8);

    // QImage image(QSize(imageWidth, imageHeight), QImage::Format_Indexed8);

    QColor color(Qt::green);
    auto r = color.redF();
    auto g = color.greenF();
    auto b = color.blueF();

    for (int c = 0; c < 256; c++) {
        QRgb col = qRgb(int(c * r), int(c * g), int(c * b));
        result.setColor(c, col);
    }

    int notNull = 0;

    for (size_t colIdx = 0; colIdx < calibrationTable->size(); ++colIdx) {
        const auto& column = calibrationTable->at(colIdx);

        for (size_t rowIdx = 0; rowIdx < column.size(); ++rowIdx) {
            bool hasValue = !qFuzzyIsNull(column.at(rowIdx));

            notNull += int(hasValue);

            result.setPixel(colIdx, rowIdx, hasValue ? 255 : 0);
            // if (column.at(rowIdx) >= 190.) {
            //     qWarning() << "invalid mm value" << column.at(rowIdx);
            //     exit(EXIT_FAILURE);
            // }
            // result.setPixel(colIdx, rowIdx, (column.at(rowIdx) / 190.) * 255);
        }
    }

    qDebug() << "not null count" << notNull << "of"
             << sizeof(CalibrationTable)
                    / sizeof(calibrationTable->at(0).at(0));

    return result;
}

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 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());

            // 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 << ";";
            // }
        }

        // std::cout << std::endl << std::flush;
    }

    return result;
    // TODO: try something interesting
    // return {};
}

CalibrationTablePtr calibrateX(const QList<Pixels>& rawProfiles)
{
    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;
            }
        }

        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);
                                                  });

        int nearestAnchorToX0Idx = nearestAnchorToX0 - xAnchors.constBegin();

        QList<double> xAnchorsMm(xAnchors.count());

        for (int i = 0; i < xAnchors.size(); ++i) {
            xAnchorsMm[i] = (i - nearestAnchorToX0Idx) * triangleBaseMm / 2.;
        }

        auto xAnchorIt = xAnchors.constBegin() + 1;
        auto xAnchorMmIt = xAnchorsMm.constBegin() + 1;

        for (size_t columnIdx = 0; columnIdx < pixels.size(); ++columnIdx) {
            // 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) {
                ++xAnchorIt;
                ++xAnchorMmIt;
            }
            const auto xLeft = *(xAnchorIt - 1);
            const auto xRight = *(xAnchorIt);

            if (columnX < xLeft || columnX > xRight) {
                if (rawProfile.counters.encoderPosition >= 0) {
                    qWarning()
                        << "x anchor not found" << xLeft << columnX << xRight;
                    continue;
                }
            }

            const auto& pixelValue = pixels.at(columnIdx);
            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());

            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;
        }
    }

    return result;
}

void dumpCalibrationPixels(std::vector<Pixels>&& calibrationPixels)
{
    std::vector<Pixels> rawProfiles;

    std::swap(rawProfiles, calibrationPixels);

    const QString dumpSubdir{
        QDateTime::currentDateTime().toString("yyyy.MM.dd_hh.mm.ss")};
    const QDir dumpPath{dumpsRoot + "/" + dumpSubdir};

    if (!dumpPath.mkdir(dumpPath.path())) {
        qWarning() << "cannot create dir: " << dumpPath.path();

        return;
    }

    for (const auto& rawProfile : rawProfiles) {
        const auto filename = QLatin1String("raw_profile_meas_%1_enc_%2")
                                  .arg(QString::number(
                                      rawProfile.counters.measurementCounter))
                                  .arg(rawProfile.counters.encoderPosition);
        const auto filepath = dumpPath.path() + "/" + filename;

        QFile f{filepath};

        if (!f.open(QFile::WriteOnly)) {
            qWarning() << "cannot open dump dump file" << f.fileName();
            qWarning() << "error is:" << f.errorString();

            return;
        }

        QJsonObject jsonCounters{
            {"timestampUs", qint64(rawProfile.counters.timestampUs)},
            {"measurementCounter",
             qint64(rawProfile.counters.measurementCounter)},
            {"encoderPosition", qint64(rawProfile.counters.encoderPosition)},
        };

        QJsonObject json;

        json["counters"] = jsonCounters;

        QJsonArray jsonPixels;

        for (const auto& pixel : rawProfile.pixels) {
            jsonPixels << pixel;
        }

        json["pixels"] = jsonPixels;

        if (!f.write(QJsonDocument(json).toJson())) {
            qWarning() << "cannot write file" << f.fileName();
            qWarning() << "error is" << f.errorString();

            return;
        }

        qDebug() << "file written: " << f.fileName();
    }

    qDebug() << "dump finished";
}