ENIG/Cs_HMI/AGVNavigationCore/PathFinding/AdvancedAGVPathfinder.cs

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

namespace AGVNavigationCore.PathFinding
{
    /// <summary>
    /// 고급 AGV 경로 계획기
    /// 물리적 제약사항과 마그넷 센서를 고려한 실제 AGV 경로 생성
    /// </summary>
    public class AdvancedAGVPathfinder
    {
        /// <summary>
        /// 고급 AGV 경로 계산 결과
        /// </summary>
        public class AdvancedPathResult
        {
            public bool Success { get; set; }
            public List<NodeMotorInfo> DetailedPath { get; set; }
            public float TotalDistance { get; set; }
            public long CalculationTimeMs { get; set; }
            public int ExploredNodeCount { get; set; }
            public string ErrorMessage { get; set; }
            public string PlanDescription { get; set; }
            public bool RequiredDirectionChange { get; set; }
            public string DirectionChangeNode { get; set; }

            public AdvancedPathResult()
            {
                DetailedPath = new List<NodeMotorInfo>();
                ErrorMessage = string.Empty;
                PlanDescription = string.Empty;
            }

            public static AdvancedPathResult CreateSuccess(List<NodeMotorInfo> path, float distance, long time, int explored, string description, bool directionChange = false, string changeNode = null)
            {
                return new AdvancedPathResult
                {
                    Success = true,
                    DetailedPath = path,
                    TotalDistance = distance,
                    CalculationTimeMs = time,
                    ExploredNodeCount = explored,
                    PlanDescription = description,
                    RequiredDirectionChange = directionChange,
                    DirectionChangeNode = changeNode
                };
            }

            public static AdvancedPathResult CreateFailure(string error, long time, int explored)
            {
                return new AdvancedPathResult
                {
                    Success = false,
                    ErrorMessage = error,
                    CalculationTimeMs = time,
                    ExploredNodeCount = explored
                };
            }

            /// <summary>
            /// 단순 경로 목록 반환 (호환성용)
            /// </summary>
            public List<string> GetSimplePath()
            {
                return DetailedPath.Select(n => n.NodeId).ToList();
            }
        }

        private readonly List<MapNode> _mapNodes;
        private readonly AStarPathfinder _basicPathfinder;
        private readonly JunctionAnalyzer _junctionAnalyzer;
        private readonly DirectionChangePlanner _directionChangePlanner;

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

        /// <summary>
        /// 고급 AGV 경로 계산
        /// </summary>
        public AdvancedPathResult FindAdvancedPath(string startNodeId, string targetNodeId, AgvDirection currentDirection = AgvDirection.Forward)
        {
            var stopwatch = System.Diagnostics.Stopwatch.StartNew();

            try
            {
                // 1. 목적지 도킹 방향 요구사항 확인
                var requiredDirection = _directionChangePlanner.GetRequiredDockingDirection(targetNodeId);

                // 2. 방향 전환이 필요한지 확인
                bool needDirectionChange = (currentDirection != requiredDirection);

                AdvancedPathResult result;
                if (needDirectionChange)
                {
                    // 방향 전환이 필요한 경우
                    result = PlanPathWithDirectionChange(startNodeId, targetNodeId, currentDirection, requiredDirection);
                }
                else
                {
                    // 직접 경로 계산
                    result = PlanDirectPath(startNodeId, targetNodeId, currentDirection);
                }

                result.CalculationTimeMs = stopwatch.ElapsedMilliseconds;
                return result;
            }
            catch (Exception ex)
            {
                return AdvancedPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
            }
        }

        /// <summary>
        /// 직접 경로 계획
        /// </summary>
        private AdvancedPathResult PlanDirectPath(string startNodeId, string targetNodeId, AgvDirection currentDirection)
        {
            var basicResult = _basicPathfinder.FindPath(startNodeId, targetNodeId);

            if (!basicResult.Success)
            {
                return AdvancedPathResult.CreateFailure(basicResult.ErrorMessage, basicResult.CalculationTimeMs, basicResult.ExploredNodeCount);
            }

            // 기본 경로를 상세 경로로 변환
            var detailedPath = ConvertToDetailedPath(basicResult.Path, currentDirection);

            return AdvancedPathResult.CreateSuccess(
                detailedPath,
                basicResult.TotalDistance,
                basicResult.CalculationTimeMs,
                basicResult.ExploredNodeCount,
                "직접 경로 - 방향 전환 불필요"
            );
        }

        /// <summary>
        /// 방향 전환을 포함한 경로 계획
        /// </summary>
        private AdvancedPathResult PlanPathWithDirectionChange(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
        {
            var directionChangePlan = _directionChangePlanner.PlanDirectionChange(startNodeId, targetNodeId, currentDirection, requiredDirection);

            if (!directionChangePlan.Success)
            {
                return AdvancedPathResult.CreateFailure(directionChangePlan.ErrorMessage, 0, 0);
            }

            // 방향 전환 경로를 상세 경로로 변환
            var detailedPath = ConvertDirectionChangePath(directionChangePlan, currentDirection, requiredDirection);

            // 거리 계산
            float totalDistance = CalculatePathDistance(detailedPath);

            return AdvancedPathResult.CreateSuccess(
                detailedPath,
                totalDistance,
                0,
                0,
                directionChangePlan.PlanDescription,
                true,
                directionChangePlan.DirectionChangeNode
            );
        }

        /// <summary>
        /// 기본 경로를 상세 경로로 변환
        /// </summary>
        private List<NodeMotorInfo> ConvertToDetailedPath(List<string> simplePath, AgvDirection initialDirection)
        {
            var detailedPath = new List<NodeMotorInfo>();
            var currentDirection = initialDirection;

            for (int i = 0; i < simplePath.Count; i++)
            {
                string currentNodeId = simplePath[i];
                string nextNodeId = (i + 1 < simplePath.Count) ? simplePath[i + 1] : null;

                // 마그넷 방향 계산
                MagnetDirection magnetDirection = MagnetDirection.Straight;
                if (i > 0 && nextNodeId != null)
                {
                    string prevNodeId = simplePath[i - 1];
                    magnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, currentNodeId, nextNodeId);
                }

                // 노드 정보 생성
                var nodeMotorInfo = new NodeMotorInfo(
                    currentNodeId,
                    currentDirection,
                    nextNodeId,
                    magnetDirection
                );

                // 회전 가능 노드 설정
                var mapNode = _mapNodes.FirstOrDefault(n => n.NodeId == currentNodeId);
                if (mapNode != null)
                {
                    nodeMotorInfo.CanRotate = mapNode.CanRotate;
                }

                detailedPath.Add(nodeMotorInfo);
            }

            return detailedPath;
        }

        /// <summary>
        /// 방향 전환 경로를 상세 경로로 변환
        /// </summary>
        private List<NodeMotorInfo> ConvertDirectionChangePath(DirectionChangePlanner.DirectionChangePlan plan, AgvDirection startDirection, AgvDirection endDirection)
        {
            var detailedPath = new List<NodeMotorInfo>();
            var currentDirection = startDirection;

            for (int i = 0; i < plan.DirectionChangePath.Count; i++)
            {
                string currentNodeId = plan.DirectionChangePath[i];
                string nextNodeId = (i + 1 < plan.DirectionChangePath.Count) ? plan.DirectionChangePath[i + 1] : null;

                // 방향 전환 노드에서 방향 변경
                if (currentNodeId == plan.DirectionChangeNode && currentDirection != endDirection)
                {
                    currentDirection = endDirection;
                }

                // 마그넷 방향 계산
                MagnetDirection magnetDirection = MagnetDirection.Straight;
                if (i > 0 && nextNodeId != null)
                {
                    string prevNodeId = plan.DirectionChangePath[i - 1];
                    magnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, currentNodeId, nextNodeId);
                }

                // 특수 동작 확인
                bool requiresSpecialAction = false;
                string specialActionDescription = "";

                if (currentNodeId == plan.DirectionChangeNode)
                {
                    requiresSpecialAction = true;
                    specialActionDescription = $"방향전환: {startDirection} → {endDirection}";
                }

                // 노드 정보 생성
                var nodeMotorInfo = new NodeMotorInfo(
                    currentNodeId,
                    currentDirection,
                    nextNodeId,
                    true, // 방향 전환 경로의 경우 회전 가능으로 설정
                    currentNodeId == plan.DirectionChangeNode,
                    magnetDirection,
                    requiresSpecialAction,
                    specialActionDescription
                );

                detailedPath.Add(nodeMotorInfo);
            }

            return detailedPath;
        }

        /// <summary>
        /// 경로 총 거리 계산
        /// </summary>
        private float CalculatePathDistance(List<NodeMotorInfo> detailedPath)
        {
            float totalDistance = 0;

            for (int i = 0; i < detailedPath.Count - 1; i++)
            {
                var currentNode = _mapNodes.FirstOrDefault(n => n.NodeId == detailedPath[i].NodeId);
                var nextNode = _mapNodes.FirstOrDefault(n => n.NodeId == detailedPath[i + 1].NodeId);

                if (currentNode != null && nextNode != null)
                {
                    float dx = nextNode.Position.X - currentNode.Position.X;
                    float dy = nextNode.Position.Y - currentNode.Position.Y;
                    totalDistance += (float)Math.Sqrt(dx * dx + dy * dy);
                }
            }

            return totalDistance;
        }

        /// <summary>
        /// 경로 유효성 검증
        /// </summary>
        public bool ValidatePath(List<NodeMotorInfo> detailedPath)
        {
            if (detailedPath == null || detailedPath.Count == 0)
                return false;

            // 1. 모든 노드가 존재하는지 확인
            foreach (var nodeInfo in detailedPath)
            {
                if (!_mapNodes.Any(n => n.NodeId == nodeInfo.NodeId))
                    return false;
            }

            // 2. 연결성 확인
            for (int i = 0; i < detailedPath.Count - 1; i++)
            {
                string currentId = detailedPath[i].NodeId;
                string nextId = detailedPath[i + 1].NodeId;

                if (!_basicPathfinder.AreNodesConnected(currentId, nextId))
                    return false;
            }

            // 3. 물리적 제약사항 확인
            return ValidatePhysicalConstraints(detailedPath);
        }

        /// <summary>
        /// 물리적 제약사항 검증
        /// </summary>
        private bool ValidatePhysicalConstraints(List<NodeMotorInfo> detailedPath)
        {
            for (int i = 1; i < detailedPath.Count; i++)
            {
                var prevNode = detailedPath[i - 1];
                var currentNode = detailedPath[i];

                // 급작스러운 방향 전환 검증
                if (prevNode.MotorDirection != currentNode.MotorDirection)
                {
                    // 방향 전환은 반드시 회전 가능 노드에서만
                    if (!currentNode.CanRotate && !currentNode.IsDirectionChangePoint)
                    {
                        return false;
                    }
                }
            }

            return true;
        }

        /// <summary>
        /// 경로 최적화
        /// </summary>
        public AdvancedPathResult OptimizePath(AdvancedPathResult originalResult)
        {
            if (!originalResult.Success)
                return originalResult;

            // TODO: 경로 최적화 로직 구현
            // - 불필요한 중간 노드 제거
            // - 마그넷 방향 최적화
            // - 방향 전환 최소화

            return originalResult;
        }

        /// <summary>
        /// 디버깅용 경로 정보
        /// </summary>
        public string GetPathSummary(AdvancedPathResult result)
        {
            if (!result.Success)
                return $"경로 계산 실패: {result.ErrorMessage}";

            var summary = new List<string>
            {
                $"=== AGV 고급 경로 계획 결과 ===",
                $"총 노드 수: {result.DetailedPath.Count}",
                $"총 거리: {result.TotalDistance:F1}px",
                $"계산 시간: {result.CalculationTimeMs}ms",
                $"방향 전환: {(result.RequiredDirectionChange ? $"필요 (노드: {result.DirectionChangeNode})" : "불필요")}",
                $"설명: {result.PlanDescription}",
                "",
                "=== 상세 경로 ===",
            };

            foreach (var nodeInfo in result.DetailedPath)
            {
                summary.Add(nodeInfo.ToString());
            }

            return string.Join("\n", summary);
        }
    }
}