ENIG/Cs_HMI/AGVNavigationCore/PathFinding/JunctionAnalyzer.cs

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

namespace AGVNavigationCore.PathFinding
{
    /// <summary>
    /// AGV 갈림길 분석 및 마그넷 센서 방향 계산 시스템
    /// </summary>
    public class JunctionAnalyzer
    {
        /// <summary>
        /// 갈림길 정보
        /// </summary>
        public class JunctionInfo
        {
            public string NodeId { get; set; }
            public List<string> ConnectedNodes { get; set; }
            public Dictionary<string, MagnetDirection> PathDirections { get; set; }
            public bool IsJunction => ConnectedNodes.Count > 2;

            public JunctionInfo(string nodeId)
            {
                NodeId = nodeId;
                ConnectedNodes = new List<string>();
                PathDirections = new Dictionary<string, MagnetDirection>();
            }

            public override string ToString()
            {
                if (!IsJunction)
                    return $"{NodeId}: 일반노드 ({ConnectedNodes.Count}연결)";

                var paths = string.Join(", ", PathDirections.Select(p => $"{p.Key}({p.Value})"));
                return $"{NodeId}: 갈림길 - {paths}";
            }
        }

        private readonly List<MapNode> _mapNodes;
        private readonly Dictionary<string, JunctionInfo> _junctions;

        public JunctionAnalyzer(List<MapNode> mapNodes)
        {
            _mapNodes = mapNodes ?? new List<MapNode>();
            _junctions = new Dictionary<string, JunctionInfo>();
            AnalyzeJunctions();
        }

        /// <summary>
        /// 모든 갈림길 분석
        /// </summary>
        private void AnalyzeJunctions()
        {
            foreach (var node in _mapNodes)
            {
                if (node.IsNavigationNode())
                {
                    var junctionInfo = AnalyzeNode(node);
                    _junctions[node.NodeId] = junctionInfo;
                }
            }
        }

        /// <summary>
        /// 개별 노드의 갈림길 정보 분석
        /// </summary>
        private JunctionInfo AnalyzeNode(MapNode node)
        {
            var junction = new JunctionInfo(node.NodeId);

            // 양방향 연결을 고려하여 모든 연결된 노드 찾기
            var connectedNodes = GetAllConnectedNodes(node);
            junction.ConnectedNodes = connectedNodes;

            if (connectedNodes.Count > 2)
            {
                // 갈림길인 경우 각 방향별 마그넷 센서 방향 계산
                CalculateMagnetDirections(node, connectedNodes, junction);
            }

            return junction;
        }

        /// <summary>
        /// 양방향 연결을 고려한 모든 연결 노드 검색
        /// </summary>
        private List<string> GetAllConnectedNodes(MapNode node)
        {
            var connected = new HashSet<string>();

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

            // 역방향 연결된 노드들 (다른 노드에서 이 노드로 연결)
            foreach (var otherNode in _mapNodes)
            {
                if (otherNode.NodeId != node.NodeId && otherNode.ConnectedNodes.Contains(node.NodeId))
                {
                    connected.Add(otherNode.NodeId);
                }
            }

            return connected.ToList();
        }

        /// <summary>
        /// 갈림길에서 각 방향별 마그넷 센서 방향 계산
        /// </summary>
        private void CalculateMagnetDirections(MapNode junctionNode, List<string> connectedNodes, JunctionInfo junction)
        {
            if (connectedNodes.Count < 3) return;

            // 각 연결 노드의 각도 계산
            var nodeAngles = new List<(string NodeId, double Angle)>();

            foreach (var connectedId in connectedNodes)
            {
                var connectedNode = _mapNodes.FirstOrDefault(n => n.NodeId == connectedId);
                if (connectedNode != null)
                {
                    double angle = CalculateAngle(junctionNode.Position, connectedNode.Position);
                    nodeAngles.Add((connectedId, angle));
                }
            }

            // 각도순으로 정렬
            nodeAngles.Sort((a, b) => a.Angle.CompareTo(b.Angle));

            // 마그넷 방향 할당
            AssignMagnetDirections(nodeAngles, junction);
        }

        /// <summary>
        /// 두 점 사이의 각도 계산 (라디안)
        /// </summary>
        private double CalculateAngle(Point from, Point to)
        {
            double deltaX = to.X - from.X;
            double deltaY = to.Y - from.Y;
            double angle = Math.Atan2(deltaY, deltaX);

            // 0~2π 범위로 정규화
            if (angle < 0)
                angle += 2 * Math.PI;

            return angle;
        }

        /// <summary>
        /// 갈림길에서 마그넷 센서 방향 할당
        /// </summary>
        private void AssignMagnetDirections(List<(string NodeId, double Angle)> sortedNodes, JunctionInfo junction)
        {
            int nodeCount = sortedNodes.Count;

            for (int i = 0; i < nodeCount; i++)
            {
                string nodeId = sortedNodes[i].NodeId;
                MagnetDirection direction;

                if (nodeCount == 3)
                {
                    // 3갈래: 직진, 좌측, 우측
                    switch (i)
                    {
                        case 0: direction = MagnetDirection.Straight; break;
                        case 1: direction = MagnetDirection.Left; break;
                        case 2: direction = MagnetDirection.Right; break;
                        default: direction = MagnetDirection.Straight; break;
                    }
                }
                else if (nodeCount == 4)
                {
                    // 4갈래: 교차로
                    switch (i)
                    {
                        case 0: direction = MagnetDirection.Straight; break;
                        case 1: direction = MagnetDirection.Left; break;
                        case 2: direction = MagnetDirection.Straight; break;  // 반대편
                        case 3: direction = MagnetDirection.Right; break;
                        default: direction = MagnetDirection.Straight; break;
                    }
                }
                else
                {
                    // 5갈래 이상: 각도 기반 배정
                    double angleStep = 2 * Math.PI / nodeCount;
                    double normalizedIndex = (double)i / nodeCount;

                    if (normalizedIndex < 0.33)
                        direction = MagnetDirection.Left;
                    else if (normalizedIndex < 0.67)
                        direction = MagnetDirection.Straight;
                    else
                        direction = MagnetDirection.Right;
                }

                junction.PathDirections[nodeId] = direction;
            }
        }

        /// <summary>
        /// 특정 경로에서 요구되는 마그넷 방향 계산 (전진 방향 기준)
        /// </summary>
        public MagnetDirection GetRequiredMagnetDirection(string fromNodeId, string currentNodeId, string toNodeId)
        {
            if (!_junctions.ContainsKey(currentNodeId))
                return MagnetDirection.Straight;

            var junction = _junctions[currentNodeId];
            if (!junction.IsJunction)
                return MagnetDirection.Straight;

            // 실제 각도 기반으로 마그넷 방향 계산
            var fromNode = _mapNodes.FirstOrDefault(n => n.NodeId == fromNodeId);
            var currentNode = _mapNodes.FirstOrDefault(n => n.NodeId == currentNodeId);
            var toNode = _mapNodes.FirstOrDefault(n => n.NodeId == toNodeId);

            if (fromNode == null || currentNode == null || toNode == null)
                return MagnetDirection.Straight;

            // 전진 방향(진행 방향) 계산
            double incomingAngle = CalculateAngle(fromNode.Position, currentNode.Position);

            // 목표 방향 계산
            double outgoingAngle = CalculateAngle(currentNode.Position, toNode.Position);

            // 각도 차이 계산 (전진 방향 기준)
            double angleDiff = outgoingAngle - incomingAngle;

            // 각도를 -π ~ π 범위로 정규화
            while (angleDiff > Math.PI) angleDiff -= 2 * Math.PI;
            while (angleDiff < -Math.PI) angleDiff += 2 * Math.PI;

            // 전진 방향 기준으로 마그넷 방향 결정
            // 각도 차이가 작으면 직진, 음수면 왼쪽, 양수면 오른쪽
            if (Math.Abs(angleDiff) < Math.PI / 6)  // 30도 이내는 직진
                return MagnetDirection.Straight;
            else if (angleDiff < 0)  // 음수면 왼쪽 회전
                return MagnetDirection.Left;
            else  // 양수면 오른쪽 회전
                return MagnetDirection.Right;
        }

        /// <summary>
        /// 방향 전환 가능한 갈림길 검색
        /// </summary>
        public List<string> FindDirectionChangeJunctions(AgvDirection currentDirection, AgvDirection targetDirection)
        {
            var availableJunctions = new List<string>();

            if (currentDirection == targetDirection)
                return availableJunctions;

            foreach (var junction in _junctions.Values)
            {
                if (junction.IsJunction)
                {
                    // 갈림길에서 방향 전환이 가능한지 확인
                    // (실제로는 더 복잡한 로직이 필요하지만, 일단 모든 갈림길을 후보로 함)
                    availableJunctions.Add(junction.NodeId);
                }
            }

            return availableJunctions;
        }

        /// <summary>
        /// 갈림길 정보 반환
        /// </summary>
        public JunctionInfo GetJunctionInfo(string nodeId)
        {
            return _junctions.ContainsKey(nodeId) ? _junctions[nodeId] : null;
        }

        /// <summary>
        /// 모든 갈림길 목록 반환
        /// </summary>
        public List<JunctionInfo> GetAllJunctions()
        {
            return _junctions.Values.Where(j => j.IsJunction).ToList();
        }

        /// <summary>
        /// 디버깅용 갈림길 정보 출력
        /// </summary>
        public List<string> GetJunctionSummary()
        {
            var summary = new List<string>();

            foreach (var junction in _junctions.Values.Where(j => j.IsJunction))
            {
                summary.Add(junction.ToString());
            }

            return summary;
        }
    }
}