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

namespace AGVNavigationCore.Utils
{
    /// <summary>
    /// AGV 방향 기반 다음 노드 계산기
    /// VirtualAGV 또는 실제 AGV 시스템에서 현재 방향을 알 때, 다음 목적지 노드를 결정
    /// </summary>
    public class AGVDirectionCalculator
    {
        private DirectionalPathfinder _pathfinder;

        public AGVDirectionCalculator(DirectionalPathfinder.DirectionWeights weights = null)
        {
            _pathfinder = new DirectionalPathfinder(weights);
        }

        /// <summary>
        /// 이전 RFID 위치 + 현재 위치 + 현재 방향을 기반으로 다음 노드 ID를 반환
        ///
        /// 사용 예시:
        /// - 001에서 002로 이동 후 GetNextNodeId(001_pos, 002_node, 002_pos, Forward) → 003
        /// - 003에서 004로 이동 후, Left 선택 → 030
        /// - 004에서 003으로 이동(Backward) 후, GetNextNodeId(..., Backward) → 002
        /// </summary>
        /// <param name="previousRfidPos">이전 RFID 감지 위치</param>
        /// <param name="currentNode">현재 RFID 노드</param>
        /// <param name="currentRfidPos">현재 RFID 감지 위치</param>
        /// <param name="direction">이동 방향</param>
        /// <param name="allNodes">맵의 모든 노드</param>
        /// <returns>다음 노드 ID (실패 시 null)</returns>
        public string GetNextNodeId(
            Point previousRfidPos,
            MapNode currentNode,
            Point currentRfidPos,
            AgvDirection direction,
            List<MapNode> allNodes)
        {
            // 유효성 검사
            if (previousRfidPos == Point.Empty)
            {
                throw new ArgumentException("previousRfidPos는 빈 값일 수 없습니다. 최소 2개의 위치 히스토리가 필요합니다.");
            }

            if (currentNode == null)
            {
                throw new ArgumentNullException(nameof(currentNode), "currentNode는 null일 수 없습니다.");
            }

            if (allNodes == null || allNodes.Count == 0)
            {
                throw new ArgumentException("allNodes는 비어있을 수 없습니다.");
            }

            return _pathfinder.GetNextNodeId(
                previousRfidPos,
                currentNode,
                currentRfidPos,
                direction,
                allNodes
            );
        }

        /// <summary>
        /// 현재 모터 상태를 기반으로 실제 선택된 방향을 분석
        /// VirtualAGV의 현재/이전 상태로부터 선택된 방향을 역추적
        /// </summary>
        public AgvDirection AnalyzeSelectedDirection(
            Point previousPos,
            Point currentPos,
            MapNode selectedNextNode,
            List<MapNode> connectedNodes)
        {
            if (previousPos == Point.Empty || currentPos == Point.Empty || selectedNextNode == null)
            {
                return AgvDirection.Forward;
            }

            // 이동 벡터
            var movementVector = new PointF(
                currentPos.X - previousPos.X,
                currentPos.Y - previousPos.Y
            );

            // 다음 노드 벡터
            var nextVector = new PointF(
                selectedNextNode.Position.X - currentPos.X,
                selectedNextNode.Position.Y - currentPos.Y
            );

            // 내적 계산 (유사도)
            float dotProduct = (movementVector.X * nextVector.X) +
                              (movementVector.Y * nextVector.Y);

            // 외적 계산 (좌우 판별)
            float crossProduct = (movementVector.X * nextVector.Y) -
                                (movementVector.Y * nextVector.X);

            // 진행 방향 판별
            if (dotProduct > 0)  // 같은 방향으로 진행
            {
                if (Math.Abs(crossProduct) < 0.1f)  // 거의 직진
                {
                    return AgvDirection.Forward;
                }
                else if (crossProduct > 0)  // 좌측으로 회전
                {
                    return AgvDirection.Left;
                }
                else  // 우측으로 회전
                {
                    return AgvDirection.Right;
                }
            }
            else  // 반대 방향으로 진행 (후진)
            {
                return AgvDirection.Backward;
            }
        }
    }
}