path: root/src/image.cpp

#include "image.h"

// #include <format>

#include <QElapsedTimer>

#include <libcamera/formats.h>

#include "macro.h"
#include "pixels.h"

uint64_t dq_elapsed_ns = 0;
uint64_t get_elapsed_ns = 0;
uint64_t sum_elapsed_ns = 0;
uint64_t corr_elapsed_ns = 0;
uint64_t max_elapsed_ns = 0;
uint64_t value_elapsed_ns = 0;
uint64_t rot_elapsed_ns = 0;
// time spent for conversion of image to pixels
uint64_t pix_elapsed_ns = 0;
uint64_t dropped_count = 0;

// float process_column(const uint8_t (&column)[])
// float process_column(const Image::row_t &column)
float process_column(const Image::column_t &column)
{
    // Image::column_t c = column;
    // start_timer(process_column);
    // QElapsedTimer t;
    // t.start();

    float result = std::numeric_limits<float>::quiet_NaN();

    // constexpr uint32_t signalThreshold = 900; // = SKO * sqrt(patternSize)
    constexpr uint32_t signalThreshold = 900; // = SKO * sqrt(patternSize)
    static constexpr uint32_t patternOffset = patternSize - ((patternSize % 2 == 1) ? 1 : 0);
    constexpr uint32_t correlationSize = img_height - patternSize
                                         + ((patternSize % 2 == 1) ? 1 : 0);
    // constexpr uint32_t correlationSize = img_height - patternSize;
    uint32_t correlation[img_height] = {0};
    // memset(correlation, 0, img_height * sizeof(correlation[0]));
    uint32_t integralSum[img_height];
    uint32_t maxTripleSum = signalThreshold * 50;
    uint32_t x1 = 0;
    int32_t y1 = 0;
    int32_t y2 = 0;

    static_assert((img_height % patternSize) == 0, "img_height % patternSize should be 0");
    // std::array<uint16_t, img_height / patternSize> sums;
    // uint16_t maxLALASum{0};
    // size_t maxLALAIdx{0};
    // uint16_t sum{0};

    // for (size_t i = 0; i < img_height; i += patternSize) {
    //     // const auto sum = std::accumulate(column.cbegin() + i,
    //     //                                  column.cbegin() + i + patternSize,
    //     //                                  0,
    //     //                                  std::plus<uint16_t>());
    //     uint16_t sum{0};

    //     for (size_t j = i; j < i + patternSize; ++j)
    //         sum += column[j];
    //     // if ((i % patternSize) == 0)
    //     //     sum = 0;
    //     // sum += column[i];

    //     // if (sum > 0xff)
    //     // std::cout << sum << ' ';

    //     if (sum > maxLALASum) {
    //         maxLALASum = sum;
    //         maxLALAIdx = i;
    //     }
    // }

    // maxLALAIdx = patternSize * 32 - 14; // crash
    // maxLALAIdx = patternSize * 32 - 14; // no crash

    // maxLALAIdx = std::clamp(uint32_t(maxLALAIdx),
    //                         uint32_t(patternSize),
    //                         uint32_t(img_height - patternSize * 2 - 14));

    // std::cout << maxLALAIdx << ' ';
    // memset(correlation, 0, img_height * sizeof(correlation[0]));
    // memset(correlation, 0, patternSize);
    // memset(correlation + correlationSize - 1, 0, patternSize);
    integralSum[0] = 0;
    // integralSum[maxLALAIdx] = 0;
    // memset(integralSum, 0, img_height * sizeof(integralSum[0]));

    for (uint32_t i = 1; i < img_height; ++i) {
        // for (uint32_t i = maxLALAIdx + 1; i < maxLALAIdx + patternSize * 3; ++i) {
        integralSum[i] = column[i] + integralSum[i - 1];
    }

    // sum_elapsed_ns += t.nsecsElapsed();
    // t.restart();

    // pixel * <sum of neighbours>
    for (uint32_t i = 0; i < correlationSize; ++i)
        // for (uint32_t i = maxLALAIdx; i < maxLALAIdx + patternSize * 2; ++i)
        correlation[i + patternSize / 2] = column[i + patternSize / 2]
                                           * (integralSum[i + patternOffset] - integralSum[i]);
    // * (integralSum[i + patternSize] - integralSum[i]);

    // corr_elapsed_ns += t.nsecsElapsed();
    // t.restart();

    uint32_t cPPP = correlation[0];
    uint32_t cPP = correlation[1];
    uint32_t cP = correlation[2];

    // uint32_t c = correlation[3];
    // uint32_t cN = correlation[4];
    // uint32_t cNN = correlation[5];

    for (uint32_t i = 3; i < img_height - 2; ++i) {
        // for (uint32_t i = maxLALAIdx + 3; i < maxLALAIdx + patternSize * 3 - 2; ++i) {
        // p - pixel, n - neighbour
        // P - pixel used in sum, N - neighbour used in sum
        // [N P N]
        const uint32_t c = correlation[i];
        const uint32_t cN = correlation[i + 1];
        const uint32_t cNN = correlation[i + 2];
        const auto sum = cP + c + cN;

        // const int32_t rioux0 = int32_t(cPPP + cPP) - int32_t(c + cN);
        // const int32_t rioux1 = int32_t(cPP + cP) - int32_t(cN + cNN);

        // if (sum > maxTripleSum && rioux0 < 0 && rioux1 >= 0) {
        //     x1 = i - 1;
        //     y1 = rioux0;
        //     y2 = rioux1;
        //     maxTripleSum = sum;
        // }

        if (sum > maxTripleSum) {
            // [N N n p] - [P N]
            const int32_t rioux0 = int32_t(cPPP + cPP) - int32_t(c + cN);

            if (rioux0 < 0) {
                // [N N p] - [p N N]
                const int32_t rioux1 = int32_t(cPP + cP) - int32_t(cN + cNN);

                if (rioux1 >= 0) {
                    x1 = i - 1;
                    y1 = rioux0;
                    y2 = rioux1;
                    maxTripleSum = sum;
                }
            }
        }

        cPPP = cPP;
        cPP = cP;
        cP = c;
        // c = cN;
        // cN = cNN;
        // cNN = correlation[i + 1];
    }

    // value_elapsed_ns += t.nsecsElapsed();
    // t.restart();

    result = (y2 != y1) ? (float(x1) - (float(y1) / (y2 - y1)))
                        : std::numeric_limits<float>::quiet_NaN();

    return result;
}

// uint8_t &Image::dataAt(size_t row, size_t col)
// {
//     const auto index = img_width * row + col;
//     return *(data + index);
// }

void Image::rotate()
{
    t.start();

#ifdef RADXA_ZERO_3E
        // const Image::row_t row = data[img_height - i - 1];
        for (size_t j = 0; j < img_width; ++j) {
            for (size_t i = 0; i < img_height; ++i) {
                rotated_cw[j][i] = data[img_height - i - 1][j];
                // rotated_cw[j][i] = row[j];
            }
    }
#else
    for (size_t j = 0; j < img_width; ++j) {
        for (size_t i = 0; i < img_height; ++i) {
            rotated_cw[j][i] = data[img_height - i - 1][j];
        }
    }
#endif // RADXA_ZERO_3E

    rot_elapsed_ns += t.nsecsElapsed();
}

// std::shared_ptr<Pixels> Image::sharedPixels()
// {
//     t.start();

//     static auto result = std::make_shared<Pixels>();
//     result->counters = counters;

//     std::transform(rotated_cw.cbegin(), rotated_cw.cend(), result->pixels.begin(), process_column);
//     pix_elapsed_ns += t.nsecsElapsed();

//     return result;
// }

Pixels &Image::getPixels()
{
    if (pixels)
        return *pixels;

    t.start();

    // overhead of this?
    pixels = Pixels{};
    pixels->counters = counters;

    std::transform(rotated_cw.cbegin(), rotated_cw.cend(), pixels->pixels.begin(), process_column);
    pix_elapsed_ns += t.nsecsElapsed();

    return *pixels;
}

void Image::copyFromData(const void *src, size_t size)
{
    if (Q_UNLIKELY(size % sizeof(data) != 0 || size < sizeof(data))) {
        throw std::logic_error(__func__ + std::string(": wrong data size"));
    }

    switch (pixelFormat) {
    case libcamera::formats::R8: {
        // std::cout << "R8" << std::endl;
        // memcpy(data, src, size);
        // memcpy(data->data(), src, size);
        memcpy(&data, src, size);
        // data = (uint8_t *) src;
        break;
    }
#define RADXA_HAS_NO_R16
#ifndef RADXA_HAS_NO_R16
    case libcamera::formats::R16: {
        // std::cout << "R16" << std::endl;
        // data_t &d = *data;
#pragma omp parallel
#pragma omp for
        for (size_t i = 0; i < img_size; i++) {
            data[i / img_width][i % img_width] = (((uint16_t *) src)[i] & 0xff00) >> 8;
            // d[i / img_width][i % img_width] = (((uint16_t *) src)[i] & 0xff00) >> 8;
            // dataAt(i / img_width, i % img_width) = (((uint16_t *) src)[i] & 0xff00) >> 8;
        }
        break;
    }
#endif // RADXA_HAS_NO_R16
    default:
        throw std::logic_error(__func__ + std::string(": unsupported pixel format"));
        break;
    }
}

void Image::reset()
{
    counters = Counters{};
    pixels = std::nullopt;
}

void Image::copy(data_t &dst, const radxa_data_t &src)
{
    for (size_t i = 0; i < img_height; ++i) {
        memcpy(dst[i].data(), src[i].data(), img_width);
    }
}