#include "image.h"

// #include <format>

#include <QElapsedTimer>

#include <libcamera/formats.h>

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

uint64_t sum_elapsed_ns = 0;
uint64_t corr_elapsed_ns = 0;
uint64_t max_elapsed_ns = 0;
uint64_t value_elapsed_ns = 0;

float process_column(const uint8_t (&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)
    static constexpr uint32_t patternOffset = patternSize -
                                              ((patternSize % 2 == 1) ? 1 : 0);
    const uint32_t correlationSize = img_height - patternSize +
                                     ((patternSize % 2 == 1) ? 1 : 0);
    uint32_t correlation[img_height];
    uint32_t integralSum[img_height];
    uint32_t maxSum = signalThreshold * 50;
    uint32_t x1 = 0;
    int32_t y1 = 0;
    int32_t y2 = 0;

    memset(correlation, 0, img_height * sizeof(correlation[0]));
    integralSum[0] = 0;

    for (uint32_t i = 1; i < img_height; ++i) {
        integralSum[i] = column[i] + integralSum[i - 1];
    }
    sum_elapsed_ns += t.nsecsElapsed();
    t.restart();

    for (uint32_t i = 0; i < correlationSize; ++i)
        correlation[i + patternSize / 2] = column[i + patternSize / 2]
                                           * (integralSum[i + patternOffset] - integralSum[i]);

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

    for (uint32_t i = 3; i < img_height - 2; ++i) {
        const auto sum = correlation[i - 1] + correlation[i] +
                         correlation[i + 1];

        if (sum > maxSum)
        {
            const int32_t rioux0 = int32_t(correlation[i - 2 - 1] +
                                           correlation[i - 1 - 1]) -
                                   int32_t(correlation[i + 1 - 1] +
                                           correlation[i + 2 - 1]);

            if (rioux0 < 0)
            {
                const int32_t rioux1 = int32_t(correlation[i - 2] +
                                               correlation[i - 1]) -
                                       int32_t(correlation[i + 1] +
                                               correlation[i + 2]);

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

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

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

    return result;
}

void Image::rotate()
{
    start_timer(rotate);

    using namespace std;

#pragma omp parallel
#pragma omp for
    for (size_t i = 0; i < img_height; ++i)
    {
        for (size_t j = 0; j < img_width; ++j)
        {
            rotated_cw[j][i] = data[img_height - i][j];
        }
    }

    stop_timer(rotate);
}

std::shared_ptr<Pixels> Image::pixels() const
{
    auto result = std::make_shared<Pixels>();
    result->counters = counters;

    start_timer(process_columns);

#pragma omp chunk
#pragma omp parallel for
    for (size_t i = 0; i < img_width; i++) {
        result->pixels[i] = process_column(rotated_cw[i]);
    }

    // for (size_t i = 640 - 5; i < 640 + 5; ++i) {
    //     std::cout << result->pixels[i] << ' ';
    // }
    // std::cout << std::endl;

    stop_timer(process_columns);

    return result;
}

void Image::copyFromData(const void *src, size_t size)
{
    if (size > sizeof(data))
    {
        // throw std::logic_error(std::format)
    }

    if (pixelFormat == libcamera::formats::R8)
    {
        memcpy(data, src, size);
    }
}