[Unity] 길찾기 알고리즘

using System;
using System.Collections.Generic;
using UnityEngine;

/// <summary>
/// A* 경로찾기 알고리즘 구현
/// 블록을 피해 최적 경로를 찾습니다.
/// </summary>
public static class AStarPathfinder
{
    /// <summary>
    /// 노드 클래스 (A* 알고리즘용)
    /// </summary>
    private class PathNode : IComparable<PathNode>
    {
        public Vector2Int position;
        public float gCost; // 시작점부터 현재 노드까지의 비용
        public float hCost; // 현재 노드부터 목표까지의 휴리스틱 비용
        public float fCost => gCost + hCost; // 총 비용
        public PathNode parent;
        
        public PathNode(Vector2Int pos)
        {
            position = pos;
            gCost = float.MaxValue;
            hCost = 0;
            parent = null;
        }
        
        public int CompareTo(PathNode other)
        {
            if (other == null) return 1;
            return fCost.CompareTo(other.fCost);
        }
    }
    
    /// <summary>
    /// 경로 찾기 메인 메서드
    /// </summary>
    /// <param name="start">시작 위치</param>
    /// <param name="end">목표 위치</param>
    /// <param name="blockSpawner">블록 스포너 참조</param>
    /// <returns>경로 (없으면 null)</returns>
    public static List<Vector2Int> FindPath(Vector2Int start, Vector2Int end, BlockSpawner blockSpawner)
    {
        if (blockSpawner == null)
        {
            Debug.LogError("BlockSpawner가 null입니다!");
            return null;
        }
        
        // 시작점과 목표점이 같은 경우
        if (start == end)
        {
            return new List<Vector2Int> { start };
        }
        
        // 시작점이나 목표점이 맵 밖이거나 블록인 경우
        bool startValid = IsValidPosition(start, blockSpawner);
        bool endValid = IsValidPosition(end, blockSpawner);
        
        if (!startValid || !endValid)
        {
            Debug.LogWarning($"유효하지 않은 위치: 시작={start}, 목표={end}");
            Debug.LogWarning($"맵 크기: {blockSpawner.MapWidth}x{blockSpawner.MapHeight}");
            Debug.LogWarning($"시작점 유효성: {startValid}, 목표점 유효성: {endValid}");
            
            // 각 점의 상세 정보 출력
            if (!startValid)
            {
                Debug.LogWarning($"시작점 {start} 상세 정보:");
                bool inBounds = start.x >= 0 && start.x < blockSpawner.MapWidth && start.y >= 0 && start.y < blockSpawner.MapHeight;
                Debug.LogWarning($"  - 맵 경계 내: {inBounds}");
                if (inBounds)
                {
                    Debug.LogWarning($"  - 블록 존재: {blockSpawner.IsBlockAt(start.x, start.y)}");
                }
            }
            
            if (!endValid)
            {
                Debug.LogWarning($"목표점 {end} 상세 정보:");
                bool inBounds = end.x >= 0 && end.x < blockSpawner.MapWidth && end.y >= 0 && end.y < blockSpawner.MapHeight;
                Debug.LogWarning($"  - 맵 경계 내: {inBounds}");
                if (inBounds)
                {
                    Debug.LogWarning($"  - 블록 존재: {blockSpawner.IsBlockAt(end.x, end.y)}");
                }
            }
            
            return null;
        }
        
        return AStar(start, end, blockSpawner);
    }
    
    /// <summary>
    /// A* 알고리즘 구현 (우선순위 큐 최적화)
    /// </summary>
    private static List<Vector2Int> AStar(Vector2Int start, Vector2Int end, BlockSpawner blockSpawner)
    {
        // 노드 맵 초기화
        Dictionary<Vector2Int, PathNode> allNodes = new Dictionary<Vector2Int, PathNode>();
        PriorityQueue<PathNode> openSet = new PriorityQueue<PathNode>();
        HashSet<Vector2Int> closedSet = new HashSet<Vector2Int>();
        HashSet<Vector2Int> openSetPositions = new HashSet<Vector2Int>(); // 위치 기반 중복 확인용
        
        // 시작 노드 생성
        PathNode startNode = new PathNode(start);
        startNode.gCost = 0;
        startNode.hCost = CalculateHeuristic(start, end);
        allNodes[start] = startNode;
        openSet.Enqueue(startNode);
        openSetPositions.Add(start);
        
        while (!openSet.IsEmpty())
        {
            // fCost가 가장 낮은 노드 선택
            PathNode currentNode = openSet.Dequeue();
            openSetPositions.Remove(currentNode.position);
            
            // 목표에 도달한 경우
            if (currentNode.position == end)
            {
                return RetracePath(currentNode);
            }
            
            // 현재 노드를 닫힌 집합으로 이동
            closedSet.Add(currentNode.position);
            
            // 이웃 노드들 확인
            foreach (Vector2Int neighborPos in GetNeighbors(currentNode.position))
            {
                // 유효하지 않은 위치이거나 이미 확인한 노드인 경우 스킵
                if (!IsValidPosition(neighborPos, blockSpawner) || closedSet.Contains(neighborPos))
                {
                    continue;
                }
                
                // 이웃 노드 생성 또는 가져오기
                if (!allNodes.ContainsKey(neighborPos))
                {
                    allNodes[neighborPos] = new PathNode(neighborPos);
                }
                
                PathNode neighborNode = allNodes[neighborPos];
                
                // 새로운 gCost 계산
                float newGCost = currentNode.gCost + CalculateDistance(currentNode.position, neighborPos);
                
                // 더 나은 경로를 찾은 경우
                if (newGCost < neighborNode.gCost)
                {
                    neighborNode.parent = currentNode;
                    neighborNode.gCost = newGCost;
                    neighborNode.hCost = CalculateHeuristic(neighborPos, end);
                    
                    // 열린 집합에 추가 (이미 있지 않은 경우)
                    if (!openSetPositions.Contains(neighborPos))
                    {
                        openSet.Enqueue(neighborNode);
                        openSetPositions.Add(neighborPos);
                    }
                }
            }
        }
        
        // 경로를 찾지 못한 경우
        Debug.LogWarning($"경로를 찾을 수 없습니다: {start} -> {end}");
        return null;
    }
    
    /// <summary>
    /// 휴리스틱 함수 (맨해튼 거리)
    /// </summary>
    private static float CalculateHeuristic(Vector2Int from, Vector2Int to)
    {
        return Mathf.Abs(from.x - to.x) + Mathf.Abs(from.y - to.y);
    }
    
    /// <summary>
    /// 두 위치 간의 거리 계산
    /// </summary>
    private static float CalculateDistance(Vector2Int from, Vector2Int to)
    {
        // 4방향 이동만 허용 (상하좌우)
        float dx = Mathf.Abs(from.x - to.x);
        float dy = Mathf.Abs(from.y - to.y);
        
        // 맨해튼 거리 사용 (대각선 이동 없음)
        return dx + dy;
    }
    
    /// <summary>
    /// 이웃 노드들 반환
    /// </summary>
    private static List<Vector2Int> GetNeighbors(Vector2Int position)
    {
        List<Vector2Int> neighbors = new List<Vector2Int>();
        
        // 4방향 이웃 (상하좌우만, 대각선 제외)
        Vector2Int[] directions = {
            new Vector2Int(0, 1),   // 위
            new Vector2Int(1, 0),   // 오른쪽
            new Vector2Int(0, -1),  // 아래
            new Vector2Int(-1, 0)   // 왼쪽
        };
        
        foreach (Vector2Int direction in directions)
        {
            neighbors.Add(position + direction);
        }
        
        return neighbors;
    }
    

    
    /// <summary>
    /// 경로 역추적
    /// </summary>
    private static List<Vector2Int> RetracePath(PathNode endNode)
    {
        List<Vector2Int> path = new List<Vector2Int>();
        PathNode currentNode = endNode;
        
        while (currentNode != null)
        {
            path.Add(currentNode.position);
            currentNode = currentNode.parent;
        }
        
        // 경로를 올바른 순서로 뒤집기
        path.Reverse();
        
        return path;
    }
    
    /// <summary>
    /// 위치가 유효한지 확인 (이동 가능한지)
    /// </summary>
    private static bool IsValidPosition(Vector2Int position, BlockSpawner blockSpawner)
    {
        // BlockSpawner의 IsWalkableAt 메서드 사용 (좌표계 통일)
        return blockSpawner.IsWalkableAt(position.x, position.y);
    }
    
    /// <summary>
    /// 경로 최적화 (불필요한 중간점 제거)
    /// </summary>
    public static List<Vector2Int> OptimizePath(List<Vector2Int> path, BlockSpawner blockSpawner)
    {
        if (path == null || path.Count <= 2)
        {
            return path;
        }
        
        List<Vector2Int> optimizedPath = new List<Vector2Int>();
        optimizedPath.Add(path[0]);
        
        for (int i = 1; i < path.Count - 1; i++)
        {
            Vector2Int prev = path[i - 1];
            Vector2Int current = path[i];
            Vector2Int next = path[i + 1];
            
            // 현재 점이 불필요한지 확인 (직선 경로인지)
            if (!IsDirectPath(prev, next, blockSpawner))
            {
                optimizedPath.Add(current);
            }
        }
        
        optimizedPath.Add(path[path.Count - 1]);
        
        return optimizedPath;
    }
    
    /// <summary>
    /// 두 점 사이에 직선 경로가 있는지 확인
    /// </summary>
    private static bool IsDirectPath(Vector2Int from, Vector2Int to, BlockSpawner blockSpawner)
    {
        // Bresenham 알고리즘을 사용하여 직선 경로 확인
        int dx = Mathf.Abs(to.x - from.x);
        int dy = Mathf.Abs(to.y - from.y);
        int sx = from.x < to.x ? 1 : -1;
        int sy = from.y < to.y ? 1 : -1;
        int err = dx - dy;
        
        int x = from.x;
        int y = from.y;
        
        while (x != to.x || y != to.y)
        {
            // 중간점에 블록이 있는지 확인
            if (blockSpawner.IsBlockAt(x, y))
            {
                return false;
            }
            
            int e2 = 2 * err;
            if (e2 > -dy)
            {
                err -= dy;
                x += sx;
            }
            if (e2 < dx)
            {
                err += dx;
                y += sy;
            }
        }
        
        return true;
    }
}