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

namespace AGVNavigationCore.PathFinding.Planning
{
    /// <summary>
    /// 방향 기반 경로 탐색기
    /// 이전 위치 + 현재 위치 + 이동 방향을 기반으로 다음 노드를 결정
    /// </summary>
    public class DirectionalPathfinder
    {
        /// <summary>
        /// 이동 방향별 가중치
        /// </summary>
        public class DirectionWeights
        {
            public float ForwardWeight { get; set; } = 1.0f;   // 직진
            public float LeftWeight { get; set; } = 1.5f;      // 좌측
            public float RightWeight { get; set; } = 1.5f;     // 우측
            public float BackwardWeight { get; set; } = 2.0f;  // 후진
        }

        private readonly DirectionWeights _weights;

        public DirectionalPathfinder(DirectionWeights weights = null)
        {
            _weights = weights ?? new DirectionWeights();
        }

        /// <summary>
        /// 이전 위치와 현재 위치, 그리고 이동 방향을 기반으로 다음 노드 ID를 반환
        /// </summary>
        /// <param name="previousPos">이전 위치 (이전 RFID 감지 위치)</param>
        /// <param name="currentNode">현재 노드 (현재 RFID 노드)</param>
        /// <param name="currentPos">현재 위치</param>
        /// <param name="direction">이동 방향 (Forward/Backward/Left/Right)</param>
        /// <param name="allNodes">맵의 모든 노드</param>
        /// <returns>다음 노드 ID (또는 null)</returns>
        public string GetNextNodeId(
            Point previousPos,
            MapNode currentNode,
            Point currentPos,
            AgvDirection direction,
            List<MapNode> allNodes)
        {
            // 전제조건: 최소 2개 위치 히스토리 필요
            if (previousPos == Point.Empty || currentPos == Point.Empty)
            {
                return null;
            }

            if (currentNode == null || allNodes == null || allNodes.Count == 0)
            {
                return null;
            }

            // 현재 노드에 연결된 노드들 가져오기
            var connectedNodeIds = currentNode.ConnectedNodes;
            if (connectedNodeIds == null || connectedNodeIds.Count == 0)
            {
                return null;
            }

            // 연결된 노드 중 현재 노드가 아닌 것들만 필터링
            var candidateNodes = allNodes.Where(n =>
                connectedNodeIds.Contains(n.Id) && n.Id != currentNode.Id
            ).ToList();

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

            // 이전→현재 벡터 계산 (진행 방향 벡터)
            var movementVector = new PointF(
                currentPos.X - previousPos.X,
                currentPos.Y - previousPos.Y
            );

            // 벡터 정규화
            var movementLength = (float)Math.Sqrt(
                movementVector.X * movementVector.X +
                movementVector.Y * movementVector.Y
            );

            if (movementLength < 0.001f) // 거의 이동하지 않음
            {
                return candidateNodes[0].Id; // 첫 번째 연결 노드 반환
            }

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

            // 각 후보 노드에 대해 방향 점수 계산
            var scoredCandidates = new List<(MapNode node, float score)>();

            foreach (var candidate in candidateNodes)
            {
                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
                );

                if (toNextLength < 0.001f)
                {
                    continue;
                }

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

                // 진행 방향 기반 점수 계산
                float score = CalculateDirectionalScore(
                    normalizedMovement,
                    normalizedToNext,
                    direction
                );

                scoredCandidates.Add((candidate, score));
            }

            if (scoredCandidates.Count == 0)
            {
                return null;
            }

            // 가장 높은 점수를 가진 노드 반환
            var bestCandidate = scoredCandidates.OrderByDescending(x => x.score).First();
            return bestCandidate.node.Id;
        }

        /// <summary>
        /// 이동 방향을 기반으로 방향 점수를 계산
        /// 높은 점수 = 더 나은 선택지
        /// </summary>
        private float CalculateDirectionalScore(
            PointF movementDirection,   // 정규화된 이전→현재 벡터
            PointF nextDirection,        // 정규화된 현재→다음 벡터
            AgvDirection requestedDir)   // 요청된 이동 방향
        {
            float baseScore = 0;

            // 벡터 간 각도 계산 (내적)
            float dotProduct = (movementDirection.X * nextDirection.X) +
                              (movementDirection.Y * nextDirection.Y);

            // 외적으로 좌우 판별 (Z 성분)
            float crossProduct = (movementDirection.X * nextDirection.Y) -
                                (movementDirection.Y * nextDirection.X);

            switch (requestedDir)
            {
                case AgvDirection.Forward:
                    // Forward: 직진 방향 선호 (dotProduct ≈ 1)
                    if (dotProduct > 0.9f)  // 거의 같은 방향
                    {
                        baseScore = 100.0f * _weights.ForwardWeight;
                    }
                    else if (dotProduct > 0.5f)  // 비슷한 방향
                    {
                        baseScore = 80.0f * _weights.ForwardWeight;
                    }
                    else if (dotProduct > 0.0f)  // 약간 다른 방향
                    {
                        baseScore = 50.0f * _weights.ForwardWeight;
                    }
                    else if (dotProduct > -0.5f)  // 거의 반대 방향 아님
                    {
                        baseScore = 20.0f * _weights.BackwardWeight;
                    }
                    else
                    {
                        baseScore = 0.0f;  // 완전 반대
                    }
                    break;

                case AgvDirection.Backward:
                    // Backward: 역진 방향 선호 (dotProduct ≈ -1)
                    if (dotProduct < -0.9f)  // 거의 반대 방향
                    {
                        baseScore = 100.0f * _weights.BackwardWeight;
                    }
                    else if (dotProduct < -0.5f)  // 비슷하게 반대
                    {
                        baseScore = 80.0f * _weights.BackwardWeight;
                    }
                    else if (dotProduct < 0.0f)  // 약간 다른 방향
                    {
                        baseScore = 50.0f * _weights.BackwardWeight;
                    }
                    else if (dotProduct < 0.5f)  // 거의 같은 방향 아님
                    {
                        baseScore = 20.0f * _weights.ForwardWeight;
                    }
                    else
                    {
                        baseScore = 0.0f;  // 완전 같은 방향
                    }
                    break;

                case AgvDirection.Left:
                    // Left: 좌측 방향 선호
                    // Forward 상태에서: crossProduct > 0 = 좌측
                    // Backward 상태에서: crossProduct < 0 = 좌측 (반대)
                    if (dotProduct > 0.0f)  // Forward 상태
                    {
                        // crossProduct > 0이면 좌측
                        if (crossProduct > 0.5f)
                        {
                            baseScore = 100.0f * _weights.LeftWeight;
                        }
                        else if (crossProduct > 0.0f)
                        {
                            baseScore = 70.0f * _weights.LeftWeight;
                        }
                        else if (crossProduct > -0.5f)
                        {
                            baseScore = 50.0f * _weights.ForwardWeight;
                        }
                        else
                        {
                            baseScore = 30.0f * _weights.RightWeight;
                        }
                    }
                    else  // Backward 상태 - 좌우 반전
                    {
                        // Backward에서 좌측 = crossProduct < 0
                        if (crossProduct < -0.5f)
                        {
                            baseScore = 100.0f * _weights.LeftWeight;
                        }
                        else if (crossProduct < 0.0f)
                        {
                            baseScore = 70.0f * _weights.LeftWeight;
                        }
                        else if (crossProduct < 0.5f)
                        {
                            baseScore = 50.0f * _weights.BackwardWeight;
                        }
                        else
                        {
                            baseScore = 30.0f * _weights.RightWeight;
                        }
                    }
                    break;

                case AgvDirection.Right:
                    // Right: 우측 방향 선호
                    // Forward 상태에서: crossProduct < 0 = 우측
                    // Backward 상태에서: crossProduct > 0 = 우측 (반대)
                    if (dotProduct > 0.0f)  // Forward 상태
                    {
                        // crossProduct < 0이면 우측
                        if (crossProduct < -0.5f)
                        {
                            baseScore = 100.0f * _weights.RightWeight;
                        }
                        else if (crossProduct < 0.0f)
                        {
                            baseScore = 70.0f * _weights.RightWeight;
                        }
                        else if (crossProduct < 0.5f)
                        {
                            baseScore = 50.0f * _weights.ForwardWeight;
                        }
                        else
                        {
                            baseScore = 30.0f * _weights.LeftWeight;
                        }
                    }
                    else  // Backward 상태 - 좌우 반전
                    {
                        // Backward에서 우측 = crossProduct > 0
                        if (crossProduct > 0.5f)
                        {
                            baseScore = 100.0f * _weights.RightWeight;
                        }
                        else if (crossProduct > 0.0f)
                        {
                            baseScore = 70.0f * _weights.RightWeight;
                        }
                        else if (crossProduct > -0.5f)
                        {
                            baseScore = 50.0f * _weights.BackwardWeight;
                        }
                        else
                        {
                            baseScore = 30.0f * _weights.LeftWeight;
                        }
                    }
                    break;
            }

            return baseScore;
        }

        /// <summary>
        /// 벡터 간 각도를 도 단위로 계산
        /// </summary>
        private float CalculateAngle(PointF vector1, PointF vector2)
        {
            float dotProduct = (vector1.X * vector2.X) + (vector1.Y * vector2.Y);
            float magnitude1 = (float)Math.Sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y);
            float magnitude2 = (float)Math.Sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y);

            if (magnitude1 < 0.001f || magnitude2 < 0.001f)
            {
                return 0;
            }

            float cosAngle = dotProduct / (magnitude1 * magnitude2);
            cosAngle = Math.Max(-1.0f, Math.Min(1.0f, cosAngle)); // 범위 제한

            return (float)(Math.Acos(cosAngle) * 180.0 / Math.PI);
        }
    }
}