#include "surface_reconstruction.h"

#include <QSet>
#include <QString>
#include <QtGlobal>
#include <QtMath>
#include <algorithm>
#include <limits>

namespace algorithms
{
namespace reconstruction
{
namespace
{
const double kEps = 1e-6;
float gNeighborRadiusScale = 3.5f;

struct Point2d
{
    double x;
    double y;
};

struct UniquePoint
{
    Point3f p;
    int originalIndex;
};

QString xyKey(const Point3f &p)
{
    const qint64 qx = qRound64(static_cast<double>(p.x) / kEps);
    const qint64 qy = qRound64(static_cast<double>(p.y) / kEps);
    return QString::number(qx) + "_" + QString::number(qy);
}

QVector<UniquePoint> uniquePointsByXY(const QVector<Point3f> &input)
{
    QVector<UniquePoint> out;
    out.reserve(input.size());
    QSet<QString> seen;

    for (int i = 0; i < input.size(); ++i) {
        const QString key = xyKey(input[i]);
        if (!seen.contains(key)) {
            seen.insert(key);
            out.push_back({input[i], i});
        }
    }
    return out;
}

double orient2d(const Point2d &a, const Point2d &b, const Point2d &c)
{
    return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x);
}

struct Neighbor
{
    int idx;
    double d2;
    double angle;
};

} // namespace

void setNeighborRadiusScale(const float scale)
{
    gNeighborRadiusScale = qBound(0.5f, scale, 20.0f);
}

float neighborRadiusScale()
{
    return gNeighborRadiusScale;
}

QVector<Triangle> buildSurfaceTriangles(const QVector<Point3f> &points)
{
    QVector<Triangle> triangles;
    QVector<UniquePoint> pts = uniquePointsByXY(points);
    if (pts.size() < 4) {
        return triangles;
    }

    QVector<Point2d> pts2d;
    pts2d.reserve(pts.size());
    for (const UniquePoint &up : pts) {
        pts2d.push_back({up.p.x, up.p.y});
    }

    auto dist2xyz = [&](const int a, const int b) -> double {
        const double dx = static_cast<double>(pts[a].p.x) - static_cast<double>(pts[b].p.x);
        const double dy = static_cast<double>(pts[a].p.y) - static_cast<double>(pts[b].p.y);
        const double dz = static_cast<double>(pts[a].p.z) - static_cast<double>(pts[b].p.z);
        return dx * dx + dy * dy + dz * dz;
    };

    QVector<double> nearestDist;
    nearestDist.reserve(pts.size());
    for (int i = 0; i < pts.size(); ++i) {
        double best = std::numeric_limits<double>::max();
        for (int j = 0; j < pts.size(); ++j) {
            if (i == j) {
                continue;
            }
            const double d2 = dist2xyz(i, j);
            if (d2 < best) {
                best = d2;
            }
        }
        if (best < std::numeric_limits<double>::max()) {
            nearestDist.push_back(qSqrt(best));
        }
    }
    if (nearestDist.isEmpty()) {
        return triangles;
    }
    std::sort(nearestDist.begin(), nearestDist.end());
    const double medianNearest = nearestDist[nearestDist.size() / 2];
    const double maxNeighborDistance = qMax(1e-6, medianNearest * static_cast<double>(gNeighborRadiusScale));
    const double maxNeighborDistance2 = maxNeighborDistance * maxNeighborDistance;
    const int kNeighbors = 10;

    QSet<QString> uniqueTriangles;
    triangles.reserve(pts.size() * 2);

    auto triangleKey = [](int a, int b, int c) -> QString {
        int v[3] = {a, b, c};
        std::sort(v, v + 3);
        return QString::number(v[0]) + "_" + QString::number(v[1]) + "_" + QString::number(v[2]);
    };

    for (int i = 0; i < pts.size(); ++i) {
        QVector<Neighbor> neighbors;
        neighbors.reserve(pts.size() - 1);
        for (int j = 0; j < pts.size(); ++j) {
            if (i == j) {
                continue;
            }
            const double d2 = dist2xyz(i, j);
            if (d2 <= maxNeighborDistance2) {
                const double angle = qAtan2(pts2d[j].y - pts2d[i].y, pts2d[j].x - pts2d[i].x);
                neighbors.push_back({j, d2, angle});
            }
        }
        if (neighbors.size() < 3) {
            continue;
        }
        std::sort(neighbors.begin(), neighbors.end(), [](const Neighbor &a, const Neighbor &b) {
            return a.d2 < b.d2;
        });
        if (neighbors.size() > kNeighbors) {
            neighbors.resize(kNeighbors);
        }
        std::sort(neighbors.begin(), neighbors.end(), [](const Neighbor &a, const Neighbor &b) {
            return a.angle < b.angle;
        });

        for (int n = 0; n < neighbors.size(); ++n) {
            const int j = neighbors[n].idx;
            const int k = neighbors[(n + 1) % neighbors.size()].idx;
            if (j == k) {
                continue;
            }
            if (dist2xyz(j, k) > maxNeighborDistance2) {
                continue;
            }

            const double area2 = orient2d(pts2d[i], pts2d[j], pts2d[k]);
            if (qAbs(area2) < kEps) {
                continue;
            }

            const QString key = triangleKey(i, j, k);
            if (uniqueTriangles.contains(key)) {
                continue;
            }
            uniqueTriangles.insert(key);

            if (area2 > 0.0) {
                triangles.push_back({pts[i].originalIndex, pts[j].originalIndex, pts[k].originalIndex});
            } else {
                triangles.push_back({pts[i].originalIndex, pts[k].originalIndex, pts[j].originalIndex});
            }
        }
    }

    return triangles;
}
} // namespace reconstruction
} // namespace algorithms