#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;
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)
{
    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);
    // constexpr uint32_t correlationSize = img_height - patternSize;
    uint32_t correlation[img_height];
    uint32_t integralSum[img_height];
    uint32_t maxTripleSum = signalThreshold * 50;
    uint32_t x1 = 0;
    int32_t y1 = 0;
    int32_t y2 = 0;

    memset(correlation, 0, img_height * sizeof(correlation[0]));
    // memset(correlation, 0, patternSize / 2);
    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();

    // pixel * <sum of neighbours>
    for (uint32_t i = 0; i < correlationSize; ++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();

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

        if (sum > maxTripleSum) {
            // [N N n p] - [P N]
            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)
            {
                // [N N p] - [p N N]
                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;
                    maxTripleSum = 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;
}

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

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

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

    rot_elapsed_ns += t.nsecsElapsed();
}

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

    start_timer(process_columns);
    // std::transform(std::execution::par_unseq,
    //                rotated_cw.cbegin(),
    //                rotated_cw.cend(),
    //                result->pixels.begin(),
    //                [](const auto &column) -> float { return process_column(column); });

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

    return result;
}

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;
    }
    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;
    }
    default:
        throw std::logic_error(__func__ + std::string(": unsupported pixel format"));
        break;
    }
}