#include "image.h"

// #include <format>

#include <QElapsedTimer>

#include <libcamera/formats.h>

#include "image.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 Image::column_t &column);
float process_column_center_of_mass(const Image::column_t &column);

// float process_column(const uint8_t (&column)[])
// float process_column(const Image::row_t &column)
float process_column(const Image::column_t &column)
{
    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;
    // 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];
    }

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

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

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

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

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

    return result;
}

float process_column_center_of_mass(const Image::column_t &column)
{
    static_assert((img_height % patternSize) == 0, "img_height % patternSize should be 0");

    // const auto &c = column;
    Image::column_t c = column;
    constexpr size_t win_size = 2;
    constexpr size_t wins_count{img_height / win_size};
    using pixel_sum_t = uint16_t;
    // using img_pixel_t = decltype(Image::row_t)::value_type;
    using img_pixel_t = uint8_t;
    static_assert(std::numeric_limits<pixel_sum_t>::max()
                      > std::numeric_limits<img_pixel_t>::max() * win_size,
                  "choose bigger type for pixels sum");
    // std::array<pixel_sum_t, wins_count> sums;
    pixel_sum_t maxSum{0};
    size_t maxWinI{0};
    constexpr img_pixel_t blackLevel{5};

    // std::for_each(c.begin(), c.end(), [](auto &v) {
    //     if (v > 20) {
    //         v = 0;
    //     }
    // });

    for (size_t w{0}; w < wins_count; ++w) {
        // option 0: manually sum
        const auto i = w * win_size;
        // sums[w] = c[i + 0] * 1 + c[i + 1] * 2 + c[i + 2] * 3 + c[i + 3] * 4 + c[i + 4] * 5
        //           + c[i + 5] * 6 + c[i + 6] * 7 + c[i + 7] * 8;
        // const auto sum{c[i + 0] + c[i + 1] + c[i + 2] + c[i + 3] + c[i + 4] + c[i + 5] + c[i + 6]
        //                + c[i + 7]};
        const auto sum{c[i + 0] + c[i + 1] + c[i + 2] + c[i + 3]};

        if (sum > maxSum) {
            maxSum = sum;
            maxWinI = w;
        }
    }

    const auto start = (maxWinI - 1) * win_size;
    const auto com_size = win_size;
    float com_sum{0};
    float com_sum_mass{0};
    constexpr uint8_t black_level{27};
    // std::array<uint8_t, win_size * 3>

    for (size_t i{start}; i < start + (win_size * 3); ++i) {
        const auto value = c[i] > black_level ? c[i] : 0;
        com_sum += value;
        com_sum_mass += value * (i);
    }

    // return com_sum / (win_size * 3);
    return com_sum_mass / com_sum;
}

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

    // TODO: get rid of this alloc
    auto result = std::make_shared<Pixels>();
    result->counters = counters;

    std::transform(rotated_cw.cbegin(), rotated_cw.cend(), result->pixels.begin(), process_column);
    // std::transform(rotated_cw.cbegin(),
    //                rotated_cw.cend(),
    //                result->pixels.begin(),
    // process_column_center_of_mass);

    static bool found{false};

    for (const auto &p : result->pixels) {
        if (p > img_height) {
            // std::cout << "AAAAAAAAAAAAAAAAAAAAA too big pixel values: " << p << std::endl;
            found = true;
        }
    }

    pix_elapsed_ns += t.nsecsElapsed();

    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;
    }
#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::copy(data_t &dst, const radxa_data_t &src)
{
    for (size_t i = 0; i < img_height; ++i) {
        const auto dst_data = static_cast<void *>(dst[i].data());
        const auto src_data = static_cast<const void *>(src[i].data());
        memcpy(dst_data, src_data, img_width);
    }
}