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

namespace AGVNavigationCore.Utils
{
    /// <summary>
    /// AGV 방향 계산 헬퍼 유틸리티
    /// 현재 위치에서 주어진 모터 방향과 마그넷 방향으로 이동할 때 다음 노드를 계산
    /// 이전 이동 방향과 마그넷 방향을 고려하여 더 정확한 경로 예측
    /// </summary>
    public static class DirectionalHelper
    {

        /// <summary>
        /// AGV방향과 일치하는지 확인한다. 단 원본위치에서 dock 위치가 Don't Care 라면 true가 반환 됩니다.
        /// </summary>
        /// <param name="dock"></param>
        /// <param name="agvdirection"></param>
        /// <returns></returns>
        public static bool MatchAGVDirection(this DockingDirection dock, AgvDirection agvdirection)
        {
            if (dock == DockingDirection.DontCare) return true;
            if (dock == DockingDirection.Forward && agvdirection == AgvDirection.Forward) return true;
            if (dock == DockingDirection.Backward && agvdirection == AgvDirection.Backward) return true;
            return false;
        }

        private static JunctionAnalyzer _junctionAnalyzer;

        /// <summary>
        /// JunctionAnalyzer 초기화 (첫 호출 시)
        /// </summary>
        private static void InitializeJunctionAnalyzer(List<MapNode> allNodes)
        {
            if (_junctionAnalyzer == null && allNodes != null)
            {
                _junctionAnalyzer = new JunctionAnalyzer(allNodes);
            }
        }

        /// <summary>
        /// 현재 노드에서 주어진 모터 방향과 마그넷 방향으로 이동할 때 다음 노드를 반환
        /// 이전 모터 방향과 마그넷 방향을 고려하여 더 정확한 경로 예측
        /// </summary>
        /// <param name="currentNode">현재 노드</param>
        /// <param name="prevNode">이전 노드 (진행 방향 기준점)</param>
        /// <param name="prevDirection">이전 구간의 모터 방향</param>
        /// <param name="direction">현재 모터 방향 (Forward 또는 Backward)</param>
        /// <param name="magnetDirection">현재 마그넷 방향 (Straight/Left/Right)</param>
        /// <param name="allNodes">모든 맵 노드</param>
        /// <returns>다음 노드 (또는 null)</returns>
        public static MapNode GetNextNodeByDirection(
            MapNode currentNode,
            MapNode prevNode,
            AgvDirection prevDirection,
            AgvDirection direction,
            MagnetDirection magnetDirection,
            List<MapNode> allNodes)
        {
            if (currentNode == null || prevNode == null || allNodes == null)
                return null;

            // JunctionAnalyzer 초기화
            InitializeJunctionAnalyzer(allNodes);

            // 현재 노드에 연결된 노드들 중 이전 노드가 아닌 노드들만 필터링
            var connectedMapNodes = currentNode.ConnectedMapNodes;
            if (connectedMapNodes == null || connectedMapNodes.Count == 0)
                return null;

            List<MapNode> candidateNodes = new List<MapNode>();
            if (prevDirection == direction)
            {
                candidateNodes = connectedMapNodes.Where(n => n.Id != prevNode.Id).ToList();
            }
            else
            {
                candidateNodes = connectedMapNodes.ToList();
            }

            if (candidateNodes.Count == 0)
                return null;

            // 이전→현재 이동 벡터
            var movementVector = new PointF(
                currentNode.Position.X - prevNode.Position.X,
                currentNode.Position.Y - prevNode.Position.Y
            );

            var movementLength = (float)Math.Sqrt(
                movementVector.X * movementVector.X +
                movementVector.Y * movementVector.Y
            );

            if (movementLength < 0.001f)
                return candidateNodes[0];

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

            // 각 후보 노드에 대해 점수 계산
            MapNode bestNode = null;
            float bestScore = float.MinValue;

            Console.WriteLine(
                $"\n[GetNextNodeByDirection] ========== 다음 노드 선택 시작 ==========");
            Console.WriteLine(
                $"  현재노드: {currentNode.RfidId}[{currentNode.Id}]({currentNode.Position.X:F1}, {currentNode.Position.Y:F1})");
            Console.WriteLine(
                $"  이전노드: {prevNode.RfidId}[{prevNode.Id}]({prevNode.Position.X:F1}, {prevNode.Position.Y:F1})");
            Console.WriteLine(
                $"  이동벡터: ({movementVector.X:F2}, {movementVector.Y:F2}) → 정규화: ({normalizedMovement.X:F3}, {normalizedMovement.Y:F3})");
            Console.WriteLine(
                $"  현재방향: {direction}, 이전방향: {prevDirection}, 마그넷방향: {magnetDirection}");
            Console.WriteLine(
                $"  후보노드 개수: {candidateNodes.Count}");

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

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

                if (toNextLength < 0.001f)
                    continue;

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

                // 내적 계산 (유사도: -1 ~ 1)
                float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
                                  (normalizedMovement.Y * normalizedToNext.Y);

                float score;
                if (direction == prevDirection)
                {
                    // Forward: 진행 방향과 유사한 방향 선택 (높은 내적 = 좋음)
                    score = dotProduct;
                }
                else // Backward
                {
                    // Backward: 진행 방향과 반대인 방향 선택 (낮은 내적 = 좋음)
                    score = -dotProduct;
                }

                Console.WriteLine(
                    $"\n  [후보] {candidate.RfidId}[{candidate.Id}]({candidate.Position.X:F1}, {candidate.Position.Y:F1})");
                Console.WriteLine(
                    $"    벡터: ({toNextVector.X:F2}, {toNextVector.Y:F2}), 길이: {toNextLength:F2}");
                Console.WriteLine(
                    $"    정규화벡터: ({normalizedToNext.X:F3}, {normalizedToNext.Y:F3})");
                Console.WriteLine(
                    $"    내적(dotProduct): {dotProduct:F4}");
                Console.WriteLine(
                    $"    기본점수 ({(direction == prevDirection ? "방향유지" : "방향변경")}): {score:F4}");

                // 이전 모터 방향이 제공된 경우: 방향 일관성 보너스 추가
                var scoreBeforeMotor = score;
                score = ApplyMotorDirectionConsistencyBonus(
                    score,
                    direction,
                    prevDirection,
                    dotProduct
                );
                Console.WriteLine(
                    $"    모터방향 적용 후: {scoreBeforeMotor:F4} → {score:F4}");

                // 마그넷 방향을 고려한 점수 조정
                var scoreBeforeMagnet = score;
                score = ApplyMagnetDirectionBonus(
                    score,
                    magnetDirection,
                    normalizedMovement,
                    normalizedToNext,
                    currentNode,
                    candidate,
                    direction
                );
                Console.WriteLine(
                    $"    마그넷방향 적용 후: {scoreBeforeMagnet:F4} → {score:F4}");

                if (score > bestScore)
                {
                    bestScore = score;
                    bestNode = candidate;
                    Console.WriteLine(
                        $"    ⭐ 현재 최고점수 선택됨!");
                }
            }

            Console.WriteLine(
                $"\n  최종선택: {bestNode?.RfidId ?? 0}[{bestNode?.Id ?? "null"}] (점수: {bestScore:F4})");
            Console.WriteLine(
                $"[GetNextNodeByDirection] ========== 다음 노드 선택 종료 ==========\n");

            return bestNode;
        }

        /// <summary>
        /// 모터 방향 일관성을 고려한 점수 보정
        /// 같은 방향으로 계속 이동하는 경우 보너스 점수 부여
        /// </summary>
        /// <param name="baseScore">기본 점수</param>
        /// <param name="currentDirection">현재 모터 방향</param>
        /// <param name="prevMotorDirection">이전 모터 방향</param>
        /// <param name="dotProduct">벡터 내적값</param>
        /// <returns>조정된 점수</returns>
        private static float ApplyMotorDirectionConsistencyBonus(
            float baseScore,
            AgvDirection currentDirection,
            AgvDirection prevMotorDirection,
            float dotProduct)
        {
            float adjustedScore = baseScore;

            // 모터 방향이 변경되지 않은 경우: 일관성 보너스
            if (currentDirection == prevMotorDirection)
            {
                // Forward 지속: 직진 방향으로의 이동 선호
                // Backward 지속: 반대 방향으로의 이동 선호
                const float CONSISTENCY_BONUS = 0.2f;
                adjustedScore += CONSISTENCY_BONUS;

                System.Diagnostics.Debug.WriteLine(
                    $"[DirectionalHelper] 모터 방향 일관성 보너스: {currentDirection} → {currentDirection} " +
                    $"(점수: {baseScore:F3} → {adjustedScore:F3})");
            }
            else
            {
                // 모터 방향이 변경된 경우: 방향 변경 페널티
                const float DIRECTION_CHANGE_PENALTY = 0.15f;
                adjustedScore -= DIRECTION_CHANGE_PENALTY;

                System.Diagnostics.Debug.WriteLine(
                    $"[DirectionalHelper] 모터 방향 변경 페널티: {prevMotorDirection} → {currentDirection} " +
                    $"(점수: {baseScore:F3} → {adjustedScore:F3})");
            }

            return adjustedScore;
        }

        /// <summary>
        /// 마그넷 방향을 고려한 점수 보정
        /// Straight/Left/Right 마그넷 방향에 따라 후보 노드를 평가
        /// </summary>
        /// <param name="baseScore">기본 점수</param>
        /// <param name="magnetDirection">마그넷 방향 (Straight/Left/Right)</param>
        /// <param name="normalizedMovement">정규화된 이동 벡터</param>
        /// <param name="normalizedToNext">정규화된 다음 이동 벡터</param>
        /// <param name="currentNode">현재 노드</param>
        /// <param name="candidate">후보 노드</param>
        /// <returns>조정된 점수</returns>
        private static float ApplyMagnetDirectionBonus(
            float baseScore,
            MagnetDirection magnetDirection,
            PointF normalizedMovement,
            PointF normalizedToNext,
            MapNode currentNode,
            MapNode candidate,
            AgvDirection direction)
        {
            float adjustedScore = baseScore;

            // Straight: 일직선 방향 (높은 내적 보너스)
            if (magnetDirection == MagnetDirection.Straight)
            {
                const float STRAIGHT_BONUS = 0.5f;
                adjustedScore += STRAIGHT_BONUS;

                Console.WriteLine(
                    $"    [마그넷 판정] Straight 보너스 +0.5: {baseScore:F4} → {adjustedScore:F4}");
            }
            // Left 또는 Right: 모터 위치에 따른 회전 방향 판단
            else if (magnetDirection == MagnetDirection.Left || magnetDirection == MagnetDirection.Right)
            {
                // 2D 외적: movement × toNext = movement.X * toNext.Y - movement.Y * toNext.X
                float crossProduct = (normalizedMovement.X * normalizedToNext.Y) -
                                    (normalizedMovement.Y * normalizedToNext.X);

                bool isLeftMotorMatch = false;
                bool isRightMotorMatch = false;

                // ===== 정방향(Forward) 이동 =====
                if (direction == AgvDirection.Forward)
                {
                    // Forward 이동 시 외적 판정:
                    // - 외적 < 0 (음수) = 반시계 회전 = Left 모터 멈춤
                    // - 외적 > 0 (양수) = 시계 회전 = Right 모터 멈춤
                    //
                    // 예: 004 → 012 → 016 (Left 모터)
                    //     외적 = -0.9407 (음수) → 반시계 → Left 일치 ✅

                    isLeftMotorMatch = crossProduct < 0;      // 음수 = 반시계 = Left 멈춤
                    isRightMotorMatch = crossProduct > 0;     // 양수 = 시계 = Right 멈춤
                }
                // ===== 역방향(Backward) 이동 =====
                else // Backward
                {
                    // Backward 이동 시 외적 판정:
                    // - 외적 < 0 (음수) = 시계 회전 = Left 모터 멈춤
                    // - 외적 > 0 (양수) = 반시계 회전 = Right 모터 멈춤
                    //
                    // 예: 012 → 004 → 003 (Left 모터)
                    //     외적 = 0.9334 (양수) → 반시계(역방향 기준 시계) → Left 일치 ✅

                    isLeftMotorMatch = crossProduct > 0;      // 양수 = 시계(역) = Left 멈춤
                    isRightMotorMatch = crossProduct < 0;     // 음수 = 반시계(역) = Right 멈춤
                }

                Console.WriteLine(
                    $"    [마그넷 판정] 외적(Cross): {crossProduct:F4}, Left모터일치: {isLeftMotorMatch}, Right모터일치: {isRightMotorMatch} [{direction}]");

                // 외적의 절대값으로 회전 강도 판단 (0에 가까우면 약함, 1에 가까우면 강함)
                float rotationStrength = Math.Abs(crossProduct);

                if ((magnetDirection == MagnetDirection.Left && isLeftMotorMatch) ||
                    (magnetDirection == MagnetDirection.Right && isRightMotorMatch))
                {
                    // 올바른 모터 방향: 회전 강도에 비례한 보너스
                    // 강한 회전(|외적| ≈ 1): +2.0
                    // 약한 회전(|외적| ≈ 0.2): +0.4
                    float magnetBonus = rotationStrength * 2.0f;
                    adjustedScore += magnetBonus;

                    Console.WriteLine(
                        $"    [마그넷 판정] ✅ {magnetDirection} 모터 일치 (회전강도: {rotationStrength:F4}, 보너스 +{magnetBonus:F4}): {baseScore:F4} → {adjustedScore:F4}");
                }
                else
                {
                    // 잘못된 모터 방향: 회전 강도에 비례한 페널티
                    // 강한 회전(|외적| ≈ 1): -2.0
                    // 약한 회전(|외적| ≈ 0.2): -0.4
                    float magnetPenalty = rotationStrength * 2.0f;
                    adjustedScore -= magnetPenalty;

                    string actualMotor = crossProduct > 0 ? "Left" : "Right";

                    Console.WriteLine(
                        $"    [마그넷 판정] ❌ {magnetDirection} 모터 불일치 (실제: {actualMotor}, 회전강도: {rotationStrength:F4}, 페널티 -{magnetPenalty:F4}): {baseScore:F4} → {adjustedScore:F4}");
                }
            }

            return adjustedScore;
        }

        /// <summary>
        /// 모터 방향을 고려한 다음 노드 선택 (디버깅/분석용)
        /// </summary>
        public static (MapNode node, float score, string reason) GetNextNodeByDirectionWithDetails(
            MapNode currentNode,
            MapNode prevNode,
            AgvDirection direction,
            List<MapNode> allNodes,
            AgvDirection? prevMotorDirection)
        {
            if (currentNode == null || prevNode == null || allNodes == null)
                return (null, 0, "입력 파라미터가 null입니다");

            var connectedMapNodes = currentNode.ConnectedMapNodes;
            if (connectedMapNodes == null || connectedMapNodes.Count == 0)
                return (null, 0, "연결된 노드가 없습니다");

            var candidateNodes = connectedMapNodes.ToList();

            if (candidateNodes.Count == 0)
                return (null, 0, "후보 노드가 없습니다");

            var movementVector = new PointF(
                currentNode.Position.X - prevNode.Position.X,
                currentNode.Position.Y - prevNode.Position.Y
            );

            var movementLength = (float)Math.Sqrt(
                movementVector.X * movementVector.X +
                movementVector.Y * movementVector.Y
            );

            if (movementLength < 0.001f)
                return (candidateNodes[0], 1.0f, "움직임이 거의 없음");

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

            MapNode bestNode = null;
            float bestScore = float.MinValue;
            string reason = "";

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

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

                if (toNextLength < 0.001f)
                    continue;

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

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

                float score = (direction == AgvDirection.Forward) ? dotProduct : -dotProduct;

                if (prevMotorDirection.HasValue)
                {
                    score = ApplyMotorDirectionConsistencyBonus(
                        score,
                        direction,
                        prevMotorDirection.Value,
                        dotProduct
                    );
                }

                if (score > bestScore)
                {
                    bestScore = score;
                    bestNode = candidate;

                    // 선택 이유 생성
                    if (prevMotorDirection.HasValue && direction == prevMotorDirection)
                    {
                        reason = $"모터 방향 일관성 유지 ({direction}) → {candidate.Id}";
                    }
                    else if (prevMotorDirection.HasValue)
                    {
                        reason = $"모터 방향 변경 ({prevMotorDirection} → {direction}) → {candidate.Id}";
                    }
                    else
                    {
                        reason = $"방향 기반 선택 ({direction}) → {candidate.Id}";
                    }
                }
            }

            return (bestNode, bestScore, reason);
        }
    }
}