#include "TrajectoryCalculator.h"
#include <algorithm>
#include <cmath>

namespace {

double planarDistance(const QVector3D &a, const QVector3D &b)
{
    const double dx = static_cast<double>(a.x()) - b.x();
    const double dy = static_cast<double>(a.y()) - b.y();
    return std::sqrt(dx * dx + dy * dy);
}

QVector3D lerpPoint(const QVector3D &a, const QVector3D &b, double t)
{
    return a * static_cast<float>(1.0 - t) + b * static_cast<float>(t);
}

QVector3D catmullRom(const QVector3D &p0, const QVector3D &p1, const QVector3D &p2, const QVector3D &p3, double t)
{
    const float tf = static_cast<float>(t);
    const float t2 = tf * tf;
    const float t3 = t2 * tf;
    return 0.5f * ((2.0f * p1) + (-p0 + p2) * tf + (2.0f * p0 - 5.0f * p1 + 4.0f * p2 - p3) * t2 +
                   (-p0 + 3.0f * p1 - 3.0f * p2 + p3) * t3);
}

} // namespace

double TrajectoryCalculator::headingAt(int traceIdx, const QVector<QVector3D> &gpsPositions)
{
    const int n = gpsPositions.size();
    if (n <= 1) return 0.0;

    if (traceIdx < n - 1) {
        double deltaX = gpsPositions[traceIdx + 1].x() - gpsPositions[traceIdx].x(); // 北向变化
        double deltaY = gpsPositions[traceIdx + 1].y() - gpsPositions[traceIdx].y(); // 东向变化
        return std::atan2(deltaY, deltaX);
    } else {
        // 最后一点复用前一点方向
        double deltaX = gpsPositions[traceIdx].x() - gpsPositions[traceIdx - 1].x();
        double deltaY = gpsPositions[traceIdx].y() - gpsPositions[traceIdx - 1].y();
        return std::atan2(deltaY, deltaX);
    }
}

bool TrajectoryCalculator::computeTrajectories(SurveyLine &line, const SurveyGeometry &geom)
{
    const QVector<QVector3D> &gps = line.gpsPositions;
    const int nRtk = gps.size();
    if (nRtk <= 0) return false;

    const int chCount = geom.channelCount;
    if (chCount <= 0) return false;

    // 通道相对x偏移优先使用头文件CH_X_OFFSETS推导出的逐通道数组；旧工程无数组时按首通道对称补齐。
    QVector<double> chXRel = geom.channelXRel;
    if (chXRel.size() != chCount) {
        chXRel.resize(chCount);
        const double lastXRel = -geom.ch1XRel;
        for (int c = 0; c < chCount; ++c) {
            chXRel[c] = (chCount > 1)
                    ? geom.ch1XRel + (lastXRel - geom.ch1XRel) * c / (chCount - 1.0)
                    : geom.ch1XRel;
        }
    }

    // 预分配 channelTrajectories
    line.channelTrajectories.resize(chCount);
    for (int c = 0; c < chCount; ++c) {
        line.channelTrajectories[c].resize(nRtk);
    }

    // 遍历每个 RTK 点
    for (int i = 0; i < nRtk; ++i) {
        double thetaY = headingAt(i, gps);
        double thetaX = thetaY + M_PI / 2.0;

        // 旋转矩阵 R = [cos(theta_x), cos(theta_y); sin(theta_x), sin(theta_y)]
        // 作用：将设备相对坐标系（x=右, y=前）转换为绝对坐标系（X=北, Y=东）
        double r11 = std::cos(thetaX);
        double r12 = std::cos(thetaY);
        double r21 = std::sin(thetaX);
        double r22 = std::sin(thetaY);

        // RTK 相对天线中心的偏移向量（设备坐标系）
        double rtkLocalX = geom.rtkOffsetX; // 横向偏移
        double rtkLocalY = geom.rtkOffsetY; // 纵向偏移（前方为正）

        // 转换为绝对坐标系偏移
        double rtkAbsOffsetX = r11 * rtkLocalX + r12 * rtkLocalY;
        double rtkAbsOffsetY = r21 * rtkLocalX + r22 * rtkLocalY;

        // 天线中心绝对坐标 = RTK - 偏移量
        double antX = gps[i].x() - rtkAbsOffsetX;
        double antY = gps[i].y() - rtkAbsOffsetY;
        double antZ = gps[i].z();

        // 遍历每个通道
        for (int c = 0; c < chCount; ++c) {
            // 通道相对坐标（设备坐标系）
            double chLocalX = chXRel[c];
            double chLocalY = 0.0; // MATLAB 中 ch_y_rel = zeros(...)

            // 转换为绝对坐标系偏移
            double chAbsOffsetX = r11 * chLocalX + r12 * chLocalY;
            double chAbsOffsetY = r21 * chLocalX + r22 * chLocalY;

            // 通道绝对坐标
            double chX = antX + chAbsOffsetX;
            double chY = antY + chAbsOffsetY;
            double chZ = antZ;

            line.channelTrajectories[c][i] = QVector3D(static_cast<float>(chX),
                                                        static_cast<float>(chY),
                                                        static_cast<float>(chZ));
        }
    }

    // 同步更新 GPRDataModel 中 traces 的 position
    // 数据存储顺序：trace0_ch0, trace0_ch1, ... trace0_chN, trace1_ch0, ...
    // 即 globalIdx = traceInChannel * chCount + channel
    const int tracesPerChannel = line.data.getTracesPerChannel();
    for (int c = 0; c < chCount; ++c) {
        for (int t = 0; t < tracesPerChannel && t < nRtk; ++t) {
            int globalIdx = t * chCount + c;
            if (globalIdx >= 0 && globalIdx < line.data.traces.size()) {
                line.data.traces[globalIdx].position = line.channelTrajectories[c][t];
            }
        }
    }

    return true;
}

QVector<QVector3D> TrajectoryCalculator::resampleTrajectoryLinear(const QVector<QVector3D> &input, int targetCount)
{
    QVector<QVector3D> output;
    if (targetCount <= 0 || input.isEmpty()) return output;
    if (input.size() == 1 || targetCount == 1) {
        output.resize(targetCount);
        std::fill(output.begin(), output.end(), input.first());
        return output;
    }

    output.reserve(targetCount);
    const double scale = static_cast<double>(input.size() - 1) / qMax(1, targetCount - 1);
    for (int i = 0; i < targetCount; ++i) {
        const double src = i * scale;
        const int i0 = qBound(0, static_cast<int>(std::floor(src)), input.size() - 1);
        const int i1 = qBound(0, i0 + 1, input.size() - 1);
        output.append(lerpPoint(input[i0], input[i1], src - i0));
    }
    return output;
}

QVector<QVector3D> TrajectoryCalculator::resampleTrajectorySpline(const QVector<QVector3D> &input, int targetCount)
{
    if (input.size() < 4) return resampleTrajectoryLinear(input, targetCount);
    QVector<QVector3D> output;
    if (targetCount <= 0) return output;
    if (targetCount == 1) return QVector<QVector3D>{input.first()};

    output.reserve(targetCount);
    const double scale = static_cast<double>(input.size() - 1) / qMax(1, targetCount - 1);
    for (int i = 0; i < targetCount; ++i) {
        const double src = i * scale;
        const int i1 = qBound(0, static_cast<int>(std::floor(src)), input.size() - 1);
        const int i2 = qBound(0, i1 + 1, input.size() - 1);
        const int i0 = qBound(0, i1 - 1, input.size() - 1);
        const int i3 = qBound(0, i2 + 1, input.size() - 1);
        output.append(catmullRom(input[i0], input[i1], input[i2], input[i3], src - i1));
    }
    return output;
}

TrajectoryFilterResult TrajectoryCalculator::filterAndInterpolateTrajectory(const QVector<QVector3D> &input,
                                                                             const TrajectoryFilterOptions &options,
                                                                             int targetCount)
{
    TrajectoryFilterResult result;
    const int n = input.size();
    result.keepMask = QVector<bool>(n, true);
    result.distanceOutlierMask = QVector<bool>(n, false);
    result.speedOutlierMask = QVector<bool>(n, false);
    result.angleOutlierMask = QVector<bool>(n, false);
    if (n <= 0 || targetCount <= 0) return result;

    for (int i = 1; i < n; ++i) {
        const double dist = planarDistance(input[i - 1], input[i]);
        if (options.distanceFilterEnabled && dist > options.maxSegmentDistanceM) {
            result.distanceOutlierMask[i] = true;
            result.keepMask[i] = false;
        }
        if (options.speedFilterEnabled && options.traceIntervalSec > 1e-9 && dist / options.traceIntervalSec > options.maxSpeedMps) {
            result.speedOutlierMask[i] = true;
            result.keepMask[i] = false;
        }
    }

    if (options.angleFilterEnabled) {
        const double thresholdRad = options.maxTurnAngleDeg * M_PI / 180.0;
        for (int i = 1; i + 1 < n; ++i) {
            const double d0 = planarDistance(input[i - 1], input[i]);
            const double d1 = planarDistance(input[i], input[i + 1]);
            if (d0 < 1e-6 || d1 < 1e-6) continue;
            const double ax = input[i].x() - input[i - 1].x();
            const double ay = input[i].y() - input[i - 1].y();
            const double bx = input[i + 1].x() - input[i].x();
            const double by = input[i + 1].y() - input[i].y();
            const double cosv = std::clamp((ax * bx + ay * by) / (d0 * d1), -1.0, 1.0);
            const double turn = std::acos(cosv);
            if (turn > thresholdRad) {
                result.angleOutlierMask[i] = true;
                result.keepMask[i] = false;
            }
        }
    }

    if (options.preserveEndpoints && n > 0) {
        result.keepMask[0] = true;
        result.keepMask[n - 1] = true;
    }

    QVector<QVector3D> kept;
    kept.reserve(n);
    for (int i = 0; i < n; ++i) {
        if (result.keepMask.value(i, true)) kept.append(input[i]);
    }
    if (kept.size() < 2) {
        kept = input;
        result.warnings.append(QStringLiteral("过滤后有效轨迹点过少，已使用原始轨迹插值。"));
    }

    if (options.interpolationMode == TrajectoryFilterOptions::InterpolationMode::Spline) {
        if (kept.size() < 4) result.warnings.append(QStringLiteral("样条插值点数不足，已回退到线性插值。"));
        result.outputPositions = resampleTrajectorySpline(kept, targetCount);
    } else {
        result.outputPositions = resampleTrajectoryLinear(kept, targetCount);
    }
    return result;
}