ENIG/Cs_HMI/AGVNavigationCore/PathFinding/AGVPathfinder.cs

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 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)
        {
            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, stopwatch);
                }
                else
                {
                    return FindNormalPath(startNodeId, endNodeId, targetDirection, 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, System.Diagnostics.Stopwatch stopwatch)
        {
            var 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, System.Diagnostics.Stopwatch stopwatch)
        {
            var requiredDirection = GetRequiredDirectionForNode(endNode);
            var actualTargetDirection = targetDirection ?? requiredDirection;

            var 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;

            for (int i = 0; i < path.Count; i++)
            {
                var currentNodeId = path[i];
                string nextNodeId = i < path.Count - 1 ? path[i + 1] : null;
                
                AgvDirection motorDirection;

                if (i == path.Count - 1)
                {
                    // 마지막 노드: 도킹/충전 노드 타입에 따라 결정
                    if (_nodeMap.ContainsKey(currentNodeId))
                    {
                        var currentNode = _nodeMap[currentNodeId];
                        motorDirection = GetRequiredDirectionForNode(currentNode);
                    }
                    else
                    {
                        motorDirection = AgvDirection.Forward;
                    }
                }
                else
                {
                    // 중간 노드: 다음 노드와의 관계를 고려한 모터방향 결정
                    motorDirection = CalculateMotorDirection(currentNodeId, nextNodeId);
                }

                nodeMotorInfos.Add(new NodeMotorInfo(currentNodeId, motorDirection, nextNodeId));
            }

            return nodeMotorInfos;
        }

        /// <summary>
        /// 현재 노드에서 다음 노드로 이동할 때의 모터방향 계산
        /// </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 currentNode = _nodeMap[currentNodeId];
            var nextNode = _nodeMap[nextNodeId];

            // 현재 노드와 다음 노드의 위치를 기반으로 이동 방향 계산
            var dx = nextNode.Position.X - currentNode.Position.X;
            var dy = nextNode.Position.Y - currentNode.Position.Y;
            var moveAngle = Math.Atan2(dy, dx);

            // AGV의 구조: 리프트 ↔ AGV 몸체 ↔ 모니터
            // 전진: 모니터 방향으로 이동 (리프트에서 멀어짐)
            // 후진: 리프트 방향으로 이동 (리프트에 가까워짐)

            // 다음 노드가 특수 노드인지 확인
            if (nextNode.Type == NodeType.Charging)
            {
                // 충전기: 전진으로 도킹
                return AgvDirection.Forward;
            }
            else if (nextNode.Type == NodeType.Docking)
            {
                // 도킹 스테이션: 후진으로 도킹
                return AgvDirection.Backward;
            }
            else
            {
                // 일반 이동: 기본적으로 전진
                // 향후 경로 패턴 분석을 통해 더 정확한 방향 결정 가능
                return AgvDirection.Forward;
            }
        }

        /// <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;
        }
    }
}