#include "navigation/pathfinder.hpp"

#include <queue>
#include <algorithm>
#include <cmath>
#include <chrono>
#include <iostream>

namespace navigation {

namespace {

inline float OctileDistance(int32_t dx, int32_t dy) {
    const float D = 1.0f;
    const float D2 = 1.41421356f;
    return D * static_cast<float>(dx + dy) + (D2 - 2.0f * D) * static_cast<float>((std::min)(dx, dy));
}

inline int32_t Sign(int32_t val) {
    return (val > 0) ? 1 : ((val < 0) ? -1 : 0);
}

struct PQElement {
    float fCost;
    int32_t gx;
    int32_t gy;

    bool operator>(const PQElement& other) const {
        return fCost > other.fCost;
    }
};

struct Dir2D {
    int32_t dx = 0;
    int32_t dy = 0;
};

} // namespace

TangentSlideResult Pathfinder::ComputeTangentSlide(
    const Vec2& currentPos,
    const Vec2& desiredDir,
    const Vec2& goalPos,
    const TerrainGrid* grid,
    int8_t preferredHand
) {
    TangentSlideResult result{};
    const float len = std::sqrt(desiredDir.x * desiredDir.x + desiredDir.y * desiredDir.y);
    if (len < 0.001f) {
        return result;
    }

    // Huong mong muon duoc chuan hoa
    const float ndx = desiredDir.x / len;
    const float ndy = desiredDir.y / len;

    // Hai vector tiep tuyen vuong goc voi huong mong muon:
    // tLeft: quay 90 do CCW (-ndy, ndx)
    // tRight: quay 90 do CW (ndy, -ndx)
    const Vec2 tLeft(-ndy, ndx);
    const Vec2 tRight(ndy, -ndx);

    // Vector huong ve dich den tu vi tri hien tai
    float toGoalX = goalPos.x - currentPos.x;
    float toGoalY = goalPos.y - currentPos.y;
    const float toGoalLen = std::sqrt(toGoalX * toGoalX + toGoalY * toGoalY);
    if (toGoalLen > 0.001f) {
        toGoalX /= toGoalLen;
        toGoalY /= toGoalLen;
    } else {
        toGoalX = ndx;
        toGoalY = ndy;
    }

    // Tinh tich vo huong giua tiep tuyen va huong dich den
    const float dotLeft = tLeft.x * toGoalX + tLeft.y * toGoalY;
    const float dotRight = tRight.x * toGoalX + tRight.y * toGoalY;

    // Kiem tra tinh walkable tren grid neu co
    bool leftWalkable = true;
    bool rightWalkable = true;
    if (grid) {
        float probeDist = grid->CellSize() * 2.0f;
        leftWalkable = grid->IsWalkable(currentPos.x + tLeft.x * probeDist, currentPos.y + tLeft.y * probeDist);
        rightWalkable = grid->IsWalkable(currentPos.x + tRight.x * probeDist, currentPos.y + tRight.y * probeDist);
    }

    int8_t chosenHand = 0;
    Vec2 chosenTangent;

    if (preferredHand != 0) {
        // Duy tri huong wall-following da chon neu co the di duoc
        if (preferredHand > 0 && leftWalkable) {
            chosenHand = 1;
            chosenTangent = tLeft;
        } else if (preferredHand < 0 && rightWalkable) {
            chosenHand = -1;
            chosenTangent = tRight;
        }
    }

    if (chosenHand == 0) {
        // Chon tiep tuyen huong ve dich tot nhat
        if (leftWalkable && (!rightWalkable || dotLeft >= dotRight)) {
            chosenHand = 1;
            chosenTangent = tLeft;
        } else if (rightWalkable) {
            chosenHand = -1;
            chosenTangent = tRight;
        } else {
            // Ca 2 phia deu vuong vat can cuc bo tren grid -> mac dinh chon ben co dot product cao hon
            chosenHand = (dotLeft >= dotRight) ? 1 : -1;
            chosenTangent = (chosenHand == 1) ? tLeft : tRight;
        }
    }

    // Blend: 75% tiep tuyen + 25% huong toi de truot sat bo vat can ma khong tach roi
    float blendX = chosenTangent.x * 0.75f + ndx * 0.25f;
    float blendY = chosenTangent.y * 0.75f + ndy * 0.25f;
    const float blendLen = std::sqrt(blendX * blendX + blendY * blendY);
    if (blendLen > 0.001f) {
        blendX /= blendLen;
        blendY /= blendLen;
    }

    result.slideVector = Vec2(blendX, blendY);
    result.slideAngle = std::atan2(blendY, blendX);
    if (result.slideAngle < 0.0f) result.slideAngle += 6.2831853f;
    result.isSliding = true;
    result.handDirection = chosenHand;

    return result;
}

std::vector<Vec2> Pathfinder::FindPath(
    const Vec2& start,
    const Vec2& goal,
    const TerrainGrid& grid,
    uint32_t maxSearchNodes
) {
    auto tStart = std::chrono::high_resolution_clock::now();
    std::vector<Vec2> path;

    if (m_algorithm == PathAlgorithm::JPS_FAST) {
        path = FindPathJPS(start, goal, grid, maxSearchNodes);
        if (!path.empty()) {
            auto tEnd = std::chrono::high_resolution_clock::now();
            m_lastComputeTimeUs = std::chrono::duration<float, std::micro>(tEnd - tStart).count();
            return path;
        }
    }

    path = FindPathAStar(start, goal, grid, maxSearchNodes);
    auto tEnd = std::chrono::high_resolution_clock::now();
    m_lastComputeTimeUs = std::chrono::duration<float, std::micro>(tEnd - tStart).count();
    return path;
}

Pathfinder::GridPoint Pathfinder::JumpStraight(
    int32_t cx, int32_t cy,
    int32_t dx, int32_t dy,
    const TerrainGrid& grid,
    int32_t goalGx, int32_t goalGy,
    int32_t dim
) {
    int32_t nx = cx;
    int32_t ny = cy;

    if (dx != 0 && dy == 0) {
        // Nhảy thẳng theo phương ngang (Tây <-> Đông)
        while (true) {
            nx += dx;
            if (nx < 0 || nx >= dim || !grid.IsWalkableGrid(nx, cy)) {
                return {-1, -1};
            }
            if (nx == goalGx && cy == goalGy) {
                return {nx, cy};
            }
            // Nếu cắt ngang cột x của đích, kiểm tra xem có đường thẳng dọc tới đích không
            if (nx == goalGx) {
                int32_t toGoalY = Sign(goalGy - cy);
                if (toGoalY != 0) {
                    int32_t testY = cy;
                    bool canReach = true;
                    while (testY != goalGy) {
                        testY += toGoalY;
                        if (testY < 0 || testY >= dim || !grid.IsWalkableGrid(nx, testY)) {
                            canReach = false;
                            break;
                        }
                    }
                    if (canReach) {
                        return {nx, cy};
                    }
                }
            }
            // Forced neighbor kiểm tra phía trên và dưới
            if (cy + 1 < dim && !grid.IsWalkableGrid(nx, cy + 1) && nx + dx >= 0 && nx + dx < dim && grid.IsWalkableGrid(nx + dx, cy + 1)) {
                return {nx, cy};
            }
            if (cy - 1 >= 0 && !grid.IsWalkableGrid(nx, cy - 1) && nx + dx >= 0 && nx + dx < dim && grid.IsWalkableGrid(nx + dx, cy - 1)) {
                return {nx, cy};
            }
        }
    } else if (dx == 0 && dy != 0) {
        // Nhảy thẳng theo phương dọc (Bắc <-> Nam)
        while (true) {
            ny += dy;
            if (ny < 0 || ny >= dim || !grid.IsWalkableGrid(cx, ny)) {
                return {-1, -1};
            }
            if (cx == goalGx && ny == goalGy) {
                return {cx, ny};
            }
            // Nếu cắt ngang hàng y của đích, kiểm tra xem có đường thẳng ngang tới đích không
            if (ny == goalGy) {
                int32_t toGoalX = Sign(goalGx - cx);
                if (toGoalX != 0) {
                    int32_t testX = cx;
                    bool canReach = true;
                    while (testX != goalGx) {
                        testX += toGoalX;
                        if (testX < 0 || testX >= dim || !grid.IsWalkableGrid(testX, ny)) {
                            canReach = false;
                            break;
                        }
                    }
                    if (canReach) {
                        return {cx, ny};
                    }
                }
            }
            // Forced neighbor kiểm tra phía phải và trái
            if (cx + 1 < dim && !grid.IsWalkableGrid(cx + 1, ny) && ny + dy >= 0 && ny + dy < dim && grid.IsWalkableGrid(cx + 1, ny + dy)) {
                return {cx, ny};
            }
            if (cx - 1 >= 0 && !grid.IsWalkableGrid(cx - 1, ny) && ny + dy >= 0 && ny + dy < dim && grid.IsWalkableGrid(cx - 1, ny + dy)) {
                return {cx, ny};
            }
        }
    }

    return {-1, -1};
}

Pathfinder::GridPoint Pathfinder::Jump(
    int32_t cx, int32_t cy,
    int32_t dx, int32_t dy,
    const TerrainGrid& grid,
    int32_t goalGx, int32_t goalGy,
    int32_t dim
) {
    if (dx == 0 && dy == 0) return {-1, -1};

    if (dx == 0 || dy == 0) {
        return JumpStraight(cx, cy, dx, dy, grid, goalGx, goalGy, dim);
    }

    // Nhảy chéo 45 độ
    int32_t nx = cx;
    int32_t ny = cy;

    while (true) {
        nx += dx;
        ny += dy;

        if (nx < 0 || nx >= dim || ny < 0 || ny >= dim || !grid.IsWalkableGrid(nx, ny)) {
            return {-1, -1};
        }

        // Không cắt qua góc tường nhọn
        if (!grid.IsWalkableGrid(nx - dx, ny) || !grid.IsWalkableGrid(nx, ny - dy)) {
            return {-1, -1};
        }

        if (nx == goalGx && ny == goalGy) {
            return {nx, ny};
        }

        // Forced neighbor trong phương chéo
        if (nx - dx >= 0 && nx - dx < dim && ny + dy >= 0 && ny + dy < dim) {
            if (!grid.IsWalkableGrid(nx - dx, ny) && grid.IsWalkableGrid(nx - dx, ny + dy)) {
                return {nx, ny};
            }
        }
        if (nx + dx >= 0 && nx + dx < dim && ny - dy >= 0 && ny - dy < dim) {
            if (!grid.IsWalkableGrid(nx, ny - dy) && grid.IsWalkableGrid(nx + dx, ny - dy)) {
                return {nx, ny};
            }
        }

        // Quét 2 phương thẳng thành phần
        if (JumpStraight(nx, ny, dx, 0, grid, goalGx, goalGy, dim).IsValid()) {
            return {nx, ny};
        }
        if (JumpStraight(nx, ny, 0, dy, grid, goalGx, goalGy, dim).IsValid()) {
            return {nx, ny};
        }
    }
}

std::vector<Vec2> Pathfinder::FindPathJPS(
    const Vec2& start,
    const Vec2& goal,
    const TerrainGrid& grid,
    uint32_t maxSearchNodes
) {
    std::vector<Vec2> path;

    // 1. Line of Sight trực tiếp (0ms)
    if (grid.HasLineOfSight(start.x, start.y, goal.x, goal.y)) {
        path.push_back(goal);
        m_lastNodesExplored = 0;
        return path;
    }

    int32_t startGx, startGy, goalGx, goalGy;
    if (!grid.WorldToGrid(start.x, start.y, startGx, startGy)) {
        path.push_back(goal);
        return path;
    }

    float adjustedGoalX = goal.x;
    float adjustedGoalY = goal.y;
    if (!grid.WorldToGrid(goal.x, goal.y, goalGx, goalGy) || !grid.IsWalkableGrid(goalGx, goalGy)) {
        if (grid.FindNearestWalkable(goal.x, goal.y, adjustedGoalX, adjustedGoalY, 35.0f)) {
            grid.WorldToGrid(adjustedGoalX, adjustedGoalY, goalGx, goalGy);
        } else {
            path.push_back(goal);
            return path;
        }
    }

    if (startGx == goalGx && startGy == goalGy) {
        path.push_back(Vec2(adjustedGoalX, adjustedGoalY));
        m_lastNodesExplored = 0;
        return path;
    }

    const int32_t dim = TerrainGrid::kGridDim;
    const size_t totalCells = static_cast<size_t>(dim) * dim;

    std::vector<float> gCosts(totalCells, 1e9f);
    std::vector<int32_t> parentNodes(totalCells, -1);
    std::vector<bool> closed(totalCells, false);

    std::priority_queue<PQElement, std::vector<PQElement>, std::greater<PQElement>> openSet;

    auto cellIndex = [dim](int32_t gx, int32_t gy) -> size_t {
        return static_cast<size_t>(gy) * dim + static_cast<size_t>(gx);
    };

    size_t startIdx = cellIndex(startGx, startGy);
    gCosts[startIdx] = 0.0f;
    float startH = OctileDistance(std::abs(startGx - goalGx), std::abs(startGy - goalGy));
    openSet.push({startH, startGx, startGy});

    bool found = false;
    uint32_t nodesExplored = 0;

    int32_t closestGx = startGx;
    int32_t closestGy = startGy;
    float closestH = startH;

    while (!openSet.empty() && nodesExplored < maxSearchNodes) {
        auto current = openSet.top();
        openSet.pop();

        int32_t cx = current.gx;
        int32_t cy = current.gy;
        size_t cIdx = cellIndex(cx, cy);

        if (cx == goalGx && cy == goalGy) {
            found = true;
            break;
        }

        if (closed[cIdx]) continue;
        closed[cIdx] = true;
        ++nodesExplored;

        float currentG = gCosts[cIdx];

        // Xác định hướng đi ứng viên (Pruning Rules của JPS)
        // Xác định hướng đi ứng viên (Pruning Rules của JPS)
        Dir2D candidateDirs[16];
        int numDirs = 0;

        int32_t pIdx = parentNodes[cIdx];
        if (pIdx < 0) {
            // Điểm xuất phát: xét cả 8 hướng
            const int32_t dirX[8] = { 0,  0,  1, -1,  1,  1, -1, -1 };
            const int32_t dirY[8] = { 1, -1,  0,  0,  1, -1,  1, -1 };
            for (int i = 0; i < 8; ++i) {
                candidateDirs[numDirs++] = {dirX[i], dirY[i]};
            }
        } else {
            int32_t px = pIdx % dim;
            int32_t py = pIdx / dim;
            int32_t pdx = Sign(cx - px);
            int32_t pdy = Sign(cy - py);

            if (pdx != 0 && pdy == 0) {
                // Di chuyển ngang: hướng tự nhiên
                candidateDirs[numDirs++] = {pdx, 0};
                if (cx == goalGx) {
                    if (goalGy > cy) candidateDirs[numDirs++] = {0, 1};
                    if (goalGy < cy) candidateDirs[numDirs++] = {0, -1};
                }
                if (cy + 1 < dim && !grid.IsWalkableGrid(cx, cy + 1)) {
                    candidateDirs[numDirs++] = {0, 1};
                    candidateDirs[numDirs++] = {pdx, 1};
                }
                if (cy - 1 >= 0 && !grid.IsWalkableGrid(cx, cy - 1)) {
                    candidateDirs[numDirs++] = {0, -1};
                    candidateDirs[numDirs++] = {pdx, -1};
                }
            } else if (pdx == 0 && pdy != 0) {
                // Di chuyển dọc: hướng tự nhiên
                candidateDirs[numDirs++] = {0, pdy};
                if (cy == goalGy) {
                    if (goalGx > cx) candidateDirs[numDirs++] = {1, 0};
                    if (goalGx < cx) candidateDirs[numDirs++] = {-1, 0};
                }
                if (cx + 1 < dim && !grid.IsWalkableGrid(cx + 1, cy)) {
                    candidateDirs[numDirs++] = {1, 0};
                    candidateDirs[numDirs++] = {1, pdy};
                }
                if (cx - 1 >= 0 && !grid.IsWalkableGrid(cx - 1, cy)) {
                    candidateDirs[numDirs++] = {-1, 0};
                    candidateDirs[numDirs++] = {-1, pdy};
                }
            } else if (pdx != 0 && pdy != 0) {
                // Di chuyển chéo: hướng tự nhiên và 2 hướng phân nhánh
                candidateDirs[numDirs++] = {pdx, pdy};
                candidateDirs[numDirs++] = {pdx, 0};
                candidateDirs[numDirs++] = {0, pdy};
                if (cx == goalGx) {
                    if (goalGy > cy) candidateDirs[numDirs++] = {0, 1};
                    if (goalGy < cy) candidateDirs[numDirs++] = {0, -1};
                }
                if (cy == goalGy) {
                    if (goalGx > cx) candidateDirs[numDirs++] = {1, 0};
                    if (goalGx < cx) candidateDirs[numDirs++] = {-1, 0};
                }
                if (!grid.IsWalkableGrid(cx - pdx, cy)) {
                    candidateDirs[numDirs++] = {-pdx, pdy};
                    candidateDirs[numDirs++] = {-pdx, 0};
                }
                if (!grid.IsWalkableGrid(cx, cy - pdy)) {
                    candidateDirs[numDirs++] = {pdx, -pdy};
                    candidateDirs[numDirs++] = {0, -pdy};
                }
            }
        }

        // Nhảy tìm các Jump Points theo các hướng ứng viên
        for (int i = 0; i < numDirs; ++i) {
            int32_t dx = candidateDirs[i].dx;
            int32_t dy = candidateDirs[i].dy;

            GridPoint jp = Jump(cx, cy, dx, dy, grid, goalGx, goalGy, dim);
            if (!jp.IsValid()) continue;

            size_t nIdx = cellIndex(jp.x, jp.y);
            if (closed[nIdx]) continue;

            float moveDist = OctileDistance(std::abs(jp.x - cx), std::abs(jp.y - cy));
            float newG = currentG + moveDist;

            if (newG < gCosts[nIdx]) {
                gCosts[nIdx] = newG;
                parentNodes[nIdx] = static_cast<int32_t>(cIdx);
                float h = OctileDistance(std::abs(jp.x - goalGx), std::abs(jp.y - goalGy));
                openSet.push({newG + h, jp.x, jp.y});

                if (h < closestH) {
                    closestH = h;
                    closestGx = jp.x;
                    closestGy = jp.y;
                }
            }
        }
    }

    m_lastNodesExplored = nodesExplored;

    if (!found) {
        return {}; // Fallback sang A* nếu JPS chưa tới đích
    }

    // Truy hồi đường đi từ Goal về Start
    int32_t traceX = goalGx;
    int32_t traceY = goalGy;

    std::vector<Vec2> rawPath;
    while (true) {
        float wx, wy;
        grid.GridToWorld(traceX, traceY, wx, wy);
        rawPath.push_back(Vec2(wx, wy));

        if (traceX == startGx && traceY == startGy) break;

        size_t idx = cellIndex(traceX, traceY);
        int32_t pIdx = parentNodes[idx];
        if (pIdx < 0) break;

        traceX = pIdx % dim;
        traceY = pIdx / dim;
    }

    // Đảm bảo có điểm xuất phát
    float startWx, startWy;
    grid.GridToWorld(startGx, startGy, startWx, startWy);
    if (rawPath.empty() || rawPath.back().Distance(Vec2(startWx, startWy)) > 1.0f) {
        rawPath.push_back(Vec2(startWx, startWy));
    }

    std::reverse(rawPath.begin(), rawPath.end());

    // Đặt start chính xác ở đầu và goal chính xác ở cuối
    if (!rawPath.empty()) {
        rawPath.front() = Vec2(start.x, start.y);
        rawPath.back() = Vec2(adjustedGoalX, adjustedGoalY);
    }

    return SmoothPath(rawPath, grid);
}

std::vector<Vec2> Pathfinder::FindPathAStar(
    const Vec2& start,
    const Vec2& goal,
    const TerrainGrid& grid,
    uint32_t maxSearchNodes
) {
    std::vector<Vec2> path;

    if (grid.HasLineOfSight(start.x, start.y, goal.x, goal.y)) {
        path.push_back(goal);
        m_lastNodesExplored = 0;
        return path;
    }

    int32_t startGx, startGy, goalGx, goalGy;
    if (!grid.WorldToGrid(start.x, start.y, startGx, startGy)) {
        path.push_back(goal);
        return path;
    }

    float adjustedGoalX = goal.x;
    float adjustedGoalY = goal.y;
    if (!grid.WorldToGrid(goal.x, goal.y, goalGx, goalGy) || !grid.IsWalkableGrid(goalGx, goalGy)) {
        if (grid.FindNearestWalkable(goal.x, goal.y, adjustedGoalX, adjustedGoalY, 35.0f)) {
            grid.WorldToGrid(adjustedGoalX, adjustedGoalY, goalGx, goalGy);
        } else {
            path.push_back(goal);
            return path;
        }
    }

    if (startGx == goalGx && startGy == goalGy) {
        path.push_back(Vec2(adjustedGoalX, adjustedGoalY));
        m_lastNodesExplored = 0;
        return path;
    }

    const int32_t dim = TerrainGrid::kGridDim;
    const size_t totalCells = static_cast<size_t>(dim) * dim;

    std::vector<float> gCosts(totalCells, 1e9f);
    std::vector<int32_t> parentNodes(totalCells, -1);
    std::vector<bool> closed(totalCells, false);

    std::priority_queue<PQElement, std::vector<PQElement>, std::greater<PQElement>> openSet;

    auto cellIndex = [dim](int32_t gx, int32_t gy) -> size_t {
        return static_cast<size_t>(gy) * dim + static_cast<size_t>(gx);
    };

    size_t startIdx = cellIndex(startGx, startGy);
    gCosts[startIdx] = 0.0f;
    float startH = OctileDistance(std::abs(startGx - goalGx), std::abs(startGy - goalGy));
    openSet.push({startH, startGx, startGy});

    const int32_t dirX[8] = { 0,  0,  1, -1,  1,  1, -1, -1 };
    const int32_t dirY[8] = { 1, -1,  0,  0,  1, -1,  1, -1 };
    const float moveCost[8] = { 1.0f, 1.0f, 1.0f, 1.0f, 1.4142f, 1.4142f, 1.4142f, 1.4142f };

    bool found = false;
    uint32_t nodesExplored = 0;

    int32_t closestGx = startGx;
    int32_t closestGy = startGy;
    float closestH = startH;

    while (!openSet.empty() && nodesExplored < maxSearchNodes) {
        auto current = openSet.top();
        openSet.pop();

        int32_t cx = current.gx;
        int32_t cy = current.gy;
        size_t cIdx = cellIndex(cx, cy);

        if (cx == goalGx && cy == goalGy) {
            found = true;
            break;
        }

        if (closed[cIdx]) continue;
        closed[cIdx] = true;
        ++nodesExplored;

        float currentG = gCosts[cIdx];

        for (int i = 0; i < 8; ++i) {
            int32_t nx = cx + dirX[i];
            int32_t ny = cy + dirY[i];

            if (nx < 0 || nx >= dim || ny < 0 || ny >= dim) continue;
            if (!grid.IsWalkableGrid(nx, ny)) continue;

            if (i >= 4) {
                if (!grid.IsWalkableGrid(cx, ny) || !grid.IsWalkableGrid(nx, cy)) {
                    continue;
                }
            }

            size_t nIdx = cellIndex(nx, ny);
            if (closed[nIdx]) continue;

            float newG = currentG + moveCost[i];
            if (newG < gCosts[nIdx]) {
                gCosts[nIdx] = newG;
                parentNodes[nIdx] = static_cast<int32_t>(cIdx);
                float h = OctileDistance(std::abs(nx - goalGx), std::abs(ny - goalGy));
                openSet.push({newG + h, nx, ny});

                if (h < closestH) {
                    closestH = h;
                    closestGx = nx;
                    closestGy = ny;
                }
            }
        }
    }

    m_lastNodesExplored = nodesExplored;

    int32_t traceX = found ? goalGx : closestGx;
    int32_t traceY = found ? goalGy : closestGy;

    std::vector<Vec2> rawPath;
    while (true) {
        float wx, wy;
        grid.GridToWorld(traceX, traceY, wx, wy);
        rawPath.push_back(Vec2(wx, wy));

        if (traceX == startGx && traceY == startGy) break;

        size_t idx = cellIndex(traceX, traceY);
        int32_t pIdx = parentNodes[idx];
        if (pIdx < 0) break;

        traceX = pIdx % dim;
        traceY = pIdx / dim;
    }

    std::reverse(rawPath.begin(), rawPath.end());

    if (rawPath.size() > 1 && rawPath.front().Distance(start) < grid.CellSize()) {
        rawPath.erase(rawPath.begin());
    }

    if (found && !rawPath.empty()) {
        rawPath.back() = Vec2(adjustedGoalX, adjustedGoalY);
    }

    return SmoothPath(rawPath, grid);
}

std::vector<Vec2> Pathfinder::SmoothPath(
    const std::vector<Vec2>& rawPath,
    const TerrainGrid& grid
) {
    if (rawPath.size() <= 2) {
        return rawPath;
    }

    std::vector<Vec2> smoothed;
    smoothed.push_back(rawPath.front());

    size_t currentIndex = 0;
    while (currentIndex < rawPath.size() - 1) {
        size_t furthestVisible = currentIndex + 1;

        for (size_t nextIndex = currentIndex + 2; nextIndex < rawPath.size(); ++nextIndex) {
            if (grid.HasLineOfSight(
                    rawPath[currentIndex].x, rawPath[currentIndex].y,
                    rawPath[nextIndex].x, rawPath[nextIndex].y)) {
                furthestVisible = nextIndex;
            } else {
                break;
            }
        }

        smoothed.push_back(rawPath[furthestVisible]);
        currentIndex = furthestVisible;
    }

    return smoothed;
}

} // namespace navigation