#include "imagealgos.h"

#include <cassert>
#include <cstdint>
#include <cstring>

#include <algorithm>
#include <chrono>
#include <iostream>
#include <limits>
#include <mutex>
#include <typeinfo>
#include <utility>

// #include <arm_neon.h>

#include "genetic_algos.h"
#include "macro.h"

uint8_t pgm_image[64 + img_width * img_height * sizeof(uint16_t)];
size_t pgm_image_size = 0;
std::mutex pgm_image_mtx;

size_t pgm_save(Image *img, FILE *outfile, bool really_save) {
    std::lock_guard<std::mutex> lg(pgm_image_mtx);

    size_t n = 0;

    // n += fprintf(outfile, "P5\n%d %d\n%d\n",
    // img->width, img->height, 0xFF);
    n += sprintf((char*)pgm_image, "P5\n%d %d\n%d\n",
                 img->width, img->height, 0xFF);

    // for (size_t i = 0; i < img->width * img->height; ++i)
    for (size_t i = 0; i < img_width * img_height; ++i)
    {
        uint16_t *pixels = (uint16_t*)img->data;
        // const auto p = pixels[i];
        uint8_t value = (pixels[i] & 0xFF00) >> 8;

        // n += fwrite(&value, 1, 1, outfile);
        memcpy((void*)(pgm_image + n), &value, sizeof(value));
        n += sizeof(value);
    }

    pgm_image_size = n;

    // std::cout << "size is " << n << std::endl;

    if (really_save)
    {
        fwrite(pgm_image, 1, pgm_image_size, outfile);
        fflush(outfile);
    }

    return n;
}

void unpack_10bit(uint8_t const *src, Image const &image, uint16_t *dest)
{
    unsigned int w_align = image.width & ~3;
    for (unsigned int y = 0; y < image.height; y++, src += image.stride)
    {
        uint8_t const *ptr = src;
        unsigned int x;
        for (x = 0; x < w_align; x += 4, ptr += 5)
        {
            *dest++ = (ptr[0] << 2) | ((ptr[4] >> 0) & 3);
            *dest++ = (ptr[1] << 2) | ((ptr[4] >> 2) & 3);
            *dest++ = (ptr[2] << 2) | ((ptr[4] >> 4) & 3);
            *dest++ = (ptr[3] << 2) | ((ptr[4] >> 6) & 3);
        }
        for (; x < image.width; x++)
            *dest++ = (ptr[x & 3] << 2) | ((ptr[4] >> ((x & 3) << 1)) & 3);
    }
}

void unpack_16bit(uint8_t const *src, Image const &image, uint16_t *dest)
{
    start_timer(unpack_16bit);
    /* Assume the pixels in memory are already in native byte order */
    unsigned int w = image.width;

    for (unsigned int y = 0; y < image.height; y++)
    {
        memcpy(dest, src, 2 * w);
        dest += w;
        src += image.stride;
    }
    stop_timer(unpack_16bit);
}

void rotate(Image &image)
{
    start_timer(rotate);

    using namespace std;

    for (size_t i = 0; i < img_height; ++i)
    {
        for (size_t j = 0; j < img_width; ++j)
        {
            image.rotated_cw[j][i] = image.data[img_height - i][j];
        }
    }

    stop_timer(rotate);
}

template<class T, size_t N>
constexpr size_t mysize(T (&)[N]) { return N; }

void smooth_column(uint16_t (&column)[]) {
    for (size_t i = 1; i < img_height - 1; ++i) {
        column[i] = median3(column[i - 1], column[i], column[i + 1]);
    }
}

float process_column(uint16_t (&column)[])
{
    start_timer(process_column);

    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) {
        // if (column[i + 0] < 100) { column[i + 0] = 0; } integralSum[i + 0] = column[i + 0] / 256 + integralSum[i + 0 - 1];
        // if (column[i + 1] < 100) { column[i + 1] = 0; } integralSum[i + 1] = column[i + 1] / 256 + integralSum[i + 1 - 1];
        // if (column[i + 2] < 100) { column[i + 2] = 0; } integralSum[i + 2] = column[i + 2] / 256 + integralSum[i + 2 - 1];
        // if (column[i + 3] < 100) { column[i + 3] = 0; } integralSum[i + 3] = column[i + 3] / 256 + integralSum[i + 3 - 1];
        // if (column[i + 4] < 100) { column[i + 4] = 0; } integralSum[i + 4] = column[i + 4] / 256 + integralSum[i + 4 - 1];
        // if (column[i + 5] < 100) { column[i + 5] = 0; } integralSum[i + 5] = column[i + 5] / 256 + integralSum[i + 5 - 1];
        // if (column[i + 6] < 100) { column[i + 6] = 0; } integralSum[i + 6] = column[i + 6] / 256 + integralSum[i + 6 - 1];
        // if (column[i + 7] < 100) { column[i + 7] = 0; } integralSum[i + 7] = column[i + 7] / 256 + integralSum[i + 7 - 1];
        // if (column[i + 8] < 100) { column[i + 8] = 0; } integralSum[i + 8] = column[i + 8] / 256 + integralSum[i + 8 - 1];
        // if (column[i + 9] < 100) { column[i + 9] = 0; } integralSum[i + 9] = column[i + 9] / 256 + integralSum[i + 9 - 1];
        // if (column[i + 10] < 100) { column[i + 10] = 0; } integralSum[i + 10] = column[i + 10] / 256 + integralSum[i + 10 - 1];
        // if (column[i + 11] < 100) { column[i + 11] = 0; } integralSum[i + 11] = column[i + 11] / 256 + integralSum[i + 11 - 1];
        // if (column[i + 12] < 100) { column[i + 12] = 0; } integralSum[i + 12] = column[i + 12] / 256 + integralSum[i + 12 - 1];
        // if (column[i + 13] < 100) { column[i + 13] = 0; } integralSum[i + 13] = column[i + 13] / 256 + integralSum[i + 13 - 1];
        // if (column[i + 14] < 100) { column[i + 14] = 0; } integralSum[i + 14] = column[i + 14] / 256 + integralSum[i + 14 - 1];
        // if (column[i + 15] < 100) { column[i + 15] = 0; } integralSum[i + 15] = column[i + 15] / 256 + integralSum[i + 15 - 1];

        if (column[i] < 100) { column[i] = 0; }
        integralSum[i] = column[i] / 256 + integralSum[i - 1];
    }

    // maxSum = 0 ;
    // size_t maxIdx { 0 };

    // for (size_t i = 0; i < img_height - patternSize; ++i) {
    //     const auto sum = integralSum[i + patternSize] - integralSum[i];
    //     if (sum > maxSum) {
    //         maxSum = sum;
    //         maxIdx = i;
    //     }
    // }

    // Algo genetic(column + maxIdx);
    // // std::cout << "maxIdx " << maxIdx << std::endl;

    // // return maxIdx + genetic.run().a;
    // return 500;
    // return img_height - maxIdx - genetic.run().a;


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

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

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


    static bool result_done = false;
    if (!result_done) {
        std::cout << "result " << result << std::endl;
        result_done = true;
    }
    // std::cout << "result is '" << result << "'\n";

    // stop_timer(process_column);

    return result;

// center of mass
#if 0
    auto max_el = std::max_element(std::begin(accumulated_sum),
                                   std::end(accumulated_sum) - window_size);

    size_t max_sum_idx = max_el - std::begin(accumulated_sum) + window_size;

    double sum_w = 0;
    double prod_wx = 0;
    double wmc = 0;

    for(int i = max_sum_idx - window_size; i < max_sum_idx; ++i)
    {
        prod_wx += column[i] * i;
        sum_w += column[i];
    }

    wmc = float(prod_wx) / float(sum_w);

    result = img_height - wmc;

    return result;
#endif
}

Pixels process_columns(Image &image)
{
    Pixels result;
    result.counters = image.counters;

    // std::cout << "here\n";
    start_timer(process_columns);

    for (size_t i = 0; i < image.width; i++)
    {
        // smooth_column(image.rotated_cw[i]);
        result.pixels[i] = process_column(image.rotated_cw[i]);
        // Algo genetic(image.rotated_cw[i]);

        // image.pixels[i] = genetic.run().a;

        // if (i == 0) {
            // std::cout << "pixel: " << image.pixels[i] << std::endl;
          // }
    }

    stop_timer(process_columns);

    return result;
}