using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using Project.StateMachine;
using COMM;
using AR;
using AGVNavigationCore.Utils;
using AGVNavigationCore.PathFinding.Core;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;

namespace Project
{
    public partial class fMain
    {

        bool CheckStopCondition()
        {

            //이머전시상태라면 stop 처리한다.
            if (PUB.AGV.error.Emergency &&
                PUB.AGV.system1.agv_stop == true &&
                PUB.AGV.system1.stop_by_front_detect == false)
            {
                PUB.Speak(Lang.비상정지로인해작업을중단합니다);
                PUB.sm.SetNewStep(eSMStep.IDLE);
                return false;
            }

            //수동충전상태라면 이동하지 못한다
            if (VAR.BOOL[eVarBool.FLAG_CHARGEONM])
            {
                PUB.Speak("수동 충전중이라 사용할 수 없습니다");
                PUB.sm.SetNewStep(eSMStep.IDLE);
                return false;
            }


            return true;
        }

        AGVPathResult CalcPath(MapNode startNode, MapNode targetNode)
        {
            var nodes = PUB._mapCanvas.Nodes;
            var _simulatorCanvas = PUB._mapCanvas;
            var _mapNodes = PUB._mapCanvas.Nodes;

            // 현재 AGV 방향 가져오기
            var selectedAGV = PUB._virtualAGV;
            var currentDirection = selectedAGV.CurrentDirection;

            // AGV의 이전 위치에서 가장 가까운 노드 찾기
            var prevNode = selectedAGV.PrevNode;
            var prevDir = selectedAGV.PrevDirection;

            // Core Logic으로 이관됨
            var pathFinder = new AGVPathfinder(nodes);
            var result = pathFinder.CalculateScriptedPath(startNode, targetNode, prevNode, prevDir);

            //게이트웨이노드를 하이라이트강조 한단
            _simulatorCanvas.HighlightNodeId = (result.Gateway?.Id ?? string.Empty);
            return result;


        }


        /// <summary>
        /// 실행스텝을 오류로 전환합니다.
        /// 로그메세지(에러)가 추가됩니다.
        /// 맵캔버스오류경고메세지가 설정됩니다
        /// </summary>
        /// <param name="ermsg"></param>
        /// <param name="ecode"></param>
        public void SetRunStepError(ENIGProtocol.AGVErrorCode ecode, string errmsg = "")
        {
            if (string.IsNullOrEmpty(errmsg))
            {
                errmsg = ENIGProtocol.AGVUtility.GetAGVErrorMessage(ecode);
            }
            PUB.Result.ResultMessage = errmsg;
            PUB.XBE.SendError(ecode);

            PUB.AGV.AGVMoveStop(errmsg);
            PUB.log.AddE(errmsg);
            PUB._mapCanvas.SetInfoMessage(errmsg);
            PUB.Result.RunStepErrorCode = ecode;
            PUB.sm.SetNewRunStep(ERunStep.ERROR);
        }

        /// <summary>
        /// 라이더멈춤이 설정되어있다면 음성으로 알려준다
        /// </summary>
        /// <returns></returns>
        public bool CheckLiderStop()
        {

            if (PUB.AGV.system1.stop_by_front_detect == true)
            {
                var tsSpeak = DateTime.Now - LastSpeakTime;
                if (tsSpeak.TotalSeconds >= PUB.setting.alarmSoundTerm)
                {
                    PUB.Speak(Lang.전방에물체가감지되었습니다);
                    LastSpeakTime = DateTime.Now;
                }
                return false;
            }
            return true;
        }

        /// <summary>
        /// 설정된 목적지까지 이동을 완료 한 후 True를 반환합니다.
        /// 목적지 : PUB._virtualAGV.TargetNode
        /// 
        /// </summary>
        /// <param name="sender"></param>
        /// <returns></returns>
        Boolean UpdateMotionPositionForMark(string sender)
        {
            ////현재위치를 모르는 상태라면 처리하지 않는다
            if (PUB._virtualAGV.CurrentNode == null || PUB._virtualAGV.PrevNode == null)
                return false;

            //현재위치노드 오류
            var currentNode = PUB.FindByNodeID(PUB._virtualAGV.CurrentNode.Id);
            if (currentNode == null)
            {
                PUB.log.AddE($"현재위치노드가 없습니다");
                PUB.sm.SetNewRunStep(ERunStep.READY);
                return false;
            }

            //시작노드값이 없다면 현재위치를 노드로 결정한다
            if (PUB._virtualAGV.StartNode == null)
                PUB._virtualAGV.StartNode = PUB.FindByNodeID(PUB._virtualAGV.CurrentNode.Id);

            //시작노드가없다면 오류
            if (PUB._virtualAGV.StartNode == null)
            {
                PUB.log.AddE($"경로시작노드가 없습니다");
                PUB.sm.SetNewRunStep(ERunStep.READY);
                return false;
            }

            //대상노드가 없다면 오류
            if (PUB._virtualAGV.TargetNode == null)
            {
                PUB.log.AddE($"경로종료노드가 없습니다");
                PUB.sm.SetNewRunStep(ERunStep.READY);
                return false;
            }

            //경로 생성(경로정보가 없거나 현재노드가 경로에 없는경우)
            if (PUB._virtualAGV.HasPath() == false ||
                PUB._virtualAGV.CurrentPath.DetailedPath.Where(t => t.NodeId.Equals(currentNode.Id)).Any() == false)
            {
                if (PUB.AGV.system1.agv_run)
                {
                    PUB.log.Add($"경로 재생성으로 인해 구동을 멈춥니다");
                    PUB.AGV.AGVMoveStop("경로재생성");
                }

                PUB._virtualAGV.StartNode = PUB._virtualAGV.CurrentNode;
                var PathResult = CalcPath(currentNode, PUB._virtualAGV.TargetNode);
                if (PathResult.Success == false)
                {
                    PUB.log.AddE($"경로가 계산되지 않았습니다");
                    PUB.sm.SetNewRunStep(ERunStep.READY);
                    return false;
                }

                PUB._mapCanvas.CurrentPath = PathResult;
                PUB._virtualAGV.SetPath(PathResult);
                PUB.log.AddI($"경로생성 {PUB._virtualAGV.StartNode.RfidId} -> {PUB._virtualAGV.TargetNode.RfidId}");
            }

            //경로에 대한 무결성 검증
            if (CheckPathIntegrity(PUB._virtualAGV.CurrentPath) == false)
            {
                if (PUB.AGV.system1.agv_run)
                {
                    PUB.AGV.AGVMoveStop("Path Integrity Fail");
                }
                PUB.log.AddE($"경로 무결성 오류로 인해 경로를 삭제 합니다");
                PUB._virtualAGV.SetPath(null);
                VAR.I32[eVarInt32.PathValidationError] += 1;
                if (VAR.I32[eVarInt32.PathValidationError] > 50)
                {
                    SetRunStepError(ENIGProtocol.AGVErrorCode.PATH_INTEGRITY_FAIL, $"연속 경로 무결성 오류로 인해 중지 합니다");
                }
                return false;
            }
            else VAR.I32[eVarInt32.PathValidationError] = 0;


            //predict 를 이용하여 다음 이동을 모두 확인한다.
            var nextAction = PUB._virtualAGV.Predict();

            // [DEBUG] 예측 결과 로그 추가
            // 너무 빈번하게 찍히지 않도록 변화가 있을 때만 찍거나, 특정 조건에서 찍는 것이 좋으나
            // 디버깅 요청이므로 일단 주요 정보 출력
            // (실제 운용시에는 Verbose 레벨로 조정 필요)
            // PUB.log.Add($"[DEBUG] Predict: Reason={nextAction.Reason}, Motor={nextAction.Motor}, Magnet={nextAction.Magnet}, Speed={nextAction.Speed}");

            if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.PathOut)
            {
                //경로이탈
                var logmessage = $"경로이탈감지 시작노드를 현재위치로 설정합니다  START:{PUB._virtualAGV.StartNode},CURRENT:{PUB._virtualAGV.CurrentNode}";
                PUB.log.AddE(logmessage);
                Console.WriteLine(logmessage);
                PUB._virtualAGV.ClearPath();//.DetailedPath.Clear();
                PUB._virtualAGV.StartNode = PUB._virtualAGV.CurrentNode;
                return false;
            }

            //마크스탑을 해야하는데 움직이지 않는다면 움직이도록 해야한다
            if (nextAction.Motor == MotorCommand.Stop && nextAction.Reason == eAGVCommandReason.MarkStop)
            {
                if (PUB.AGV.system1.agv_run == false)
                {
                    PUB.log.Add("마크스탑을 해야하는데 중지상태라서 강제 이동하도록 합니다");
                    PUB.AGV.AGVMoveRun();
                    VAR.TIME[eVarTime.LastMarkStopCommandTime] = DateTime.Now.AddDays(-1);
                    return false;
                }
            }


            //모터에서 정지를 요청했다
            if (nextAction.Motor == AGVNavigationCore.Models.MotorCommand.Stop)
            {
                if (PUB.AGV.system1.agv_run)
                {
                    // 완료(Complete) 상태라면 MarkStop 전송
                    if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.MarkStop)
                    {
                        // 턴 중이거나 턴 동작을 방금 명령했다면 MarkStop 전송 생략
                        if (nextAction.IsTurn && PUB.AGV.TurnInformation.State == arDev.eNarumiTurn.LeftIng)
                        {
                            return false;
                        }

                        if (PUB.AGV.data.Speed != 'S')
                        {
                            //2초간격으로 명령을 전송한다
                            var tsCmd = VAR.TIME.RUN(eVarTime.LastMarkStopCommandTime);
                            if (tsCmd.TotalSeconds >= 2.0)
                            {
                                PUB.log.Add("다음행동예측에서 MARK-STOP이 확인되었습니다 (자동정지시퀀스)");
                                PUB.AGV.AGVMoveStop(nextAction.Message, arDev.Narumi.eStopOpt.MarkStop);
                                VAR.TIME.Update(eVarTime.LastMarkStopCommandTime);
                                LastCommandTime = DateTime.Now;
                            }
                        }
                    }
                    else
                    {
                        //마크스탑이 아니므로 바로 멈춰야한다
                        var tsCmd = VAR.TIME.RUN(eVarTime.LastStopCommandTime);
                        if (tsCmd.TotalSeconds >= 2.0)
                        {
                            PUB.log.Add($"다음행동예측에서 멈춤이 확인되었습니다({nextAction.Reason})");
                            PUB.AGV.AGVMoveStop(nextAction.Message);
                            VAR.TIME.Update(eVarTime.LastStopCommandTime);
                            LastCommandTime = DateTime.Now;
                        }
                    }

                    //이동중이라면 멈추는 명령을 전송하거나, 마크스탑을 진행해야하낟.
                    return false;
                }

                //여기시점에서는 장비는 멈춘 상태이다
                if (nextAction.IsTurn)
                {
                    // 1. 턴 완료 상태 확인
                    if (PUB.AGV.TurnInformation.State != arDev.eNarumiTurn.Left)
                    {
                        var tsTurn = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
                        // 턴 동작 미진행 (또는 이전 명령이 무시된 경우, 쿨타임 5초)
                        if (PUB.AGV.TurnInformation.State != arDev.eNarumiTurn.LeftIng && tsTurn.TotalSeconds > 5)
                        {
                            PUB.log.Add($"[경로실행] 180도 좌회전 명령 전송");
                            PUB.AGV.AGVMoveLeft180Turn();
                            VAR.TIME.Update(eVarTime.LastTurnCommandTime);
                        }
                        else if (PUB.AGV.TurnInformation.State == arDev.eNarumiTurn.LeftIng)
                        {
                            // 턴 진행 중 (타임아웃은 30초)
                            if (tsTurn.TotalSeconds > 30)
                            {
                                SetRunStepError(ENIGProtocol.AGVErrorCode.TURN_FAIL, "경로 중 턴 동작 대기 타임아웃 (30초)");
                            }
                            else
                            {
                                PUB._mapCanvas.SetInfoMessage($"턴 진행 중...({tsTurn.TotalSeconds:F1}/30)");
                            }
                        }
                        
                        // 턴이 완료될 때까지 대기 반환
                        return false;
                    }

                    // 2. 턴 정상 완료 
                    PUB.log.Add($"[경로실행] 180도 좌회전 완료 확인. 턴 명령 노드 패스 처리");
                    PUB._virtualAGV.SetCurrentNodeMarkStop(); // 첫번째 미완료 노드(IsTurn=true인 노드)를 Pass로 전환
                    
                    // 다음 루프 명령 실행 시 영향 없도록 Turn State 초기화
                    PUB.AGV.TurnInformation.State = arDev.eNarumiTurn.None; 
                    
                    // 이번 이동 함수 종료, 다음 루프에서 갱신된 Predict 호출
                    return false;
                }

                if (nextAction.Reason == eAGVCommandReason.Complete)
                {
                    // 목적지 도착 여부 확인
                    if (PUB.AGV.signal1.mark_sensor == false)
                    {
                        if (PUB._virtualAGV.TargetNode.StationType != StationType.Normal)
                        {
                            PUB.log.AddAT($"목표({PUB._virtualAGV.TargetNode.RfidId})도착되었으나 마크센서가 감지되지 않아 완료처리 하지않습니다");
                            return false;
                        }
                        else
                        {
                            PUB.log.AddI($"일반노드({PUB._virtualAGV.TargetNode.RfidId})는 마크센서가 감지되지 않아도 완료처리 합니다");
                            return true;
                        }
                    }

                    //완료되었고 마크센서가 확인된 상태이다
                    var lastInfo = PUB._virtualAGV.CurrentPath.DetailedPath.Last();

                    // 위치와 방향이 모두 일치해야 완료된 것으로 본다.
                    if (PUB._virtualAGV.CurrentNode.Id == lastInfo.NodeId &&
                        PUB._virtualAGV.CurrentDirection == lastInfo.MotorDirection)
                    {
                        var node = PUB._mapCanvas.Nodes.Where(t => t.Id == PUB._virtualAGV.CurrentNodeId).FirstOrDefault();
                        var rfid = node?.ID2 ?? "(X)";
                        PUB.log.AddI($"목표 도착 및 정지 확인됨(MarkStop 완료) Node:{rfid}, Dir:{PUB._virtualAGV.CurrentDirection}");
                        return true;
                    }
                    else
                    {
                        var errmsg = $"[도착오류] 목적지 정보 불일치. CurNode:{PUB._virtualAGV.CurrentNode.Id}, Target:{lastInfo.NodeId}, CurDir:{PUB._virtualAGV.CurrentDirection}, TargetDir:{lastInfo.MotorDirection}";
                        SetRunStepError(ENIGProtocol.AGVErrorCode.PATH_COMPLETE_INTEGRITY_FAIL, errmsg);
                        return false;
                    }
                }

                return false;
            }
            else
            {
                //PREDICT에서 이동을 명령했따

                
                var bunki = arDev.Narumi.eBunki.Strate;
                if (nextAction.Magnet == AGVNavigationCore.Models.MagnetPosition.L) bunki = arDev.Narumi.eBunki.Left;
                else if (nextAction.Magnet == AGVNavigationCore.Models.MagnetPosition.R) bunki = arDev.Narumi.eBunki.Right;

                var dir = arDev.Narumi.eMoveDir.Forward;
                if (nextAction.Motor == AGVNavigationCore.Models.MotorCommand.Backward) dir = arDev.Narumi.eMoveDir.Backward;

                var spd = arDev.Narumi.eMoveSpd.Low;
                if (nextAction.Speed == AGVNavigationCore.Models.SpeedLevel.M) spd = arDev.Narumi.eMoveSpd.Mid;
                else if (nextAction.Speed == AGVNavigationCore.Models.SpeedLevel.H) spd = arDev.Narumi.eMoveSpd.High;

                var char_bunki = bunki.ToString()[0];
                var char_speed = spd.ToString()[0];
                var char_dir = dir.ToString()[0];
                

                // 방향 전환 시 정지 로직 추가 ( 이동 중인데 방향이 다르면 먼저 정지시킨다.)
                if (PUB.AGV.system1.agv_run)
                {
                    if ((PUB.AGV.data.Direction == 'F' && dir == arDev.Narumi.eMoveDir.Backward) ||
                        (PUB.AGV.data.Direction == 'B' && dir == arDev.Narumi.eMoveDir.Forward))
                    {
                        var tsCmd = VAR.TIME.RUN(eVarTime.LastStopCommandTime);
                        if (tsCmd.TotalSeconds >= 2.0)
                        {
                            var msg = "$[predict] Direction Change({ PUB.AGV.data.Direction} → { dir})";
                            PUB.log.Add(msg);
                            PUB.AGV.AGVMoveStop(msg);
                            VAR.TIME[eVarTime.LastStopCommandTime] = DateTime.Now;
                        }
                        return false;
                    }
                }

                // 명령 설정
                // 현재 상태와 다를 때만 전송 (불필요한 통신 부하 방지)
                if (PUB.AGV.data.Sts != char_bunki ||
                    PUB.AGV.data.Direction != char_dir ||
                    PUB.AGV.data.Speed != char_speed ||
                    PUB.AGV.PBSSensor != arDev.eNarmiPBSSensor.on)
                {
                    // 2초 쿨타임 적용
                    var tsCmd = VAR.TIME.RUN(eVarTime.LastMoveSetCommandTime);// DateTime.Now - VAR.TIME;
                    if (tsCmd.TotalSeconds >= 2.0)
                    {
                        var mp = new arDev.Narumi.BunkiData
                        {
                            Bunki = bunki,
                            Direction = dir,
                            PBSSensor = 2,
                            Speed = spd,
                        };
                        var ret = PUB.AGV.AGVMoveSet(mp);

                        var msg = $"Predict Move Setting = bunki:{mp.Bunki},dir:{mp.Direction},pbs:1,spd:{mp.Speed}";
                        if (ret == arDev.eNarumiCommandResult.Success)
                            PUB.log.Add(msg);
                        else
                            PUB.log.AddE(msg);

                        VAR.TIME[eVarTime.LastMoveSetCommandTime] = DateTime.Now;
                    }
                    return false;
                }

                // AGV가 정지 상태라면 구동 시작 (라이다가켜져있을때에만 사용한다)
                if (PUB.AGV.system1.agv_run == false )
                {
                    if(PUB.AGV.PBSSensor == arDev.eNarmiPBSSensor.on)
                    {
                        // 2초 쿨타임 적용 (AGVMoveSet과 동일한 타이머 사용)
                        var tsCmd = VAR.TIME.RUN(eVarTime.LastRunCommandTime);// DateTime.Now - LastCommandTime;
                        if (tsCmd.TotalSeconds >= 2.0)
                        {
                            var msg = $"[PREDICT] Run AGV Dir:{dir}";
                            PUB.log.Add(msg);
                            var runOpt = (dir == arDev.Narumi.eMoveDir.Forward) ? arDev.Narumi.eRunOpt.Forward : arDev.Narumi.eRunOpt.Backward;
                            PUB.AGV.AGVMoveRun(runOpt);
                            VAR.TIME[eVarTime.LastRunCommandTime] = DateTime.Now;
                        }
                    }
                    else
                    {
                        PUB._mapCanvas.SetInfoMessage("AGV가동이 필요하나 PBS가 OFF되어있습니다");
                    }
                    return false;
                }



                return false;
            }

        }

        /// <summary>
        /// 충전기검색시퀀스
        /// </summary>
        /// <param name="sender"></param>
        /// <returns></returns>
        Boolean UpdateMotionPositionForCharger(string sender)
        {
            if (VAR.BOOL[eVarBool.AGVDIR_BACK] == false)// PUB.flag.get(EFlag.FLAG_DIR_BW) == true)
            {
                //충전기 검색은 항상 앞으로 검색한다
                var tsCmd = DateTime.Now - tm_gocharge_command;
                if (tsCmd.TotalMilliseconds >= 1999 &&
                    PUB.AGV.error.Emergency == false &&
                    PUB.AGV.system1.agv_run == false)
                {
                    var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
                    {
                        Bunki = arDev.Narumi.eBunki.Strate,
                        Direction = arDev.Narumi.eMoveDir.Forward,
                        PBSSensor = 2,
                        Speed = arDev.Narumi.eMoveSpd.Low,
                    });
                    if (ret == arDev.eNarumiCommandResult.Success)
                    {
                        PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);//
                    }

                    tm_gocharge_command = DateTime.Now;
                }
            }
            else
            {
                //현재위치가 충전위치이고, 움직이지 않았다면 완료된 경우라 할수 있따
                if (PUB._virtualAGV.CurrentNode.Id.Equals(PUB.setting.NodeMAP_RFID_Charger) &&
                      VAR.BOOL[eVarBool.MARK_SENSOR] == true)
                {
                    PUB.log.AddI("충전위치 검색 완료");
                    return true;
                }
                else
                {
                    //이동중이지 않다면 항상 이동을 해줘야한다
                    var tsCmd = DateTime.Now - LastCommandTime;
                    if (tsCmd.TotalMilliseconds >= 1999 &&
                        PUB.AGV.error.Emergency == false &&
                        PUB.AGV.system1.agv_run == false)
                    {
                        var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
                        {
                            Bunki = arDev.Narumi.eBunki.Strate,
                            Direction = arDev.Narumi.eMoveDir.Backward,
                            PBSSensor = 2,
                            Speed = arDev.Narumi.eMoveSpd.Low,
                        });
                        if (ret == arDev.eNarumiCommandResult.Success)
                            PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);//
                        LastCommandTime = DateTime.Now;
                    }
                }
                //}
            }

            return false;
        }




        /// <summary>
        /// 경로 무결성(도킹방향 등) 검증
        /// </summary>
        /// <param name="pathResult"></param>
        /// <returns></returns>
        private bool CheckPathIntegrity(AGVNavigationCore.PathFinding.Core.AGVPathResult pathResult)
        {
            if (pathResult == null) return false;

            // CalcPath에서 이미 DockingValidator를 수행했을 수 있음.
            // 만약 수행되지 않았다면 여기서 수행.
            if (pathResult.DockingValidation == null)
            {
                pathResult.DockingValidation = AGVNavigationCore.Utils.DockingValidator.ValidateDockingDirection(pathResult, PUB._mapCanvas.Nodes);
            }

            // 검증 결과 확인
            if (pathResult.DockingValidation != null && pathResult.DockingValidation.IsValidationRequired)
            {
                if (pathResult.DockingValidation.IsValid == false)
                {
                    PUB.log.AddE($"[경로무결성오류] {pathResult.DockingValidation.ValidationError}");
                    return false;
                }
            }

            return true;
        }


    }//cvass
}