using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.Utils;
using AGVNavigationCore.PathFinding.Core;
using AGVNavigationCore.PathFinding.Analysis;

namespace AGVNavigationCore.PathFinding.Planning
{
    /// <summary>
    /// AGV 경로 계획기
    /// 물리적 제약사항과 마그넷 센서를 고려한 실제 AGV 경로 생성
    /// </summary>
    public class AGVPathfinder
    {

        private readonly List<MapNode> _mapNodes;
        private readonly AStarPathfinder _basicPathfinder;
        private readonly DirectionalPathfinder _directionPathfinder;
        private readonly JunctionAnalyzer _junctionAnalyzer;
        private readonly DirectionChangePlanner _directionChangePlanner;

        public AGVPathfinder(List<MapNode> mapNodes)
        {
            _mapNodes = mapNodes ?? new List<MapNode>();
            _basicPathfinder = new AStarPathfinder();
            _basicPathfinder.SetMapNodes(_mapNodes);
            _junctionAnalyzer = new JunctionAnalyzer(_mapNodes);
            _directionChangePlanner = new DirectionChangePlanner(_mapNodes);
            _directionPathfinder = new DirectionalPathfinder();
        }

        /// <summary>
        /// 지정한 노드에서 가장 가까운 교차로(3개 이상 연결된 노드)를 찾는다.
        /// </summary>
        /// <param name="startNode">기준이 되는 노드</param>
        /// <returns>가장 가까운 교차로 노드 (또는 null)</returns>
        public MapNode FindNearestJunction(MapNode startNode)
        {
            if (startNode == null || _mapNodes == null || _mapNodes.Count == 0)
                return null;

            // 교차로: 3개 이상의 노드가 연결된 노드
            var junctions = _mapNodes.Where(n =>
                n.IsActive &&
                n.IsNavigationNode() &&
                n.ConnectedNodes != null &&
                n.DisableCross == false &&
                n.ConnectedNodes.Count >= 3 &&
                n.ConnectedMapNodes.Where(t => t.CanDocking).Any() == false &&
                n.NodeId != startNode.NodeId
            ).ToList();

            // docking 포인트가 연결된 노드는 제거한다.


            if (junctions.Count == 0)
                return null;

            // 직선 거리 기반으로 가장 가까운 교차로 찾기
            MapNode nearestJunction = null;
            float minDistance = float.MaxValue;

            foreach (var junction in junctions)
            {
                float dx = junction.Position.X - startNode.Position.X;
                float dy = junction.Position.Y - startNode.Position.Y;
                float distance = (float)Math.Sqrt(dx * dx + dy * dy);

                if (distance < minDistance)
                {
                    minDistance = distance;
                    nearestJunction = junction;
                }
            }

            return nearestJunction;
        }

        /// <summary>
        /// 지정한 노드에서 경로상 가장 가까운 교차로를 찾는다.
        /// (최단 경로 내에서 3개 이상 연결된 교차로를 찾음)
        /// </summary>
        /// <param name="startNode">시작 노드</param>
        /// <param name="targetNode">목적지 노드</param>
        /// <returns>경로상의 가장 가까운 교차로 노드 (또는 null)</returns>
        public MapNode FindNearestJunctionOnPath(AGVPathResult pathResult)
        {
            if (pathResult == null || !pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
                return null;

            // 경로상의 모든 노드 중 교차로(3개 이상 연결) 찾기
            var StartNode = pathResult.Path.First();
            foreach (var pathNode in pathResult.Path)
            {
                if (pathNode != null &&
                    pathNode.IsActive &&
                    pathNode.IsNavigationNode() &&
                    pathNode.DisableCross == false &&
                    pathNode.ConnectedNodes != null &&
                    pathNode.ConnectedNodes.Count >= 3 &&
                    pathNode.ConnectedMapNodes.Where(t => t.CanDocking).Any() == false)
                {
                    if (pathNode.NodeId.Equals(StartNode.NodeId) == false)
                        return pathNode;
                }
            }

            return null;
        }

        public AGVPathResult FindPath(MapNode startNode, MapNode targetNode,
            MapNode prevNode, AgvDirection prevDirection, AgvDirection currentDirection, bool crossignore = false)
        {
            // 입력 검증
            if (startNode == null)
                return AGVPathResult.CreateFailure("시작 노드가 null입니다.", 0, 0);
            if (targetNode == null)
                return AGVPathResult.CreateFailure("목적지 노드가 null입니다.", 0, 0);
            if (prevNode == null)
                return AGVPathResult.CreateFailure("이전위치 노드가 null입니다.", 0, 0);
            if (startNode.NodeId == targetNode.NodeId && targetNode.DockDirection.MatchAGVDirection(prevDirection))
                return AGVPathResult.CreateSuccess(new List<MapNode> { startNode, startNode }, new List<AgvDirection>(), 0, 0);

            var ReverseDirection = (currentDirection == AgvDirection.Forward ? AgvDirection.Backward : AgvDirection.Forward);

            //1.목적지까지의 최단거리 경로를 찾는다.
            var pathResult = _basicPathfinder.FindPathAStar(startNode.NodeId, targetNode.NodeId);
            pathResult.PrevNode = prevNode;
            pathResult.PrevDirection = prevDirection;
            if (!pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
                return AGVPathResult.CreateFailure("각 노드간 최단 경로 계산이 실패되었습니다", 0, 0);

            //정방향/역방향 이동 시 다음 노드 확인
            // 경로 계획 단계에서는 마그넷 방향이 미리 알려지지 않으므로 Straight로 기본값 사용

            // ✅ 현재 방향 유지: prevDirection = currentDirection (방향 일관성)
            var nextNodeForward = DirectionalHelper.GetNextNodeByDirection(
                startNode, prevNode, currentDirection, currentDirection, MagnetDirection.Straight, _mapNodes);

            // ✅ 방향 전환: prevDirection = currentDirection, direction = ReverseDirection
            var nextNodeBackward = DirectionalHelper.GetNextNodeByDirection(
                startNode, prevNode, currentDirection, ReverseDirection, MagnetDirection.Straight, _mapNodes);


            //2.AGV방향과 목적지에 설정된 방향이 일치하면 그대로 진행하면된다.(목적지에 방향이 없는 경우에도 그대로 진행)
            if (targetNode.DockDirection == DockingDirection.DontCare ||
                (targetNode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Forward) ||
                (targetNode.DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Backward))
            {
                if ((nextNodeForward?.NodeId ?? string.Empty) == pathResult.Path[1].NodeId) //예측경로와 다음진행방향 경로가 일치하면 해당 방향이 맞다
                {
                    MakeDetailData(pathResult, currentDirection);
                    MakeMagnetDirection(pathResult);
                    for (int i = 0; i < pathResult.DetailedPath.Count; i++)
                        pathResult.DetailedPath[i].seq = i + 1;
                    return pathResult;
                }
            }


            //2-1 현재위치의 반대방향과 대상의 방향이 맞는 경우에도 그대로 사용가능하다.
            //if (targetNode.DockDirection == DockingDirection.DontCare ||
            //    (targetNode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Backward) ||
            //    (targetNode.DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Forward))
            //{
            //    // 반대 방향으로 이동하여 목적지에 진입해야 함
            //    // 현재 방향으로 가면서 역방향으로 도킹
            //    MakeDetailData(pathResult, ReverseDirection);
            //    MakeMagnetDirection(pathResult);
            //    return pathResult;
            //}


            //뒤로 이동시 경로상의 처음 만나는 노드가 같다면 그 방향으로 이동하면 된다.
            // ⚠️ 단, 현재 방향과 목적지 도킹 방향이 일치해야 함!
            if (nextNodeBackward != null && pathResult.Path.Count > 1 &&
                nextNodeBackward.NodeId == pathResult.Path[1].NodeId) // ✅ 추가: 현재도 Backward여야 함
            {
                if (targetNode.DockDirection == DockingDirection.Forward && ReverseDirection == AgvDirection.Forward ||
                   targetNode.DockDirection == DockingDirection.Backward && ReverseDirection == AgvDirection.Backward)
                {
                    MakeDetailData(pathResult, ReverseDirection);
                    MakeMagnetDirection(pathResult);
                    for (int i = 0; i < pathResult.DetailedPath.Count; i++)
                        pathResult.DetailedPath[i].seq = i + 1;
                    return pathResult;
                }
            }

            if (nextNodeForward != null && pathResult.Path.Count > 1 &&
                nextNodeForward.NodeId == pathResult.Path[1].NodeId &&
                targetNode.DockDirection == DockingDirection.Forward &&
                currentDirection == AgvDirection.Forward) // ✅ 추가: 현재도 Forward여야 함
            {
                if (targetNode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Forward ||
                    targetNode.DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Backward)
                {
                    MakeDetailData(pathResult, currentDirection);
                    MakeMagnetDirection(pathResult);
                    for (int i = 0; i < pathResult.DetailedPath.Count; i++)
                        pathResult.DetailedPath[i].seq = i + 1;
                    return pathResult;
                }

            }

            //if(nextNodeForward.NodeId == pathResult.Path[1])
            //{
            //    MakeDetailData(pathResult, currentDirection);
            //    MakeMagnetDirection(pathResult);
            //    return pathResult;
            //}

            //현재 내 포인트가 교차로라면.. 무조건 왓던 방향 혹은 그 반대방향으로 이동해서 경로를 계산해야한다.
            //교차로에 멈춰있을때에는 바로 방향전환을 할 수없으니. 정/역(straight)로 이동해서 다시 계산을 해야한다
            if (crossignore == false && startNode.ConnectedNodes.Count > 2)
            {
                //진행방향으로 이동했을때 나오는 노드를 사용한다.
                if (nextNodeForward != null)
                {
                    var Path0 = _basicPathfinder.FindPathAStar(startNode.NodeId, nextNodeForward.NodeId);
                    Path0.PrevNode = prevNode;
                    Path0.PrevDirection = prevDirection;
                    MakeDetailData(Path0, prevDirection);

                    var Path1 = FindPath(nextNodeForward, targetNode, startNode, prevDirection, currentDirection, true);
                    Path1.PrevNode = startNode;
                    Path1.PrevDirection = prevDirection;
                    //MakeDetailData(Path1, ReverseDirection);

                    var combinedResult0 = Path0;
                    combinedResult0 = _basicPathfinder.CombineResults(combinedResult0, Path1);
                    MakeMagnetDirection(combinedResult0);

                    for (int i = 0; i < combinedResult0.DetailedPath.Count; i++)
                        combinedResult0.DetailedPath[i].seq = i + 1;
                    return combinedResult0;
                }
                else if (nextNodeBackward != null)
                {
                    return AGVPathResult.CreateFailure("backward 처리코드가 없습니다 오류", 0, 0);
                }
                else
                {
                    return AGVPathResult.CreateFailure("교차로에서 시작하는 조건중 forward/backrad 노드 검색 실패", 0, 0);
                }
            }


            //3. 도킹방향이 일치하지 않으니 교차로에서 방향을 회전시켜야 한다
            //최단거리(=minpath)경로에 속하는 교차로가 있다면 그것을 사용하고 없다면 가장 가까운 교차로를 찾는다.
            var JunctionInPath = FindNearestJunctionOnPath(pathResult);
            if (JunctionInPath == null)
            {
                //시작노드로부터 가까운 교차로 검색
                JunctionInPath = FindNearestJunction(startNode);

                //종료노드로부터 가까운 교차로 검색
                if (JunctionInPath == null) JunctionInPath = FindNearestJunction(targetNode);
            }
            if (JunctionInPath == null)
                return AGVPathResult.CreateFailure("교차로가 없어 경로계산을 할 수 없습니다", 0, 0);

            //경유지를 포함하여 경로를 다시 계산한다.

            //1.시작위치 - 교차로(여기까지는 현재 방향으로 그대로 이동을 한다)
            var path1 = _basicPathfinder.FindPathAStar(startNode.NodeId, JunctionInPath.NodeId);
            path1.PrevNode = prevNode;
            path1.PrevDirection = prevDirection;

            //다음좌표를 보고 교차로가 진행방향인지 반대방향인지 체크한다.
            bool ReverseCheck = false;
            //if (path1.Path.Count > 1 && nextNodeForward != null && nextNodeForward.NodeId.Equals(path1.Path[1].NodeId))
            //{
            //    ReverseCheck = false; //현재 진행 방향으로 이동해야한다
            //    MakeDetailData(path1, currentDirection); // path1의 상세 경로 정보 채우기 (모터 방향 설정)
            //}
            if (path1.Path.Count > 1 && nextNodeBackward != null && nextNodeBackward.NodeId.Equals(path1.Path[1].NodeId))
            {
                ReverseCheck = true;    //현재 방향의 반대방향으로 이동해야한다
                MakeDetailData(path1, ReverseDirection); // path1의 상세 경로 정보 채우기 (모터 방향 설정)
            }
            else
            {
                ReverseCheck = false; //현재 진행 방향으로 이동해야한다
                MakeDetailData(path1, currentDirection); // path1의 상세 경로 정보 채우기 (모터 방향 설정)
            }






            //2.교차로 - 종료위치
            var path2 = _basicPathfinder.FindPathAStar(JunctionInPath.NodeId, targetNode.NodeId);
            path2.PrevNode = prevNode;
            path2.PrevDirection = prevDirection;

            //2번paths느최종 목적지 이므로 목적지와 도킹방향 확인해서 결정한다
            if ((path2.Path.Last().DockDirection == DockingDirection.Forward && ReverseDirection == AgvDirection.Forward) ||
                (path2.Path.Last().DockDirection == DockingDirection.Backward && ReverseDirection == AgvDirection.Backward))
            {
                MakeDetailData(path2, ReverseDirection);
            }
            else if ((path2.Path.Last().DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Forward) ||
                    (path2.Path.Last().DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Backward))
            {
                MakeDetailData(path2, currentDirection);
            }

            MapNode tempNode = null;


            //3.방향전환을 위환 대체 노드찾기
            tempNode = _basicPathfinder.FindAlternateNodeForDirectionChange(JunctionInPath.NodeId,
               path1.Path[path1.Path.Count - 2].NodeId,
               path2.Path[1].NodeId);

            //4. path1 + tempnode + path2 가 최종 위치가 된다.
            if (tempNode == null)
                return AGVPathResult.CreateFailure("방향 전환을 위한 대체 노드를 찾을 수 없습니다.", 0, 0);



            // path1 (시작 → 교차로)
            var combinedResult = path1;

            //교차로 대체노드를 사용한 경우
            //if (tempNode != null)
            {
                // 교차로 → 대체노드 경로 계산
                var pathToTemp = _basicPathfinder.FindPathAStar(JunctionInPath.NodeId, tempNode.NodeId);
                pathToTemp.PrevNode = JunctionInPath;
                pathToTemp.PrevDirection = (ReverseCheck ? ReverseDirection : currentDirection);
                if (!pathToTemp.Success)
                    return AGVPathResult.CreateFailure("교차로에서 대체 노드까지의 경로를 찾을 수 없습니다.", 0, 0);
                if (ReverseCheck) MakeDetailData(pathToTemp, ReverseDirection);
                else MakeDetailData(pathToTemp, currentDirection);

                //교차로찍고 원래방향으로 돌어가야한다.
                if (pathToTemp.DetailedPath.Count > 1)
                    pathToTemp.DetailedPath[pathToTemp.DetailedPath.Count - 1].MotorDirection = currentDirection;

                // path1 + pathToTemp 합치기
                combinedResult = _basicPathfinder.CombineResults(combinedResult, pathToTemp);

                // 대체노드 → 교차로 경로 계산 (역방향)
                var pathFromTemp = _basicPathfinder.FindPathAStar(tempNode.NodeId, JunctionInPath.NodeId);
                pathFromTemp.PrevNode = JunctionInPath;
                pathFromTemp.PrevDirection = (ReverseCheck ? ReverseDirection : currentDirection);
                if (!pathFromTemp.Success)
                    return AGVPathResult.CreateFailure("대체 노드에서 교차로까지의 경로를 찾을 수 없습니다.", 0, 0);

                if (ReverseCheck) MakeDetailData(pathFromTemp, currentDirection);
                else MakeDetailData(pathFromTemp, ReverseDirection);

                // (path1 + pathToTemp) + pathFromTemp 합치기
                combinedResult = _basicPathfinder.CombineResults(combinedResult, pathFromTemp);

                //현재까지 노드에서 목적지까지의 방향이 일치하면 그대로 사용한다.
                bool temp3ok = false;
                var TempCheck3 = _basicPathfinder.FindPathAStar(combinedResult.Path.Last().NodeId, targetNode.NodeId);
                if (TempCheck3.Path.First().NodeId.Equals(combinedResult.Path.Last().NodeId))
                {
                    if (targetNode.DockDirection == DockingDirection.Forward && combinedResult.DetailedPath.Last().MotorDirection == AgvDirection.Forward)
                    {
                        temp3ok = true;
                    }
                    else if (targetNode.DockDirection == DockingDirection.Backward && combinedResult.DetailedPath.Last().MotorDirection == AgvDirection.Backward)
                    {
                        temp3ok = true;
                    }
                }



                //대체노드에서 최종 목적지를 다시 확인한다.
                if (temp3ok == false)
                {
                    //목적지와 방향이 맞지 않다. 그러므로 대체노드를 추가로 더 찾아야한다.
                    var tempNode2 = _basicPathfinder.FindAlternateNodeForDirectionChange(JunctionInPath.NodeId,
               combinedResult.Path[combinedResult.Path.Count - 2].NodeId,
               path2.Path[1].NodeId);

                    var pathToTemp2 = _basicPathfinder.FindPathAStar(JunctionInPath.NodeId, tempNode2.NodeId);
                    if (ReverseCheck) MakeDetailData(pathToTemp2, currentDirection);
                    else MakeDetailData(pathToTemp2, ReverseDirection);

                    combinedResult = _basicPathfinder.CombineResults(combinedResult, pathToTemp2);

                    //교차로찍고 원래방향으로 돌어가야한다.
                    if (combinedResult.DetailedPath.Count > 1)
                    {
                        if (ReverseCheck)
                            combinedResult.DetailedPath[combinedResult.DetailedPath.Count - 1].MotorDirection = ReverseDirection;
                        else
                            combinedResult.DetailedPath[combinedResult.DetailedPath.Count - 1].MotorDirection = currentDirection;
                    }

                    var pathToTemp3 = _basicPathfinder.FindPathAStar(tempNode2.NodeId, JunctionInPath.NodeId);
                    if (ReverseCheck) MakeDetailData(pathToTemp3, ReverseDirection);
                    else MakeDetailData(pathToTemp3, currentDirection);

                    combinedResult = _basicPathfinder.CombineResults(combinedResult, pathToTemp3);
                }
            }

            // (path1 + pathToTemp + pathFromTemp) + path2 합치기
            combinedResult = _basicPathfinder.CombineResults(combinedResult, path2);

            MakeMagnetDirection(combinedResult);

            for (int i = 0; i < combinedResult.DetailedPath.Count; i++)
                combinedResult.DetailedPath[i].seq = i + 1;
            return combinedResult;


        }

        /// <summary>
        /// 이 작업후에 MakeMagnetDirection 를 추가로 실행 하세요
        /// </summary>
        /// <param name="path1"></param>
        /// <param name="currentDirection"></param>
        private void MakeDetailData(AGVPathResult path1, AgvDirection currentDirection)
        {
            if (path1.Success && path1.Path != null && path1.Path.Count > 0)
            {
                var detailedPath1 = new List<NodeMotorInfo>();
                for (int i = 0; i < path1.Path.Count; i++)
                {
                    var node = path1.Path[i];
                    string nodeId = node.NodeId;
                    string RfidId = node.RfidId;
                    string nextNodeId = (i + 1 < path1.Path.Count) ? path1.Path[i + 1].NodeId : null;

                    // 노드 정보 생성 (현재 방향 유지)
                    var nodeInfo = new NodeMotorInfo(i + 1,
                        nodeId, RfidId,
                        currentDirection,
                        nextNodeId,
                        MagnetDirection.Straight
                    );

                    detailedPath1.Add(nodeInfo);
                }

                // path1에 상세 경로 정보 설정
                path1.DetailedPath = detailedPath1;
            }
        }


        /// <summary>
        /// Path에 등록된 방향을 확인하여 마그넷정보를 업데이트 합니다
        /// </summary>
        /// <param name="path1"></param>
        private void MakeMagnetDirection(AGVPathResult path1)
        {
            if (path1.Success && path1.DetailedPath != null && path1.DetailedPath.Count > 0)
            {
                for (int i = 0; i < path1.DetailedPath.Count; i++)
                {
                    var detailPath = path1.DetailedPath[i];
                    string nodeId = path1.Path[i].NodeId;
                    string nextNodeId = (i + 1 < path1.Path.Count) ? path1.Path[i + 1].NodeId : null;

                    // 마그넷 방향 계산 (3개 이상 연결된 교차로에서만 좌/우 가중치 적용)
                    if (i > 0 && nextNodeId != null)
                    {
                        string prevNodeId = path1.Path[i - 1].NodeId;
                        if (path1.DetailedPath[i - 1].MotorDirection != detailPath.MotorDirection)
                            detailPath.MagnetDirection = MagnetDirection.Straight;
                        else
                            detailPath.MagnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, nodeId, nextNodeId, detailPath.MotorDirection);
                    }
                    else detailPath.MagnetDirection = MagnetDirection.Straight;
                }

            }
        }


    }
}