#include "imagealgos.h"

#include <QDebug>

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

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

#include "macro.h"

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

template<typename T>
T median3(const T &a, const T &b, const T &c)
{
    using namespace std;
    return max(min(a, b), min(max(a, b), c));
}

size_t pgm_save(std::shared_ptr<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)
    {
        uint8_t *pixels = (uint8_t *) img->data;
        // const auto p = pixels[i];
        // uint8_t value = (pixels[i] & 0xFF00) >> 8;
        uint8_t value = pixels[i];

        // 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)
    {
        if (outfile) {
            fwrite(pgm_image, 1, pgm_image_size, outfile);
            fflush(outfile);

        } else {
            std::cerr << __func__ << ": output filename is nullptr, cannot save"
                      << std::endl;
        }
    }

    // std::cout << "written pgm image" << std::endl;
    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);
// }

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

// void smooth_column(uint8_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(uint8_t (&column)[])
// {
//     std::cout << "aaaaaaaaaaa\n";
//     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] + 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) ? (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;
// }

QList<QLineF> pixelsToLines(const Pixels &rawProfile)
{
    const auto& pixels = rawProfile.pixels;
    QList<QPointF> points(pixels.size());

    for (int i = 0; i < pixels.size(); ++i)
    {
        points[i] = { qreal(i) - img_width / 2, pixels.at(i) };
    }

    // qDebug() << "mid points" << points.mid(points.count() - 3, 6);

    return pointsToLines(std::move(points));
}

QList<QLineF> pointsToLines(const QList<QPointF> &points)
{
    constexpr double maxDistanceFromLine { 3 };
    constexpr double minLineLength { 10 };

    QList<int> lineAnchors{0, int(points.count() - 1)};

    auto vecLength = [](const QPointF& vector) {
        return std::hypot(vector.x(), vector.y());
    };

    auto distanceToLine = [vecLength](const QPointF& point, const QLineF& line) {
        // const auto d1 = point - line.p1();
        // const auto d2 = line.p2() - line.p1();

        // const auto norm = vecLength(d2);
        // qDebug() << "norm" << norm;

        // if (norm <= std::numeric_limits<double>::epsilon())
        // {
        //     qDebug() << "AAAAAAAAAAAAAAAAAA";
        //     return vecLength(d1);
        // }

        // const auto result = std::fabs(d1.x() * d2.y() - d1.y() * d2.x()) / norm;

        // if (!qFuzzyIsNull(result))
        // {
        //     qDebug() << "NOT NULL" << result << point << line;
        // }

        // return result;
        // transform to loocal coordinates system (0,0) - (lx, ly)
        QPointF p1 = line.p1();
        QPointF p2 = line.p2();
        const auto& p = point;
        qreal x = p.x() - p1.x();
        qreal y = p.y() - p1.y();
        qreal x2 = p2.x() - p1.x();
        qreal y2 = p2.y() - p1.y();

        // if line is a point (nodes are the same) =>
        // just return distance between point and one line node
        qreal norm = sqrt(x2*x2 + y2*y2);
        if (norm <= std::numeric_limits<int>::epsilon())
            return sqrt(x*x + y*y);

        // distance
        // qDebug() << "dist" << fabs(x*y2 - y*x2) / norm << point << line;
        return fabs(x * y2 - y * x2) / norm;
    };

    // for (const auto& p : points)
    // {
    //     qDebug() << "\t" << p;
    // }

    for (int i = 0; i < lineAnchors.count() - 1; ++i)
    {
        const auto &lineFirst = i;
        const auto &lineLast = i + 1;
        const auto& leftPointIdx = lineAnchors.at(lineFirst);
        const auto& rightPointIdx = lineAnchors.at(lineLast);

        if (rightPointIdx - leftPointIdx < 2)
        {
            continue;
        }

        const QLineF line { points.at(leftPointIdx), points.at(rightPointIdx) };

        // std::cout << "max between " << leftPointIdx + 1 << " and "
        // << rightPointIdx - 1 << std::endl;

        const auto farthest = std::max_element(
            // std::execution::par_unseq,
            points.cbegin() + leftPointIdx + 1,
            points.cbegin() + rightPointIdx - 1,
            [line, distanceToLine](const QPointF& a, const QPointF& b) {
                return distanceToLine(a, line) < distanceToLine(b, line);
            }
        );

        // std::cout << "done max" << std::endl;
        // auto tmp = *farthest;
        // std::cout << "done farthest" << std::endl;

        // qDebug() << "farthest point" << distanceToLine(*farthest, line);
        // qDebug() << "farthest dist" << distanceToLine(*farthest, line);
        // qDebug() << "that's from line" << line;

        if (distanceToLine(*farthest, line) > maxDistanceFromLine)
        {
            // std::cout << "try insert at " << i + 1 << ". lineAnchors.size is
            // "
            //           << lineAnchors.size()
            //           << ". inserting this: " << farthest - points.cbegin()
            //           << std::endl;
            lineAnchors.insert(i + 1, farthest - points.cbegin());
            // std::cout << "done insert" << std::endl;
            --i;
            // qDebug() << "I'm here" << i;
            continue;
        }
    }

    struct LineAB
    {
        double a { std::numeric_limits<double>::quiet_NaN() };
        double b { std::numeric_limits<double>::quiet_NaN() };

        LineAB(const QList<QPointF>& points) {
            if (points.isEmpty())
            {
                return;
            }

            double sumX { 0 };
            double sumY { 0 };
            double sumXY { 0 };
            double sumXX { 0 };

            // FIXME: validate
            // for (const auto& point : points)
            const int delta = points.count() * 0.15;
            for (int i = delta; i < points.count() - delta; ++i)
            {
                const auto& point = points.at(i);
                sumX += point.x();
                sumY += point.y();
                sumXY += point.x() * point.y();
                sumXX += point.x() * point.x();
            }

            // sumX /= points.count();
            // sumY /= points.count();
            // sumXY /= points.count();
            // sumXX /= points.count();

            const int n = points.count() - delta * 2;
            Q_ASSERT_X(n > 0, Q_FUNC_INFO, "n <= 0");

            a = (n * sumXY - sumX * sumY) /
                (n * sumXX - sumX * sumX);
            b = (sumY - a * sumX) / n;
        }
    };

    // auto pointsToLineAB =

    // qDebug() << "line anchors count is" << lineAnchors.count();

    constexpr bool useLsd = true;

    QList<QLineF> result { lineAnchors.length() - 1 };

    for (int i = 0; i < lineAnchors.count() - 1; ++i)
    {
        const auto& leftPointIdx = lineAnchors.at(i);
        const auto& rightPointIdx = lineAnchors.at(i + 1);

        QPointF leftPoint = points.at(leftPointIdx);
        QPointF rightPoint = points.at(rightPointIdx);

        LineAB lineAB(points.mid(leftPointIdx, rightPointIdx - leftPointIdx));

        leftPoint.setY(lineAB.a * leftPoint.x() + lineAB.b);
        rightPoint.setY(lineAB.a * rightPoint.x() + lineAB.b);

        if (useLsd)
            result[i] = QLineF{ std::move(leftPoint), std::move(rightPoint) };
        else
            result[i] = QLineF{ points.at(lineAnchors.at(i)), points.at(lineAnchors.at(i + 1)) };
    }

    if (useLsd)
    {
        for (int i = 0; i < result.count() - 1; ++i)
        {
            auto& lineA = result[i];
            auto& lineB = result[i + 1];

            QPointF intersectionPoint {};

            if (lineA.intersects(lineB, &intersectionPoint) != QLineF::NoIntersection)
            {
                lineA.setP2(intersectionPoint);
                lineB.setP1(intersectionPoint);
            }
        }
    }

    // qDebug() << result;

    return result;
}