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 << __func__ << ": 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 (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;
}

QList<Pixels> filter(
    const QList<Pixels>& rawProfiles)
{
    QList<Pixels> result;

    QList<Pixels>::const_iterator it = rawProfiles.constBegin();

    while (it != rawProfiles.constEnd()) {
        Pixels sum = *it;

        size_t count{1};
        ++it;

        while (it != rawProfiles.constEnd() &&
               it->counters.encoderPosition == sum.counters.encoderPosition) {
            sum += *it;
            ++count;
            ++it;
        }

        sum /= float(count);
        result << sum;
    }

    return result;
}

CalibrationTablePtr calibrateZ(
    const QList<Pixels>& rawProfiles, const uint32_t& stepsPerMm)
{
    CalibrationTablePtr result{new CalibrationTable{{0}}};

    for (const auto& rawProfile : rawProfiles) {
        const float positionMm{float(rawProfile.counters.encoderPosition) /
                               float(stepsPerMm)};

        const auto& pixels = rawProfile.pixels;

        for (size_t columnIdx = 0; columnIdx < pixels.size(); ++columnIdx) {
            const auto& pixelValue = pixels.at(columnIdx);
            const uint16_t discretePixelValue{
                uint16_t(pixelValue * discretesInRage / img_height)};

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

            auto& calibrationColumn = (*result)[columnIdx];
            calibrationColumn[discretePixelValue] = positionMm;
        }
    }

    return result;
}

CalibrationTablePtr calibrateX(
    const QList<Pixels>& rawProfiles)
{
    // TODO: move to settings
    constexpr double triangleBaseMm{8.};
    CalibrationTablePtr result{new CalibrationTable{{0}}};

    for (const auto& rawProfile : rawProfiles) {
        const auto& pixels = rawProfile.pixels;
        auto lines = pixelsToLines(rawProfile);

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

        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 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 - centralAnchorIdx) * 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 [...(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) {
                    // FIXME: xRight can be smaller than xLeft
                    // 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)};

            // 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.);

            (*result)[columnIdx][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";
}