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

namespace AGVNavigationCore.PathFinding.Core
{
    /// <summary>
    /// A* 알고리즘 기반 경로 탐색기
    /// </summary>
    public class AStarPathfinder
    {
        private Dictionary<string, PathNode> _nodeMap;
        private List<MapNode> _mapNodes;
        private Dictionary<string, MapNode> _mapNodeLookup; // Quick lookup for node ID -> MapNode

        /// <summary>
        /// 휴리스틱 가중치 (기본값: 1.0)
        /// 값이 클수록 목적지 방향을 우선시하나 최적 경로를 놓칠 수 있음
        /// </summary>
        public float HeuristicWeight { get; set; } = 1.0f;

        /// <summary>
        /// 최대 탐색 노드 수 (무한 루프 방지)
        /// </summary>
        public int MaxSearchNodes { get; set; } = 1000;

        /// <summary>
        /// 생성자
        /// </summary>
        public AStarPathfinder()
        {
            _nodeMap = new Dictionary<string, PathNode>();
            _mapNodes = new List<MapNode>();
            _mapNodeLookup = new Dictionary<string, MapNode>();
        }

        /// <summary>
        /// 맵 노드 설정
        /// </summary>
        /// <param name="mapNodes">맵 노드 목록</param>
        public void SetMapNodes(List<MapNode> mapNodes)
        {
            _mapNodes = mapNodes ?? new List<MapNode>();
            _nodeMap.Clear();
            _mapNodeLookup.Clear();

            // 모든 네비게이션 노드를 PathNode로 변환하고 양방향 연결 생성
            foreach (var mapNode in _mapNodes)
            {
                _mapNodeLookup[mapNode.Id] = mapNode; // Add to lookup table

                if (mapNode.IsNavigationNode())
                {
                    var pathNode = new PathNode(mapNode.Id, mapNode.Position);
                    _nodeMap[mapNode.Id] = pathNode;
                }
            }

            // 단일 연결을 양방향으로 확장
            foreach (var mapNode in _mapNodes)
            {
                if (mapNode.IsNavigationNode() && _nodeMap.ContainsKey(mapNode.Id))
                {
                    var pathNode = _nodeMap[mapNode.Id];

                    foreach (var connectedNode in mapNode.ConnectedMapNodes)
                    {
                        if (connectedNode != null && _nodeMap.ContainsKey(connectedNode.Id))
                        {
                            // 양방향 연결 생성 (단일 연결이 양방향을 의미)
                            if (!pathNode.ConnectedNodes.Contains(connectedNode.Id))
                            {
                                pathNode.ConnectedNodes.Add(connectedNode.Id);
                            }

                            var connectedPathNode = _nodeMap[connectedNode.Id];
                            if (!connectedPathNode.ConnectedNodes.Contains(mapNode.Id))
                            {
                                connectedPathNode.ConnectedNodes.Add(mapNode.Id);
                            }
                        }
                    }
                }
            }
        }

        /// <summary>
        /// 노드 ID로 MapNode 가져오기 (헬퍼 메서드)
        /// </summary>
        private MapNode GetMapNode(string nodeId)
        {
            return _mapNodeLookup.ContainsKey(nodeId) ? _mapNodeLookup[nodeId] : null;
        }

        /// <summary>
        /// 경로 찾기 (A* 알고리즘)
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="endNodeId">목적지 노드 ID</param>
        /// <returns>경로 계산 결과</returns>
        public AGVPathResult FindPathAStar(MapNode start, MapNode end)
        {
            var stopwatch = System.Diagnostics.Stopwatch.StartNew();

            try
            {
                if (!_nodeMap.ContainsKey(start.Id))
                {
                    return AGVPathResult.CreateFailure($"시작 노드를 찾을 수 없습니다: {start.Id}", stopwatch.ElapsedMilliseconds, 0);
                }

                if (!_nodeMap.ContainsKey(end.Id))
                {
                    return AGVPathResult.CreateFailure($"목적지 노드를 찾을 수 없습니다: {end.Id}", stopwatch.ElapsedMilliseconds, 0);
                }

                //출발지와 목적지가 동일한 경우
                if (start.Id == end.Id)
                {
                    //var startMapNode = GetMapNode(start);
                    var singlePath = new List<MapNode> { start };
                    return AGVPathResult.CreateSuccess(singlePath, new List<AgvDirection>(), 0, stopwatch.ElapsedMilliseconds);
                }

                var startNode = _nodeMap[start.Id];
                var endNode = _nodeMap[end.Id];
                var openSet = new List<PathNode>();
                var closedSet = new HashSet<string>();
                var exploredCount = 0;

                startNode.GCost = 0;
                startNode.HCost = CalculateHeuristic(startNode, endNode);
                startNode.Parent = null;
                openSet.Add(startNode);

                while (openSet.Count > 0 && exploredCount < MaxSearchNodes)
                {
                    var currentNode = GetLowestFCostNode(openSet);
                    openSet.Remove(currentNode);
                    closedSet.Add(currentNode.NodeId);
                    exploredCount++;

                    if (currentNode.NodeId == end.Id)
                    {
                        var path = ReconstructPath(currentNode);
                        var totalDistance = CalculatePathDistance(path);
                        return AGVPathResult.CreateSuccess(path, new List<AgvDirection>(), totalDistance, stopwatch.ElapsedMilliseconds);
                    }

                    foreach (var neighborId in currentNode.ConnectedNodes)
                    {
                        if (closedSet.Contains(neighborId) || !_nodeMap.ContainsKey(neighborId))
                            continue;

                        var neighbor = _nodeMap[neighborId];
                        var tentativeGCost = currentNode.GCost + currentNode.DistanceTo(neighbor);

                        if (!openSet.Contains(neighbor))
                        {
                            neighbor.Parent = currentNode;
                            neighbor.GCost = tentativeGCost;
                            neighbor.HCost = CalculateHeuristic(neighbor, endNode);
                            openSet.Add(neighbor);
                        }
                        else if (tentativeGCost < neighbor.GCost)
                        {
                            neighbor.Parent = currentNode;
                            neighbor.GCost = tentativeGCost;
                        }
                    }
                }

                return AGVPathResult.CreateFailure("경로를 찾을 수 없습니다", stopwatch.ElapsedMilliseconds, exploredCount);
            }
            catch (Exception ex)
            {
                return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
            }
        }

        /// <summary>
        /// 휴리스틱 거리 계산 (유클리드 거리)
        /// </summary>
        private float CalculateHeuristic(PathNode from, PathNode to)
        {
            return from.DistanceTo(to) * HeuristicWeight;
        }

        /// <summary>
        /// F cost가 가장 낮은 노드 선택
        /// </summary>
        private PathNode GetLowestFCostNode(List<PathNode> nodes)
        {
            PathNode lowest = nodes[0];
            foreach (var node in nodes)
            {
                if (node.FCost < lowest.FCost || 
                    (Math.Abs(node.FCost - lowest.FCost) < 0.001f && node.HCost < lowest.HCost))
                {
                    lowest = node;
                }
            }
            return lowest;
        }

        /// <summary>
        /// 경로 재구성 (부모 노드를 따라 역추적)
        /// </summary>
        private List<MapNode> ReconstructPath(PathNode endNode)
        {
            var path = new List<MapNode>();
            var current = endNode;

            while (current != null)
            {
                var mapNode = GetMapNode(current.NodeId);
                if (mapNode != null)
                {
                    path.Add(mapNode);
                }
                current = current.Parent;
            }

            path.Reverse();
            return path;
        }

        /// <summary>
        /// 경로의 총 거리 계산
        /// </summary>
        private float CalculatePathDistance(List<MapNode> path)
        {
            if (path.Count < 2) return 0;

            float totalDistance = 0;
            for (int i = 0; i < path.Count - 1; i++)
            {
                var nodeId1 = path[i].Id;
                var nodeId2 = path[i + 1].Id;

                if (_nodeMap.ContainsKey(nodeId1) && _nodeMap.ContainsKey(nodeId2))
                {
                    totalDistance += _nodeMap[nodeId1].DistanceTo(_nodeMap[nodeId2]);
                }
            }

            return totalDistance;
        }

        /// <summary>
        /// 두 노드가 연결되어 있는지 확인
        /// </summary>
        /// <param name="nodeId1">노드 1 ID</param>
        /// <param name="nodeId2">노드 2 ID</param>
        /// <returns>연결 여부</returns>
        public bool AreNodesConnected(string nodeId1, string nodeId2)
        {
            if (!_nodeMap.ContainsKey(nodeId1) || !_nodeMap.ContainsKey(nodeId2))
                return false;

            return _nodeMap[nodeId1].ConnectedNodes.Contains(nodeId2);
        }

        
        /// <summary>
        /// 네비게이션 가능한 노드 목록 반환
        /// </summary>
        /// <returns>노드 ID 목록</returns>
        public List<string> GetNavigationNodes()
        {
            return _nodeMap.Keys.ToList();
        }

        /// <summary>
        /// 노드 정보 반환
        /// </summary>
        /// <param name="nodeId">노드 ID</param>
        /// <returns>노드 정보 또는 null</returns>
        public PathNode GetNode(string nodeId)
        {
            return _nodeMap.ContainsKey(nodeId) ? _nodeMap[nodeId] : null;
        }

        /// <summary>
        /// 방향 전환을 위한 대체 노드 찾기
        /// 교차로에 연결된 노드 중에서 왔던 길과 갈 길이 아닌 다른 노드를 찾음
        /// 방향 전환 시 왕복 경로에 사용될 노드
        /// </summary>
        /// <param name="junctionNodeId">교차로 노드 ID (B)</param>
        /// <param name="previousNodeId">이전 노드 ID (A - 왔던 길)</param>
        /// <param name="targetNodeId">목표 노드 ID (C - 갈 길)</param>
        /// <param name="mapNodes">전체 맵 노드 목록</param>
        /// <returns>방향 전환에 사용할 대체 노드, 없으면 null</returns>
        public  MapNode FindAlternateNodeForDirectionChange(
            string junctionNodeId,
            string previousNodeId,
            string targetNodeId)
        {
            // 입력 검증
            if (string.IsNullOrEmpty(junctionNodeId) || string.IsNullOrEmpty(previousNodeId) || string.IsNullOrEmpty(targetNodeId))
                return null;

            if (_mapNodes == null || _mapNodes.Count == 0)
                return null;

            // 교차로 노드 찾기
            var junctionNode = _mapNodes.FirstOrDefault(n => n.Id == junctionNodeId);
            if (junctionNode == null || junctionNode.ConnectedNodes == null || junctionNode.ConnectedNodes.Count == 0)
                return null;

            // 교차로에 연결된 모든 노드 중에서 조건을 만족하는 노드 찾기
            // 조건:
            // 1. 이전 노드(왔던 길)가 아님
            // 2. 목표 노드(갈 길)가 아님
            // 3. 실제로 존재하는 노드
            // 4. 활성 상태인 노드
            // 5. 네비게이션 가능한 노드

            var alternateNodes = new List<MapNode>();

            foreach (var connectedNodeId in junctionNode.ConnectedNodes)
            {
                if (connectedNodeId == null) continue;

                // 조건 1: 왔던 길이 아님
                if (connectedNodeId == previousNodeId) continue;

                // 조건 2: 갈 길이 아님
                if (connectedNodeId == targetNodeId) continue;

                // 조건 3, 4, 5: 존재하고, 활성 상태이고, 네비게이션 가능
                var connectedNode = _mapNodes.FirstOrDefault(n => n.Id == connectedNodeId);
                if (connectedNode != null && connectedNode.IsActive && connectedNode.IsNavigationNode())
                {
                    alternateNodes.Add(connectedNode);
                }
            }

            // 찾은 노드가 없으면 null 반환
            if (alternateNodes.Count == 0)
                return null;

            // 여러 개 찾았으면 첫 번째 노드 반환
            // (필요시 거리 기반으로 가장 가까운 노드를 선택할 수도 있음)
            return alternateNodes[0];
        }
    }
}