#include "combat/predictive_aim_controller.hpp"
#include "navigation/terrain_grid.hpp"
#include <algorithm>
#include <cmath>

namespace combat {

void TrackedEntityState::PushSample(uint64_t nowMs, float x, float y) {
    samples[head] = EntitySample{ nowMs, x, y };
    head = static_cast<uint8_t>((head + 1) % kMaxSamplesPerEntity);
    if (count < kMaxSamplesPerEntity) {
        ++count;
    }
    currentX = x;
    currentY = y;
    lastSeenMs = nowMs;
}

void TrackedEntityState::UpdateVelocity(uint32_t maxSamples) {
    if (count < 2) {
        velocityX = 0.0f;
        velocityY = 0.0f;
        speed = 0.0f;
        return;
    }

    const uint32_t sampleCount = (std::min)(static_cast<uint32_t>(count), maxSamples);
    if (sampleCount < 2) {
        velocityX = 0.0f;
        velocityY = 0.0f;
        speed = 0.0f;
        return;
    }

    // Trích xuất các mẫu gần nhất theo thứ tự thời gian tăng dần
    // Head trỏ vào vị trí ghi kế tiếp, nên mẫu mới nhất là (head - 1 + Cap) % Cap
    float times[kMaxSamplesPerEntity]{};
    float xs[kMaxSamplesPerEntity]{};
    float ys[kMaxSamplesPerEntity]{};

    const uint64_t baseTimeMs = samples[(head - sampleCount + kMaxSamplesPerEntity) % kMaxSamplesPerEntity].timestampMs;

    for (uint32_t i = 0; i < sampleCount; ++i) {
        const size_t idx = (head - sampleCount + i + kMaxSamplesPerEntity) % kMaxSamplesPerEntity;
        const auto& s = samples[idx];
        times[i] = static_cast<float>(s.timestampMs - baseTimeMs) / 1000.0f; // Chuyển sang giây
        xs[i] = s.posX;
        ys[i] = s.posY;
    }

    const float totalDt = times[sampleCount - 1] - times[0];
    if (totalDt < 0.008f) { // Quá ngắn (< 8ms, tức dưới 1 tick)
        return;
    }

    // Hồi quy tuyến tính Ordinary Least Squares (OLS)
    float meanT = 0.0f, meanX = 0.0f, meanY = 0.0f;
    for (uint32_t i = 0; i < sampleCount; ++i) {
        meanT += times[i];
        meanX += xs[i];
        meanY += ys[i];
    }
    const float invN = 1.0f / static_cast<float>(sampleCount);
    meanT *= invN;
    meanX *= invN;
    meanY *= invN;

    float varT = 0.0f;
    float covTX = 0.0f;
    float covTY = 0.0f;
    for (uint32_t i = 0; i < sampleCount; ++i) {
        const float dt = times[i] - meanT;
        varT += dt * dt;
        covTX += dt * (xs[i] - meanX);
        covTY += dt * (ys[i] - meanY);
    }

    if (varT > 1e-6f) {
        velocityX = covTX / varT;
        velocityY = covTY / varT;
    } else {
        // Fallback: sai phân hữu hạn đơn giản
        velocityX = (xs[sampleCount - 1] - xs[0]) / totalDt;
        velocityY = (ys[sampleCount - 1] - ys[0]) / totalDt;
    }

    speed = std::sqrt(velocityX * velocityX + velocityY * velocityY);
}

PredictiveAimController::PredictiveAimController(const PredictiveAimConfig& config)
    : m_config(config) {}

int PredictiveAimController::FindEntitySlot(uint32_t entityId) const {
    for (size_t i = 0; i < kMaxTrackedEntities; ++i) {
        if (m_trackedTable[i].entityId == entityId && m_trackedTable[i].count > 0) {
            return static_cast<int>(i);
        }
    }
    return -1;
}

int PredictiveAimController::AllocateEntitySlot(uint32_t entityId, uint64_t nowMs) {
    // 1. Tìm slot trống
    for (size_t i = 0; i < kMaxTrackedEntities; ++i) {
        if (m_trackedTable[i].entityId == 0) {
            m_trackedTable[i] = TrackedEntityState{};
            m_trackedTable[i].entityId = entityId;
            ++m_activeTrackedCount;
            return static_cast<int>(i);
        }
    }

    // 2. Tìm slot quá hạn (quái đã chết hoặc ngoài tầm nhìn)
    int oldestSlot = 0;
    uint64_t oldestSeenMs = UINT64_MAX;
    for (size_t i = 0; i < kMaxTrackedEntities; ++i) {
        if (nowMs >= m_trackedTable[i].lastSeenMs + m_config.sampleExpiryMs) {
            m_trackedTable[i] = TrackedEntityState{};
            m_trackedTable[i].entityId = entityId;
            return static_cast<int>(i);
        }
        if (m_trackedTable[i].lastSeenMs < oldestSeenMs) {
            oldestSeenMs = m_trackedTable[i].lastSeenMs;
            oldestSlot = static_cast<int>(i);
        }
    }

    // 3. Thay thế slot cũ nhất (LRU eviction)
    m_trackedTable[oldestSlot] = TrackedEntityState{};
    m_trackedTable[oldestSlot].entityId = entityId;
    return oldestSlot;
}

void PredictiveAimController::UpdateEntityTracking(const TelemetryPacket& packet, uint64_t nowMs) {
    if (!m_config.enabled) return;

    for (uint32_t i = 0; i < packet.entityCount; ++i) {
        const auto& ent = packet.entities[i];
        if (ent.type != 1) continue; // Chỉ theo dõi quái vật (Monster)
        if (ent.extraFlags & 4) continue; // Bỏ qua quái đã chết (Dead)
        if (ent.maxHP == 0 || ent.currentHP == 0) continue;

        int slot = FindEntitySlot(ent.id);
        if (slot < 0) {
            slot = AllocateEntitySlot(ent.id, nowMs);
        }

        if (slot >= 0 && slot < static_cast<int>(kMaxTrackedEntities)) {
            auto& state = m_trackedTable[slot];
            state.PushSample(nowMs, ent.posX, ent.posY);
            state.UpdateVelocity(m_config.historyTicks);

            // Giới hạn tốc độ phi lý (teleport hoặc data glitch)
            if (state.speed > m_config.maxReasonableSpeed) {
                const float scale = m_config.maxReasonableSpeed / state.speed;
                state.velocityX *= scale;
                state.velocityY *= scale;
                state.speed = m_config.maxReasonableSpeed;
            }
        }
    }
}

bool PredictiveAimController::GetEntityVelocity(uint32_t targetId, float& outVx, float& outVy, float& outSpeed) const {
    const int slot = FindEntitySlot(targetId);
    if (slot < 0) return false;
    const auto& state = m_trackedTable[slot];
    outVx = state.velocityX;
    outVy = state.velocityY;
    outSpeed = state.speed;
    return true;
}

AimSolution PredictiveAimController::SolveAim(
    uint32_t targetId,
    Vector2D playerPos,
    const BallisticParams& params,
    const navigation::TerrainGrid* terrainGrid
) const {
    const int slot = FindEntitySlot(targetId);
    if (slot < 0) {
        AimSolution fail{};
        return fail;
    }

    const auto& state = m_trackedTable[slot];
    Vector2D targetPos{ state.currentX, state.currentY };
    Vector2D targetVel{ state.velocityX, state.velocityY };

    return SolveDirect(playerPos, targetPos, targetVel, params, terrainGrid);
}

AimSolution PredictiveAimController::SolveDirect(
    Vector2D playerPos,
    Vector2D targetPos,
    Vector2D targetVel,
    const BallisticParams& params,
    const navigation::TerrainGrid* terrainGrid
) const {
    AimSolution sol{};
    sol.targetVelocityX = targetVel.x;
    sol.targetVelocityY = targetVel.y;
    sol.targetSpeed = std::sqrt(targetVel.x * targetVel.x + targetVel.y * targetVel.y);

    const float dx0 = targetPos.x - playerPos.x;
    const float dy0 = targetPos.y - playerPos.y;
    const float currentDist = std::sqrt(dx0 * dx0 + dy0 * dy0);

    // Nếu quái đứng yên hoặc tốc độ dưới ngưỡng tối thiểu: ngắm thẳng vào vị trí hiện tại
    if (sol.targetSpeed < m_config.minSpeedThreshold) {
        sol.valid = (currentDist <= params.maxRange);
        sol.aimTargetX = targetPos.x;
        sol.aimTargetY = targetPos.y;
        sol.leadDistance = 0.0f;
        sol.flightTime = (params.projectileSpeed > 0.0f) ? (currentDist / params.projectileSpeed) : 0.0f;
        sol.timeToImpact = params.windupSeconds + sol.flightTime;
        return sol;
    }

    const float Tw = (std::max)(0.0f, params.windupSeconds);
    const float Vp = params.projectileSpeed;

    float flightTime = 0.0f;

    if (Vp <= 0.0f || Vp >= 10000.0f) {
        // Chiêu cận chiến hoặc đòn diện rộng tức thời (Hitscan / Ground Slam sau windup)
        flightTime = 0.0f;
    } else {
        // Giải phương trình đạn đạo bậc 2:
        // ||(targetPos + targetVel*(Tw + tf)) - playerPos|| = Vp * tf
        // Đặt D0 = targetPos + targetVel*Tw - playerPos
        const float D0x = targetPos.x + targetVel.x * Tw - playerPos.x;
        const float D0y = targetPos.y + targetVel.y * Tw - playerPos.y;

        const float VmSq = targetVel.x * targetVel.x + targetVel.y * targetVel.y;
        const float VpSq = Vp * Vp;

        const float A = VmSq - VpSq;
        const float B = 2.0f * (D0x * targetVel.x + D0y * targetVel.y);
        const float C = D0x * D0x + D0y * D0y;

        const float discr = B * B - 4.0f * A * C;

        if (discr < 0.0f) {
            // Không có nghiệm thực (quái di chuyển nhanh hơn đạn rời khỏi tầm ngắm)
            flightTime = (std::sqrt(C) / Vp);
        } else if (std::fabs(A) < 1e-4f) {
            // Nghiệm suy biến bậc 1: B * tf + C = 0
            if (B < -1e-4f) {
                flightTime = -C / B;
            } else {
                flightTime = 0.0f;
            }
        } else {
            const float sqrtD = std::sqrt(discr);
            const float t1 = (-B - sqrtD) / (2.0f * A);
            const float t2 = (-B + sqrtD) / (2.0f * A);

            if (t1 > 0.0f && t2 > 0.0f) {
                flightTime = (std::min)(t1, t2);
            } else if (t1 > 0.0f) {
                flightTime = t1;
            } else if (t2 > 0.0f) {
                flightTime = t2;
            } else {
                // Hai nghiệm đều âm -> Quái đang chạy lại gần hoặc quá nhanh, fallback thời gian bay ước lượng
                flightTime = (std::sqrt(C) / Vp);
            }
        }
    }

    // Giới hạn thời gian đón đầu tối đa tránh dự đoán quá xa
    const float maxFlight = (std::max)(0.0f, m_config.maxPredictTime - Tw);
    flightTime = (std::clamp)(flightTime, 0.0f, maxFlight);

    const float totalImpactTime = Tw + flightTime;

    // Tọa độ đón đầu dự kiến
    float predX = targetPos.x + targetVel.x * totalImpactTime;
    float predY = targetPos.y + targetVel.y * totalImpactTime;

    const float leadDx = predX - targetPos.x;
    const float leadDy = predY - targetPos.y;
    sol.leadDistance = std::sqrt(leadDx * leadDx + leadDy * leadDy);
    sol.flightTime = flightTime;
    sol.timeToImpact = totalImpactTime;

    // Kiểm tra cự ly tối đa của kỹ năng
    const float predDx = predX - playerPos.x;
    const float predDy = predY - playerPos.y;
    const float predDist = std::sqrt(predDx * predDx + predDy * predDy);

    if (predDist > params.maxRange && predDist > 0.001f) {
        // Kẹp cự ly tối đa theo hướng ngắm
        const float scale = params.maxRange / predDist;
        predX = playerPos.x + predDx * scale;
        predY = playerPos.y + predDy * scale;
    }

    // Kẹp tọa độ vào địa hình có thể đi được (TerrainGrid)
    if (m_config.clampToTerrain && terrainGrid != nullptr) {
        if (!terrainGrid->IsWalkable(predX, predY)) {
            // Lùi dần từ điểm dự đoán về phía vị trí thực tại của quái
            bool foundWalkable = false;
            for (int step = 1; step <= 8; ++step) {
                const float alpha = static_cast<float>(step) / 8.0f;
                const float probeX = predX + (targetPos.x - predX) * alpha;
                const float probeY = predY + (targetPos.y - predY) * alpha;
                if (terrainGrid->IsWalkable(probeX, probeY)) {
                    predX = probeX;
                    predY = probeY;
                    foundWalkable = true;
                    sol.clampedToWalkable = true;
                    break;
                }
            }
            if (!foundWalkable) {
                predX = targetPos.x;
                predY = targetPos.y;
                sol.clampedToWalkable = true;
            }
        }
    }

    sol.aimTargetX = predX;
    sol.aimTargetY = predY;
    sol.valid = true;
    return sol;
}

void PredictiveAimController::Reset() {
    m_trackedTable.fill(TrackedEntityState{});
    m_activeTrackedCount = 0;
}

BallisticParams PredictiveAimController::GetMonkIceStrikeParams() {
    BallisticParams p{};
    p.windupSeconds = 0.110f;       // Ice Strike nhịp đánh vung trượng rất nhanh (110ms)
    p.projectileSpeed = 0.0f;       // Melee Cone - đòn đánh tức thời ngay sau windup
    p.maxRange = 75.0f;             // Cự ly đánh trượng cận chiến
    p.targetHitboxRadius = 16.0f;
    return p;
}

BallisticParams PredictiveAimController::GetMonkTempestBellParams() {
    BallisticParams p{};
    p.windupSeconds = 0.180f;       // Thời gian cắm Chuông Bão xuống đất (180ms)
    p.projectileSpeed = 0.0f;       // Chuông đặt trực tiếp xuống đất
    p.maxRange = 180.0f;            // Cự ly đặt chuông trung bình
    p.targetHitboxRadius = 24.0f;
    return p;
}

BallisticParams PredictiveAimController::GetMonkChargedStaffWaveParams() {
    BallisticParams p{};
    p.windupSeconds = 0.140f;       // Tích tụ sóng sấm sét (140ms)
    p.projectileSpeed = 900.0f;     // Sóng xung kích lôi bão bay 900 units/s
    p.maxRange = 420.0f;            // Chiêu dọn quái tầm xa
    p.targetHitboxRadius = 18.0f;
    return p;
}

BallisticParams PredictiveAimController::GetMonkWolfLeapParams() {
    BallisticParams p{};
    p.windupSeconds = 0.080f;       // Bật nhảy gần như tức thì (80ms)
    p.projectileSpeed = 800.0f;     // Tốc độ lao tới trong không trung (800 units/s)
    p.maxRange = 320.0f;            // Cự ly nhảy áp sát
    p.targetHitboxRadius = 20.0f;
    return p;
}

} // namespace combat