ENIG/Cs_HMI/AGVMapEditor/Models/PathCalculator.cs

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Linq;

namespace AGVMapEditor.Models
{
    /// <summary>
    /// AGV 전용 경로 계산기 (A* 알고리즘 기반)
    /// AGV의 방향성, 도킹 제약, 회전 제약을 고려한 경로 계산
    /// </summary>
    public class PathCalculator
    {
        #region Constants

        private const float BASE_MOVE_COST = 1.0f;          // 기본 이동 비용
        private const float ROTATION_COST = 0.5f;           // 회전 비용
        private const float DOCKING_APPROACH_COST = 0.2f;   // 도킹 접근 추가 비용
        private const float HEURISTIC_WEIGHT = 1.0f;        // 휴리스틱 가중치

        #endregion

        #region Fields

        private List<MapNode> _mapNodes;
        private NodeResolver _nodeResolver;

        #endregion

        #region Constructor

        /// <summary>
        /// 생성자
        /// </summary>
        /// <param name="mapNodes">맵 노드 목록</param>
        /// <param name="nodeResolver">노드 해결기</param>
        public PathCalculator(List<MapNode> mapNodes, NodeResolver nodeResolver)
        {
            _mapNodes = mapNodes ?? throw new ArgumentNullException(nameof(mapNodes));
            _nodeResolver = nodeResolver ?? throw new ArgumentNullException(nameof(nodeResolver));
        }

        #endregion

        #region Public Methods

        /// <summary>
        /// 경로 계산 (메인 메서드)
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="targetNodeId">목표 노드 ID</param>
        /// <param name="currentDirection">현재 AGV 방향</param>
        /// <returns>경로 계산 결과</returns>
        public PathResult CalculatePath(string startNodeId, string targetNodeId, AgvDirection currentDirection)
        {
            var stopwatch = Stopwatch.StartNew();
            
            try
            {
                // 입력 검증
                var validationResult = ValidateInput(startNodeId, targetNodeId, currentDirection);
                if (!validationResult.Success)
                {
                    stopwatch.Stop();
                    validationResult.CalculationTime = stopwatch.ElapsedMilliseconds;
                    return validationResult;
                }

                // A* 알고리즘 실행
                var result = ExecuteAStar(startNodeId, targetNodeId, currentDirection);
                
                stopwatch.Stop();
                result.CalculationTime = stopwatch.ElapsedMilliseconds;
                
                return result;
            }
            catch (Exception ex)
            {
                stopwatch.Stop();
                return new PathResult($"경로 계산 중 오류 발생: {ex.Message}")
                {
                    CalculationTime = stopwatch.ElapsedMilliseconds
                };
            }
        }

        /// <summary>
        /// 경로 유효성 검증 (RFID 이탈 감지시 사용)
        /// </summary>
        /// <param name="currentPath">현재 경로</param>
        /// <param name="currentRfidId">현재 감지된 RFID</param>
        /// <returns>경로 유효성 여부</returns>
        public bool ValidateCurrentPath(PathResult currentPath, string currentRfidId)
        {
            if (currentPath == null || !currentPath.Success)
                return false;

            var currentNode = _nodeResolver.GetNodeByRfid(currentRfidId);
            if (currentNode == null)
                return false;

            // 현재 노드가 계획된 경로에 포함되어 있는지 확인
            return currentPath.NodeSequence.Contains(currentNode.NodeId);
        }

        /// <summary>
        /// 동적 경로 재계산 (경로 이탈시 사용)
        /// </summary>
        /// <param name="currentRfidId">현재 RFID 위치</param>
        /// <param name="targetNodeId">목표 노드 ID</param>
        /// <param name="currentDirection">현재 방향</param>
        /// <param name="originalPath">원래 경로 (참고용)</param>
        /// <returns>새로운 경로</returns>
        public PathResult RecalculatePath(string currentRfidId, string targetNodeId, 
                                        AgvDirection currentDirection, PathResult originalPath = null)
        {
            var currentNode = _nodeResolver.GetNodeByRfid(currentRfidId);
            if (currentNode == null)
            {
                return new PathResult("현재 위치를 확인할 수 없습니다.");
            }

            // 새로운 경로 계산
            var result = CalculatePath(currentNode.NodeId, targetNodeId, currentDirection);
            
            // 원래 경로와 비교 (로그용)
            if (originalPath != null && result.Success)
            {
                // TODO: 경로 변경 로그 기록
            }

            return result;
        }

        #endregion

        #region Private Methods - Input Validation

        /// <summary>
        /// 입력 값 검증
        /// </summary>
        private PathResult ValidateInput(string startNodeId, string targetNodeId, AgvDirection currentDirection)
        {
            if (string.IsNullOrEmpty(startNodeId))
                return new PathResult("시작 노드가 지정되지 않았습니다.");

            if (string.IsNullOrEmpty(targetNodeId))
                return new PathResult("목표 노드가 지정되지 않았습니다.");

            var startNode = _mapNodes.FirstOrDefault(n => n.NodeId == startNodeId);
            if (startNode == null)
                return new PathResult($"시작 노드를 찾을 수 없습니다: {startNodeId}");

            var targetNode = _mapNodes.FirstOrDefault(n => n.NodeId == targetNodeId);
            if (targetNode == null)
                return new PathResult($"목표 노드를 찾을 수 없습니다: {targetNodeId}");

            if (startNodeId == targetNodeId)
                return new PathResult("시작점과 목표점이 동일합니다.");

            return new PathResult { Success = true };
        }

        #endregion

        #region Private Methods - A* Algorithm

        /// <summary>
        /// A* 알고리즘 실행
        /// </summary>
        private PathResult ExecuteAStar(string startNodeId, string targetNodeId, AgvDirection currentDirection)
        {
            var openSet = new SortedSet<PathNode>();
            var closedSet = new HashSet<string>();
            var gScore = new Dictionary<string, float>();

            // 시작 노드 설정
            var startPathNode = new PathNode(startNodeId, currentDirection)
            {
                GCost = 0,
                HCost = CalculateHeuristic(startNodeId, targetNodeId)
            };

            openSet.Add(startPathNode);
            gScore[startPathNode.GetKey()] = 0;

            while (openSet.Count > 0)
            {
                // 가장 낮은 F 비용을 가진 노드 선택
                var current = openSet.Min;
                openSet.Remove(current);

                // 목표 도달 확인
                if (current.NodeId == targetNodeId)
                {
                    // 도킹 방향 검증
                    if (IsValidDockingApproach(targetNodeId, current.Direction))
                    {
                        return ReconstructPath(current, startNodeId, targetNodeId, currentDirection);
                    }
                    // 도킹 방향이 맞지 않으면 계속 탐색
                }

                closedSet.Add(current.GetKey());

                // 인접 노드들 처리
                ProcessNeighbors(current, targetNodeId, openSet, closedSet, gScore);
            }

            return new PathResult("경로를 찾을 수 없습니다.");
        }

        /// <summary>
        /// 인접 노드들 처리
        /// </summary>
        private void ProcessNeighbors(PathNode current, string targetNodeId,
                                    SortedSet<PathNode> openSet, HashSet<string> closedSet,
                                    Dictionary<string, float> gScore)
        {
            var currentMapNode = _mapNodes.FirstOrDefault(n => n.NodeId == current.NodeId);
            if (currentMapNode == null) return;

            foreach (var neighborId in currentMapNode.ConnectedNodes)
            {
                var neighborMapNode = _mapNodes.FirstOrDefault(n => n.NodeId == neighborId);
                if (neighborMapNode == null) continue;

                // 가능한 모든 방향으로 이웃 노드 방문
                foreach (var direction in GetPossibleDirections(current, neighborMapNode))
                {
                    var neighborPathNode = new PathNode(neighborId, direction);
                    var neighborKey = neighborPathNode.GetKey();

                    if (closedSet.Contains(neighborKey))
                        continue;

                    // 이동 비용 계산
                    var moveCost = CalculateMoveCost(current, neighborPathNode, neighborMapNode);
                    var tentativeGScore = current.GCost + moveCost;

                    // 더 좋은 경로인지 확인
                    if (!gScore.ContainsKey(neighborKey) || tentativeGScore < gScore[neighborKey])
                    {
                        // 경로 정보 업데이트
                        neighborPathNode.Parent = current;
                        neighborPathNode.GCost = tentativeGScore;
                        neighborPathNode.HCost = CalculateHeuristic(neighborId, targetNodeId);
                        neighborPathNode.RotationCount = current.RotationCount + 
                            (current.Direction != direction ? 1 : 0);

                        // 이동 명령 시퀀스 구성
                        neighborPathNode.MovementSequence = GenerateMovementSequence(current, neighborPathNode);

                        gScore[neighborKey] = tentativeGScore;

                        // openSet에 추가 (중복 제거)
                        openSet.RemoveWhere(n => n.GetKey() == neighborKey);
                        openSet.Add(neighborPathNode);
                    }
                }
            }
        }

        /// <summary>
        /// 가능한 방향들 계산
        /// </summary>
        private List<AgvDirection> GetPossibleDirections(PathNode current, MapNode neighborNode)
        {
            var directions = new List<AgvDirection>();

            // 기본적으로 전진/후진 가능
            directions.Add(AgvDirection.Forward);
            directions.Add(AgvDirection.Backward);

            // 회전 가능한 노드에서만 방향 전환 가능
            if (CanRotateAt(current.NodeId))
            {
                // 현재 방향과 다른 방향도 고려
                if (current.Direction == AgvDirection.Forward)
                    directions.Add(AgvDirection.Backward);
                else if (current.Direction == AgvDirection.Backward)
                    directions.Add(AgvDirection.Forward);
            }

            return directions;
        }

        /// <summary>
        /// 이동 비용 계산
        /// </summary>
        private float CalculateMoveCost(PathNode from, PathNode to, MapNode toMapNode)
        {
            float cost = BASE_MOVE_COST;

            // 방향 전환 비용
            if (from.Direction != to.Direction)
            {
                cost += ROTATION_COST;
            }

            // 도킹 스테이션 접근 비용
            if (toMapNode.Type == NodeType.Docking || toMapNode.Type == NodeType.Charging)
            {
                cost += DOCKING_APPROACH_COST;
            }

            // 실제 거리 기반 비용 (좌표가 있는 경우)
            var fromMapNode = _mapNodes.FirstOrDefault(n => n.NodeId == from.NodeId);
            if (fromMapNode != null && toMapNode != null)
            {
                var distance = CalculateDistance(fromMapNode.Position, toMapNode.Position);
                cost *= (distance / 100.0f); // 좌표 단위를 거리 단위로 조정
            }

            return cost;
        }

        /// <summary>
        /// 휴리스틱 함수 (목표까지의 추정 거리)
        /// </summary>
        private float CalculateHeuristic(string fromNodeId, string toNodeId)
        {
            var fromNode = _mapNodes.FirstOrDefault(n => n.NodeId == fromNodeId);
            var toNode = _mapNodes.FirstOrDefault(n => n.NodeId == toNodeId);

            if (fromNode == null || toNode == null)
                return 0;

            // 유클리드 거리 계산
            var distance = CalculateDistance(fromNode.Position, toNode.Position);
            return distance * HEURISTIC_WEIGHT / 100.0f; // 좌표 단위 조정
        }

        /// <summary>
        /// 두 점 사이의 거리 계산
        /// </summary>
        private float CalculateDistance(Point from, Point to)
        {
            var dx = to.X - from.X;
            var dy = to.Y - from.Y;
            return (float)Math.Sqrt(dx * dx + dy * dy);
        }

        /// <summary>
        /// 회전 가능한 위치인지 확인
        /// </summary>
        private bool CanRotateAt(string nodeId)
        {
            var node = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
            return node != null && (node.CanRotate || node.Type == NodeType.Rotation);
        }

        /// <summary>
        /// 도킹 접근 방향이 유효한지 확인
        /// </summary>
        private bool IsValidDockingApproach(string nodeId, AgvDirection approachDirection)
        {
            var node = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
            if (node == null) return true;

            // 도킹/충전 스테이션이 아니면 방향 제약 없음
            if (node.Type != NodeType.Docking && node.Type != NodeType.Charging)
                return true;

            // 도킹 방향 확인
            if (node.DockDirection == null)
                return true;

            // 충전기는 전진으로만, 다른 장비는 후진으로만
            if (node.Type == NodeType.Charging)
                return approachDirection == AgvDirection.Forward;
            else
                return approachDirection == AgvDirection.Backward;
        }

        /// <summary>
        /// 이동 명령 시퀀스 생성
        /// </summary>
        private List<AgvDirection> GenerateMovementSequence(PathNode from, PathNode to)
        {
            var sequence = new List<AgvDirection>();

            // 방향 전환이 필요한 경우
            if (from.Direction != to.Direction)
            {
                if (from.Direction == AgvDirection.Forward && to.Direction == AgvDirection.Backward)
                {
                    sequence.Add(AgvDirection.Right); // 180도 회전 (마크센서까지)
                }
                else if (from.Direction == AgvDirection.Backward && to.Direction == AgvDirection.Forward)
                {
                    sequence.Add(AgvDirection.Right); // 180도 회전 (마크센서까지)
                }
            }

            // 이동 명령
            sequence.Add(to.Direction);

            return sequence;
        }

        /// <summary>
        /// 경로 재구성
        /// </summary>
        private PathResult ReconstructPath(PathNode goalNode, string startNodeId, string targetNodeId, AgvDirection startDirection)
        {
            var path = new List<PathNode>();
            var current = goalNode;

            // 역순으로 경로 구성
            while (current != null)
            {
                path.Insert(0, current);
                current = current.Parent;
            }

            return new PathResult(path, startNodeId, targetNodeId, startDirection);
        }

        #endregion

        #region Public Utility Methods

        /// <summary>
        /// 노드 간 직선 거리 계산
        /// </summary>
        public float GetDistance(string fromNodeId, string toNodeId)
        {
            var fromNode = _mapNodes.FirstOrDefault(n => n.NodeId == fromNodeId);
            var toNode = _mapNodes.FirstOrDefault(n => n.NodeId == toNodeId);

            if (fromNode == null || toNode == null)
                return float.MaxValue;

            return CalculateDistance(fromNode.Position, toNode.Position);
        }

        /// <summary>
        /// 특정 노드에서 가능한 다음 노드들 조회
        /// </summary>
        public List<string> GetPossibleNextNodes(string currentNodeId, AgvDirection currentDirection)
        {
            var currentNode = _mapNodes.FirstOrDefault(n => n.NodeId == currentNodeId);
            if (currentNode == null)
                return new List<string>();

            return currentNode.ConnectedNodes.ToList();
        }

        /// <summary>
        /// 경로 최적화 (선택적 기능)
        /// </summary>
        public PathResult OptimizePath(PathResult originalPath)
        {
            if (originalPath == null || !originalPath.Success)
                return originalPath;

            // TODO: 경로 최적화 로직 구현
            // - 불필요한 중간 정지점 제거
            // - 회전 최소화
            // - 경로 단순화

            return originalPath;
        }

        #endregion
    }
}