using AGVControl.Models;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Threading;

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

        private static ManualResetEvent mrepredict = new ManualResetEvent(true);

        /// <summary>
        /// 목표지정으로 모터방향이 이동하고 있는가?
        /// history 데이터가 있어야 하며 기준데이터가 없는 경우 null 반환
        /// </summary>
        /// <returns></returns>
        public static 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 static 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 static AGVActionPrediction PredictNextAction()
        {
            if (mrepredict.WaitOne(1) == false)
            {
                PredictResult = new AGVActionPrediction
                {
                    Reason = "이전 작업이 완료되지 않았습니다",
                    ReasonCode = AGVActionReasonCode.busy,
                    MoveState = AGVMoveState.Stop,
                    Direction = agv.CurrentMOTDirection,
                };
                return PredictResult;
            }
          
            mrepredict.Reset();
            try
            {
                // 0. 설정경로와 리프트 방향 체크 (경로설정이 없을때에는 직선이동경로내의 방향들과 체크한다)
                agv.IsTargetDirectionMatch = IsLiftDirectionMatch() ?? false;

                // 1. 위치를 모를 때 (CurrentRFID가 0 또는 미설정)
                if (agv.CurrentRFID.Value == 0)
                {
                    PredictResult = new AGVActionPrediction
                    {
                        Direction = Direction.Backward,
                        NextRFID = null,
                        Reason = "AGV 위치 미확정(처음 기동)",
                        ReasonCode = AGVActionReasonCode.NoPosition,
                        MoveState = AGVMoveState.Run
                    };
                    return PredictResult;
                }

                //2. 이동방향을 모른다
                if (agv.MovementHistory.Any() == false || agv.MovementHistory.Count < 2)
                {
                    PredictResult = new AGVActionPrediction
                    {
                        Direction = Direction.Backward,
                        NextRFID = null,
                        Reason = "AGV이동방향 알수없음",
                        ReasonCode = AGVActionReasonCode.NoDirection,
                        MoveState = AGVMoveState.Run
                    };
                    return PredictResult;
                }


                // 3. 경로가 없거나 현재 위치가 경로에 없음
                if ((agv.MainPath?.Count ?? 0) < 2)
                {
                    PredictResult = new AGVActionPrediction
                    {
                        Direction = agv.CurrentMOTDirection,
                        NextRFID = null,
                        Reason = "경로 없음 또는 현재 위치 미확정",
                        ReasonCode = AGVActionReasonCode.NoPath,
                        MoveState = AGVMoveState.Stop
                    };
                    return PredictResult;
                }

                // 4. 경로상에서 다음 RFID 예측
                int idx = agv.MainPath.FindIndex(p => p.Value == agv.CurrentRFID.Value);
                if (idx < 0)
                {
                    PredictResult = new AGVActionPrediction
                    {
                        Direction = agv.CurrentMOTDirection,
                        NextRFID = null,
                        Reason = "현재 위치가 경로에 없음",
                        ReasonCode = AGVActionReasonCode.NotOnPath,
                        MoveState = AGVMoveState.Stop
                    };
                    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 - 1).First();
                var curPT = agv.MovementHistory.Last();



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



                    //1. 가까운 회전위치를 찾는다
                    var nearTurnPoint = RFIDPoints.Where(t => t.IsRotatable)?.OrderBy(t => GetDistance(t.Location, agv.CurrentRFID.Location)).FirstOrDefault() ?? null;
                    if (nearTurnPoint == null)
                    {
                        PredictResult= new AGVActionPrediction
                        {
                            Direction = agv.CurrentMOTDirection,
                            NextRFID = null,
                            Reason = "회전 가능한 위치가 없습니다",
                            ReasonCode = AGVActionReasonCode.NoTurnPoint,
                            MoveState = AGVMoveState.Stop
                        };
                        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 != PrePT.Value ||
                        agv.SubPath.Last().Value != nearTurnPoint.Value)
                        {
                            var rlt = CalculatePath(PrePT, nearTurnPoint, true);  //이전포인트도 추가를 해준다
                            if (rlt.Success) agv.SubPath = rlt.Path;
                            else
                            {
                                agv.SubPath.Clear();
                                PredictResult= new AGVActionPrediction
                                {
                                    Direction = agv.CurrentMOTDirection,
                                    NextRFID = nearTurnPoint,
                                    Reason = "회전 위치까지의 경로를 계산할 수 없습니다",
                                    ReasonCode = AGVActionReasonCode.PathCalcError,
                                    MoveState = AGVMoveState.Stop
                                };
                                return PredictResult;
                            }
                        }


                        //현재 모터방향을 확인하여 대상까지 이동하도록 해야한다
                        var curidx = agv.SubPath.FindIndex(t => t.Value == curPT.Value);
                        var preidx = agv.SubPath.FindIndex(t => t.Value == PrePT.Value);
                        Direction newdirection = agv.CurrentMOTDirection;
                        string message = "턴위치로 이동중";
                        if (preidx > curidx)
                        {
                            //지정경로를 거꾸로 이동하고 있다
                            if (agv.CurrentMOTDirection == Direction.Forward)
                                newdirection = Direction.Backward;
                            else
                                newdirection = Direction.Forward;
                            message += "(방향전환)";
                        }

                        PredictResult= new AGVActionPrediction
                        {
                            Direction = newdirection,
                            NextRFID = nearTurnPoint,
                            Reason = message,
                            ReasonCode = AGVActionReasonCode.MoveForTurn,
                            MoveState = AGVMoveState.Run,
                        };
                        return PredictResult;
                    }


                    PredictResult =  new AGVActionPrediction
                    {
                        Direction = agv.CurrentMOTDirection,
                        NextRFID = nearTurnPoint,
                        Reason = "턴 완료 대기",
                        ReasonCode = AGVActionReasonCode.NeedTurn,
                        MoveState = AGVMoveState.Stop
                    };
                    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);
                    Direction newdirection = agv.CurrentMOTDirection;
                    string message = "목적지 이동중";
                    if (preidx > curidx)
                    {
                        //지정경로를 거꾸로 이동하고 있다
                        if (agv.CurrentMOTDirection == Direction.Forward)
                            newdirection = Direction.Backward;
                        else
                            newdirection = Direction.Forward;
                        message += "(방향전환)";
                    }

                    //경로상 바로 다음 위치를 확인한다
                    var nexstRFID = agv.MainPath.Skip(agv.MainPath.FindIndex(t => t.Value == curPT.Value) + 1).First();
                    PredictResult =  new AGVActionPrediction
                    {
                        Direction = newdirection,
                        NextRFID = nexstRFID,
                        Reason = message,
                        ReasonCode = AGVActionReasonCode.Normal,
                        MoveState = AGVMoveState.Run,
                    };
                    return PredictResult;
                }


                // 5. 목적지 도달 시
                PredictResult = new AGVActionPrediction
                {
                    Direction = agv.CurrentMOTDirection,
                    NextRFID = destRFID,
                    Reason = "경로의 마지막 지점(목적지 도달)",
                    ReasonCode = AGVActionReasonCode.Arrived,
                    MoveState = AGVMoveState.Stop
                };
                return PredictResult;
            }
            catch (Exception ex)
            {
                PredictResult = new AGVActionPrediction
                {
                    Direction = agv.CurrentMOTDirection,
                    NextRFID = null,
                    Reason = $"ERR:{ex.Message}",
                    ReasonCode = AGVActionReasonCode.Unknown,
                    MoveState = AGVMoveState.Stop
                };
                return PredictResult;
            }
            finally
            {
                mrepredict.Set();
            }

        }


        /// <summary>
        /// 이웃포인터를 반환합니다
        /// </summary>
        /// <param name="point"></param>
        /// <returns></returns>
        public static 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 static RFIDPoint FindRFIDPoint(uint rfidValue)
        {
            if (RFIDPoints == null || RFIDPoints.Any() == false) return null;
            return RFIDPoints.FirstOrDefault(r => r.Value == rfidValue);
        }
        private static 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 static 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 static PathResult CalculatePath(RFIDPoint start, RFIDPoint end, bool autorun)
        {
            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 static 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, false);
            if (retval.Success == false)
                retval.Message = "경로를 찾을 수 없습니다";

            return retval;
        }

        /// <summary>
        /// 리프트방향과 대상위치와의 방향이 일치하는가?
        /// 목적지경로가 셋팅된 경우 현재 이동방향이 목적지방향과 일치하는가?
        /// 이동경로정보가 없거나 목적지가 없으면 null 이 반환됨
        /// </summary>
        /// <returns></returns>
        public static 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 == Direction.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;
            }
        }



    }
}