path: root/src/calibration.cpp

#include "calibration.h"

#include "fuck_intel.h"

#include <execution>
#include <iostream>
#include <numeric>

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

#include "imagealgos.h"

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

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

        return false;
    }

    auto bytes = f.read((char *) table.data(), sizeof(op::CalibrationTable));

    if (bytes != sizeof(op::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];
    // }

    qDebug() << "calibration table opened:" << filename;

    return true;
}

bool op::dump(const op::CalibrationTablePtr &table, const QString &filename)
{
    qDebug() << Q_FUNC_INFO << "size is" << sizeof(op::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(op::CalibrationTable));

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

    return true;
}

void op::interpolate(const op::CalibrationTablePtr table)
{
    std::for_each(std::execution::par, table->begin(), table->end(), [](auto &column) {
        op::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 op::interpolate(op::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 op::calibrationTableToImage(const op::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) {
            const auto &v = column.at(rowIdx);
            bool hasValue = !qFuzzyIsNull(v);

            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(op::CalibrationTable) / sizeof(calibrationTable->at(0).at(0));

    return result;
}

QList<Pixels> op::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};
        // qDebug() << "\t" << __func__ << "enc pos:" << it->counters.encoderPosition;
        ++it;

        while (it != rawProfiles.constEnd()
               && it->counters.encoderPosition == sum.counters.encoderPosition) {
            sum += *it;
            ++count;
            // qDebug() << "\t\t" << __func__ << "enc pos:" << it->counters.encoderPosition;
            ++it;
        }

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

    return result;
}

bool op::calibrateZ(const QList<Pixels> &rawProfiles,
                    const uint32_t &stepsPerMm,
                    op::CalibrationTablePtr table)
{
    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 >= op::calibrationColumnHeight)) {
                std::cerr << __func__
                          << ":/tinvalid discrete value. col: " << columnIdx
                          << ", val: " << pixelValue << std::endl;

                return false;
            }

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

    return true;
}

bool op::calibrateX(const QList<Pixels> &rawProfiles, op::CalibrationTablePtr table)
{
    // TODO: move to settings
    constexpr double triangleBaseMm{8.};

    int i{-1};

    for (const auto &rawProfile : rawProfiles) {
        ++i;
        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();
        // qDebug() << "anchors (discretes):" << xAnchors;

        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;
        // qDebug() << "anchors (mm):" << xAnchors;

        if (rawProfile.counters.encoderPosition > 6000) {
            qt_noop();
        }

        std::array<uint16_t, img_width> dl = {uint16_t{0}};

        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 >= op::calibrationColumnHeight)) {
                std::cerr << __func__
                          << ":/tinvalid discrete value. col: " << columnIdx
                          << ", val: " << pixelValue << std::endl;
                return false;
            }

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

            // TODO: check if value exists
            (*table)[columnIdx][discretePixelValue] = xMmValue;
            dl[columnIdx] = discretePixelValue;
        }

        // const auto avg_dpv = std::accumulate(dl.cbegin(), dl.cend(), uint64_t{0}) / img_width;
        // const auto avg_p = std::accumulate(pixels.cbegin(), pixels.cend(), double{0.}) / img_width;
        // std::cout << avg_dpv << '/' << avg_p << std::endl;
        // qDebug() << rawProfile;
    }

    std::cout << std::endl;

    return true;
}

void op::dumpCalibrationPixels(const std::vector<std::shared_ptr<Pixels> > &calibrationPixels,
                               const op::DumpFormat format)
{
    const auto &rawProfiles = 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;

        // TODO: move out of `for` loop
        // TODO: use switch, or remove enum and create a separate function
        if (format == op::DumpFormat::Json) {
            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();
        } else if (format == op::DumpFormat::Binary) {
            if (!rawProfile->save(filepath)) {
                return;
            }
        }
    }

    qDebug() << "dump finished";
}

bool areAllEncPosIdentical(const std::vector<Pixels> &rawProfiles)
{
    if (rawProfiles.empty()) {
        return false;
    }

    if (rawProfiles.size() == 1) {
        return true;
    }

    // const auto equalEncPos = [](const Pixels &a, const Pixels &b) {
    //     return a.counters.encoderPosition == b.counters.encoderPosition;
    // };
    const auto nonEqualEncPos = [](const Pixels &a, const Pixels &b) {
        return a.counters.encoderPosition != b.counters.encoderPosition;
    };

    return std::ranges::adjacent_find(rawProfiles, nonEqualEncPos) == rawProfiles.end();
}

std::optional<Pixels> op::avgFilterAtPos(const std::vector<Pixels> &rawProfiles)
{
    if (rawProfiles.empty()) {
        return std::nullopt;
    }

    if (rawProfiles.size() == 1) {
        return rawProfiles.front();
    }

    if (!areAllEncPosIdentical(rawProfiles)) {
        return std::nullopt;
    }

    Pixels sum;
    sum.counters = rawProfiles.front().counters;

    // TODO: think about std::accumulate
    // const auto tmp = std::accumulate(rawProfiles.begin(), rawProfiles.end(), Pixels{});

    for (const auto &rp : rawProfiles) {
        sum += rp;
    }

    sum /= rawProfiles.size();

    return sum;
}

std::optional<Pixels> op::medianFilterAtPos(const std::vector<Pixels> &rawProfiles)
{
    const auto &rps = rawProfiles;

    if (rps.empty()) {
        return std::nullopt;
    }

    if (rps.size() == 1) {
        return rps.front();
    }

    if (!areAllEncPosIdentical(rps)) {
        return std::nullopt;
    }

    using pixel_t = decltype(Pixels::pixels)::value_type;
    // std::array<std::vector<pixel_t>, img_width> forMedian;

    Pixels result;

    for (size_t i{0}; i < img_width; ++i) {
        std::vector<pixel_t> pixelsAtI;
        pixelsAtI.reserve(rps.size());

        for (const auto &rp : rps) {
            const auto &p = rp.pixels.at(i);

            if (qFuzzyIsNull(p) || qIsNaN(p)) {
                continue;
            }
            pixelsAtI.push_back(p);
        }

        // result.pixels.at(i) = pixelsAtI.empty() ? 0 :
        if (pixelsAtI.empty()) {
            continue;
        }

        if (pixelsAtI.size() == 1) {
            result.pixels.at(i) = pixelsAtI.front();
            continue;
        }

        std::sort(std::execution::par_unseq, pixelsAtI.begin(), pixelsAtI.end());

        if (pixelsAtI.size() % 2 == 1) {
            result.pixels.at(i) = pixelsAtI.at(pixelsAtI.size() / 2);
        } else {
            result.pixels.at(i) = (pixelsAtI.at(pixelsAtI.size() / 2 - 1)
                                   + pixelsAtI.at(pixelsAtI.size() / 2))
                                  / 2;
        }
    }

    result.counters = rps.front().counters;

    return result;
}