using PathFinding;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using UnityEngine;
public struct PathfindingMetrics
{
    public float timeTaken;      // in milliseconds
    public int pathLength;      // number of nodes in path
    public long memoryUsed;     // memory used by pathfinding in bytes
    public int maxOpenListSize; // maximum size of open list during pathfinding
    public int maxClosedListSize; // maximum size of closed list during pathfinding

    // Tambahan untuk cost metrics
    public float totalGCost;    // Total biaya G untuk jalur (jarak sebenarnya)
    public float totalHCost;    // Total biaya H untuk jalur (heuristik)
    public float totalFCost;    // Total biaya F untuk jalur (G + H)

    // Tambahan metrik untuk analisis akademis
    public int totalExpandedNodes;  // Node yang dikeluarkan dari open list dan diproses (ClosedList)
    public int totalGeneratedNodes; // Node yang ditambahkan ke open list
    public int totalTouchedNodes;   // Node yang dievaluasi/diupdate (termasuk yang di-skip)
}

public class NPC : MonoBehaviour
{
    public float speed = 2.0f;
    public Queue<Vector2> wayPoints = new Queue<Vector2>();

    // Event that fires when pathfinding is complete with performance metrics
    public event Action<PathfindingMetrics> OnPathfindingComplete;

    public enum PathFinderType
    {
        ASTAR,
        DIJKSTRA,
        GREEDY,
        BACKTRACKING,
        BFS,
    }

    // Mode pengukuran memori
    public enum MemoryMeasurementMode
    {
        UNITY_PROFILER        // Pengukuran dari Unity Profiler
    }

    [SerializeField]
    public PathFinderType pathFinderType = PathFinderType.ASTAR;

    // Mode pengukuran memori
    [SerializeField]
    public MemoryMeasurementMode memoryMeasurementMode = MemoryMeasurementMode.UNITY_PROFILER;

    PathFinder<Vector2Int> pathFinder = null;

    public GridMap Map { get; set; }

    // List to store all steps for visualization playback
    private List<PathfindingVisualizationStep> visualizationSteps = new List<PathfindingVisualizationStep>();
    private bool isVisualizingPath = false;

    // Properties to control visualization
    [SerializeField]

    // Visualization speed is time between visualization steps
    public float visualizationSpeed = 0.0f; // Default 0; set higher for slower visualization

    // Visualization batch is the number of steps to visualize at once
    public int visualizationBatch = 1; // Default 1; set higher value for faster visualization

    [SerializeField]
    public bool showVisualization = true; // Whether to show visualization at all

    // Struct to store each step of the pathfinding process for visualization
    private struct PathfindingVisualizationStep
    {
        public enum StepType { CurrentNode, OpenList, ClosedList, FinalPath }
        public StepType type;
        public Vector2Int position;

        public PathfindingVisualizationStep(StepType type, Vector2Int position)
        {
            this.type = type;
            this.position = position;
        }
    }

    // Tambahkan counter untuk metrik baru
    private int expandedNodesCount = 0;
    private int generatedNodesCount = 0;
    private int touchedNodesCount = 0;

    private IEnumerator Coroutine_MoveOverSeconds(
      GameObject objectToMove,
      Vector3 end,
      float seconds)
    {
        float elaspedTime = 0.0f;
        Vector3 startingPos = objectToMove.transform.position;

        while (elaspedTime < seconds)
        {
            objectToMove.transform.position =
              Vector3.Lerp(startingPos, end, elaspedTime / seconds);
            elaspedTime += Time.deltaTime;

            yield return new WaitForEndOfFrame();
        }
        objectToMove.transform.position = end;
    }

    IEnumerator Coroutine_MoveToPoint(Vector2 p, float speed)
    {
        Vector3 endP = new Vector3(p.x, p.y, transform.position.z);
        float duration = (transform.position - endP).magnitude / speed;

        yield return StartCoroutine(
          Coroutine_MoveOverSeconds(
            transform.gameObject, endP, duration));
    }

    public IEnumerator Coroutine_MoveTo()
    {
        while (true)
        {
            while (wayPoints.Count > 0)
            {
                yield return StartCoroutine(
                  Coroutine_MoveToPoint(
                    wayPoints.Dequeue(),
                    speed));
            }
            yield return null;
        }
    }

    private void AddWayPoint(GridNode node)
    {
        wayPoints.Enqueue(new Vector2(
          node.Value.x * Map.GridNodeWidth,
          node.Value.y * Map.GridNodeHeight));

        // We set a color to show the path.
        GridNodeView gnv = Map.GetGridNodeView(node.Value.x, node.Value.y);
        gnv.SetInnerColor(Map.COLOR_PATH);
    }

    public void SetStartNode(GridNode node)
    {
        wayPoints.Clear();
        transform.position = new Vector3(
          node.Value.x * Map.GridNodeWidth,
          node.Value.y * Map.GridNodeHeight,
          transform.position.z);
    }

    private void InitializePathFinder()
    {
        // Hitung perkiraan jumlah node dalam grid
        int estimatedNodeCount = 0;
        if (Map != null)
        {
            estimatedNodeCount = Map.NumX * Map.NumY;
        }

        // Log informasi ukuran grid dan strategi optimisasi
        bool isLargeGrid = estimatedNodeCount > 2500;
        UnityEngine.Debug.Log($"Grid Size: {Map?.NumX ?? 0}x{Map?.NumY ?? 0} ({estimatedNodeCount} nodes) - " +
                             $"Using {(isLargeGrid ? "optimized" : "simplified")} pathfinding strategy");

        // Create new pathfinder instance
        switch (pathFinderType)
        {
            case PathFinderType.ASTAR:
                pathFinder = new AStarPathFinder<Vector2Int>(estimatedNodeCount);
                break;
            case PathFinderType.DIJKSTRA:
                pathFinder = new DijkstraPathFinder<Vector2Int>(estimatedNodeCount);
                break;
            case PathFinderType.GREEDY:
                pathFinder = new GreedyPathFinder<Vector2Int>();
                break;
            case PathFinderType.BACKTRACKING:
                pathFinder = new BacktrackingPathFinder<Vector2Int>();
                break;
            case PathFinderType.BFS:
                pathFinder = new BFSPathFinder<Vector2Int>();
                break;
        }

        // Set up callbacks
        pathFinder.onSuccess = OnSuccessPathFinding;
        pathFinder.onFailure = OnFailurePathFinding;

        // Gunakan setting asli
        pathFinder.HeuristicCost = GridMap.GetManhattanCost;
        pathFinder.NodeTraversalCost = GridMap.GetEuclideanCost;

        UnityEngine.Debug.Log($"Initialized pathfinder with algorithm: {pathFinderType}");
    }

    public void MoveTo(GridNode destination, bool silentMode = false)
    {
        if (pathFinder == null)
        {
            UnityEngine.Debug.LogError("Pathfinder is not initialized!");
            InitializePathFinder();
        }

        if (pathFinder.Status == PathFinderStatus.RUNNING)
        {
            UnityEngine.Debug.Log("PathFinder is running. Cannot start a new pathfinding now");
            return;
        }

        GridNode start = Map.GetGridNode(
          (int)(transform.position.x / Map.GridNodeWidth),
          (int)(transform.position.y / Map.GridNodeHeight));

        if (start == null || destination == null)
        {
            UnityEngine.Debug.LogError($"Invalid start or destination node. Start: {start}, Destination: {destination}");
            return;
        }

        if (!silentMode)
        {
            UnityEngine.Debug.Log($"Starting pathfinding from ({start.Value.x}, {start.Value.y}) to ({destination.Value.x}, {destination.Value.y})");
        }

        SetStartNode(start);

        // Reset grid colors
        if (!silentMode)
        {
            Map.ResetGridNodeColours();
        }

        visualizationSteps.Clear();
        isVisualizingPath = false;

        // Initialize pathfinding
        if (!pathFinder.Initialise(start, destination))
        {
            UnityEngine.Debug.LogError("Failed to initialize pathfinder!");
            return;
        }

        StartCoroutine(Coroutine_FindPathStep(silentMode));
    }

    IEnumerator Coroutine_FindPathStep(bool silentMode = false)
    {
        yield return StartCoroutine(MeasurePerformance(silentMode));

        // Start visualization after calculation is complete
        if (pathFinder.Status == PathFinderStatus.SUCCESS && showVisualization && !silentMode)
        {
            yield return StartCoroutine(VisualizePathfinding());
        }
    }

    IEnumerator MeasurePerformance(bool silentMode = false)
    {
        // Memory tracking for pathfinding structures - tetap untuk visualisasi
        int maxOpenListSize = 0;
        int currentOpenListSize = 0;
        int maxClosedListSize = 0;
        int currentClosedListSize = 0;

        // Pre-allocate visualizationSteps with estimated capacity to avoid reallocations
        visualizationSteps = new List<PathfindingVisualizationStep>(1000);

        // ===== MEMORY MEASUREMENT START: Ukur memory sebelum algoritma =====
        System.GC.Collect(); // Force garbage collection untuk pengukuran yang akurat
        System.GC.WaitForPendingFinalizers();
        long memoryBefore = System.GC.GetTotalMemory(false);

        // Setup callbacks before running algorithm
        SetupCallbacks(silentMode, ref maxOpenListSize, ref currentOpenListSize,
                       ref maxClosedListSize, ref currentClosedListSize);

        // ===== STOPWATCH START: Pengukuran waktu algoritma =====
        float startTime = Time.realtimeSinceStartup;

        // Counter untuk jumlah step yang dilakukan algoritma
        int stepCount = 0;

        // Execute the pathfinding algorithm synchronously in a single frame without visualization
        while (pathFinder.Status == PathFinderStatus.RUNNING)
        {
            stepCount++;
            pathFinder.Step();
        }

        // ===== STOPWATCH STOP: Akhir pengukuran waktu algoritma =====
        float endTime = Time.realtimeSinceStartup;
        float duration = (endTime - startTime) * 1000f; // Convert to milliseconds

        // ===== MEMORY MEASUREMENT END: Ukur memory setelah algoritma =====
        long memoryAfter = System.GC.GetTotalMemory(false);
        long memoryUsed = memoryAfter - memoryBefore;

        UnityEngine.Debug.Log($"Memory used: {memoryUsed} bytes");
        UnityEngine.Debug.Log($"Algorithm execution time: {duration:F2} ms");

        // Calculate path length once and reuse
        int pathLength = 0;
        float totalGCost = 0;
        float totalHCost = 0;
        float totalFCost = 0;

        // Add memory for path reconstruction (final path)
        if (pathFinder.Status == PathFinderStatus.SUCCESS)
        {
            pathLength = CalculatePathLength();

            // Hitung total G, H, dan F cost
            CalculatePathCosts(out totalGCost, out totalHCost, out totalFCost);
        }

        // Koreksi estimasi touched nodes berdasarkan jumlah langkah dan expanded nodes
        // Pendekatan heuristik: touched nodes = expanded nodes * rata-rata branching factor
        float estimatedBranchingFactor = 4.0f; // Untuk grid 4-connected
        if (pathFinderType == PathFinderType.ASTAR || pathFinderType == PathFinderType.GREEDY)
        {
            // A* dan Greedy biasanya memeriksa lebih sedikit node karena heuristik
            estimatedBranchingFactor = 3.5f;
        }
        // Koreksi touchedNodesCount jika terlalu rendah, karena estimasi minimal
        int estimatedTouchedNodes = Mathf.RoundToInt(expandedNodesCount * estimatedBranchingFactor);
        touchedNodesCount = Mathf.Max(touchedNodesCount, estimatedTouchedNodes);

        // Create and send metrics - waktu pengukuran algoritma yang tepat
        PathfindingMetrics metrics = new PathfindingMetrics
        {
            timeTaken = duration,  // Waktu algoritma yang diukur dengan Time.realtimeSinceStartup
            pathLength = pathLength,
            memoryUsed = memoryUsed,
            maxOpenListSize = maxOpenListSize,
            maxClosedListSize = maxClosedListSize,
            totalGCost = totalGCost,
            totalHCost = totalHCost,
            totalFCost = totalFCost,
            // Tambahkan metrik baru
            totalExpandedNodes = expandedNodesCount,
            totalGeneratedNodes = generatedNodesCount,
            totalTouchedNodes = touchedNodesCount
        };

        // Report metrics before visualization
        if (!silentMode)
        {
            OnPathfindingComplete?.Invoke(metrics);
        }

        // Path visualization and handling
        HandlePathFindingResult(silentMode, pathLength);

        // Pastikan untuk mengembalikan nilai di akhir coroutine
        yield return null;
    }

    /// <summary>
    /// Setup callbacks for tracking nodes in open/closed lists and visualization
    /// </summary>
    private void SetupCallbacks(bool silentMode, ref int maxOpenListSize, ref int currentOpenListSize,
                               ref int maxClosedListSize, ref int currentClosedListSize)
    {
        // Reset counters setiap kali pathfinding dimulai
        expandedNodesCount = 0;
        generatedNodesCount = 0;
        touchedNodesCount = 0;

        // Buat variabel lokal untuk menghindari masalah dengan ref parameter dalam lambda
        int localCurrentOpenListSize = currentOpenListSize;
        int localMaxOpenListSize = maxOpenListSize;
        int localCurrentClosedListSize = currentClosedListSize;
        int localMaxClosedListSize = maxClosedListSize;

        if (silentMode)
        {
            // In silent mode, just set minimal callbacks for metrics
            pathFinder.onAddToOpenList = (node) =>
            {
                // Menghitung node yang ditambahkan ke open list (generated)
                generatedNodesCount++;

                localCurrentOpenListSize++;
                if (localCurrentOpenListSize > localMaxOpenListSize)
                    localMaxOpenListSize = localCurrentOpenListSize;
            };

            pathFinder.onAddToClosedList = (node) =>
            {
                // Menghitung node yang dipindahkan ke closed list (expanded)
                expandedNodesCount++;

                localCurrentClosedListSize++;
                if (localCurrentClosedListSize > localMaxClosedListSize)
                    localMaxClosedListSize = localCurrentClosedListSize;
                localCurrentOpenListSize--; // When a node is moved from open to closed list
            };

            // For touched nodes, kita menghitung semua node yang dievaluasi
            // Karena kita tidak bisa memodifikasi PathFinder class, kita estimasi ini
            // berdasarkan jumlah Step yang dilakukan algoritma
            pathFinder.onChangeCurrentNode = (node) =>
            {
                // Setiap kali node diproses, node tetangga dievaluasi (touched)
                // Kita akan mengestimasi jumlah node tetangga (umumnya 4-8 untuk grid)
                touchedNodesCount += 4; // Estimasi minimal, akan dikoreksi nanti
            };
        }
        else
        {
            // In regular mode, track and prepare for visualization
            pathFinder.onAddToOpenList = (node) =>
            {
                // Menghitung node yang ditambahkan ke open list (generated)
                generatedNodesCount++;

                visualizationSteps.Add(new PathfindingVisualizationStep(
                    PathfindingVisualizationStep.StepType.OpenList,
                    node.Location.Value));

                localCurrentOpenListSize++;
                if (localCurrentOpenListSize > localMaxOpenListSize)
                    localMaxOpenListSize = localCurrentOpenListSize;
            };

            pathFinder.onAddToClosedList = (node) =>
            {
                // Menghitung node yang dipindahkan ke closed list (expanded)
                expandedNodesCount++;

                visualizationSteps.Add(new PathfindingVisualizationStep(
                    PathfindingVisualizationStep.StepType.ClosedList,
                    node.Location.Value));

                localCurrentClosedListSize++;
                if (localCurrentClosedListSize > localMaxClosedListSize)
                    localMaxClosedListSize = localCurrentClosedListSize;

                localCurrentOpenListSize--; // When a node is moved from open to closed list
            };

            pathFinder.onChangeCurrentNode = (node) =>
            {
                // Setiap kali node diproses, node tetangga dievaluasi (touched)
                touchedNodesCount += 4; // Estimasi minimal

                visualizationSteps.Add(new PathfindingVisualizationStep(
                    PathfindingVisualizationStep.StepType.CurrentNode,
                    node.Location.Value));
            };
        }

        // Setelah lambda selesai dijalankan, perbarui variabel ref
        maxOpenListSize = localMaxOpenListSize;
        currentOpenListSize = localCurrentOpenListSize;
        maxClosedListSize = localMaxClosedListSize;
        currentClosedListSize = localCurrentClosedListSize;
    }

    /// <summary>
    /// Handle path finding result (success or failure)
    /// </summary>
    private void HandlePathFindingResult(bool silentMode, int pathLength)
    {
        if (pathFinder.Status == PathFinderStatus.SUCCESS)
        {
            OnSuccessPathFinding();

            // In non-silent mode, prepare visualization data for the path
            if (!silentMode && showVisualization)
            {
                // Add the path nodes for visualization in efficient batched way
                PathFinder<Vector2Int>.PathFinderNode node = pathFinder.CurrentNode;
                List<Vector2Int> pathPositions = new List<Vector2Int>(pathLength); // Pre-allocate with known size

                // Build path in reverse order
                while (node != null)
                {
                    pathPositions.Add(node.Location.Value);
                    node = node.Parent;
                }

                // Process path in correct order
                for (int i = pathPositions.Count - 1; i >= 0; i--)
                {
                    visualizationSteps.Add(new PathfindingVisualizationStep(
                        PathfindingVisualizationStep.StepType.FinalPath,
                        pathPositions[i]));
                }
            }
        }
        else if (pathFinder.Status == PathFinderStatus.FAILURE)
        {
            OnFailurePathFinding();
        }
    }

    /// <summary>
    /// Memformat ukuran byte menjadi string yang lebih mudah dibaca
    /// </summary>

    void OnSuccessPathFinding()
    {
        float totalGCost = 0;
        float totalHCost = 0;
        float totalFCost = 0;

        // Hitung biaya-biaya path menggunakan metode yang sudah ada
        CalculatePathCosts(out totalGCost, out totalHCost, out totalFCost);

        // Informasi dasar
        int pathLength = CalculatePathLength();

        // Log informasi lengkap hanya jika dibutuhkan
        if (UnityEngine.Debug.isDebugBuild)
        {
            UnityEngine.Debug.Log($"Pathfinding Success - Algorithm: {pathFinderType}");
            UnityEngine.Debug.Log($"Final Path length: {pathLength} nodes");
            UnityEngine.Debug.Log($"Nodes explored: {expandedNodesCount}");

            // Log node metrics untuk analisis akademis
            UnityEngine.Debug.Log($"Generated nodes: {generatedNodesCount}");
            UnityEngine.Debug.Log($"Touched nodes: {touchedNodesCount}");

            // Log cost metrics dengan format yang lebih jelas
            bool usesHeuristic = pathFinderType == PathFinderType.ASTAR || pathFinderType == PathFinderType.GREEDY;

            UnityEngine.Debug.Log($"Total G-Cost: {totalGCost:F2} (Jarak sebenarnya)");

            if (usesHeuristic)
            {
                UnityEngine.Debug.Log($"Total H-Cost: {totalHCost:F2} (Estimasi heuristik)");
                UnityEngine.Debug.Log($"Total F-Cost: {totalFCost:F2} (G+H, Biaya total)");
            }

            // Log detail tambahan jika perlu
            UnityEngine.Debug.Log($"Average cost per step: {(pathLength > 0 ? totalGCost / pathLength : 0):F2}");
        }
    }

    void OnFailurePathFinding()
    {
        if (UnityEngine.Debug.isDebugBuild)
        {
            UnityEngine.Debug.Log("Cannot find path. No valid path exists!");
            UnityEngine.Debug.Log($"Nodes explored: {expandedNodesCount}");
        }
    }

    /// <summary>
    /// Changes the pathfinding algorithm at runtime
    /// </summary>
    public void ChangeAlgorithm(PathFinderType newType)
    {
        // Don't change if pathfinding is in progress
        if (pathFinder != null && pathFinder.Status == PathFinderStatus.RUNNING)
        {
            UnityEngine.Debug.Log("Cannot change algorithm while pathfinding is running");
            return;
        }

        pathFinderType = newType;

        // Hitung perkiraan jumlah node dalam grid
        int estimatedNodeCount = 0;
        if (Map != null)
        {
            estimatedNodeCount = Map.NumX * Map.NumY;
        }

        // Create new pathfinder instance
        switch (pathFinderType)
        {
            case PathFinderType.ASTAR:
                pathFinder = new AStarPathFinder<Vector2Int>(estimatedNodeCount);
                break;
            case PathFinderType.DIJKSTRA:
                pathFinder = new DijkstraPathFinder<Vector2Int>(estimatedNodeCount);
                break;
            case PathFinderType.GREEDY:
                pathFinder = new GreedyPathFinder<Vector2Int>();
                break;
            case PathFinderType.BACKTRACKING:
                pathFinder = new BacktrackingPathFinder<Vector2Int>();
                break;
            case PathFinderType.BFS:
                pathFinder = new BFSPathFinder<Vector2Int>();
                break;
        }

        // Set up callbacks
        pathFinder.onSuccess = OnSuccessPathFinding;
        pathFinder.onFailure = OnFailurePathFinding;

        // Gunakan setting asli
        pathFinder.HeuristicCost = GridMap.GetManhattanCost;
        pathFinder.NodeTraversalCost = GridMap.GetEuclideanCost;

        UnityEngine.Debug.Log($"Changed pathfinding algorithm to {pathFinderType}");
    }

    private int CalculatePathLength()
    {
        int pathLength = 0;
        PathFinder<Vector2Int>.PathFinderNode node = pathFinder.CurrentNode;
        while (node != null)
        {
            pathLength++;
            node = node.Parent;
        }
        return pathLength;
    }

    IEnumerator VisualizePathfinding()
    {
        if (!showVisualization)
            yield break;

        isVisualizingPath = true;

        // First, ensure grid is reset
        Map.ResetGridNodeColours();

        // Visualize each step with a delay - use batch processing for efficiency
        int stepCount = visualizationSteps.Count;
        int batchSize = Mathf.Min(visualizationBatch, stepCount); // set higher value for faster visualization

        for (int i = 0; i < stepCount; i += batchSize)
        {
            int end = Mathf.Min(i + batchSize, stepCount);

            // Process a batch of steps
            for (int j = i; j < end; j++)
            {
                var step = visualizationSteps[j];
                GridNodeView gnv = Map.GetGridNodeView(step.position.x, step.position.y);
                if (gnv != null)
                {
                    switch (step.type)
                    {
                        case PathfindingVisualizationStep.StepType.CurrentNode:
                            gnv.SetInnerColor(Map.COLOR_CURRENT_NODE);
                            break;
                        case PathfindingVisualizationStep.StepType.OpenList:
                            gnv.SetInnerColor(Map.COLOR_ADD_TO_OPENLIST);
                            break;
                        case PathfindingVisualizationStep.StepType.ClosedList:
                            gnv.SetInnerColor(Map.COLOR_ADD_TO_CLOSEDLIST);
                            break;
                        case PathfindingVisualizationStep.StepType.FinalPath:
                            gnv.SetInnerColor(Map.COLOR_PATH);
                            // Also add the waypoint when we process the path
                            if (step.type == PathfindingVisualizationStep.StepType.FinalPath)
                            {
                                GridNode pathNode = Map.GetGridNode(step.position.x, step.position.y);
                                AddWayPoint(pathNode);
                            }
                            break;
                    }
                }
            }

            // Yield after each batch to prevent frame drops
            yield return new WaitForSeconds(visualizationSpeed);
        }

        isVisualizingPath = false;
    }

    /// <summary>
    /// Menghitung biaya G, H, dan F untuk jalur
    /// </summary>
    private void CalculatePathCosts(out float totalGCost, out float totalHCost, out float totalFCost)
    {
        // Inisialisasi nilai awal
        totalGCost = 0;
        totalHCost = 0;
        totalFCost = 0;

        // Jika tidak ada path yang ditemukan, return nilai 0
        if (pathFinder.CurrentNode == null)
            return;

        // Untuk algoritma yang menggunakan heuristik
        bool usesHeuristic = pathFinderType == PathFinderType.ASTAR ||
                            pathFinderType == PathFinderType.GREEDY;

        // Node final berisi total cost jalur
        PathFinder<Vector2Int>.PathFinderNode finalNode = pathFinder.CurrentNode;

        // G cost adalah biaya sebenarnya dari start ke goal, sudah terakumulasi di node akhir
        totalGCost = finalNode.GCost;

        // H cost di node final idealnya 0 (sudah di tujuan), 
        // tapi untuk info lengkap, kita dapat path's H cost dari node awal
        if (usesHeuristic)
        {
            // H cost dari node awal ke tujuan (untuk referensi)
            totalHCost = finalNode.HCost;

            // F cost adalah G + H di node akhir
            totalFCost = finalNode.FCost;

            // Log additional debug info
            UnityEngine.Debug.Log($"Final node - G:{finalNode.GCost:F2}, H:{finalNode.HCost:F2}, F:{finalNode.FCost:F2}");
        }
        else
        {
            // Algoritma tanpa heuristik (seperti Dijkstra)
            totalFCost = totalGCost;
        }

        // Hitung rata-rata biaya per langkah untuk analisis
        int pathLength = CalculatePathLength();
        float avgCostPerStep = pathLength > 0 ? totalGCost / pathLength : 0;

        UnityEngine.Debug.Log($"[COST] Path: {pathLength} steps, Avg cost/step: {avgCostPerStep:F2}");
    }
}