ENIG/Cs_HMI/AGVMapEditor/Models/PathCalculator.cs

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

namespace AGVMapEditor.Models
{
    /// <summary>
    /// AGV 전용 경로 계산기 (AGVNavigationCore 래퍼)
    /// AGVMapEditor와 AGVNavigationCore 간의 호환성 제공
    /// RFID 기반 경로 계산을 우선 사용
    /// </summary>
    public class PathCalculator
    {
        private AGVPathfinder _agvPathfinder;
        private AStarPathfinder _astarPathfinder;
        private RfidBasedPathfinder _rfidPathfinder;

        /// <summary>
        /// 생성자
        /// </summary>
        public PathCalculator()
        {
            _agvPathfinder = new AGVPathfinder();
            _astarPathfinder = new AStarPathfinder();
            _rfidPathfinder = new RfidBasedPathfinder();
        }

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

        /// <summary>
        /// 맵 데이터 설정
        /// </summary>
        /// <param name="mapNodes">맵 노드 목록</param>
        public void SetMapData(List<MapNode> mapNodes)
        {
            _agvPathfinder.SetMapNodes(mapNodes);
            _astarPathfinder.SetMapNodes(mapNodes);
            // RfidPathfinder는 MapNode의 RFID 정보를 직접 사용
            _rfidPathfinder.SetMapNodes(mapNodes);
            // 도킹 조건 검색용 내부 노드 목록 업데이트
            UpdateInternalMapNodes(mapNodes);
        }

        /// <summary>
        /// AGV 경로 계산
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="endNodeId">목적지 노드 ID</param>
        /// <param name="targetDirection">목적지 도착 방향</param>
        /// <returns>AGV 경로 계산 결과</returns>
        public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? targetDirection = null)
        {
            return _agvPathfinder.FindAGVPath(startNodeId, endNodeId, targetDirection);
        }

        /// <summary>
        /// AGV 경로 계산 (옵션 지정 가능)
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="endNodeId">목적지 노드 ID</param>
        /// <param name="targetDirection">목적지 도착 방향</param>
        /// <param name="options">경로 탐색 옵션</param>
        /// <returns>AGV 경로 계산 결과</returns>
        public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? targetDirection, PathfindingOptions options)
        {
            return _agvPathfinder.FindAGVPath(startNodeId, endNodeId, targetDirection, options);
        }

        /// <summary>
        /// AGV 경로 계산 (현재 방향 및 PathfindingOptions 지원)
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="endNodeId">목적지 노드 ID</param>
        /// <param name="currentDirection">현재 AGV 방향</param>
        /// <param name="targetDirection">목적지 도착 방향</param>
        /// <param name="options">경로 탐색 옵션</param>
        /// <returns>AGV 경로 계산 결과</returns>
        public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? currentDirection, AgvDirection? targetDirection, PathfindingOptions options)
        {
            return _agvPathfinder.FindAGVPath(startNodeId, endNodeId, currentDirection, targetDirection, options);
        }

        /// <summary>
        /// 충전 스테이션으로의 경로 찾기
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <returns>AGV 경로 계산 결과</returns>
        public AGVPathResult FindPathToChargingStation(string startNodeId)
        {
            return _agvPathfinder.FindPathToChargingStation(startNodeId);
        }

        /// <summary>
        /// 도킹 스테이션으로의 경로 찾기
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="stationType">장비 타입</param>
        /// <returns>AGV 경로 계산 결과</returns>
        public AGVPathResult FindPathToDockingStation(string startNodeId, StationType stationType)
        {
            return _agvPathfinder.FindPathToDockingStation(startNodeId, stationType);
        }

        /// <summary>
        /// 여러 목적지 중 가장 가까운 노드로의 경로 찾기
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="targetNodeIds">목적지 후보 노드 ID 목록</param>
        /// <returns>경로 계산 결과</returns>
        public PathResult FindNearestPath(string startNodeId, List<string> targetNodeIds)
        {
            return _astarPathfinder.FindNearestPath(startNodeId, targetNodeIds);
        }

        /// <summary>
        /// 두 노드가 연결되어 있는지 확인
        /// </summary>
        /// <param name="nodeId1">노드 1 ID</param>
        /// <param name="nodeId2">노드 2 ID</param>
        /// <returns>연결 여부</returns>
        public bool AreNodesConnected(string nodeId1, string nodeId2)
        {
            return _astarPathfinder.AreNodesConnected(nodeId1, nodeId2);
        }

        /// <summary>
        /// 경로 유효성 검증
        /// </summary>
        /// <param name="path">검증할 경로</param>
        /// <returns>유효성 검증 결과</returns>
        public bool ValidatePath(List<string> path)
        {
            return _agvPathfinder.ValidatePath(path);
        }

        /// <summary>
        /// 네비게이션 가능한 노드 목록 반환
        /// </summary>
        /// <returns>노드 ID 목록</returns>
        public List<string> GetNavigationNodes()
        {
            return _astarPathfinder.GetNavigationNodes();
        }

        /// <summary>
        /// AGV 현재 방향 설정
        /// </summary>
        /// <param name="direction">현재 방향</param>
        public void SetCurrentDirection(AgvDirection direction)
        {
            _agvPathfinder.CurrentDirection = direction;
        }

        /// <summary>
        /// 회전 비용 가중치 설정
        /// </summary>
        /// <param name="weight">회전 비용 가중치</param>
        public void SetRotationCostWeight(float weight)
        {
            _agvPathfinder.RotationCostWeight = weight;
        }

        /// <summary>
        /// 휴리스틱 가중치 설정
        /// </summary>
        /// <param name="weight">휴리스틱 가중치</param>
        public void SetHeuristicWeight(float weight)
        {
            _astarPathfinder.HeuristicWeight = weight;
        }

        /// <summary>
        /// 최대 탐색 노드 수 설정
        /// </summary>
        /// <param name="maxNodes">최대 탐색 노드 수</param>
        public void SetMaxSearchNodes(int maxNodes)
        {
            _astarPathfinder.MaxSearchNodes = maxNodes;
        }

        // ==================== RFID 기반 경로 계산 메서드들 ====================

        /// <summary>
        /// RFID 기반 AGV 경로 계산
        /// </summary>
        /// <param name="startRfidId">시작 RFID</param>
        /// <param name="endRfidId">목적지 RFID</param>
        /// <param name="targetDirection">목적지 도착 방향</param>
        /// <returns>RFID 기반 AGV 경로 계산 결과</returns>
        public RfidPathResult FindAGVPathByRfid(string startRfidId, string endRfidId, AgvDirection? targetDirection = null)
        {
            return _rfidPathfinder.FindAGVPath(startRfidId, endRfidId, targetDirection);
        }

        /// <summary>
        /// RFID 기반 충전소 경로 찾기
        /// </summary>
        /// <param name="startRfidId">시작 RFID</param>
        /// <returns>RFID 기반 경로 계산 결과</returns>
        public RfidPathResult FindPathToChargingStationByRfid(string startRfidId)
        {
            return _rfidPathfinder.FindPathToChargingStation(startRfidId);
        }

        /// <summary>
        /// RFID 기반 도킹 스테이션 경로 찾기
        /// </summary>
        /// <param name="startRfidId">시작 RFID</param>
        /// <param name="stationType">장비 타입</param>
        /// <returns>RFID 기반 경로 계산 결과</returns>
        public RfidPathResult FindPathToDockingStationByRfid(string startRfidId, StationType stationType)
        {
            return _rfidPathfinder.FindPathToDockingStation(startRfidId, stationType);
        }

        /// <summary>
        /// 여러 RFID 목적지 중 가장 가까운 곳으로의 경로 찾기
        /// </summary>
        /// <param name="startRfidId">시작 RFID</param>
        /// <param name="targetRfidIds">목적지 후보 RFID 목록</param>
        /// <returns>RFID 기반 경로 계산 결과</returns>
        public RfidPathResult FindNearestPathByRfid(string startRfidId, List<string> targetRfidIds)
        {
            return _rfidPathfinder.FindNearestPath(startRfidId, targetRfidIds);
        }

        /// <summary>
        /// RFID 매핑 정보 조회 (MapNode 반환)
        /// </summary>
        /// <param name="rfidId">RFID</param>
        /// <returns>MapNode 또는 null</returns>
        public MapNode GetRfidMapping(string rfidId)
        {
            return _rfidPathfinder.GetRfidMapping(rfidId);
        }

        /// <summary>
        /// RFID로 NodeId 조회
        /// </summary>
        /// <param name="rfidId">RFID</param>
        /// <returns>NodeId 또는 null</returns>
        public string GetNodeIdByRfid(string rfidId)
        {
            return _rfidPathfinder.GetNodeIdByRfid(rfidId);
        }

        /// <summary>
        /// NodeId로 RFID 조회
        /// </summary>
        /// <param name="nodeId">NodeId</param>
        /// <returns>RFID 또는 null</returns>
        public string GetRfidByNodeId(string nodeId)
        {
            return _rfidPathfinder.GetRfidByNodeId(nodeId);
        }

        /// <summary>
        /// 활성화된 RFID 목록 반환
        /// </summary>
        /// <returns>활성화된 RFID 목록</returns>
        public List<string> GetActiveRfidList()
        {
            return _rfidPathfinder.GetActiveRfidList();
        }

        /// <summary>
        /// RFID pathfinder의 AGV 현재 방향 설정
        /// </summary>
        /// <param name="direction">현재 방향</param>
        public void SetRfidPathfinderCurrentDirection(AgvDirection direction)
        {
            _rfidPathfinder.CurrentDirection = direction;
        }

        /// <summary>
        /// RFID pathfinder의 회전 비용 가중치 설정
        /// </summary>
        /// <param name="weight">회전 비용 가중치</param>
        public void SetRfidPathfinderRotationCostWeight(float weight)
        {
            _rfidPathfinder.RotationCostWeight = weight;
        }

        #region 도킹 조건 검색 기능

        // 내부 노드 목록 저장
        private List<MapNode> _mapNodes;

        /// <summary>
        /// 맵 노드 설정 (도킹 조건 검색용)
        /// </summary>
        private void UpdateInternalMapNodes(List<MapNode> mapNodes)
        {
            _mapNodes = mapNodes;
        }

        /// <summary>
        /// 도킹 방향 기반 노드 검색
        /// </summary>
        /// <param name="dockingDirection">도킹 방향</param>
        /// <returns>해당 도킹 방향의 노드 목록</returns>
        public List<MapNode> GetNodesByDockingDirection(DockingDirection dockingDirection)
        {
            if (_mapNodes == null) return new List<MapNode>();
            
            var result = new List<MapNode>();

            foreach (var node in _mapNodes)
            {
                if (!node.IsActive) continue;

                var nodeDockingDirection = GetNodeDockingDirection(node);
                if (nodeDockingDirection == dockingDirection)
                {
                    result.Add(node);
                }
            }

            return result;
        }

        /// <summary>
        /// 노드의 도킹 방향 결정
        /// </summary>
        /// <param name="node">노드</param>
        /// <returns>도킹 방향</returns>
        public DockingDirection GetNodeDockingDirection(MapNode node)
        {
            switch (node.Type)
            {
                case NodeType.Charging:
                    return DockingDirection.Forward;   // 충전기: 전진 도킹
                case NodeType.Docking:
                    return DockingDirection.Backward;  // 장비: 후진 도킹
                default:
                    return DockingDirection.Forward;   // 기본값: 전진
            }
        }

        /// <summary>
        /// 도킹 방향과 장비 타입에 맞는 노드들로의 경로 검색
        /// </summary>
        /// <param name="startRfidId">시작 RFID</param>
        /// <param name="dockingDirection">필요한 도킹 방향</param>
        /// <param name="stationType">장비 타입 (선택사항)</param>
        /// <returns>경로 계산 결과 목록 (거리 순 정렬)</returns>
        public List<RfidPathResult> FindPathsByDockingCondition(string startRfidId, DockingDirection dockingDirection, StationType? stationType = null)
        {
            var targetNodes = GetNodesByDockingDirection(dockingDirection);
            var results = new List<RfidPathResult>();

            // 장비 타입 필터링 (필요시)
            if (stationType.HasValue && dockingDirection == DockingDirection.Backward)
            {
                // 후진 도킹이면서 특정 장비 타입이 지정된 경우
                // 이 부분은 추후 StationMapping 정보가 있을 때 구현
                // 현재는 모든 도킹 노드를 대상으로 함
            }

            foreach (var targetNode in targetNodes)
            {
                if (!targetNode.HasRfid()) continue;

                try
                {
                    var pathResult = _rfidPathfinder.FindAGVPath(startRfidId, targetNode.RfidId);
                    if (pathResult.Success)
                    {
                        results.Add(pathResult);
                    }
                }
                catch (Exception ex)
                {
                    // 개별 경로 계산 실패는 무시하고 계속 진행
                    System.Diagnostics.Debug.WriteLine($"Path calculation failed from {startRfidId} to {targetNode.RfidId}: {ex.Message}");
                }
            }

            // 거리 순으로 정렬
            return results.OrderBy(r => r.TotalDistance).ToList();
        }

        /// <summary>
        /// 가장 가까운 충전기 경로 찾기 (전진 도킹)
        /// </summary>
        /// <param name="startRfidId">시작 RFID</param>
        /// <returns>가장 가까운 충전기로의 경로</returns>
        public RfidPathResult FindNearestChargingStationPath(string startRfidId)
        {
            var chargingPaths = FindPathsByDockingCondition(startRfidId, DockingDirection.Forward);
            var chargingNodes = chargingPaths.Where(p => p.Success).ToList();

            return chargingNodes.FirstOrDefault() ?? new RfidPathResult
            {
                Success = false,
                ErrorMessage = "충전 가능한 충전기를 찾을 수 없습니다."
            };
        }

        /// <summary>
        /// 가장 가까운 장비 도킹 경로 찾기 (후진 도킹)
        /// </summary>
        /// <param name="startRfidId">시작 RFID</param>
        /// <param name="stationType">장비 타입 (선택사항)</param>
        /// <returns>가장 가까운 장비로의 경로</returns>
        public RfidPathResult FindNearestEquipmentPath(string startRfidId, StationType? stationType = null)
        {
            var equipmentPaths = FindPathsByDockingCondition(startRfidId, DockingDirection.Backward, stationType);
            var equipmentNodes = equipmentPaths.Where(p => p.Success).ToList();

            return equipmentNodes.FirstOrDefault() ?? new RfidPathResult
            {
                Success = false,
                ErrorMessage = $"도킹 가능한 장비를 찾을 수 없습니다. ({stationType?.ToString() ?? "모든 타입"})"
            };
        }

        #endregion
    }
}