#include "CScanGridder.h"

#include <QHash>
#include <QPair>
#include <QtGlobal>
#include <algorithm>
#include <cmath>
#include <limits>

namespace {

struct CScanSamplePoint {
    double x = 0.0;
    double y = 0.0;
    float amplitude = 0.0f;
    int line = -1;
    int channel = -1;
    int trace = -1;
};

struct BinKey {
    int x = 0;
    int y = 0;

    bool operator==(const BinKey &other) const
    {
        return x == other.x && y == other.y;
    }
};

uint qHash(const BinKey &key, uint seed = 0)
{
    return ::qHash(key.x, seed) ^ (::qHash(key.y, seed + 0x9e3779b9U) << 1);
}

static double distanceSquared(double ax, double ay, double bx, double by)
{
    const double dx = ax - bx;
    const double dy = ay - by;
    return dx * dx + dy * dy;
}

static int gridIndex(int x, int y, int width)
{
    return y * width + x;
}

static QVector<float> gaussianKernel(double sigma)
{
    sigma = std::max(0.1, sigma);
    const int radius = std::max(1, static_cast<int>(std::ceil(sigma * 3.0)));
    QVector<float> kernel(radius * 2 + 1);
    double sum = 0.0;
    for (int i = -radius; i <= radius; ++i) {
        const double v = std::exp(-(i * i) / (2.0 * sigma * sigma));
        kernel[i + radius] = static_cast<float>(v);
        sum += v;
    }
    if (sum > 0.0) {
        for (float &v : kernel) v = static_cast<float>(v / sum);
    }
    return kernel;
}

static void smoothMasked(CScanGridResult &grid, double sigma)
{
    if (!grid.valid || grid.width <= 0 || grid.height <= 0) return;

    const QVector<float> kernel = gaussianKernel(sigma);
    const int radius = kernel.size() / 2;
    const int count = grid.width * grid.height;

    QVector<float> tmpValues(count, 0.0f);
    QVector<float> tmpWeights(count, 0.0f);

    for (int y = 0; y < grid.height; ++y) {
        for (int x = 0; x < grid.width; ++x) {
            double sum = 0.0;
            double weight = 0.0;
            for (int k = -radius; k <= radius; ++k) {
                const int sx = x + k;
                if (sx < 0 || sx >= grid.width) continue;
                const int idx = gridIndex(sx, y, grid.width);
                if (!grid.validMask[idx]) continue;
                const double w = kernel[k + radius];
                sum += grid.values[idx] * w;
                weight += w;
            }
            const int outIdx = gridIndex(x, y, grid.width);
            if (weight > 1e-6) {
                tmpValues[outIdx] = static_cast<float>(sum / weight);
                tmpWeights[outIdx] = static_cast<float>(weight);
            }
        }
    }

    QVector<float> outValues(count, 0.0f);
    QVector<unsigned char> outMask(count, 0);

    for (int y = 0; y < grid.height; ++y) {
        for (int x = 0; x < grid.width; ++x) {
            double sum = 0.0;
            double weight = 0.0;
            for (int k = -radius; k <= radius; ++k) {
                const int sy = y + k;
                if (sy < 0 || sy >= grid.height) continue;
                const int idx = gridIndex(x, sy, grid.width);
                if (tmpWeights[idx] <= 1e-6f) continue;
                const double w = kernel[k + radius] * tmpWeights[idx];
                sum += tmpValues[idx] * w;
                weight += w;
            }
            const int outIdx = gridIndex(x, y, grid.width);
            if (weight > 1e-4) {
                outValues[outIdx] = static_cast<float>(sum / weight);
                outMask[outIdx] = 1;
            }
        }
    }

    grid.values = std::move(outValues);
    grid.validMask = std::move(outMask);
}

static float percentile(QVector<float> values, double percent)
{
    if (values.isEmpty()) return 0.0f;
    std::sort(values.begin(), values.end());
    percent = std::clamp(percent, 0.0, 100.0);
    const double pos = percent / 100.0 * (values.size() - 1);
    const int lo = static_cast<int>(std::floor(pos));
    const int hi = static_cast<int>(std::ceil(pos));
    if (lo == hi) return values[lo];
    const double t = pos - lo;
    return static_cast<float>(values[lo] * (1.0 - t) + values[hi] * t);
}

static void updateDisplayRange(CScanGridResult &grid, double lowPercent, double highPercent)
{
    QVector<float> validValues;
    validValues.reserve(grid.values.size());
    for (int i = 0; i < grid.values.size(); ++i) {
        if (i < grid.validMask.size() && grid.validMask[i]) {
            validValues.append(grid.values[i]);
        }
    }

    if (validValues.isEmpty()) {
        grid.displayMin = 0.0f;
        grid.displayMax = 1.0f;
        return;
    }

    grid.displayMin = percentile(validValues, lowPercent);
    grid.displayMax = percentile(validValues, highPercent);
    if (std::abs(grid.displayMax - grid.displayMin) < 1e-6f) {
        grid.displayMax = grid.displayMin + 1.0f;
    }
}

} // namespace

CScanGridResult CScanGridder::build(const QVector<SurveyLine> &surveyLines, CScanGridOptions options)
{
    CScanGridResult result;

    options.cellSizeM = std::max(0.005, options.cellSizeM);
    options.searchRadiusM = std::max(options.cellSizeM, options.searchRadiusM);
    options.idwPower = std::max(0.1, options.idwPower);
    options.smoothingSigmaCells = std::clamp(options.smoothingSigmaCells, 0.1, 5.0);
    if (options.clipLowPercent >= options.clipHighPercent) {
        options.clipLowPercent = 2.0;
        options.clipHighPercent = 98.0;
    }
    options.maxGridWidth = std::max(64, options.maxGridWidth);
    options.maxGridHeight = std::max(64, options.maxGridHeight);
    options.maxGridCells = std::max(4096, options.maxGridCells);

    QVector<CScanSamplePoint> samples;
    double minX = std::numeric_limits<double>::max();
    double minY = std::numeric_limits<double>::max();
    double maxX = std::numeric_limits<double>::lowest();
    double maxY = std::numeric_limits<double>::lowest();

    for (int li = 0; li < surveyLines.size(); ++li) {
        const SurveyLine &line = surveyLines[li];
        if (!line.hasValidTrajectories()) continue;

        const GPRDataModel &data = line.data;

        const int channelCount = std::min(static_cast<int>(line.channelTrajectories.size()), data.getChannelCount());
        const int traceCount = data.getTraceCountPerChannel();
        const int sampleCount = data.getSampleCount();
        if (channelCount <= 0 || traceCount <= 0 || sampleCount <= 0) continue;

        const int sampleIndex = qBound(0, options.sampleIndex, sampleCount - 1);
        for (int c = 0; c < channelCount; ++c) {
            const QVector<QVector3D> &traj = line.channelTrajectories[c];
            const int n = std::min(traceCount, static_cast<int>(traj.size()));
            for (int t = 0; t < n; ++t) {
                const RadarTrace *trace = data.getTrace(c, t);
                if (!trace || sampleIndex >= trace->amplitudes.size()) continue;

                const QVector3D &pos = traj[t];
                const float amp = static_cast<float>(trace->amplitudes[sampleIndex]);
                // Grid X uses CGCS easting (Y), grid Y uses CGCS northing (X),
                // so the generated C-scan image aligns with map screen axes.
                samples.append({pos.y(), pos.x(), amp, li, c, t});
                minX = std::min(minX, static_cast<double>(pos.y()));
                minY = std::min(minY, static_cast<double>(pos.x()));
                maxX = std::max(maxX, static_cast<double>(pos.y()));
                maxY = std::max(maxY, static_cast<double>(pos.x()));
            }
        }
    }

    if (samples.isEmpty() || maxX <= minX || maxY <= minY) {
        return result;
    }

    result.dataMinX = minX;
    result.dataMinY = minY;
    result.dataMaxX = maxX;
    result.dataMaxY = maxY;

    minX -= options.searchRadiusM;
    minY -= options.searchRadiusM;
    maxX += options.searchRadiusM;
    maxY += options.searchRadiusM;

    double cellSize = options.cellSizeM;
    int width = static_cast<int>(std::ceil((maxX - minX) / cellSize)) + 1;
    int height = static_cast<int>(std::ceil((maxY - minY) / cellSize)) + 1;
    while ((width > options.maxGridWidth || height > options.maxGridHeight
            || static_cast<qint64>(width) * height > options.maxGridCells) && cellSize < 10.0) {
        cellSize *= 1.5;
        width = static_cast<int>(std::ceil((maxX - minX) / cellSize)) + 1;
        height = static_cast<int>(std::ceil((maxY - minY) / cellSize)) + 1;
    }

    if (width <= 0 || height <= 0 || width > options.maxGridWidth || height > options.maxGridHeight
        || static_cast<qint64>(width) * height > options.maxGridCells) {
        return result;
    }

    result.valid = true;
    result.originX = minX;
    result.originY = minY;
    result.cellSizeM = cellSize;
    result.width = width;
    result.height = height;

    const int cellCount = width * height;
    result.values.fill(0.0f, cellCount);
    result.validMask.fill(0, cellCount);
    result.nearestLine.fill(-1, cellCount);
    result.nearestChannel.fill(-1, cellCount);
    result.nearestTrace.fill(-1, cellCount);

    const double binSize = options.searchRadiusM;
    QHash<BinKey, QVector<int>> bins;
    bins.reserve(samples.size());
    for (int i = 0; i < samples.size(); ++i) {
        const CScanSamplePoint &p = samples[i];
        const BinKey key{static_cast<int>(std::floor((p.x - minX) / binSize)),
                         static_cast<int>(std::floor((p.y - minY) / binSize))};
        bins[key].append(i);
    }

    const double radiusSq = options.searchRadiusM * options.searchRadiusM;
    const int searchBins = std::max(1, static_cast<int>(std::ceil(options.searchRadiusM / binSize)));

    for (int y = 0; y < height; ++y) {
        const double cy = minY + y * cellSize;
        const int by = static_cast<int>(std::floor((cy - minY) / binSize));
        for (int x = 0; x < width; ++x) {
            const double cx = minX + x * cellSize;
            const int bx = static_cast<int>(std::floor((cx - minX) / binSize));

            double sum = 0.0;
            double weightSum = 0.0;
            double bestDistSq = std::numeric_limits<double>::max();
            int bestIndex = -1;
            bool exact = false;
            float exactValue = 0.0f;

            for (int oy = -searchBins; oy <= searchBins; ++oy) {
                for (int ox = -searchBins; ox <= searchBins; ++ox) {
                    const BinKey key{bx + ox, by + oy};
                    auto it = bins.constFind(key);
                    if (it == bins.constEnd()) continue;

                    for (int sampleIdx : it.value()) {
                        const CScanSamplePoint &p = samples[sampleIdx];
                        const double d2 = distanceSquared(cx, cy, p.x, p.y);
                        if (d2 > radiusSq) continue;
                        if (d2 < bestDistSq) {
                            bestDistSq = d2;
                            bestIndex = sampleIdx;
                        }
                        if (d2 < 1e-8) {
                            exact = true;
                            exactValue = p.amplitude;
                            bestIndex = sampleIdx;
                            break;
                        }
                        const double w = 1.0 / std::pow(std::sqrt(d2), options.idwPower);
                        sum += p.amplitude * w;
                        weightSum += w;
                    }
                    if (exact) break;
                }
                if (exact) break;
            }

            const int idx = gridIndex(x, y, width);
            if (exact || weightSum > 0.0) {
                result.values[idx] = exact ? exactValue : static_cast<float>(sum / weightSum);
                result.validMask[idx] = 1;
                if (bestIndex >= 0) {
                    const CScanSamplePoint &p = samples[bestIndex];
                    result.nearestLine[idx] = p.line;
                    result.nearestChannel[idx] = p.channel;
                    result.nearestTrace[idx] = p.trace;
                }
            }
        }
    }

    if (options.smoothingEnabled) {
        smoothMasked(result, options.smoothingSigmaCells);
    }
    updateDisplayRange(result, options.clipLowPercent, options.clipHighPercent);

    return result;
}