ENIG/Cs_HMI/AGVNavigationCore/PathFinding/DirectionChangePlanner.cs

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

namespace AGVNavigationCore.PathFinding
{
    /// <summary>
    /// AGV 방향 전환 경로 계획 시스템
    /// 물리적 제약사항을 고려한 방향 전환 경로 생성
    /// </summary>
    public class DirectionChangePlanner
    {
        /// <summary>
        /// 방향 전환 계획 결과
        /// </summary>
        public class DirectionChangePlan
        {
            public bool Success { get; set; }
            public List<string> DirectionChangePath { get; set; }
            public string DirectionChangeNode { get; set; }
            public string ErrorMessage { get; set; }
            public string PlanDescription { get; set; }

            public DirectionChangePlan()
            {
                DirectionChangePath = new List<string>();
                ErrorMessage = string.Empty;
                PlanDescription = string.Empty;
            }

            public static DirectionChangePlan CreateSuccess(List<string> path, string changeNode, string description)
            {
                return new DirectionChangePlan
                {
                    Success = true,
                    DirectionChangePath = path,
                    DirectionChangeNode = changeNode,
                    PlanDescription = description
                };
            }

            public static DirectionChangePlan CreateFailure(string error)
            {
                return new DirectionChangePlan
                {
                    Success = false,
                    ErrorMessage = error
                };
            }
        }

        private readonly List<MapNode> _mapNodes;
        private readonly JunctionAnalyzer _junctionAnalyzer;
        private readonly AStarPathfinder _pathfinder;

        public DirectionChangePlanner(List<MapNode> mapNodes)
        {
            _mapNodes = mapNodes ?? new List<MapNode>();
            _junctionAnalyzer = new JunctionAnalyzer(_mapNodes);
            _pathfinder = new AStarPathfinder();
            _pathfinder.SetMapNodes(_mapNodes);
        }

        /// <summary>
        /// 방향 전환이 필요한 경로 계획
        /// </summary>
        public DirectionChangePlan PlanDirectionChange(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
        {
            // 방향이 같으면 직접 경로 계산
            if (currentDirection == requiredDirection)
            {
                var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
                if (directPath.Success)
                {
                    return DirectionChangePlan.CreateSuccess(
                        directPath.Path,
                        null,
                        "방향 전환 불필요 - 직접 경로 사용"
                    );
                }
            }

            // 방향 전환이 필요한 경우
            return PlanDirectionChangeRoute(startNodeId, targetNodeId, currentDirection, requiredDirection);
        }

        /// <summary>
        /// 방향 전환 경로 계획
        /// </summary>
        private DirectionChangePlan PlanDirectionChangeRoute(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
        {
            // 1. 방향 전환 가능한 갈림길 찾기
            var changeJunctions = FindSuitableChangeJunctions(startNodeId, targetNodeId, currentDirection, requiredDirection);

            if (changeJunctions.Count == 0)
            {
                return DirectionChangePlan.CreateFailure("방향 전환 가능한 갈림길을 찾을 수 없습니다.");
            }

            // 2. 각 갈림길에 대해 경로 계획 시도
            foreach (var junction in changeJunctions)
            {
                var plan = TryDirectionChangeAtJunction(startNodeId, targetNodeId, junction, currentDirection, requiredDirection);
                if (plan.Success)
                {
                    return plan;
                }
            }

            return DirectionChangePlan.CreateFailure("모든 갈림길에서 방향 전환 경로 계획이 실패했습니다.");
        }

        /// <summary>
        /// 방향 전환에 적합한 갈림길 검색
        /// </summary>
        private List<string> FindSuitableChangeJunctions(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
        {
            var suitableJunctions = new List<string>();

            // 시작점과 목표점 사이의 경로에 있는 갈림길들 우선 검색
            var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
            if (directPath.Success)
            {
                foreach (var nodeId in directPath.Path)
                {
                    var junctionInfo = _junctionAnalyzer.GetJunctionInfo(nodeId);
                    if (junctionInfo != null && junctionInfo.IsJunction)
                    {
                        suitableJunctions.Add(nodeId);
                    }
                }
            }

            // 추가로 시작점 주변의 갈림길들도 검색
            var nearbyJunctions = FindNearbyJunctions(startNodeId, 3); // 3단계 내의 갈림길
            foreach (var junction in nearbyJunctions)
            {
                if (!suitableJunctions.Contains(junction))
                {
                    suitableJunctions.Add(junction);
                }
            }

            // 거리순으로 정렬 (시작점에서 가까운 순)
            return SortJunctionsByDistance(startNodeId, suitableJunctions);
        }

        /// <summary>
        /// 특정 노드 주변의 갈림길 검색
        /// </summary>
        private List<string> FindNearbyJunctions(string nodeId, int maxSteps)
        {
            var junctions = new List<string>();
            var visited = new HashSet<string>();
            var queue = new Queue<(string NodeId, int Steps)>();

            queue.Enqueue((nodeId, 0));
            visited.Add(nodeId);

            while (queue.Count > 0)
            {
                var (currentNodeId, steps) = queue.Dequeue();

                if (steps > maxSteps) continue;

                var junctionInfo = _junctionAnalyzer.GetJunctionInfo(currentNodeId);
                if (junctionInfo != null && junctionInfo.IsJunction && currentNodeId != nodeId)
                {
                    junctions.Add(currentNodeId);
                }

                // 연결된 노드들을 큐에 추가
                var connectedNodes = GetAllConnectedNodes(currentNodeId);
                foreach (var connectedId in connectedNodes)
                {
                    if (!visited.Contains(connectedId))
                    {
                        visited.Add(connectedId);
                        queue.Enqueue((connectedId, steps + 1));
                    }
                }
            }

            return junctions;
        }

        /// <summary>
        /// 양방향 연결을 고려한 연결 노드 검색
        /// </summary>
        private List<string> GetAllConnectedNodes(string nodeId)
        {
            var node = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
            if (node == null) return new List<string>();

            var connected = new HashSet<string>();

            // 직접 연결
            foreach (var connectedId in node.ConnectedNodes)
            {
                connected.Add(connectedId);
            }

            // 역방향 연결
            foreach (var otherNode in _mapNodes)
            {
                if (otherNode.NodeId != nodeId && otherNode.ConnectedNodes.Contains(nodeId))
                {
                    connected.Add(otherNode.NodeId);
                }
            }

            return connected.ToList();
        }

        /// <summary>
        /// 갈림길을 거리순으로 정렬
        /// </summary>
        private List<string> SortJunctionsByDistance(string startNodeId, List<string> junctions)
        {
            var distances = new List<(string NodeId, double Distance)>();

            foreach (var junction in junctions)
            {
                var path = _pathfinder.FindPath(startNodeId, junction);
                double distance = path.Success ? path.TotalDistance : double.MaxValue;
                distances.Add((junction, distance));
            }

            return distances.OrderBy(d => d.Distance).Select(d => d.NodeId).ToList();
        }

        /// <summary>
        /// 특정 갈림길에서 방향 전환 시도
        /// </summary>
        private DirectionChangePlan TryDirectionChangeAtJunction(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
        {
            try
            {
                // 방향 전환 경로 생성
                var changePath = GenerateDirectionChangePath(startNodeId, targetNodeId, junctionNodeId, currentDirection, requiredDirection);

                if (changePath.Count > 0)
                {
                    // 실제 방향 전환 노드 찾기 (우회 노드)
                    string actualDirectionChangeNode = FindActualDirectionChangeNode(changePath, junctionNodeId);

                    string description = $"갈림길 {junctionNodeId}를 통해 {actualDirectionChangeNode}에서 방향 전환: {currentDirection} → {requiredDirection}";
                    return DirectionChangePlan.CreateSuccess(changePath, actualDirectionChangeNode, description);
                }

                return DirectionChangePlan.CreateFailure($"갈림길 {junctionNodeId}에서 방향 전환 경로 생성 실패");
            }
            catch (Exception ex)
            {
                return DirectionChangePlan.CreateFailure($"갈림길 {junctionNodeId}에서 오류: {ex.Message}");
            }
        }

        /// <summary>
        /// 방향 전환 경로 생성
        /// </summary>
        private List<string> GenerateDirectionChangePath(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
        {
            var fullPath = new List<string>();

            // 1. 시작점에서 갈림길까지의 경로
            var toJunctionPath = _pathfinder.FindPath(startNodeId, junctionNodeId);
            if (!toJunctionPath.Success)
                return fullPath;

            fullPath.AddRange(toJunctionPath.Path);

            // 2. 갈림길에서 방향 전환 처리
            if (currentDirection != requiredDirection)
            {
                // AGV가 어느 노드에서 갈림길로 왔는지 파악
                string fromNodeId = toJunctionPath.Path.Count >= 2 ?
                    toJunctionPath.Path[toJunctionPath.Path.Count - 2] : startNodeId;

                var changeSequence = GenerateDirectionChangeSequence(junctionNodeId, fromNodeId, currentDirection, requiredDirection);
                if (changeSequence.Count > 1) // 첫 번째는 갈림길 자체이므로 제외
                {
                    fullPath.AddRange(changeSequence.Skip(1));
                }
            }

            // 3. 갈림길에서 목표점까지의 경로
            string lastNode = fullPath.LastOrDefault() ?? junctionNodeId;
            var fromJunctionPath = _pathfinder.FindPath(lastNode, targetNodeId);
            if (fromJunctionPath.Success && fromJunctionPath.Path.Count > 1)
            {
                fullPath.AddRange(fromJunctionPath.Path.Skip(1)); // 중복 노드 제거
            }

            return fullPath;
        }

        /// <summary>
        /// 갈림길에서 방향 전환 시퀀스 생성
        /// 물리적으로 실현 가능한 방향 전환 경로 생성
        /// </summary>
        private List<string> GenerateDirectionChangeSequence(string junctionNodeId, string fromNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
        {
            var sequence = new List<string> { junctionNodeId };

            // 방향이 같으면 변경 불필요
            if (currentDirection == requiredDirection)
                return sequence;

            var junctionInfo = _junctionAnalyzer.GetJunctionInfo(junctionNodeId);
            if (junctionInfo == null || !junctionInfo.IsJunction)
                return sequence;

            // 물리적으로 실현 가능한 방향 전환 시퀀스 생성
            // 핵심 원리: AGV는 RFID 태그를 읽자마자 바로 방향전환하면 안됨
            // 왔던 길로 되돌아가지 않도록 다른 노드로 우회한 후 방향전환
            var connectedNodes = junctionInfo.ConnectedNodes;

            // 왔던 노드(fromNodeId)를 제외한 연결 노드들만 후보로 선택
            // 이렇게 해야 AGV가 되돌아가는 것을 방지할 수 있음
            var availableNodes = connectedNodes.Where(nodeId => nodeId != fromNodeId).ToList();

            if (availableNodes.Count > 0)
            {
                // 방향 전환을 위한 우회 경로 생성
                // 예시: 003→004(전진) 상태에서 후진 필요한 경우
                // 잘못된 방법: 004→003 (왔던 길로 되돌아감)
                // 올바른 방법: 004→005→004 (005로 우회하여 방향전환)

                // 가장 적합한 우회 노드 선택 (직진 방향 우선, 각도 변화 최소)
                string detourNode = FindBestDetourNode(junctionNodeId, availableNodes, fromNodeId);
                if (!string.IsNullOrEmpty(detourNode))
                {
                    // 1단계: 갈림길에서 우회 노드로 이동 (현재 방향 유지)
                    // AGV는 계속 전진하여 한 태그 더 지나감
                    sequence.Add(detourNode);

                    // 2단계: 우회 노드에서 갈림길로 다시 돌아옴 (요구 방향으로 변경)
                    // 이때 AGV는 안전한 위치에서 방향을 전환할 수 있음
                    sequence.Add(junctionNodeId);
                }
            }
            else
            {
                // 사용 가능한 우회 노드가 없는 경우 (2갈래 길목)
                // 이 경우 물리적으로 방향 전환이 불가능할 수 있음
                // 별도의 처리 로직이 필요할 수 있음
                return sequence;
            }

            return sequence;
        }

        /// <summary>
        /// 방향 전환을 위한 최적의 우회 노드 선택
        /// AGV의 물리적 특성을 고려한 각도 기반 선택
        /// </summary>
        private string FindBestDetourNode(string junctionNodeId, List<string> availableNodes, string excludeNodeId)
        {
            // 왔던 길(excludeNodeId)를 제외한 노드 중에서 최적의 우회 노드 선택
            // 우선순위: 1) 직진방향 2) 가장 작은 각도 변화 3) 막다른 길이 아닌 노드

            var junctionNode = _mapNodes.FirstOrDefault(n => n.NodeId == junctionNodeId);
            var fromNode = _mapNodes.FirstOrDefault(n => n.NodeId == excludeNodeId);

            if (junctionNode == null || fromNode == null)
                return availableNodes.FirstOrDefault();

            string bestNode = null;
            double minAngleChange = double.MaxValue;
            bool foundNonDeadEnd = false;

            // AGV가 들어온 방향 벡터 계산 (fromNode → junctionNode)
            double incomingAngle = CalculateAngle(fromNode.Position, junctionNode.Position);

            foreach (var nodeId in availableNodes)
            {
                if (nodeId == excludeNodeId) continue; // 왔던 길 제외

                var candidateNode = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
                if (candidateNode == null) continue;

                // 갈림길에서 후보 노드로의 방향 벡터 계산 (junctionNode → candidateNode)
                double outgoingAngle = CalculateAngle(junctionNode.Position, candidateNode.Position);

                // 방향 변화 각도 계산 (0도가 직진, 180도가 유턴)
                double angleChange = CalculateAngleChange(incomingAngle, outgoingAngle);

                // 막다른 길 여부 확인
                var nodeConnections = GetAllConnectedNodes(nodeId);
                bool isDeadEnd = nodeConnections.Count <= 1;

                // 최적 노드 선택 로직
                bool shouldUpdate = false;

                if (!foundNonDeadEnd && !isDeadEnd)
                {
                    // 첫 번째 막다른 길이 아닌 노드 발견
                    shouldUpdate = true;
                    foundNonDeadEnd = true;
                }
                else if (foundNonDeadEnd && isDeadEnd)
                {
                    // 이미 막다른 길이 아닌 노드를 찾았으므로 막다른 길은 제외
                    continue;
                }
                else if (foundNonDeadEnd == isDeadEnd)
                {
                    // 같은 조건(둘 다 막다른길 or 둘 다 아님)에서는 각도가 작은 것 선택
                    shouldUpdate = angleChange < minAngleChange;
                }

                if (shouldUpdate)
                {
                    minAngleChange = angleChange;
                    bestNode = nodeId;
                }
            }

            return bestNode ?? availableNodes.FirstOrDefault(n => n != excludeNodeId);
        }

        /// <summary>
        /// 두 점 사이의 각도 계산 (라디안 단위)
        /// </summary>
        private double CalculateAngle(System.Drawing.Point from, System.Drawing.Point to)
        {
            double dx = to.X - from.X;
            double dy = to.Y - from.Y;
            return Math.Atan2(dy, dx);
        }

        /// <summary>
        /// 두 방향 사이의 각도 변화량 계산 (0~180도 범위)
        /// 0도에 가까울수록 직진, 180도에 가까울수록 유턴
        /// </summary>
        private double CalculateAngleChange(double fromAngle, double toAngle)
        {
            // 각도 차이 계산
            double angleDiff = Math.Abs(toAngle - fromAngle);

            // 0~π 범위로 정규화 (0~180도)
            if (angleDiff > Math.PI)
            {
                angleDiff = 2 * Math.PI - angleDiff;
            }

            return angleDiff;
        }

        /// <summary>
        /// 실제 방향 전환이 일어나는 노드 찾기
        /// </summary>
        private string FindActualDirectionChangeNode(List<string> changePath, string junctionNodeId)
        {
            // 방향전환 경로 구조: [start...junction, detourNode, junction...target]
            // 실제 방향전환은 detourNode에서 일어남 (AGV가 한 태그 더 지나간 후)

            if (changePath.Count < 3)
                return junctionNodeId; // 기본값으로 갈림길 반환

            // 갈림길이 두 번 나타나는 위치 찾기
            int firstJunctionIndex = changePath.IndexOf(junctionNodeId);
            int lastJunctionIndex = changePath.LastIndexOf(junctionNodeId);

            // 갈림길이 두 번 나타나고, 그 사이에 노드가 있는 경우
            if (firstJunctionIndex != lastJunctionIndex && lastJunctionIndex - firstJunctionIndex == 2)
            {
                // 첫 번째와 두 번째 갈림길 사이에 있는 노드가 실제 방향전환 노드
                string detourNode = changePath[firstJunctionIndex + 1];
                return detourNode;
            }

            // 방향전환 구조를 찾지 못한 경우 기본값 반환
            return junctionNodeId;
        }

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

        /// <summary>
        /// 목적지 도킹 방향 요구사항 확인
        /// </summary>
        public AgvDirection GetRequiredDockingDirection(string targetNodeId)
        {
            var targetNode = _mapNodes.FirstOrDefault(n => n.NodeId == targetNodeId);
            if (targetNode == null)
                return AgvDirection.Forward;

            switch (targetNode.Type)
            {
                case NodeType.Charging:
                    return AgvDirection.Forward; // 충전기는 전진 도킹
                case NodeType.Docking:
                    return AgvDirection.Backward; // 일반 도킹은 후진 도킹
                default:
                    return AgvDirection.Forward; // 기본은 전진
            }
        }

        /// <summary>
        /// 경로 계획 요약 정보
        /// </summary>
        public string GetPlanSummary()
        {
            var junctions = _junctionAnalyzer.GetJunctionSummary();
            return string.Join("\n", junctions);
        }
    }
}