#pragma once

#ifndef WIN32_LEAN_AND_MEAN
#define WIN32_LEAN_AND_MEAN
#endif

#include <vector>
#include <cmath>
#include <cstdint>
#include "navigation/terrain_grid.hpp"

namespace navigation {

struct Vec2 {
    float x = 0.0f;
    float y = 0.0f;

    Vec2() = default;
    Vec2(float _x, float _y) : x(_x), y(_y) {}

    float Distance(const Vec2& other) const {
        float dx = x - other.x;
        float dy = y - other.y;
        return std::sqrt(dx * dx + dy * dy);
    }

    float DistanceSq(const Vec2& other) const {
        float dx = x - other.x;
        float dy = y - other.y;
        return dx * dx + dy * dy;
    }
};

enum class PathAlgorithm : uint8_t {
    ASTAR_CLASSIC = 0,
    JPS_FAST = 1
};

// [INV-NAV-TERRAIN-COMMERCIAL] Ket qua tinh toan vector truot tiep tuyen men theo bo vat can
struct TangentSlideResult {
    Vec2 slideVector{0.0f, 0.0f};  // Vector huong truot tiep tuyen da duoc chuan hoa
    float slideAngle = 0.0f;       // Goc huong truot (radians)
    bool isSliding = false;         // Co dang truot tiep tuyen hay khong
    int8_t handDirection = 0;      // +1: Left/CCW, -1: Right/CW
};

class Pathfinder {
public:
    Pathfinder() = default;

    void SetAlgorithm(PathAlgorithm algo) { m_algorithm = algo; }
    PathAlgorithm GetAlgorithm() const { return m_algorithm; }

    // [INV-NAV-TERRAIN-COMMERCIAL]
    // Thuat toan truot tiep tuyen men theo bo vat can (Tangent Vector Sliding / Wall Following)
    // khi nhan vat va cham vat can hoac go da (Delta XYZ xap xi 0).
    TangentSlideResult ComputeTangentSlide(
        const Vec2& currentPos,
        const Vec2& desiredDir,
        const Vec2& goalPos,
        const TerrainGrid* grid = nullptr,
        int8_t preferredHand = 0
    );

    // Tìm đường đi tối ưu từ start tới goal tránh các vật cản trong grid
    // Trả về danh sách các waypoint theo tọa độ thế giới (world coordinates)
    std::vector<Vec2> FindPath(
        const Vec2& start,
        const Vec2& goal,
        const TerrainGrid& grid,
        uint32_t maxSearchNodes = 3500
    );

    // Tìm đường bằng Jump Point Search (JPS) tốc độ cực cao (< 0.25ms)
    std::vector<Vec2> FindPathJPS(
        const Vec2& start,
        const Vec2& goal,
        const TerrainGrid& grid,
        uint32_t maxSearchNodes = 1500
    );

    // Tìm đường bằng Grid A* truyền thống
    std::vector<Vec2> FindPathAStar(
        const Vec2& start,
        const Vec2& goal,
        const TerrainGrid& grid,
        uint32_t maxSearchNodes = 3500
    );

    // Kéo thẳng dây (String Pulling / Funnel Algorithm) làm mượt đường đi tự nhiên
    std::vector<Vec2> SmoothPath(
        const std::vector<Vec2>& rawPath,
        const TerrainGrid& grid
    );

    uint32_t LastNodesExplored() const { return m_lastNodesExplored; }
    float LastComputeTimeUs() const { return m_lastComputeTimeUs; }

private:
    struct GridPoint {
        int32_t x = -1;
        int32_t y = -1;
        bool IsValid() const { return x >= 0 && y >= 0; }
        bool operator==(const GridPoint& other) const { return x == other.x && y == other.y; }
    };

    GridPoint 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
    );

    GridPoint 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
    );

    PathAlgorithm m_algorithm = PathAlgorithm::JPS_FAST;
    uint32_t m_lastNodesExplored = 0;
    float m_lastComputeTimeUs = 0.0f;
};

} // namespace navigation