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

namespace AGVNavigationCore.PathFinding
{
    /// <summary>
    /// AGV 특화 경로 탐색기 (방향성 및 도킹 제약 고려)
    /// </summary>
    public class AGVPathfinder
    {
        private AStarPathfinder _pathfinder;
        private Dictionary<string, MapNode> _nodeMap;

        /// <summary>
        /// AGV 현재 방향
        /// </summary>
        public AgvDirection CurrentDirection { get; set; } = AgvDirection.Forward;

        /// <summary>
        /// 회전 비용 가중치 (회전이 비싼 동작임을 반영)
        /// </summary>
        public float RotationCostWeight { get; set; } = 50.0f;

        /// <summary>
        /// 도킹 접근 거리 (픽셀 단위)
        /// </summary>
        public float DockingApproachDistance { get; set; } = 100.0f;

        /// <summary>
        /// 경로 탐색 옵션
        /// </summary>
        public PathfindingOptions Options { get; set; } = PathfindingOptions.Default;

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

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

            foreach (var node in mapNodes ?? new List<MapNode>())
            {
                _nodeMap[node.NodeId] = node;
            }
        }

        /// <summary>
        /// AGV 경로 계산 (방향성 및 도킹 제약 고려)
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="endNodeId">목적지 노드 ID</param>
        /// <param name="targetDirection">목적지 도착 방향 (null이면 자동 결정)</param>
        /// <returns>AGV 경로 계산 결과</returns>
        public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? targetDirection = null)
        {
            return FindAGVPath(startNodeId, endNodeId, targetDirection, Options);
        }

        /// <summary>
        /// AGV 경로 계산 (현재 방향 및 옵션 지정 가능)
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="endNodeId">목적지 노드 ID</param>
        /// <param name="currentDirection">현재 AGV 방향</param>
        /// <param name="targetDirection">목적지 도착 방향 (null이면 자동 결정)</param>
        /// <param name="options">경로 탐색 옵션</param>
        /// <returns>AGV 경로 계산 결과</returns>
        public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? currentDirection, AgvDirection? targetDirection, PathfindingOptions options)
        {
            var result = FindAGVPath(startNodeId, endNodeId, targetDirection, options);
            
            if (!result.Success || currentDirection == null || result.Commands.Count == 0)
                return result;
            
            // 경로의 첫 번째 방향과 현재 방향 비교
            var firstDirection = result.Commands[0];
            
            if (RequiresDirectionChange(currentDirection.Value, firstDirection))
            {
                return InsertDirectionChangeCommands(result, currentDirection.Value, firstDirection);
            }
            
            return result;
        }

        /// <summary>
        /// AGV 경로 계산 (옵션 지정 가능)
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="endNodeId">목적지 노드 ID</param>
        /// <param name="targetDirection">목적지 도착 방향 (null이면 자동 결정)</param>
        /// <param name="options">경로 탐색 옵션</param>
        /// <returns>AGV 경로 계산 결과</returns>
        public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? targetDirection, PathfindingOptions options)
        {
            var stopwatch = System.Diagnostics.Stopwatch.StartNew();

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

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

                var endNode = _nodeMap[endNodeId];

                if (IsSpecialNode(endNode))
                {
                    return FindPathToSpecialNode(startNodeId, endNode, targetDirection, options, stopwatch);
                }
                else
                {
                    return FindNormalPath(startNodeId, endNodeId, targetDirection, options, stopwatch);
                }
            }
            catch (Exception ex)
            {
                return AGVPathResult.CreateFailure($"AGV 경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds);
            }
        }

        /// <summary>
        /// 충전 스테이션으로의 경로 찾기
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <returns>AGV 경로 계산 결과</returns>
        public AGVPathResult FindPathToChargingStation(string startNodeId)
        {
            var chargingStations = _nodeMap.Values
                .Where(n => n.Type == NodeType.Charging && n.IsActive)
                .Select(n => n.NodeId)
                .ToList();

            if (chargingStations.Count == 0)
            {
                return AGVPathResult.CreateFailure("사용 가능한 충전 스테이션이 없습니다", 0);
            }

            var nearestResult = _pathfinder.FindNearestPath(startNodeId, chargingStations);
            if (!nearestResult.Success)
            {
                return AGVPathResult.CreateFailure("충전 스테이션으로의 경로를 찾을 수 없습니다", nearestResult.CalculationTimeMs);
            }

            var targetNodeId = nearestResult.Path.Last();
            return FindAGVPath(startNodeId, targetNodeId, AgvDirection.Forward);
        }

        /// <summary>
        /// 특정 타입의 도킹 스테이션으로의 경로 찾기
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="stationType">장비 타입</param>
        /// <returns>AGV 경로 계산 결과</returns>
        public AGVPathResult FindPathToDockingStation(string startNodeId, StationType stationType)
        {
            var dockingStations = _nodeMap.Values
                .Where(n => n.Type == NodeType.Docking && n.StationType == stationType && n.IsActive)
                .Select(n => n.NodeId)
                .ToList();

            if (dockingStations.Count == 0)
            {
                return AGVPathResult.CreateFailure($"{stationType} 타입의 사용 가능한 도킹 스테이션이 없습니다", 0);
            }

            var nearestResult = _pathfinder.FindNearestPath(startNodeId, dockingStations);
            if (!nearestResult.Success)
            {
                return AGVPathResult.CreateFailure($"{stationType} 도킹 스테이션으로의 경로를 찾을 수 없습니다", nearestResult.CalculationTimeMs);
            }

            var targetNodeId = nearestResult.Path.Last();
            return FindAGVPath(startNodeId, targetNodeId, AgvDirection.Backward);
        }

        /// <summary>
        /// 일반 노드로의 경로 계산
        /// </summary>
        private AGVPathResult FindNormalPath(string startNodeId, string endNodeId, AgvDirection? targetDirection, PathfindingOptions options, System.Diagnostics.Stopwatch stopwatch)
        {
            PathResult result;

            // 회전 회피 옵션이 활성화되어 있으면 회전 노드 회피 시도
            if (options.AvoidRotationNodes)
            {
                result = FindPathAvoidingRotation(startNodeId, endNodeId, options);
                
                // 회전 회피 경로를 찾지 못하면 일반 경로 계산
                if (!result.Success)
                {
                    result = _pathfinder.FindPath(startNodeId, endNodeId);
                }
            }
            else
            {
                result = _pathfinder.FindPath(startNodeId, endNodeId);
            }

            if (!result.Success)
            {
                return AGVPathResult.CreateFailure(result.ErrorMessage, stopwatch.ElapsedMilliseconds);
            }

            var agvCommands = GenerateAGVCommands(result.Path, targetDirection ?? AgvDirection.Forward);
            var nodeMotorInfos = GenerateNodeMotorInfos(result.Path);
            return AGVPathResult.CreateSuccess(result.Path, agvCommands, nodeMotorInfos, result.TotalDistance, stopwatch.ElapsedMilliseconds);
        }

        /// <summary>
        /// 특수 노드(도킹/충전)로의 경로 계산
        /// </summary>
        private AGVPathResult FindPathToSpecialNode(string startNodeId, MapNode endNode, AgvDirection? targetDirection, PathfindingOptions options, System.Diagnostics.Stopwatch stopwatch)
        {
            var requiredDirection = GetRequiredDirectionForNode(endNode);
            var actualTargetDirection = targetDirection ?? requiredDirection;

            PathResult result;

            // 회전 회피 옵션이 활성화되어 있으면 회전 노드 회피 시도
            if (options.AvoidRotationNodes)
            {
                result = FindPathAvoidingRotation(startNodeId, endNode.NodeId, options);
                
                // 회전 회피 경로를 찾지 못하면 일반 경로 계산
                if (!result.Success)
                {
                    result = _pathfinder.FindPath(startNodeId, endNode.NodeId);
                }
            }
            else
            {
                result = _pathfinder.FindPath(startNodeId, endNode.NodeId);
            }

            if (!result.Success)
            {
                return AGVPathResult.CreateFailure(result.ErrorMessage, stopwatch.ElapsedMilliseconds);
            }

            if (actualTargetDirection != requiredDirection)
            {
                return AGVPathResult.CreateFailure($"{endNode.NodeId}는 {requiredDirection} 방향으로만 접근 가능합니다", stopwatch.ElapsedMilliseconds);
            }

            var agvCommands = GenerateAGVCommands(result.Path, actualTargetDirection);
            var nodeMotorInfos = GenerateNodeMotorInfos(result.Path);
            return AGVPathResult.CreateSuccess(result.Path, agvCommands, nodeMotorInfos, result.TotalDistance, stopwatch.ElapsedMilliseconds);
        }

        /// <summary>
        /// 노드가 특수 노드(도킹/충전)인지 확인
        /// </summary>
        private bool IsSpecialNode(MapNode node)
        {
            return node.Type == NodeType.Docking || node.Type == NodeType.Charging;
        }

        /// <summary>
        /// 노드에 필요한 접근 방향 반환
        /// </summary>
        private AgvDirection GetRequiredDirectionForNode(MapNode node)
        {
            switch (node.Type)
            {
                case NodeType.Charging:
                    return AgvDirection.Forward;
                case NodeType.Docking:
                    return node.DockDirection == DockingDirection.Forward ? AgvDirection.Forward : AgvDirection.Backward;
                default:
                    return AgvDirection.Forward;
            }
        }

        /// <summary>
        /// 경로에서 AGV 명령어 생성
        /// </summary>
        private List<AgvDirection> GenerateAGVCommands(List<string> path, AgvDirection targetDirection)
        {
            var commands = new List<AgvDirection>();
            if (path.Count < 2) return commands;

            var currentDir = CurrentDirection;

            for (int i = 0; i < path.Count - 1; i++)
            {
                var currentNodeId = path[i];
                var nextNodeId = path[i + 1];

                if (_nodeMap.ContainsKey(currentNodeId) && _nodeMap.ContainsKey(nextNodeId))
                {
                    var currentNode = _nodeMap[currentNodeId];
                    var nextNode = _nodeMap[nextNodeId];

                    if (currentNode.CanRotate && ShouldRotate(currentDir, targetDirection))
                    {
                        commands.Add(GetRotationCommand(currentDir, targetDirection));
                        currentDir = targetDirection;
                    }

                    commands.Add(currentDir);
                }
            }

            return commands;
        }

        /// <summary>
        /// 회전이 필요한지 판단
        /// </summary>
        private bool ShouldRotate(AgvDirection current, AgvDirection target)
        {
            return current != target && (current == AgvDirection.Forward && target == AgvDirection.Backward ||
                                       current == AgvDirection.Backward && target == AgvDirection.Forward);
        }

        /// <summary>
        /// 회전 명령어 반환
        /// </summary>
        private AgvDirection GetRotationCommand(AgvDirection from, AgvDirection to)
        {
            if (from == AgvDirection.Forward && to == AgvDirection.Backward)
                return AgvDirection.Right;
            if (from == AgvDirection.Backward && to == AgvDirection.Forward)
                return AgvDirection.Right;
            
            return AgvDirection.Right;
        }

        /// <summary>
        /// 노드별 모터방향 정보 생성 (방향 전환 로직 개선)
        /// </summary>
        /// <param name="path">경로 노드 목록</param>
        /// <returns>노드별 모터방향 정보 목록</returns>
        private List<NodeMotorInfo> GenerateNodeMotorInfos(List<string> path)
        {
            var nodeMotorInfos = new List<NodeMotorInfo>();
            if (path.Count < 2) return nodeMotorInfos;

            // 전체 경로에 대한 방향 전환 계획 수립
            var directionPlan = PlanDirectionChanges(path);
            
            for (int i = 0; i < path.Count; i++)
            {
                var currentNodeId = path[i];
                string nextNodeId = i < path.Count - 1 ? path[i + 1] : null;
                
                // 계획된 방향 사용
                var motorDirection = directionPlan.ContainsKey(currentNodeId) 
                    ? directionPlan[currentNodeId] 
                    : AgvDirection.Forward;

                // 노드 특성 정보 수집
                bool canRotate = false;
                bool isDirectionChangePoint = false;
                bool requiresSpecialAction = false;
                string specialActionDescription = "";

                if (_nodeMap.ContainsKey(currentNodeId))
                {
                    var currentNode = _nodeMap[currentNodeId];
                    canRotate = currentNode.CanRotate;
                    
                    // 방향 전환 감지
                    if (i > 0 && directionPlan.ContainsKey(path[i - 1]))
                    {
                        var prevDirection = directionPlan[path[i - 1]];
                        isDirectionChangePoint = prevDirection != motorDirection;
                    }
                    
                    // 특수 동작 필요 여부 감지
                    if (!canRotate && isDirectionChangePoint)
                    {
                        requiresSpecialAction = true;
                        specialActionDescription = "갈림길 전진/후진 반복";
                    }
                    else if (canRotate && isDirectionChangePoint)
                    {
                        specialActionDescription = "회전 노드 방향전환";
                    }
                }

                var nodeMotorInfo = new NodeMotorInfo(currentNodeId, motorDirection, nextNodeId,
                    canRotate, isDirectionChangePoint, MagnetDirection.Straight, requiresSpecialAction, specialActionDescription);
                nodeMotorInfos.Add(nodeMotorInfo);
            }

            return nodeMotorInfos;
        }

        /// <summary>
        /// 경로 전체에 대한 방향 전환 계획 수립
        /// </summary>
        /// <param name="path">경로 노드 목록</param>
        /// <returns>노드별 모터 방향 계획</returns>
        private Dictionary<string, AgvDirection> PlanDirectionChanges(List<string> path)
        {
            var directionPlan = new Dictionary<string, AgvDirection>();
            if (path.Count < 2) return directionPlan;

            // 1단계: 목적지 노드의 요구사항 분석
            var targetNodeId = path[path.Count - 1];
            var targetDirection = AgvDirection.Forward;
            
            if (_nodeMap.ContainsKey(targetNodeId))
            {
                var targetNode = _nodeMap[targetNodeId];
                targetDirection = GetRequiredDirectionForNode(targetNode);
            }

            // 2단계: 역방향으로 방향 전환점 찾기
            var currentDirection = targetDirection;
            directionPlan[targetNodeId] = currentDirection;

            // 마지막에서 두 번째부터 역순으로 처리
            for (int i = path.Count - 2; i >= 0; i--)
            {
                var currentNodeId = path[i];
                var nextNodeId = path[i + 1];
                
                if (_nodeMap.ContainsKey(currentNodeId))
                {
                    var currentNode = _nodeMap[currentNodeId];
                    
                    // 방법1: 회전 가능 노드에서 방향 전환
                    if (currentNode.CanRotate && NeedDirectionChange(currentNodeId, nextNodeId, currentDirection))
                    {
                        // 회전 가능한 노드에서 방향 전환 수행
                        var optimalDirection = CalculateOptimalDirection(currentNodeId, nextNodeId, path, i);
                        directionPlan[currentNodeId] = optimalDirection;
                        currentDirection = optimalDirection;
                    }
                    else
                    {
                        // 일반 노드: 연속성 유지
                        directionPlan[currentNodeId] = currentDirection;
                    }
                }
                else
                {
                    directionPlan[currentNodeId] = currentDirection;
                }
            }

            // 3단계: 방법2 적용 - 불가능한 방향 전환 감지 및 수정
            ApplyDirectionChangeCorrection(path, directionPlan);

            return directionPlan;
        }

        /// <summary>
        /// 방향 전환이 필요한지 판단
        /// </summary>
        private bool NeedDirectionChange(string currentNodeId, string nextNodeId, AgvDirection currentDirection)
        {
            if (!_nodeMap.ContainsKey(nextNodeId)) return false;
            
            var nextNode = _nodeMap[nextNodeId];
            var requiredDirection = GetRequiredDirectionForNode(nextNode);
            
            return currentDirection != requiredDirection;
        }

        /// <summary>
        /// 회전 가능 노드에서 최적 방향 계산
        /// </summary>
        private AgvDirection CalculateOptimalDirection(string nodeId, string nextNodeId, List<string> path, int nodeIndex)
        {
            if (!_nodeMap.ContainsKey(nextNodeId)) return AgvDirection.Forward;
            
            var nextNode = _nodeMap[nextNodeId];
            
            // 목적지까지의 경로를 고려한 최적 방향 결정
            if (nextNode.Type == NodeType.Charging)
            {
                return AgvDirection.Forward; // 충전기는 전진 접근
            }
            else if (nextNode.Type == NodeType.Docking)
            {
                return AgvDirection.Backward; // 도킹은 후진 접근
            }
            else
            {
                // 경로 패턴 분석을 통한 방향 결정
                return AnalyzePathPattern(path, nodeIndex);
            }
        }

        /// <summary>
        /// 경로 패턴 분석을 통한 방향 결정
        /// </summary>
        private AgvDirection AnalyzePathPattern(List<string> path, int startIndex)
        {
            // 남은 경로에서 도킹/충전 스테이션이 있는지 확인
            for (int i = startIndex + 1; i < path.Count; i++)
            {
                if (_nodeMap.ContainsKey(path[i]))
                {
                    var node = _nodeMap[path[i]];
                    if (node.Type == NodeType.Docking)
                    {
                        return AgvDirection.Backward; // 도킹 준비를 위해 후진 방향
                    }
                    else if (node.Type == NodeType.Charging)
                    {
                        return AgvDirection.Forward; // 충전 준비를 위해 전진 방향
                    }
                }
            }
            
            return AgvDirection.Forward; // 기본값
        }

        /// <summary>
        /// 방법2: 불가능한 방향 전환 감지 및 보정 (갈림길 전진/후진 반복)
        /// </summary>
        private void ApplyDirectionChangeCorrection(List<string> path, Dictionary<string, AgvDirection> directionPlan)
        {
            for (int i = 0; i < path.Count - 1; i++)
            {
                var currentNodeId = path[i];
                var nextNodeId = path[i + 1];
                
                if (directionPlan.ContainsKey(currentNodeId) && directionPlan.ContainsKey(nextNodeId))
                {
                    var currentDir = directionPlan[currentNodeId];
                    var nextDir = directionPlan[nextNodeId];
                    
                    // 급격한 방향 전환 감지 (전진→후진, 후진→전진)
                    if (IsImpossibleDirectionChange(currentDir, nextDir))
                    {
                        var currentNode = _nodeMap.ContainsKey(currentNodeId) ? _nodeMap[currentNodeId] : null;
                        
                        if (currentNode != null && !currentNode.CanRotate)
                        {
                            // 회전 불가능한 노드에서 방향 전환 시도 → 특수 동작 추가
                            AddTurnAroundSequence(currentNodeId, directionPlan);
                        }
                    }
                }
            }
        }

        /// <summary>
        /// 불가능한 방향 전환인지 확인
        /// </summary>
        private bool IsImpossibleDirectionChange(AgvDirection current, AgvDirection next)
        {
            return (current == AgvDirection.Forward && next == AgvDirection.Backward) ||
                   (current == AgvDirection.Backward && next == AgvDirection.Forward);
        }

        /// <summary>
        /// 회전 불가능한 노드에서 방향 전환을 위한 특수 동작 시퀀스 추가
        /// </summary>
        private void AddTurnAroundSequence(string nodeId, Dictionary<string, AgvDirection> directionPlan)
        {
            // 갈림길에서 전진/후진 반복 작업으로 방향 변경
            // 실제 구현시에는 NodeMotorInfo에 특수 플래그나 추가 동작 정보 포함 필요
            // 현재는 안전한 방향으로 보정
            if (directionPlan.ContainsKey(nodeId))
            {
                // 일단 연속성을 유지하도록 보정 (실제로는 특수 회전 동작 필요)
                directionPlan[nodeId] = AgvDirection.Forward;
            }
        }

        /// <summary>
        /// 현재 노드에서 다음 노드로 이동할 때의 모터방향 계산 (레거시 - 새로운 PlanDirectionChanges 사용)
        /// </summary>
        /// <param name="currentNodeId">현재 노드 ID</param>
        /// <param name="nextNodeId">다음 노드 ID</param>
        /// <returns>모터방향</returns>
        private AgvDirection CalculateMotorDirection(string currentNodeId, string nextNodeId)
        {
            if (!_nodeMap.ContainsKey(currentNodeId) || !_nodeMap.ContainsKey(nextNodeId))
            {
                return AgvDirection.Forward;
            }

            var nextNode = _nodeMap[nextNodeId];

            // 다음 노드가 특수 노드인지 확인
            if (nextNode.Type == NodeType.Charging)
            {
                // 충전기: 전진으로 도킹
                return AgvDirection.Forward;
            }
            else if (nextNode.Type == NodeType.Docking)
            {
                // 도킹 스테이션: 후진으로 도킹
                return AgvDirection.Backward;
            }
            else
            {
                // 일반 이동: 기본적으로 전진 (실제로는 PlanDirectionChanges에서 결정됨)
                return AgvDirection.Forward;
            }
        }

        /// <summary>
        /// 회전 노드를 회피하는 경로 탐색
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="endNodeId">목적지 노드 ID</param>
        /// <param name="options">경로 탐색 옵션</param>
        /// <returns>회전 회피 경로 결과</returns>
        private PathResult FindPathAvoidingRotation(string startNodeId, string endNodeId, PathfindingOptions options)
        {
            try
            {
                // 회전 가능한 노드들을 필터링하여 임시로 비활성화
                var rotationNodes = _nodeMap.Values.Where(n => n.CanRotate).Select(n => n.NodeId).ToList();
                var originalConnections = new Dictionary<string, List<string>>();

                // 시작과 끝 노드가 회전 노드인 경우는 제외
                var nodesToDisable = rotationNodes.Where(nodeId => nodeId != startNodeId && nodeId != endNodeId).ToList();

                foreach (var nodeId in nodesToDisable)
                {
                    // 임시로 연결 해제 (실제로는 pathfinder에서 해당 노드를 높은 비용으로 설정해야 함)
                    originalConnections[nodeId] = _nodeMap[nodeId].ConnectedNodes.ToList();
                }

                // 회전 노드 회피 경로 계산 시도
                var result = _pathfinder.FindPath(startNodeId, endNodeId);
                
                // 결과에서 회전 노드가 포함되어 있으면 실패로 간주
                if (result.Success && result.Path != null)
                {
                    var pathContainsRotation = result.Path.Any(nodeId => nodesToDisable.Contains(nodeId));
                    if (pathContainsRotation)
                    {
                        return PathResult.CreateFailure("회전 노드를 회피하는 경로를 찾을 수 없습니다.", 0, 0);
                    }
                }

                return result;
            }
            catch (Exception ex)
            {
                return PathResult.CreateFailure($"회전 회피 경로 계산 중 오류: {ex.Message}", 0, 0);
            }
        }

        /// <summary>
        /// 경로 유효성 검증
        /// </summary>
        /// <param name="path">검증할 경로</param>
        /// <returns>유효성 검증 결과</returns>
        public bool ValidatePath(List<string> path)
        {
            if (path == null || path.Count < 2) return true;

            for (int i = 0; i < path.Count - 1; i++)
            {
                if (!_pathfinder.AreNodesConnected(path[i], path[i + 1]))
                {
                    return false;
                }
            }

            return true;
        }

        /// <summary>
        /// 현재 방향에서 목표 방향으로 변경하기 위해 방향 전환이 필요한지 판단
        /// </summary>
        /// <param name="currentDirection">현재 방향</param>
        /// <param name="targetDirection">목표 방향</param>
        /// <returns>방향 전환 필요 여부</returns>
        private bool RequiresDirectionChange(AgvDirection currentDirection, AgvDirection targetDirection)
        {
            // 같은 방향이면 변경 불필요
            if (currentDirection == targetDirection)
                return false;

            // 전진 <-> 후진 전환은 방향 전환 필요
            return (currentDirection == AgvDirection.Forward && targetDirection == AgvDirection.Backward) ||
                   (currentDirection == AgvDirection.Backward && targetDirection == AgvDirection.Forward);
        }

        /// <summary>
        /// 방향 전환을 위한 명령어를 기존 경로에 삽입
        /// </summary>
        /// <param name="originalResult">원래 경로 결과</param>
        /// <param name="currentDirection">현재 AGV 방향</param>
        /// <param name="targetDirection">목표 방향</param>
        /// <returns>방향 전환 명령이 포함된 경로 결과</returns>
        private AGVPathResult InsertDirectionChangeCommands(AGVPathResult originalResult, AgvDirection currentDirection, AgvDirection targetDirection)
        {
            if (originalResult.Path.Count < 1)
                return originalResult;

            // 시작 노드를 찾아서 회전 가능한지 확인
            var startNodeId = originalResult.Path[0];
            if (!_nodeMap.ContainsKey(startNodeId))
                return originalResult;

            var startNode = _nodeMap[startNodeId];
            
            // 시작 노드에서 회전이 불가능하면 회전 가능한 가까운 노드 찾기
            if (!startNode.CanRotate)
            {
                var rotationNode = FindNearestRotationNode(startNodeId);
                if (rotationNode == null)
                {
                    return AGVPathResult.CreateFailure("방향 전환을 위한 회전 가능 노드를 찾을 수 없습니다.", originalResult.CalculationTimeMs);
                }

                // 회전 노드로의 경로 추가
                var pathToRotationNode = _pathfinder.FindPath(startNodeId, rotationNode.NodeId);
                if (!pathToRotationNode.Success)
                {
                    return AGVPathResult.CreateFailure("방향 전환 노드로의 경로를 찾을 수 없습니다.", originalResult.CalculationTimeMs);
                }

                // 회전 노드에서 원래 목적지로의 경로 계산
                var pathFromRotationNode = _pathfinder.FindPath(rotationNode.NodeId, originalResult.Path.Last());
                if (!pathFromRotationNode.Success)
                {
                    return AGVPathResult.CreateFailure("방향 전환 후 목적지로의 경로를 찾을 수 없습니다.", originalResult.CalculationTimeMs);
                }

                // 전체 경로 조합
                var combinedPath = new List<string>();
                combinedPath.AddRange(pathToRotationNode.Path);
                combinedPath.AddRange(pathFromRotationNode.Path.Skip(1)); // 중복 노드 제거

                var combinedDistance = pathToRotationNode.TotalDistance + pathFromRotationNode.TotalDistance;
                var combinedCommands = GenerateAGVCommandsWithDirectionChange(combinedPath, currentDirection, targetDirection, rotationNode.NodeId);
                var nodeMotorInfos = GenerateNodeMotorInfos(combinedPath);

                return AGVPathResult.CreateSuccess(combinedPath, combinedCommands, nodeMotorInfos, combinedDistance, originalResult.CalculationTimeMs);
            }
            else
            {
                // 시작 노드에서 바로 방향 전환
                var commandsWithRotation = GenerateAGVCommandsWithDirectionChange(originalResult.Path, currentDirection, targetDirection, startNodeId);
                return AGVPathResult.CreateSuccess(originalResult.Path, commandsWithRotation, originalResult.NodeMotorInfos, originalResult.TotalDistance, originalResult.CalculationTimeMs);
            }
        }

        /// <summary>
        /// 가장 가까운 회전 가능 노드 찾기
        /// </summary>
        /// <param name="fromNodeId">시작 노드 ID</param>
        /// <returns>가장 가까운 회전 가능 노드</returns>
        private MapNode FindNearestRotationNode(string fromNodeId)
        {
            var rotationNodes = _nodeMap.Values.Where(n => n.CanRotate && n.IsActive).ToList();
            
            MapNode nearestNode = null;
            var shortestDistance = float.MaxValue;

            foreach (var rotationNode in rotationNodes)
            {
                var pathResult = _pathfinder.FindPath(fromNodeId, rotationNode.NodeId);
                if (pathResult.Success && pathResult.TotalDistance < shortestDistance)
                {
                    shortestDistance = pathResult.TotalDistance;
                    nearestNode = rotationNode;
                }
            }

            return nearestNode;
        }

        /// <summary>
        /// 방향 전환을 포함한 AGV 명령어 생성
        /// </summary>
        /// <param name="path">경로</param>
        /// <param name="currentDirection">현재 방향</param>
        /// <param name="targetDirection">목표 방향</param>
        /// <param name="rotationNodeId">방향 전환 노드 ID</param>
        /// <returns>AGV 명령어 목록</returns>
        private List<AgvDirection> GenerateAGVCommandsWithDirectionChange(List<string> path, AgvDirection currentDirection, AgvDirection targetDirection, string rotationNodeId)
        {
            var commands = new List<AgvDirection>();
            
            int rotationIndex = path.IndexOf(rotationNodeId);
            
            // 회전 노드까지는 현재 방향으로 이동
            for (int i = 0; i < rotationIndex; i++)
            {
                commands.Add(currentDirection);
            }
            
            // 회전 명령 추가 (전진->후진 또는 후진->전진)
            if (currentDirection == AgvDirection.Forward && targetDirection == AgvDirection.Backward)
            {
                commands.Add(AgvDirection.Left);
                commands.Add(AgvDirection.Left);  // 180도 회전
            }
            else if (currentDirection == AgvDirection.Backward && targetDirection == AgvDirection.Forward)
            {
                commands.Add(AgvDirection.Right);
                commands.Add(AgvDirection.Right); // 180도 회전
            }
            
            // 회전 노드부터 목적지까지는 목표 방향으로 이동
            for (int i = rotationIndex; i < path.Count - 1; i++)
            {
                commands.Add(targetDirection);
            }
            
            return commands;
        }
    }
}