ENIG/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs

using System;
using System.Collections.Generic;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Core;
using AGVNavigationCore.PathFinding.Validation;

namespace AGVNavigationCore.Utils
{
    /// <summary>
    /// AGV 도킹 방향 검증 유틸리티
    /// 경로 계산 후 마지막 도킹 방향이 올바른지 검증
    /// </summary>
    public static class DockingValidator
    {
        /// <summary>
        /// 경로의 도킹 방향 검증
        /// </summary>
        /// <param name="pathResult">경로 계산 결과</param>
        /// <param name="mapNodes">맵 노드 목록</param>
        /// <param name="currentDirection">AGV 현재 방향</param>
        /// <returns>도킹 검증 결과</returns>
        public static DockingValidationResult ValidateDockingDirection(AGVPathResult pathResult, List<MapNode> mapNodes)
        {
            // 경로가 없거나 실패한 경우
            if (pathResult == null || !pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
            {
                System.Diagnostics.Debug.WriteLine($"[DockingValidator] 도킹 검증 불필요: 경로 없음");
                return DockingValidationResult.CreateNotRequired();
            }
            if (pathResult.DetailedPath.Any() == false && pathResult.Path.Any() && pathResult.Path.Count == 2 &&
                pathResult.Path[0].Id == pathResult.Path[1].Id)
            {
                System.Diagnostics.Debug.WriteLine($"[DockingValidator] 도킹 검증 불필요: 동일포인트");
                return DockingValidationResult.CreateNotRequired();
            }

            // 목적지 노드 가져오기 (Path는 이제 List<MapNode>)
            var LastNode = pathResult.Path[pathResult.Path.Count - 1];

            if (LastNode == null)
            {
                System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드가 null입니다");
                return DockingValidationResult.CreateNotRequired();
            }

            System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드: {LastNode.Id} 타입:{LastNode.Type} ({(int)LastNode.Type})");

            ////detail 경로 이동 예측 검증
            //for (int i = 0; i < pathResult.DetailedPath.Count - 1; i++)
            //{
            //    var curNodeId = pathResult.DetailedPath[i].NodeId;
            //    var nextNodeId = pathResult.DetailedPath[i + 1].NodeId;

            //    var curNode = mapNodes?.FirstOrDefault(n => n.Id == curNodeId);
            //    var nextNode = mapNodes?.FirstOrDefault(n => n.Id == nextNodeId);

            //    if (curNode != null && nextNode != null)
            //    {
            //        MapNode prevNode = null;
            //        AgvDirection prevDir = AgvDirection.Stop;
            //        if (i == 0)
            //        {
            //            prevNode = pathResult.PrevNode;
            //            prevDir = pathResult.PrevDirection;
            //        }
            //        else
            //        {
            //            var prevNodeId = pathResult.DetailedPath[i - 1].NodeId;
            //            prevNode = mapNodes?.FirstOrDefault(n => n.Id == prevNodeId);
            //            prevDir = pathResult.DetailedPath[i - 1].MotorDirection;
            //        }


            //        if (prevNode != null)
            //        {
            //            // DirectionalHelper를 사용하여 예상되는 다음 노드 확인
            //            Console.WriteLine(
            //                $"\n[ValidateDockingDirection] 경로 검증 단계 {i}:");
            //            Console.WriteLine(
            //                $"  이전→현재→다음: {prevNode.Id}({prevNode.RfidId}) → {curNode.Id}({curNode.RfidId}) → {nextNode.Id}({nextNode.RfidId})");
            //            Console.WriteLine(
            //                $"  현재 노드 위치: ({curNode.Position.X:F1}, {curNode.Position.Y:F1})");
            //            Console.WriteLine(
            //                $"  이전 모터방향: {prevDir}, 현재 모터방향: {pathResult.DetailedPath[i].MotorDirection}");
            //            Console.WriteLine(
            //                $"  마그넷방향: {pathResult.DetailedPath[i].MagnetDirection}");

            //            var expectedNextNode = DirectionalHelper.GetNextNodeByDirection(
            //                curNode,
            //                prevNode,
            //                prevDir,
            //                pathResult.DetailedPath[i].MotorDirection,
            //                pathResult.DetailedPath[i].MagnetDirection,
            //                mapNodes
            //            );

            //            var expectedNextNodeL = DirectionalHelper.GetNextNodeByDirection(
            //       curNode,
            //       prevNode,
            //       prevDir,
            //       pathResult.DetailedPath[i].MotorDirection,
            //        PathFinding.Planning.MagnetDirection.Left,
            //       mapNodes
            //   );

            //            var expectedNextNodeR = DirectionalHelper.GetNextNodeByDirection(
            //       curNode,
            //       prevNode,
            //       prevDir,
            //       pathResult.DetailedPath[i].MotorDirection,
            //        PathFinding.Planning.MagnetDirection.Right,
            //       mapNodes
            //   );

            //            var expectedNextNodeS = DirectionalHelper.GetNextNodeByDirection(
            //           curNode,
            //           prevNode,
            //           prevDir,
            //           pathResult.DetailedPath[i].MotorDirection,
            //            PathFinding.Planning.MagnetDirection.Straight,
            //           mapNodes
            //       );


            //            Console.WriteLine(
            //                $"  [예상] GetNextNodeByDirection 결과: {expectedNextNode?.Id ?? "null"}");
            //            Console.WriteLine(
            //                $"  [실제] DetailedPath 다음 노드: {nextNode.RfidId}[{nextNode.Id}]");

            //            if (expectedNextNode != null && !expectedNextNode.Id.Equals(nextNode.Id))
            //            {
            //                string error =
            //                    $"[DockingValidator] ⚠️ 경로 방향 불일치" +
            //                    $"\n현재={curNode.RfidId}[{curNodeId}] 이전={prevNode.RfidId}[{(prevNode?.Id ?? string.Empty)}] " +
            //                    $"\n예상다음={expectedNextNode.RfidId}[{expectedNextNode.Id}] 실제다음={nextNode.RfidId}[{nextNodeId}]";
            //                Console.WriteLine(
            //                    $"[ValidateDockingDirection] ❌ 경로 방향 불일치 검출!");
            //                Console.WriteLine(
            //                    $"  이동 벡터:");
            //                Console.WriteLine(
            //                    $"    이전→현재: ({(curNode.Position.X - prevNode.Position.X):F2}, {(curNode.Position.Y - prevNode.Position.Y):F2})");
            //                Console.WriteLine(
            //                    $"    현재→예상: ({(expectedNextNode.Position.X - curNode.Position.X):F2}, {(expectedNextNode.Position.Y - curNode.Position.Y):F2})");
            //                Console.WriteLine(
            //                    $"    현재→실제: ({(nextNode.Position.X - curNode.Position.X):F2}, {(nextNode.Position.Y - curNode.Position.Y):F2})");
            //                Console.WriteLine($"[ValidateDockingDirection] 에러메시지: {error}");
            //                return DockingValidationResult.CreateInvalid(
            //                    LastNode.Id,
            //                    LastNode.Type,
            //                    pathResult.DetailedPath[i].MotorDirection,
            //                    pathResult.DetailedPath[i].MotorDirection,
            //                    error);


            //            }
            //            else
            //            {
            //                Console.WriteLine(
            //                    $"  ✅ 경로 방향 일치!");
            //            }
            //        }
            //    }
            //}


            // 도킹이 필요한 노드인지 확인 (DockDirection이 DontCare가 아닌 경우)
            if (LastNode.DockDirection == DockingDirection.DontCare)
            {
                System.Diagnostics.Debug.WriteLine($"[DockingValidator] 도킹 불필요: {LastNode.DockDirection}");
                return DockingValidationResult.CreateNotRequired();
            }



            // 필요한 도킹 방향 확인
            var requiredDirection = GetRequiredDockingDirection(LastNode.DockDirection);
            System.Diagnostics.Debug.WriteLine($"[DockingValidator] 필요한 도킹 방향: {requiredDirection}");

            var LastNodeInfo = pathResult.DetailedPath.Last();
            if (LastNodeInfo.NodeId != LastNode.Id)
            {
                string error = $"마지막 노드의 도킹방향과 경로정보의 노드ID 불일치: 필요={LastNode.Id}, 계산됨={LastNodeInfo.NodeId }";
                System.Diagnostics.Debug.WriteLine($"[DockingValidator] ❌ 도킹 검증 실패: {error}");
                return DockingValidationResult.CreateInvalid(
                    LastNode.Id,
                    LastNode.Type,
                    requiredDirection,
                    LastNodeInfo.MotorDirection,
                    error);
            }

            // 검증 수행
            if (LastNodeInfo.MotorDirection == requiredDirection && pathResult.DetailedPath[pathResult.DetailedPath.Count - 1].MotorDirection == requiredDirection)
            {
                System.Diagnostics.Debug.WriteLine($"[DockingValidator] ✅ 도킹 검증 성공");
                return DockingValidationResult.CreateValid(
                    LastNode.Id,
                    LastNode.Type,
                    requiredDirection,
                    LastNodeInfo.MotorDirection);
            }
            else
            {
                string error = $"도킹 방향 불일치: 필요={GetDirectionText(requiredDirection)}, 계산됨={GetDirectionText(LastNodeInfo.MotorDirection)}";
                System.Diagnostics.Debug.WriteLine($"[DockingValidator] ❌ 도킹 검증 실패: {error}");
                return DockingValidationResult.CreateInvalid(
                    LastNode.Id,
                    LastNode.Type,
                    requiredDirection,
                    LastNodeInfo.MotorDirection,
                    error);
            }
        }

        /// <summary>
        /// 도킹이 필요한 노드인지 확인 (도킹방향이 DontCare가 아닌 경우)
        /// </summary>
        private static bool IsDockingRequired(DockingDirection dockDirection)
        {
            return dockDirection != DockingDirection.DontCare;
        }

        /// <summary>
        /// 노드 도킹 방향에 따른 필요한 AGV 방향 반환
        /// </summary>
        private static AgvDirection GetRequiredDockingDirection(DockingDirection dockDirection)
        {
            switch (dockDirection)
            {
                case DockingDirection.Forward:
                    return AgvDirection.Forward; // 전진 도킹
                case DockingDirection.Backward:
                    return AgvDirection.Backward; // 후진 도킹
                case DockingDirection.DontCare:
                default:
                    return AgvDirection.Forward; // 기본값 (사실상 사용되지 않음)
            }
        }

        /// <summary>
        /// 경로 기반 최종 방향 계산
        /// 개선된 구현: 경로 진행 방향과 목적지 노드 타입을 고려
        /// </summary>
        private static AgvDirection CalculateFinalDirection(List<MapNode> path, List<MapNode> mapNodes, AgvDirection currentDirection)
        {
            System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 입력 - 경로 수: {path?.Count}, 현재 방향: {currentDirection}");

            // 경로가 1개 이하면 현재 방향 유지
            if (path.Count < 2)
            {
                System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 경로가 짧음, 현재 방향 유지: {currentDirection}");
                return currentDirection;
            }

            // 목적지 노드 확인 (Path는 이제 List<MapNode>)
            var lastNode = path[path.Count - 1];

            if (lastNode == null)
            {
                System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 목적지 노드가 null입니다");
                return currentDirection;
            }

            // 도킹 노드인 경우, 필요한 도킹 방향으로 설정
            if (IsDockingRequired(lastNode.DockDirection))
            {
                var requiredDockingDirection = GetRequiredDockingDirection(lastNode.DockDirection);
                System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 도킹 노드(DockDirection={lastNode.DockDirection}) 감지, 필요 방향: {requiredDockingDirection}");

                // 현재 방향이 필요한 도킹 방향과 다르면 경고 로그
                if (currentDirection != requiredDockingDirection)
                {
                    System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] ⚠️ 현재 방향({currentDirection})과 필요 도킹 방향({requiredDockingDirection}) 불일치");
                }

                // 도킹 노드의 경우 항상 필요한 도킹 방향 반환
                return requiredDockingDirection;
            }

            // 일반 노드인 경우 마지막 구간의 이동 방향 분석
            var secondLastNode = path[path.Count - 2];

            if (secondLastNode == null)
            {
                System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 이전 노드가 null입니다");
                return currentDirection;
            }

            // 마지막 구간의 이동 벡터 계산
            var deltaX = lastNode.Position.X - secondLastNode.Position.X;
            var deltaY = lastNode.Position.Y - secondLastNode.Position.Y;
            var distance = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);

            System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 마지막 구간: {secondLastNode.Id} → {lastNode.Id}, 벡터: ({deltaX}, {deltaY}), 거리: {distance:F2}");

            // 이동 거리가 매우 작으면 현재 방향 유지
            if (distance < 1.0)
            {
                System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 이동 거리 너무 짧음, 현재 방향 유지: {currentDirection}");
                return currentDirection;
            }

            // 일반 노드의 경우 현재 방향 유지 (방향 전환은 회전 노드에서만 발생)
            System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 일반 노드, 현재 방향 유지: {currentDirection}");
            return currentDirection;
        }

        /// <summary>
        /// 방향을 텍스트로 변환
        /// </summary>
        private static string GetDirectionText(AgvDirection direction)
        {
            switch (direction)
            {
                case AgvDirection.Forward:
                    return "전진";
                case AgvDirection.Backward:
                    return "후진";
                default:
                    return direction.ToString();
            }
        }

        /// <summary>
        /// 도킹 검증 결과를 문자열로 변환 (디버깅용)
        /// </summary>
        public static string GetValidationSummary(DockingValidationResult validation)
        {
            if (validation == null)
                return "검증 결과 없음";

            if (!validation.IsValidationRequired)
                return "도킹 검증 불필요";

            if (validation.IsValid)
            {
                return $"도킹 검증 통과: {validation.TargetNodeId}({validation.TargetNodeType}) - {GetDirectionText(validation.RequiredDockingDirection)} 도킹";
            }
            else
            {
                return $"도킹 검증 실패: {validation.TargetNodeId}({validation.TargetNodeType}) - {validation.ValidationError}";
            }
        }
    }
}