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.NodeId] = mapNode; // Add to lookup table

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

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

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

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

        /// <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(string startNodeId, string endNodeId)
        {
            var stopwatch = System.Diagnostics.Stopwatch.StartNew();

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

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

                if (startNodeId == endNodeId)
                {
                    var startMapNode = GetMapNode(startNodeId);
                    var singlePath = new List<MapNode> { startMapNode };
                    return AGVPathResult.CreateSuccess(singlePath, new List<AgvDirection>(), 0, stopwatch.ElapsedMilliseconds);
                }

                var startNode = _nodeMap[startNodeId];
                var endNode = _nodeMap[endNodeId];
                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 == endNodeId)
                    {
                        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>
        /// 경유지를 거쳐 경로 찾기 (오버로드)
        /// 여러 경유지를 순차적으로 거쳐서 최종 목적지까지의 경로를 계산합니다.
        /// 기존 FindPath를 여러 번 호출하여 각 구간의 경로를 합칩니다.
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="endNodeId">최종 목적지 노드 ID</param>
        /// <param name="waypointNodeIds">경유지 노드 ID 배열 (선택사항)</param>
        /// <returns>경로 계산 결과 (모든 경유지를 거친 전체 경로)</returns>
        public AGVPathResult FindPath(string startNodeId, string endNodeId, params string[] waypointNodeIds)
        {
            var stopwatch = System.Diagnostics.Stopwatch.StartNew();

            try
            {
                // 경유지가 없으면 기본 FindPath 호출
                if (waypointNodeIds == null || waypointNodeIds.Length == 0)
                {
                    return FindPathAStar(startNodeId, endNodeId);
                }

                // 경유지 유효성 검증
                var validWaypoints = new List<string>();
                foreach (var waypointId in waypointNodeIds)
                {
                    if (string.IsNullOrEmpty(waypointId))
                        continue;

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

                    validWaypoints.Add(waypointId);
                }

                // 경유지가 없으면 기본 경로 계산
                if (validWaypoints.Count == 0)
                {
                    return FindPathAStar(startNodeId, endNodeId);
                }

                // 첫 번째 경유지가 시작노드와 같은지 검사
                if (validWaypoints[0] == startNodeId)
                {
                    return AGVPathResult.CreateFailure(
                        $"첫 번째 경유지({validWaypoints[0]})가 시작 노드({startNodeId})와 동일합니다. 경유지는 시작노드와 달라야 합니다.",
                        stopwatch.ElapsedMilliseconds, 0);
                }

                // 마지막 경유지가 목적지노드와 같은지 검사
                if (validWaypoints[validWaypoints.Count - 1] == endNodeId)
                {
                    return AGVPathResult.CreateFailure(
                        $"마지막 경유지({validWaypoints[validWaypoints.Count - 1]})가 목적지 노드({endNodeId})와 동일합니다. 경유지는 목적지노드와 달라야 합니다.",
                        stopwatch.ElapsedMilliseconds, 0);
                }

                // 연속된 중복만 제거 (순서 유지)
                // 예: [1, 2, 2, 3, 2] -> [1, 2, 3, 2] (연속 중복만 제거)
                var deduplicatedWaypoints = new List<string>();
                string lastWaypoint = null;
                foreach (var waypoint in validWaypoints)
                {
                    if (waypoint != lastWaypoint)
                    {
                        deduplicatedWaypoints.Add(waypoint);
                        lastWaypoint = waypoint;
                    }
                }
                validWaypoints = deduplicatedWaypoints;

                // 최종 경로 리스트와 누적 값
                var combinedPath = new List<MapNode>();
                float totalDistance = 0;
                long totalCalculationTime = 0;

                // 현재 시작점
                string currentStart = startNodeId;

                // 1단계: 각 경유지까지의 경로 계산
                for (int i = 0; i < validWaypoints.Count; i++)
                {
                    string waypoint = validWaypoints[i];

                    // 현재 위치에서 경유지까지의 경로 계산
                    var segmentResult = FindPathAStar(currentStart, waypoint);

                    if (!segmentResult.Success)
                    {
                        return AGVPathResult.CreateFailure(
                            $"경유지 {i + 1}({waypoint})까지의 경로 계산 실패: {segmentResult.ErrorMessage}",
                            stopwatch.ElapsedMilliseconds, 0);
                    }

                    // 경로 합치기 (첫 번째 구간이 아니면 시작점 제거하여 중복 방지)
                    if (combinedPath.Count > 0 && segmentResult.Path.Count > 0)
                    {
                        // 시작 노드 제거 (이전 경로의 마지막 노드와 동일)
                        combinedPath.AddRange(segmentResult.Path.Skip(1));
                    }
                    else
                    {
                        combinedPath.AddRange(segmentResult.Path);
                    }

                    totalDistance += segmentResult.TotalDistance;
                    totalCalculationTime += segmentResult.CalculationTimeMs;

                    // 다음 경유지의 시작점은 현재 경유지
                    currentStart = waypoint;
                }

                // 2단계: 마지막 경유지에서 최종 목적지까지의 경로 계산
                var finalSegmentResult = FindPathAStar(currentStart, endNodeId);

                if (!finalSegmentResult.Success)
                {
                    return AGVPathResult.CreateFailure(
                        $"최종 목적지까지의 경로 계산 실패: {finalSegmentResult.ErrorMessage}",
                        stopwatch.ElapsedMilliseconds, 0);
                }

                // 최종 경로 합치기 (시작점 제거)
                if (combinedPath.Count > 0 && finalSegmentResult.Path.Count > 0)
                {
                    combinedPath.AddRange(finalSegmentResult.Path.Skip(1));
                }
                else
                {
                    combinedPath.AddRange(finalSegmentResult.Path);
                }

                totalDistance += finalSegmentResult.TotalDistance;
                totalCalculationTime += finalSegmentResult.CalculationTimeMs;

                stopwatch.Stop();

                // 결과 생성
                return AGVPathResult.CreateSuccess(
                    combinedPath,
                    new List<AgvDirection>(),
                    totalDistance,
                    totalCalculationTime
                );
            }
            catch (Exception ex)
            {
                return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
            }
        }

        /// <summary>
        /// 두 경로 결과를 합치기
        /// 이전 경로의 마지막 노드와 현재 경로의 시작 노드가 같으면 시작 노드를 제거하고 합침
        /// </summary>
        /// <param name="previousResult">이전 경로 결과</param>
        /// <param name="currentResult">현재 경로 결과</param>
        /// <returns>합쳐진 경로 결과</returns>
        public  AGVPathResult CombineResults( AGVPathResult previousResult, AGVPathResult currentResult)
        {
            // 입력 검증
            if (previousResult == null)
                return currentResult;

            if (currentResult == null)
                return previousResult;

            if (!previousResult.Success)
                return AGVPathResult.CreateFailure(
                    $"이전 경로 결과 실패: {previousResult.ErrorMessage}",
                    previousResult.CalculationTimeMs);

            if (!currentResult.Success)
                return AGVPathResult.CreateFailure(
                    $"현재 경로 결과 실패: {currentResult.ErrorMessage}",
                    currentResult.CalculationTimeMs);

            // 경로가 비어있는 경우 처리
            if (previousResult.Path == null || previousResult.Path.Count == 0)
                return currentResult;

            if (currentResult.Path == null || currentResult.Path.Count == 0)
                return previousResult;

            // 합친 경로 생성
            var combinedPath = new List<MapNode>(previousResult.Path);
            var combinedCommands = new List<AgvDirection>(previousResult.Commands);
            var combinedDetailedPath = new List<NodeMotorInfo>(previousResult.DetailedPath ?? new List<NodeMotorInfo>());

            // 이전 경로의 마지막 노드와 현재 경로의 시작 노드 비교
            string lastNodeOfPrevious = previousResult.Path[previousResult.Path.Count - 1].NodeId;
            string firstNodeOfCurrent = currentResult.Path[0].NodeId;

            if (lastNodeOfPrevious == firstNodeOfCurrent)
            {
                // 첫 번째 노드 제거 (중복 제거)
                combinedPath.RemoveAt(combinedPath.Count - 1);
                combinedPath.AddRange(currentResult.Path);

                // DetailedPath도 첫 번째 노드 제거
                if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0)
                {
                    combinedDetailedPath.RemoveAt(combinedDetailedPath.Count - 1);
                    combinedDetailedPath.AddRange(currentResult.DetailedPath);
                }
            }
            else
            {
                // 그대로 붙임
                combinedPath.AddRange(currentResult.Path);

                // DetailedPath도 그대로 붙임
                if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0)
                {
                    combinedDetailedPath.AddRange(currentResult.DetailedPath);
                }
            }

            // 명령어 합치기
            combinedCommands.AddRange(currentResult.Commands);

            // 총 거리 합산
            float combinedDistance = previousResult.TotalDistance + currentResult.TotalDistance;

            // 계산 시간 합산
            long combinedCalculationTime = previousResult.CalculationTimeMs + currentResult.CalculationTimeMs;

            // 합쳐진 결과 생성
            var result = AGVPathResult.CreateSuccess(
                combinedPath,
                combinedCommands,
                combinedDistance,
                combinedCalculationTime
            );

            // DetailedPath 설정
            result.DetailedPath = combinedDetailedPath;
            result.PrevNode = previousResult.PrevNode;
            result.PrevDirection = previousResult.PrevDirection;

            return result;
        }


        /// <summary>
        /// 여러 목적지 중 가장 가까운 노드로의 경로 찾기
        /// </summary>
        /// <param name="startNodeId">시작 노드 ID</param>
        /// <param name="targetNodeIds">목적지 후보 노드 ID 목록</param>
        /// <returns>경로 계산 결과</returns>
        public AGVPathResult FindNearestPath(string startNodeId, List<string> targetNodeIds)
        {
            if (targetNodeIds == null || targetNodeIds.Count == 0)
            {
                return AGVPathResult.CreateFailure("목적지 노드가 지정되지 않았습니다", 0, 0);
            }

            AGVPathResult bestResult = null;
            foreach (var targetId in targetNodeIds)
            {
                var result = FindPathAStar(startNodeId, targetId);
                if (result.Success && (bestResult == null || result.TotalDistance < bestResult.TotalDistance))
                {
                    bestResult = result;
                }
            }

            return bestResult ?? AGVPathResult.CreateFailure("모든 목적지로의 경로를 찾을 수 없습니다", 0, 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].NodeId;
                var nodeId2 = path[i + 1].NodeId;

                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.NodeId == 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.NodeId == connectedNodeId);
                if (connectedNode != null && connectedNode.IsActive && connectedNode.IsNavigationNode())
                {
                    alternateNodes.Add(connectedNode);
                }
            }

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

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