using AGVControl.Models;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Security.Cryptography.X509Certificates;
using System.Threading;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.TextBox;

namespace AGVControl
{
    public class MapControlManager
    {
        public AGVActionPrediction PredictResult = null;
        public AGV agv = new AGV();
        public List<RFIDPoint> RFIDPoints = new List<RFIDPoint>();
        public HashSet<RFIDConnection> rfidConnections = new HashSet<RFIDConnection>();

        private ManualResetEvent mrepredict = new ManualResetEvent(true);

        public MapControlManager()
        {

        }
        ~MapControlManager()
        {

        }

        /// <summary>
        /// 목표지정으로 모터방향이 이동하고 있는가?
        /// history 데이터가 있어야 하며 기준데이터가 없는 경우 null 반환
        /// </summary>
        /// <returns></returns>
        public bool? IsMotDirection_To_Target()
        {
            if (agv.MovementHistory.Any() == false || agv.MovementHistory.Count < 2) return null;
            if (agv.MainPath.Any() == false) return null;


            var prept = agv.MovementHistory.Skip(agv.MovementHistory.Count - 2).First();
            var lstpt = agv.MovementHistory.Last();

            //현재 이후의 경로를 가져온다
            var curidx = agv.MainPath.FindIndex(t => t.Value == lstpt.Value);
            var preidx = agv.MainPath.FindIndex(t => t.Value == prept.Value);
            if (curidx == -1 || preidx == -1) return null;

            //지정된경로 반대방향으로 이동하고 있다
            return preidx < curidx;
        }
        public float GetDistance(Point p1, Point p2)
        {
            float dx = p1.X - p2.X;
            float dy = p1.Y - p2.Y;
            return (float)Math.Sqrt(dx * dx + dy * dy);  // double을 float로 명시적 캐스팅
        }

        public AGVActionPrediction PredictNextAction()
        {
            if (mrepredict.WaitOne(1) == false)
            {
                PredictResult = CreatePrediction("이전 작업이 완료되지 않았습니다",
                 AGVActionReasonCode.busy,
                 AGVMoveState.Stop,
                 agv.Current_Motor_Direction, false);
                return PredictResult;
            }

            mrepredict.Reset();
            try
            {
                // 0. 설정경로와 리프트 방향 체크 (경로설정이 없을때에는 직선이동경로내의 방향들과 체크한다)
                agv.IsTargetDirectionMatch = IsLiftDirectionMatch() ?? false;

                // 1. 위치를 모를 때 (CurrentRFID가 0 또는 미설정)
                if (agv.CurrentRFID.Value == 0)
                {
                    PredictResult = CreatePrediction("AGV 위치 미확정(처음 기동)",
                    AGVActionReasonCode.NoPosition,
                    AGVMoveState.Run,
                    AgvDir.Backward, true,
                    moveSpeed: AgvSpeed.Low,
                    moveDiv: AgvSts.Straight);
                    return PredictResult;
                }

                //2. 이동방향을 모른다
                if (agv.MovementHistory.Any() == false || agv.MovementHistory.Count < 2)
                {
                    PredictResult = CreatePrediction("AGV이동방향 알수없음",
                    AGVActionReasonCode.NoDirection,
                    AGVMoveState.Run,
                    AgvDir.Backward, true,
                    moveSpeed: AgvSpeed.Low,
                    moveDiv: AgvSts.Straight);
                    return PredictResult;
                }

                // 3. 경로가 없거나 현재 위치가 경로에 없음
                if ((agv.MainPath?.Count ?? 0) < 2 || agv.MainPath.Last().Value != agv.TargetRFID.Value)
                {
                    //목적지가 없다면 진행할 수 없다
                    if (agv.TargetRFID == null)
                    {
                        PredictResult = CreatePrediction("경로 없음 또는 현재 위치 미확정",
                        AGVActionReasonCode.NoPath,
                        AGVMoveState.Stop,
                        agv.Current_Motor_Direction, true);
                        return PredictResult;
                    }
                    else
                    {
                        //목적지가 있는데 경로가 없다면 경로 예측을 진행해야한다.
                        var CurPt = agv.MovementHistory.Last();
                        var prlt = CalculatePath(CurPt, agv.TargetRFID);
                        if (prlt.Success == false)
                        {
                            PredictResult = CreatePrediction("목적지 경로 예측 실패",
                            AGVActionReasonCode.Unknown,
                            AGVMoveState.Stop,
                            agv.Current_Motor_Direction, true,
                            nextRFID: agv.TargetRFID);
                            return PredictResult;
                        }
                        else
                        {
                            //신규목적지에 대한 경로예측이 성공했다
                            agv.SubPath.Clear();
                            agv.MainPath = prlt.Path;
                        }
                    }
                }

                // 4. 경로상에서 다음 RFID 예측
                int idx = agv.MainPath.FindIndex(p => p.Value == agv.CurrentRFID.Value);
                if (idx < 0)
                {
                    PredictResult = CreatePrediction("현재 위치가 경로에 없음",
                    AGVActionReasonCode.NotOnPath,
                    AGVMoveState.Stop,
                    agv.Current_Motor_Direction, true);
                    return PredictResult;
                }

                // 4. 목적지 도달 전, 회전이 필요한경우인가?
                // 목적지 RFID 정보
                var destRFID = agv.MainPath.Last();

                //리프트 방향이 맞는가?
                var IsLiftDir = IsLiftDirectionMatch() ?? false;

                //모션이동방향이 맞는가?
                var IsMotDir = IsMotDirection_To_Target() ?? false;

                var PrePT = agv.MovementHistory.Skip(agv.MovementHistory.Count - 2).First();
                var curPT = agv.MovementHistory.Last();

                //리프트방향이 맞지 않다면 회전가능한 위치로 이동을 해야한다
                if (IsLiftDir == false)
                {
                    AgvSpeed? agv_spd = null;
                    AgvSts? agv_dir = null;

                    //회전가능한 위치로 이동을 해야한다

                    //1. 가까운 회전위치를 찾는다
                    var nearTurnPoint = RFIDPoints.Where(t => t.IsRotatable)?.OrderBy(t => GetDistance(t.Location, agv.CurrentRFID.Location)).FirstOrDefault() ?? null;
                    if (nearTurnPoint == null)
                    {
                        PredictResult = CreatePrediction("회전 가능한 위치가 없습니다",
                        AGVActionReasonCode.NoTurnPoint,
                        AGVMoveState.Stop,
                        agv.Current_Motor_Direction, true);
                        return PredictResult;
                    }

                    //2. 이동하기위한 경로계산 및 이동을 한다 (생성조건)
                    //2-1. 서브경로가없는경우
                    //2-2. 시작과 종료번호가 다른 경우(경로가 변경이 되는 조건이다)
                    if (agv.CurrentRFID.Value != nearTurnPoint.Value)
                    {
                        if (agv.SubPath.Any() == false || agv.SubPath.Count < 2 ||
                        agv.SubPath.First().Value != agv.CurrentRFID.Value ||
                        agv.SubPath.Last().Value != nearTurnPoint.Value)
                        {
                            var rlt = CalculatePath(agv.CurrentRFID, nearTurnPoint);  //이전포인트도 추가를 해준다
                            if (rlt.Success) agv.SubPath = rlt.Path;
                            else
                            {
                                agv.SubPath.Clear();
                                PredictResult = CreatePrediction("회전 위치까지의 경로를 계산할 수 없습니다",
                                AGVActionReasonCode.PathCalcError,
                                AGVMoveState.Stop,
                                agv.Current_Motor_Direction, true,
                                nextRFID: nearTurnPoint);
                                return PredictResult;
                            }
                        }


                        //현재 모터방향을 확인하여 대상까지 이동하도록 해야한다
                        var curidx = agv.SubPath.FindIndex(t => t.Value == curPT.Value);
                        var preidx = agv.SubPath.FindIndex(t => t.Value == PrePT.Value);
                        AgvDir newdirection = agv.Current_Motor_Direction;
                        string message = "턴위치로 이동중";



                        if (preidx > curidx)
                        {
                            //지정경로를 거꾸로 이동하고 있다
                            if (agv.Current_Motor_Direction == AgvDir.Forward)
                                newdirection = AgvDir.Backward;
                            else
                                newdirection = AgvDir.Forward;
                            message += "(방향전환)";
                        }

                        //도로정보를 확인하여 속도와 분기명령을 실행한다
                        var roadinfo = GetRoadInfo(agv.SubPath, curPT);
                        agv_spd = roadinfo.spd;
                        agv_dir = roadinfo.dir;

                        PredictResult = CreatePrediction(message,
                        AGVActionReasonCode.MoveForTurn,
                        AGVMoveState.Run,
                        newdirection, true,
                        moveSpeed: agv_spd,
                        moveDiv: agv_dir,
                        nextRFID: nearTurnPoint);
                        return PredictResult;
                    }

                    var roadinfo2 = GetRoadInfo(agv.SubPath.Any() ? agv.SubPath : agv.MainPath, curPT);
                    agv_spd = roadinfo2.spd;
                    agv_dir = roadinfo2.dir;


                    PredictResult = CreatePrediction("턴(이동) 완료 대기",
                    AGVActionReasonCode.NeedTurnMove,
                    AGVMoveState.Stop,
                    agv.Current_Motor_Direction, true,
                    moveSpeed: agv_spd,
                    moveDiv: agv_dir,
                    nextRFID: nearTurnPoint);
                    return PredictResult;
                }

                //보조이동선제거
                if (agv.SubPath != null && agv.SubPath.Any())
                    agv.SubPath.Clear();

                //현재위치의 RFID에서 턴이 필요한경우
                if (agv.CurrentRFID.NeedTurn)
                {
                    //Turn이 완료되지 않았다면 턴 완료를 대기한다.
                    if (agv.CurrentRFID.TurnOK == false)
                    {
                        //아직 턴위치에 멈추지 않았다
                        if (agv.CurrentRFID.TurnStop == false)
                        {
                            //현재위치로 마크스탑이동을 하게한다
                            PredictResult = CreatePrediction("Wait for Turn(P)-Mark Stop",
                                            AGVActionReasonCode.WaitForMarkStop,
                                            AGVMoveState.Run,
                                            agv.Current_Motor_Direction, true,                                            
                                            moveSpeed: agv.CurrentSpeed,
                                            moveDiv: agv.CurrentSTS);
                        }
                        else
                        {
                            //턴위치에 정지했으니. 턴완료를 기다려야 한다
                            PredictResult = CreatePrediction("Wait for Turn(P)",
                                            AGVActionReasonCode.NeedTurnPoint,
                                            AGVMoveState.Run,
                                            agv.Current_Motor_Direction, true,
                                            moveSpeed: agv.CurrentSpeed,
                                            moveDiv: agv.CurrentSTS);
                        }
                        return PredictResult;
                    }
                }


                //3. 목적지위치까지 이동이 완료되지 않았다면 계속 이동을 하게한다
                if (agv.CurrentRFID.Value != destRFID.Value)
                {
                    //현재 모터방향을 확인하여 대상까지 이동하도록 해야한다
                    var curidx = agv.MainPath.FindIndex(t => t.Value == curPT.Value);
                    var preidx = agv.MainPath.FindIndex(t => t.Value == PrePT.Value);
                    AgvDir newdirection = agv.Current_Motor_Direction;
                    string message = "목적지 이동중";
                    if (preidx > curidx)
                    {
                        //지정경로를 거꾸로 이동하고 있다
                        if (agv.Current_Motor_Direction == AgvDir.Forward)
                            newdirection = AgvDir.Backward;
                        else
                            newdirection = AgvDir.Forward;
                        message += "(방향전환)";
                    }

                    //경로상 바로 다음 위치를 확인한다
                    var nexstRFID = agv.MainPath.Skip(agv.MainPath.FindIndex(t => t.Value == curPT.Value) + 1).First();
                    var roadinfo = GetRoadInfo(agv.MainPath, curPT);


                    PredictResult = CreatePrediction(message,
                    AGVActionReasonCode.Normal,
                    AGVMoveState.Run,
                    newdirection, true,
                    moveSpeed: roadinfo.spd,
                    moveDiv: roadinfo.dir,
                    nextRFID: nexstRFID);
                    return PredictResult;
                }


                // 5. 목적지 도달 시
                PredictResult = CreatePrediction("경로의 마지막 지점(목적지 도달)",
                AGVActionReasonCode.Arrived,
                AGVMoveState.Stop,
                agv.Current_Motor_Direction, true,
                nextRFID: destRFID);
                return PredictResult;
            }
            catch (Exception ex)
            {

                PredictResult = CreatePrediction($"ERR:{ex.Message}",
                AGVActionReasonCode.Unknown,
                AGVMoveState.Stop,
                agv.Current_Motor_Direction, true);
                return PredictResult;
            }
            finally
            {
                mrepredict.Set();
            }

        }

        (AgvSpeed? spd, AgvSts? dir, RFIDConnection info) GetRoadInfo(List<RFIDPoint> paths, RFIDPoint curPT)
        {
            //도로정보를 확인하여 속도와 분기명령을 실행한다
            AgvSpeed? agv_spd = null;
            AgvSts? agv_div = null;
            RFIDConnection info = null;

            var nextpt = paths.Skip(paths.FindIndex(t => t.Value == curPT.Value) + 1).FirstOrDefault();
            if (nextpt != null)
            {
                var p1 = rfidConnections.Where(t => t.P1.Value == curPT.Value && t.P2.Value == nextpt.Value).FirstOrDefault();
                if (p1 != null)
                {
                    //positive
                    agv_spd = p1.MoveSpeedP;
                    agv_div = p1.MoveDirectionP;
                    info = p1;
                }

                var p2 = rfidConnections.Where(t => t.P2.Value == curPT.Value && t.P1.Value == nextpt.Value).FirstOrDefault();
                if (p2 != null)
                {
                    //negative
                    agv_spd = p2.MoveSpeedN;
                    agv_div = p2.MoveDirectionN;
                    info = p2;
                }
            }
            return (agv_spd, agv_div, info);
        }

        /// <summary>
        /// 이웃포인터를 반환합니다
        /// </summary>
        /// <param name="point"></param>
        /// <returns></returns>
        public List<RFIDPoint> GetNeighbors(RFIDPoint pt)
        {
            var neighbors = new List<RFIDPoint>();

            //값이 없는 경우 오류 반환
            if (pt == null) return neighbors;

            //연결정보에서 데이터를 찾은 후 반환한다
            foreach (var connection in rfidConnections)
            {
                RFIDPoint nPT = null;
                if (connection.P1.Value == pt.Value)
                {
                    nPT = connection.P2;
                }
                else if (connection.P2.Value == pt.Value)
                {
                    nPT = connection.P1;
                }

                if (nPT != null) neighbors.Add(nPT);
            }

            //중복제거후 반한
            return neighbors.Distinct().ToList();
        }

        public RFIDPoint FindRFIDPoint(uint rfidValue)
        {
            if (RFIDPoints == null || RFIDPoints.Any() == false) return null;
            return RFIDPoints.FirstOrDefault(r => r.Value == rfidValue);
        }
        private float Heuristic(Point a, Point b)
        {
            return (float)Math.Sqrt(Math.Pow(a.X - b.X, 2) + Math.Pow(a.Y - b.Y, 2));
        }

        private PathResult ReconstructPath(Dictionary<uint, RFIDPoint> cameFrom, RFIDPoint current)
        {
            var path = new List<RFIDPoint> { current };
            while (cameFrom.ContainsKey(current.Value))
            {
                current = cameFrom[current.Value];
                if (path.Contains(current) == false)
                    path.Insert(0, current);
                else
                    break;  //왜인지 반복되는경우가있어 종료한다
            }
            return new PathResult
            {
                Path = path,
            };
        }
        public PathResult CalculatePath(RFIDPoint start, RFIDPoint end)
        {
            var openList = new List<RFIDPoint> { start };
            var closedList = new List<RFIDPoint>();
            var cameFrom = new Dictionary<uint, RFIDPoint>();
            var gScore = new Dictionary<RFIDPoint, float> { { start, 0 } };
            var fScore = new Dictionary<RFIDPoint, float> { { start, Heuristic(start.Location, end.Location) } };



            if (start.Value == end.Value)
            {
                return new PathResult
                {
                    Message = "시작위치와 대상위치가 동일 합니다",
                    Path = null,
                };
            }

            //if (autorun) openList.Clear();


            while (openList.Count > 0)
            {
                var current = openList.OrderBy(p => fScore.ContainsKey(p) ? fScore[p] : float.MaxValue).First();

                if (current.Value == end.Value)
                {
                    return ReconstructPath(cameFrom, current);
                }

                openList.Remove(current);
                closedList.Add(current);

                var nb = GetNeighbors(current);
                foreach (var neighbor in nb)
                {
                    if (closedList.Contains(neighbor))
                        continue;

                    float tentativeGScore = gScore[current] + GetDistance(current.Location, neighbor.Location);

                    if (!openList.Contains(neighbor))
                        openList.Add(neighbor);
                    else if (tentativeGScore >= gScore[neighbor])
                        continue;

                    cameFrom[neighbor.Value] = (RFIDPoint)current;
                    gScore[neighbor] = tentativeGScore;
                    fScore[neighbor] = gScore[neighbor] + Heuristic(neighbor.Location, end.Location);


                }

            }

            return new PathResult
            {
                Path = openList,
            };

        }
        public PathResult CalculatePath(uint tagStrt, uint tagEnd)
        {
            var retval = new PathResult
            {
                Message = string.Empty,
                Path = new List<RFIDPoint>(),
            };

            var startPoint = FindRFIDPoint(tagStrt);
            var endPoint = FindRFIDPoint(tagEnd);
            if (startPoint == null || endPoint == null)
            {
                retval.Message = "유효한 RFID 값을 입력해주세요.";
                return retval;
            }

            retval = CalculatePath(startPoint, endPoint);
            if (retval.Success == false)
                retval.Message = "경로를 찾을 수 없습니다";

            return retval;
        }

        /// <summary>
        /// 리프트방향과 대상위치와의 방향이 일치하는가?
        /// 목적지경로가 셋팅된 경우 현재 이동방향이 목적지방향과 일치하는가?
        /// 이동경로정보가 없거나 목적지가 없으면 null 이 반환됨
        /// </summary>
        /// <returns></returns>
        public bool? IsLiftDirectionMatch()
        {

            if (agv.MovementHistory.Any() && agv.MovementHistory.Count > 1)
            {
                RFIDPoint TargetPT = null;
                var prept = agv.MovementHistory.Skip(agv.MovementHistory.Count - 2).First();
                var lstpt = agv.MovementHistory.Last();

                //뒤로이동하는경우라면 이전위치에 리프트가 있다.
                if (lstpt.Direction == AgvDir.Backward)
                {
                    TargetPT = prept;
                }
                else //앞으로 이동한다면 이동방향과 동일하다
                {
                    //이전위치는 제거 하고 처음발견된 것을 대상으로 한다
                    TargetPT = GetNeighbors(lstpt).Where(t => t.Value != prept.Value).FirstOrDefault();
                }

                //목적지가 있다면 목적지의 방향과 일치하는지 확인해야한다
                //남은경로중에 방향이 고정된 핀이 있다면 그것과 일치하는지 확인해야 한다
                if (agv.MainPath.Any())
                {
                    //지정된경로 반대방향으로 이동하고 있다
                    if ((IsMotDirection_To_Target() ?? false) == false)
                    {
                        return false;
                    }
                    else
                    {
                        var nextRoutes = agv.MainPath.Skip(agv.MainPath.FindIndex(t => t.Value == lstpt.Value) + 1).ToList();
                        var DirectionMatch = true;
                        foreach (var item in nextRoutes)
                        {
                            if (item.FixedDirection != null && item.FixedDirection != lstpt.Direction)
                            {
                                DirectionMatch = false;
                                break;
                            }
                        }
                        return DirectionMatch;
                    }
                }
                else
                {
                    //대상포인트와의 방향만 체크한다.
                    //고정대상이없다면 방향이 맞는것으로 한다
                    return (TargetPT.FixedDirection ?? lstpt.Direction) == lstpt.Direction;
                }
            }
            else
            {
                //이동된경로정보가 없다면 리프트 방향을 체크할 수 없으므로 대상과 위치가 맞지 않는걸로 기본값을 설정한다
                //이렇게 설정하면 대상으로 이동불가하고 뒤로 가도록 유도된다
                return null;
            }
        }

        // Changed 속성 설정을 위한 헬퍼 메서드
        private bool IsPredictionChanged(AGVActionPrediction newPrediction)
        {
            if (PredictResult == null) return true; // 이전 예측이 없으면 변경됨

            // Idx와 Changed를 제외하고 비교
            return !PredictResult.Equals(newPrediction);
        }

        private AGVActionPrediction CreatePrediction(
            string reason,
            AGVActionReasonCode reasonCode,
            AGVMoveState moveState,
            AgvDir direction, bool IDXUpdate = true,
            AgvSpeed? moveSpeed = null,
            AgvSts? moveDiv = null,
            RFIDPoint nextRFID = null
            )
        {
            var newPrediction = new AGVActionPrediction
            {
                NextRFID = nextRFID,
                Reason = reason,
                ReasonCode = reasonCode,
                MoveState = moveState,
                Direction = direction,
                MoveSpeed = moveSpeed,
                MoveDiv = moveDiv,
                Idx = IDXUpdate ? (PredictResult?.Idx + 1 ?? 1) : (PredictResult?.Idx ?? 0),
                CreateTime = DateTime.Now,
            };

            newPrediction.Changed = IsPredictionChanged(newPrediction);
            return newPrediction;
        }

    }
}