ENIG/Cs_HMI/AGVNavigationCore/PathFinding/AStarPathfinder.cs

using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;

namespace AGVNavigationCore.PathFinding
{
    /// <summary>
    /// A* 알고리즘 기반 경로 탐색기
    /// </summary>
    public class AStarPathfinder
    {
        private Dictionary<string, PathNode> _nodeMap;
        private List<MapNode> _mapNodes;

        /// <summary>
        /// 휴리스틱 가중치 (기본값: 1.0)
        /// 값이 클수록 목적지 방향을 우선시하나 최적 경로를 놓칠 수 있음
        /// </summary>
        public float HeuristicWeight { get; set; } = 1.0f;

        /// <summary>
        /// 최대 탐색 노드 수 (무한 루프 방지)
        /// </summary>
        public int MaxSearchNodes { get; set; } = 1000;

        /// <summary>
        /// 생성자
        /// </summary>
        public AStarPathfinder()
        {
            _nodeMap = new Dictionary<string, PathNode>();
            _mapNodes = new List<MapNode>();
        }

        /// <summary>
        /// 맵 노드 설정
        /// </summary>
        /// <param name="mapNodes">맵 노드 목록</param>
        public void SetMapNodes(List<MapNode> mapNodes)
        {
            _mapNodes = mapNodes ?? new List<MapNode>();
            _nodeMap.Clear();

            // 1단계: 모든 네비게이션 노드를 PathNode로 변환
            foreach (var mapNode in _mapNodes)
            {
                if (mapNode.IsNavigationNode())
                {
                    var pathNode = new PathNode(mapNode.NodeId, mapNode.Position);
                    pathNode.ConnectedNodes = new List<string>(mapNode.ConnectedNodes);
                    _nodeMap[mapNode.NodeId] = pathNode;
                }
            }

            // 2단계: 양방향 연결 자동 생성 (A→B 연결이 있으면 B→A도 추가)
            EnsureBidirectionalConnections();
        }

        /// <summary>
        /// 단방향 연결을 양방향으로 자동 변환
        /// A→B 연결이 있으면 B→A 연결도 자동 생성
        /// </summary>
        private void EnsureBidirectionalConnections()
        {
            foreach (var nodeId in _nodeMap.Keys.ToList())
            {
                var node = _nodeMap[nodeId];
                foreach (var connectedNodeId in node.ConnectedNodes.ToList())
                {
                    // 연결된 노드가 존재하고 네비게이션 가능한 노드인지 확인
                    if (_nodeMap.ContainsKey(connectedNodeId))
                    {
                        var connectedNode = _nodeMap[connectedNodeId];
                        // 역방향 연결이 없으면 추가
                        if (!connectedNode.ConnectedNodes.Contains(nodeId))
                        {
                            connectedNode.ConnectedNodes.Add(nodeId);
                        }
                    }
                }
            }
        }

        /// <summary>
        /// 경로 찾기 (A* 알고리즘)
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="endNodeId">목적지 노드 ID</param>
        /// <returns>경로 계산 결과</returns>
        public PathResult FindPath(string startNodeId, string endNodeId)
        {
            var stopwatch = System.Diagnostics.Stopwatch.StartNew();

            try
            {
                if (!_nodeMap.ContainsKey(startNodeId))
                {
                    return PathResult.CreateFailure($"시작 노드를 찾을 수 없습니다: {startNodeId}", stopwatch.ElapsedMilliseconds, 0);
                }

                if (!_nodeMap.ContainsKey(endNodeId))
                {
                    return PathResult.CreateFailure($"목적지 노드를 찾을 수 없습니다: {endNodeId}", stopwatch.ElapsedMilliseconds, 0);
                }

                if (startNodeId == endNodeId)
                {
                    return PathResult.CreateSuccess(new List<string> { startNodeId }, 0, stopwatch.ElapsedMilliseconds, 1);
                }

                var startNode = _nodeMap[startNodeId];
                var endNode = _nodeMap[endNodeId];

                var openSet = new List<PathNode>();
                var closedSet = new HashSet<string>();
                var exploredCount = 0;

                startNode.GCost = 0;
                startNode.HCost = CalculateHeuristic(startNode, endNode);
                startNode.Parent = null;
                openSet.Add(startNode);

                while (openSet.Count > 0 && exploredCount < MaxSearchNodes)
                {
                    var currentNode = GetLowestFCostNode(openSet);
                    openSet.Remove(currentNode);
                    closedSet.Add(currentNode.NodeId);
                    exploredCount++;

                    if (currentNode.NodeId == endNodeId)
                    {
                        var path = ReconstructPath(currentNode);
                        var totalDistance = CalculatePathDistance(path);
                        return PathResult.CreateSuccess(path, totalDistance, stopwatch.ElapsedMilliseconds, exploredCount);
                    }

                    foreach (var neighborId in currentNode.ConnectedNodes)
                    {
                        if (closedSet.Contains(neighborId) || !_nodeMap.ContainsKey(neighborId))
                            continue;

                        var neighbor = _nodeMap[neighborId];
                        var tentativeGCost = currentNode.GCost + currentNode.DistanceTo(neighbor);

                        if (!openSet.Contains(neighbor))
                        {
                            neighbor.Parent = currentNode;
                            neighbor.GCost = tentativeGCost;
                            neighbor.HCost = CalculateHeuristic(neighbor, endNode);
                            openSet.Add(neighbor);
                        }
                        else if (tentativeGCost < neighbor.GCost)
                        {
                            neighbor.Parent = currentNode;
                            neighbor.GCost = tentativeGCost;
                        }
                    }
                }

                return PathResult.CreateFailure("경로를 찾을 수 없습니다", stopwatch.ElapsedMilliseconds, exploredCount);
            }
            catch (Exception ex)
            {
                return PathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
            }
        }

        /// <summary>
        /// 여러 목적지 중 가장 가까운 노드로의 경로 찾기
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="targetNodeIds">목적지 후보 노드 ID 목록</param>
        /// <returns>경로 계산 결과</returns>
        public PathResult FindNearestPath(string startNodeId, List<string> targetNodeIds)
        {
            if (targetNodeIds == null || targetNodeIds.Count == 0)
            {
                return PathResult.CreateFailure("목적지 노드가 지정되지 않았습니다", 0, 0);
            }

            PathResult bestResult = null;
            foreach (var targetId in targetNodeIds)
            {
                var result = FindPath(startNodeId, targetId);
                if (result.Success && (bestResult == null || result.TotalDistance < bestResult.TotalDistance))
                {
                    bestResult = result;
                }
            }

            return bestResult ?? PathResult.CreateFailure("모든 목적지로의 경로를 찾을 수 없습니다", 0, 0);
        }

        /// <summary>
        /// 휴리스틱 거리 계산 (유클리드 거리)
        /// </summary>
        private float CalculateHeuristic(PathNode from, PathNode to)
        {
            return from.DistanceTo(to) * HeuristicWeight;
        }

        /// <summary>
        /// F cost가 가장 낮은 노드 선택
        /// </summary>
        private PathNode GetLowestFCostNode(List<PathNode> nodes)
        {
            PathNode lowest = nodes[0];
            foreach (var node in nodes)
            {
                if (node.FCost < lowest.FCost || 
                    (Math.Abs(node.FCost - lowest.FCost) < 0.001f && node.HCost < lowest.HCost))
                {
                    lowest = node;
                }
            }
            return lowest;
        }

        /// <summary>
        /// 경로 재구성 (부모 노드를 따라 역추적)
        /// </summary>
        private List<string> ReconstructPath(PathNode endNode)
        {
            var path = new List<string>();
            var current = endNode;

            while (current != null)
            {
                path.Add(current.NodeId);
                current = current.Parent;
            }

            path.Reverse();
            return path;
        }

        /// <summary>
        /// 경로의 총 거리 계산
        /// </summary>
        private float CalculatePathDistance(List<string> path)
        {
            if (path.Count < 2) return 0;

            float totalDistance = 0;
            for (int i = 0; i < path.Count - 1; i++)
            {
                if (_nodeMap.ContainsKey(path[i]) && _nodeMap.ContainsKey(path[i + 1]))
                {
                    totalDistance += _nodeMap[path[i]].DistanceTo(_nodeMap[path[i + 1]]);
                }
            }

            return totalDistance;
        }

        /// <summary>
        /// 두 노드가 연결되어 있는지 확인
        /// </summary>
        /// <param name="nodeId1">노드 1 ID</param>
        /// <param name="nodeId2">노드 2 ID</param>
        /// <returns>연결 여부</returns>
        public bool AreNodesConnected(string nodeId1, string nodeId2)
        {
            if (!_nodeMap.ContainsKey(nodeId1) || !_nodeMap.ContainsKey(nodeId2))
                return false;

            return _nodeMap[nodeId1].ConnectedNodes.Contains(nodeId2);
        }

        /// <summary>
        /// 네비게이션 가능한 노드 목록 반환
        /// </summary>
        /// <returns>노드 ID 목록</returns>
        public List<string> GetNavigationNodes()
        {
            return _nodeMap.Keys.ToList();
        }

        /// <summary>
        /// 노드 정보 반환
        /// </summary>
        /// <param name="nodeId">노드 ID</param>
        /// <returns>노드 정보 또는 null</returns>
        public PathNode GetNode(string nodeId)
        {
            return _nodeMap.ContainsKey(nodeId) ? _nodeMap[nodeId] : null;
        }
    }
}