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

namespace AGVNavigationCore.Utils
{
    /// <summary>
    /// AGV 리프트 방향 계산 유틸리티 클래스
    /// 모든 리프트 방향 계산 로직을 중앙화하여 일관성 보장
    /// </summary>
    public static class LiftCalculator
    {

        /// <summary>
        /// 경로 예측 기반 리프트 방향 계산
        /// 현재 노드에서 연결된 다음 노드들을 분석하여 리프트 방향 결정
        /// </summary>
        /// <param name="currentPos">현재 위치</param>
        /// <param name="previousPos">이전 위치</param>
        /// <param name="motorDirection">모터 방향</param>
        /// <param name="mapNodes">맵 노드 리스트 (경로 예측용)</param>
        /// <param name="tolerance">위치 허용 오차</param>
        /// <returns>리프트 계산 결과</returns>
        public static LiftCalculationResult CalculateLiftInfoWithPathPrediction(
            Point currentPos, Point previousPos, AgvDirection motorDirection,
            List<MapNode> mapNodes, int tolerance = 10)
        {
            if (mapNodes == null || mapNodes.Count == 0)
            {
                // 맵 노드 정보가 없으면 기존 방식 사용
                return CalculateLiftInfo(previousPos, currentPos, motorDirection);
            }

            // 현재 위치에 해당하는 노드 찾기
            var currentNode = FindNodeByPosition(mapNodes, currentPos, tolerance);

            if (currentNode == null)
            {
                // 현재 노드를 찾을 수 없으면 기존 방식 사용
                return CalculateLiftInfo(previousPos, currentPos, motorDirection);
            }

            // 이전 위치에 해당하는 노드 찾기
            var previousNode = FindNodeByPosition(mapNodes, previousPos, tolerance);

            Point targetPosition;
            string calculationMethod;

            // 모터 방향에 따른 예측 방향 결정
            if (motorDirection == AgvDirection.Backward)
            {
                // 후진 모터: AGV가 리프트 쪽(목표 위치)으로 이동
                // 경로 예측 없이 단순히 현재→목표 방향 사용
                return CalculateLiftInfo(currentPos, previousPos, motorDirection);
            }
            else
            {
                // 전진 모터: 기존 로직 (다음 노드 예측)
                var nextNodes = GetConnectedNodes(mapNodes, currentNode);

                // 이전 노드 제외 (되돌아가는 방향 제외)
                if (previousNode != null)
                {
                    nextNodes = nextNodes.Where(n => n.Id != previousNode.Id).ToList();
                }

                if (nextNodes.Count == 1)
                {
                    // 직선 경로: 다음 노드 방향으로 예측
                    targetPosition = nextNodes.First().Position;
                    calculationMethod = $"전진 경로 예측 ({currentNode.Id}→{nextNodes.First().Id})";
                }
                else if (nextNodes.Count > 1)
                {
                    // 갈래길: 이전 위치 기반 계산 사용
                    var prevResult = CalculateLiftInfo(previousPos, currentPos, motorDirection);
                    prevResult.CalculationMethod += " (전진 갈래길)";
                    return prevResult;
                }
                else
                {
                    // 연결된 노드가 없으면 기존 방식 사용
                    return CalculateLiftInfo(previousPos, currentPos, motorDirection);
                }
            }

            // 리프트 각도 계산
            var angleRadians = CalculateLiftAngleRadians(currentPos, targetPosition, motorDirection);
            var angleDegrees = angleRadians * 180.0 / Math.PI;

            // 0-360도 범위로 정규화
            while (angleDegrees < 0) angleDegrees += 360;
            while (angleDegrees >= 360) angleDegrees -= 360;

            var directionString = AngleToDirectionString(angleDegrees);

            return new LiftCalculationResult
            {
                AngleRadians = angleRadians,
                AngleDegrees = angleDegrees,
                DirectionString = directionString,
                CalculationMethod = calculationMethod,
                MotorDirection = motorDirection
            };
        }

        /// <summary>
        /// AGV 이동 방향과 모터 방향을 기반으로 리프트 각도 계산
        /// </summary>
        /// <param name="currentPos">현재 위치</param>
        /// <param name="targetPos">목표 위치</param>
        /// <param name="motorDirection">모터 방향</param>
        /// <returns>리프트 각도 (라디안)</returns>
        public static double CalculateLiftAngleRadians(Point currentPos, Point targetPos, AgvDirection motorDirection)
        {
            // 모터 방향에 따른 리프트 위치 계산
            if (motorDirection == AgvDirection.Forward)
            {
                // 전진 모터: AGV가 앞으로 가므로 리프트는 뒤쪽 (타겟 → 현재 방향)
                var dx = currentPos.X - targetPos.X;
                var dy = currentPos.Y - targetPos.Y;
                return Math.Atan2(dy, dx);
            }
            else if (motorDirection == AgvDirection.Backward)
            {
                // 후진 모터: AGV가 리프트 쪽으로 이동하므로 리프트는 AGV 이동 방향에 위치
                // 007→006 후진시: 리프트는 006방향(이동방향)을 향해야 함 (타겟→현재 반대방향)
                var dx = currentPos.X - targetPos.X;
                var dy = currentPos.Y - targetPos.Y;
                return Math.Atan2(dy, dx);
            }
            else
            {
                // 기본값: 전진 모터와 동일
                var dx = currentPos.X - targetPos.X;
                var dy = currentPos.Y - targetPos.Y;
                return Math.Atan2(dy, dx);
            }
        }

        /// <summary>
        /// AGV 이동 방향과 모터 방향을 기반으로 리프트 각도 계산 (도 단위)
        /// </summary>
        /// <param name="currentPos">현재 위치</param>
        /// <param name="targetPos">목표 위치</param>
        /// <param name="motorDirection">모터 방향</param>
        /// <returns>리프트 각도 (도)</returns>
        public static double CalculateLiftAngleDegrees(Point currentPos, Point targetPos, AgvDirection motorDirection)
        {
            var radians = CalculateLiftAngleRadians(currentPos, targetPos, motorDirection);
            var degrees = radians * 180.0 / Math.PI;
            
            // 0-360도 범위로 정규화
            while (degrees < 0) degrees += 360;
            while (degrees >= 360) degrees -= 360;
            
            return degrees;
        }

        /// <summary>
        /// 각도를 8방향 문자열로 변환 (화면 좌표계 기준)
        /// 화면 좌표계: 0°=동쪽, 90°=남쪽, 180°=서쪽, 270°=북쪽
        /// </summary>
        /// <param name="angleDegrees">각도 (도)</param>
        /// <returns>방향 문자열</returns>
        public static string AngleToDirectionString(double angleDegrees)
        {
            // 0-360도 범위로 정규화
            while (angleDegrees < 0) angleDegrees += 360;
            while (angleDegrees >= 360) angleDegrees -= 360;

            // 8방향으로 분류 (화면 좌표계)
            if (angleDegrees >= 337.5 || angleDegrees < 22.5)
                return "동쪽(→)";
            else if (angleDegrees >= 22.5 && angleDegrees < 67.5)
                return "남동쪽(↘)";
            else if (angleDegrees >= 67.5 && angleDegrees < 112.5)
                return "남쪽(↓)";
            else if (angleDegrees >= 112.5 && angleDegrees < 157.5)
                return "남서쪽(↙)";
            else if (angleDegrees >= 157.5 && angleDegrees < 202.5)
                return "서쪽(←)";
            else if (angleDegrees >= 202.5 && angleDegrees < 247.5)
                return "북서쪽(↖)";
            else if (angleDegrees >= 247.5 && angleDegrees < 292.5)
                return "북쪽(↑)";
            else if (angleDegrees >= 292.5 && angleDegrees < 337.5)
                return "북동쪽(↗)";
            else
                return "알 수 없음";
        }

        /// <summary>
        /// 리프트 계산 결과 정보
        /// </summary>
        public class LiftCalculationResult
        {
            public double AngleRadians { get; set; }
            public double AngleDegrees { get; set; }
            public string DirectionString { get; set; }
            public string CalculationMethod { get; set; }
            public AgvDirection MotorDirection { get; set; }
        }

        /// <summary>
        /// 종합적인 리프트 계산 (모든 정보 포함)
        /// </summary>
        /// <param name="currentPos">현재 위치</param>
        /// <param name="targetPos">목표 위치</param>
        /// <param name="motorDirection">모터 방향</param>
        /// <returns>리프트 계산 결과</returns>
        public static LiftCalculationResult CalculateLiftInfo(Point currentPos, Point targetPos, AgvDirection motorDirection)
        {
            var angleRadians = CalculateLiftAngleRadians(currentPos, targetPos, motorDirection);
            var angleDegrees = angleRadians * 180.0 / Math.PI;
            
            // 0-360도 범위로 정규화
            while (angleDegrees < 0) angleDegrees += 360;
            while (angleDegrees >= 360) angleDegrees -= 360;
            
            var directionString = AngleToDirectionString(angleDegrees);
            
            string calculationMethod;
            if (motorDirection == AgvDirection.Forward)
                calculationMethod = "이동방향 + 180도 (전진모터)";
            else if (motorDirection == AgvDirection.Backward)
                calculationMethod = "이동방향과 동일 (후진모터 - 리프트는 이동방향에 위치)";
            else
                calculationMethod = "기본값 (전진모터)";

            return new LiftCalculationResult
            {
                AngleRadians = angleRadians,
                AngleDegrees = angleDegrees,
                DirectionString = directionString,
                CalculationMethod = calculationMethod,
                MotorDirection = motorDirection
            };
        }

  

        /// <summary>
        /// 위치 기반 노드 찾기
        /// </summary>
        /// <param name="mapNodes">맵 노드 리스트</param>
        /// <param name="position">찾을 위치</param>
        /// <param name="tolerance">허용 오차</param>
        /// <returns>해당하는 노드 또는 null</returns>
        private static MapNode FindNodeByPosition(List<MapNode> mapNodes, Point position, int tolerance)
        {
            return mapNodes.FirstOrDefault(node =>
                Math.Abs(node.Position.X - position.X) <= tolerance &&
                Math.Abs(node.Position.Y - position.Y) <= tolerance);
        }

        /// <summary>
        /// 노드에서 연결된 다른 노드들 찾기
        /// </summary>
        /// <param name="mapNodes">맵 노드 리스트</param>
        /// <param name="currentNode">현재 노드</param>
        /// <returns>연결된 노드 리스트</returns>
        private static List<MapNode> GetConnectedNodes(List<MapNode> mapNodes, MapNode currentNode)
        {
            var connectedNodes = new List<MapNode>();
            
            foreach (var nodeId in currentNode.ConnectedNodes)
            {
                var connectedNode = mapNodes.FirstOrDefault(n => n.Id == nodeId);
                if (connectedNode != null)
                {
                    connectedNodes.Add(connectedNode);
                }
            }
            
            return connectedNodes;
        }
    }
}