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

namespace AGVNavigationCore.Utils
{
    /// <summary>
    /// GetNextNodeId() 메서드의 동작을 검증하는 테스트 클래스
    ///
    /// 테스트 시나리오:
    /// - 001(65,229) → 002(206,244) → Forward → 003이 나와야 함
    /// - 001(65,229) → 002(206,244) → Backward → 001이 나와야 함
    /// - 002(206,244) → 003(278,278) → Forward → 004가 나와야 함
    /// - 002(206,244) → 003(278,278) → Backward → 002가 나와야 함
    /// </summary>
    public class GetNextNodeIdTest
    {
        /// <summary>
        /// 가상의 VirtualAGV 상태를 시뮬레이션하여 GetNextNodeId 테스트
        /// </summary>
        public void TestGetNextNodeId()
        {
            Console.WriteLine("\n================================================");
            Console.WriteLine("GetNextNodeId() 동작 검증");
            Console.WriteLine("================================================\n");

            // 테스트 노드 생성
            var node001 = new MapNode { Id = "N001", RfidId = 001, Position = new Point(65, 229), ConnectedNodes = new List<string> { "N002" } };
            var node002 = new MapNode { Id = "N002", RfidId = 002, Position = new Point(206, 244), ConnectedNodes = new List<string> { "N001", "N003" } };
            var node003 = new MapNode { Id = "N003", RfidId = 003, Position = new Point(278, 278), ConnectedNodes = new List<string> { "N002", "N004" } };
            var node004 = new MapNode { Id = "N004", RfidId = 004, Position = new Point(380, 340), ConnectedNodes = new List<string> { "N003", "N022", "N031" } };

            var allNodes = new List<MapNode> { node001, node002, node003, node004 };

            // VirtualAGV 시뮬레이션 (실제 인스턴스 생성 불가하므로 로직만 재현)
            Console.WriteLine("테스트 시나리오 1: 001 → 002 → Forward");
            Console.WriteLine("─────────────────────────────────────────");
            TestScenario(
                "Forward 이동: 001에서 002로, 다음은 Forward",
                node001.Position, node002, node003,
                AgvDirection.Forward, allNodes,
                "003 (예상)"
            );

            Console.WriteLine("\n테스트 시나리오 2: 001 → 002 → Backward");
            Console.WriteLine("─────────────────────────────────────────");
            TestScenario(
                "Backward 이동: 001에서 002로, 다음은 Backward",
                node001.Position, node002, node001,
                AgvDirection.Backward, allNodes,
                "001 (예상)"
            );

            Console.WriteLine("\n테스트 시나리오 3: 002 → 003 → Forward");
            Console.WriteLine("─────────────────────────────────────────");
            TestScenario(
                "Forward 이동: 002에서 003으로, 다음은 Forward",
                node002.Position, node003, node004,
                AgvDirection.Forward, allNodes,
                "004 (예상)"
            );

            Console.WriteLine("\n테스트 시나리오 4: 002 → 003 Forward → Backward");
            Console.WriteLine("─────────────────────────────────────────");
            TestScenario(
                "Forward 이동: 002에서 003으로, 다음은 Backward (경로 반대)",
                node002.Position, node003, node002,
                AgvDirection.Backward, allNodes,
                "002 (예상 - 경로 반대)"
            );

            Console.WriteLine("\n테스트 시나리오 5: 002 → 003 Backward → Forward");
            Console.WriteLine("─────────────────────────────────────────");
            TestScenario(
                "Backward 이동: 002에서 003으로, 다음은 Forward (경로 반대)",
                node002.Position, node003, node002,
                AgvDirection.Forward, allNodes,
                "002 (예상 - 경로 반대)",
                AgvDirection.Backward  // 현재 모터 방향
            );

            Console.WriteLine("\n테스트 시나리오 6: 002 → 003 Backward → Backward");
            Console.WriteLine("─────────────────────────────────────────");
            TestScenario(
                "Backward 이동: 002에서 003으로, 다음은 Backward (경로 계속)",
                node002.Position, node003, node004,
                AgvDirection.Backward, allNodes,
                "004 (예상 - 경로 계속)",
                AgvDirection.Backward  // 현재 모터 방향
            );

            Console.WriteLine("\n\n================================================");
            Console.WriteLine("테스트 완료");
            Console.WriteLine("================================================\n");
        }

        /// <summary>
        /// 개별 테스트 시나리오 실행
        /// </summary>
        private void TestScenario(
            string description,
            Point prevPos,
            MapNode currentNode,
            MapNode expectedNextNode,
            AgvDirection direction,
            List<MapNode> allNodes,
            string expectedNodeIdStr,
            AgvDirection? currentMotorDirection = null)
        {
            // 현재 모터 방향이 지정되지 않으면 direction과 동일하다고 가정
            AgvDirection motorDir = currentMotorDirection ?? direction;

            Console.WriteLine($"설명: {description}");
            Console.WriteLine($"이전 위치: {prevPos} (RFID: {allNodes.First(n => n.Position == prevPos)?.RfidId.ToString("0000") ?? "?"})");
            Console.WriteLine($"현재 노드: {currentNode.Id} (RFID: {currentNode.RfidId}) - 위치: {currentNode.Position}");
            Console.WriteLine($"현재 모터 방향: {motorDir}");
            Console.WriteLine($"요청 방향: {direction}");

            // 이동 벡터 계산
            var movementVector = new PointF(
                currentNode.Position.X - prevPos.X,
                currentNode.Position.Y - prevPos.Y
            );

            Console.WriteLine($"이동 벡터: ({movementVector.X}, {movementVector.Y})");

            // 각 후보 노드에 대한 점수 계산
            Console.WriteLine($"\n현재 노드({currentNode.Id})의 ConnectedNodes: {string.Join(", ", currentNode.ConnectedNodes)}");
            Console.WriteLine($"가능한 다음 노드들:");

            var candidateNodes = allNodes.Where(n =>
                currentNode.ConnectedNodes.Contains(n.Id) && n.Id != currentNode.Id
            ).ToList();

            foreach (var candidate in candidateNodes)
            {
                var score = CalculateScoreAndPrint(movementVector, currentNode.Position, candidate, direction);
                string isExpected = (candidate.Id == expectedNextNode.Id) ? " ← 예상 노드" : "";
                Console.WriteLine($"  {candidate.Id} (RFID: {candidate.RfidId}) - 위치: {candidate.Position} - 점수: {score:F1}{isExpected}");
            }

            // 최고 점수 노드 선택
            var bestCandidate = GetBestCandidate(movementVector, currentNode.Position, candidateNodes, direction);

            Console.WriteLine($"\n✓ 선택된 노드: {bestCandidate.Id} (RFID: {bestCandidate.RfidId})");

            if (bestCandidate.Id == expectedNextNode.Id)
            {
                Console.WriteLine($"✅ 정답! ({expectedNodeIdStr})");
            }
            else
            {
                Console.WriteLine($"❌ 오답! 예상: {expectedNextNode.Id}, 실제: {bestCandidate.Id}");
            }
        }

        /// <summary>
        /// 점수 계산 및 상세 정보 출력
        /// </summary>
        private float CalculateScoreAndPrint(PointF movementVector, Point currentPos, MapNode candidate, AgvDirection direction)
        {
            // 벡터 정규화
            var movementLength = (float)Math.Sqrt(
                movementVector.X * movementVector.X +
                movementVector.Y * movementVector.Y
            );

            var normalizedMovement = new PointF(
                movementVector.X / movementLength,
                movementVector.Y / movementLength
            );

            // 다음 벡터
            var toNextVector = new PointF(
                candidate.Position.X - currentPos.X,
                candidate.Position.Y - currentPos.Y
            );

            var toNextLength = (float)Math.Sqrt(
                toNextVector.X * toNextVector.X +
                toNextVector.Y * toNextVector.Y
            );

            var normalizedToNext = new PointF(
                toNextVector.X / toNextLength,
                toNextVector.Y / toNextLength
            );

            // 내적 및 외적 계산
            float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
                              (normalizedMovement.Y * normalizedToNext.Y);

            float crossProduct = (normalizedMovement.X * normalizedToNext.Y) -
                                (normalizedMovement.Y * normalizedToNext.X);

            float score = CalculateDirectionalScore(dotProduct, crossProduct, direction);

            return score;
        }

        /// <summary>
        /// 점수 계산 (VirtualAGV.CalculateDirectionalScore()와 동일)
        /// </summary>
        private float CalculateDirectionalScore(float dotProduct, float crossProduct, AgvDirection direction)
        {
            float baseScore = 0;

            switch (direction)
            {
                case AgvDirection.Forward:
                    if (dotProduct > 0.9f)
                        baseScore = 100.0f;
                    else if (dotProduct > 0.5f)
                        baseScore = 80.0f;
                    else if (dotProduct > 0.0f)
                        baseScore = 50.0f;
                    else if (dotProduct > -0.5f)
                        baseScore = 20.0f;
                    break;

                case AgvDirection.Backward:
                    if (dotProduct < -0.9f)
                        baseScore = 100.0f;
                    else if (dotProduct < -0.5f)
                        baseScore = 80.0f;
                    else if (dotProduct < 0.0f)
                        baseScore = 50.0f;
                    else if (dotProduct < 0.5f)
                        baseScore = 20.0f;
                    break;

                case AgvDirection.Left:
                    if (dotProduct > 0.0f)
                    {
                        if (crossProduct > 0.5f)
                            baseScore = 100.0f;
                        else if (crossProduct > 0.0f)
                            baseScore = 70.0f;
                        else if (crossProduct > -0.5f)
                            baseScore = 50.0f;
                        else
                            baseScore = 30.0f;
                    }
                    else
                    {
                        if (crossProduct < -0.5f)
                            baseScore = 100.0f;
                        else if (crossProduct < 0.0f)
                            baseScore = 70.0f;
                        else if (crossProduct < 0.5f)
                            baseScore = 50.0f;
                        else
                            baseScore = 30.0f;
                    }
                    break;

                case AgvDirection.Right:
                    if (dotProduct > 0.0f)
                    {
                        if (crossProduct < -0.5f)
                            baseScore = 100.0f;
                        else if (crossProduct < 0.0f)
                            baseScore = 70.0f;
                        else if (crossProduct < 0.5f)
                            baseScore = 50.0f;
                        else
                            baseScore = 30.0f;
                    }
                    else
                    {
                        if (crossProduct > 0.5f)
                            baseScore = 100.0f;
                        else if (crossProduct > 0.0f)
                            baseScore = 70.0f;
                        else if (crossProduct > -0.5f)
                            baseScore = 50.0f;
                        else
                            baseScore = 30.0f;
                    }
                    break;
            }

            return baseScore;
        }

        /// <summary>
        /// 최고 점수 노드 반환
        /// </summary>
        private MapNode GetBestCandidate(PointF movementVector, Point currentPos, List<MapNode> candidates, AgvDirection direction)
        {
            var movementLength = (float)Math.Sqrt(
                movementVector.X * movementVector.X +
                movementVector.Y * movementVector.Y
            );

            var normalizedMovement = new PointF(
                movementVector.X / movementLength,
                movementVector.Y / movementLength
            );

            MapNode bestCandidate = null;
            float bestScore = -1;

            foreach (var candidate in candidates)
            {
                var toNextVector = new PointF(
                    candidate.Position.X - currentPos.X,
                    candidate.Position.Y - currentPos.Y
                );

                var toNextLength = (float)Math.Sqrt(
                    toNextVector.X * toNextVector.X +
                    toNextVector.Y * toNextVector.Y
                );

                var normalizedToNext = new PointF(
                    toNextVector.X / toNextLength,
                    toNextVector.Y / toNextLength
                );

                float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
                                  (normalizedMovement.Y * normalizedToNext.Y);

                float crossProduct = (normalizedMovement.X * normalizedToNext.Y) -
                                    (normalizedMovement.Y * normalizedToNext.X);

                float score = CalculateDirectionalScore(dotProduct, crossProduct, direction);

                if (score > bestScore)
                {
                    bestScore = score;
                    bestCandidate = candidate;
                }
            }

            return bestCandidate;
        }
    }
}