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.CalculatePath(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.SetAlertMessage(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;

            //현재위치 기준으로 재 계산하여. 더 최적화된 루트가 있다면 처리를 해준다.
            if (PUB._virtualAGV.CurrentPath.DetailedPath.Count > 5)
            {
                var PathResult2 = CalcPath(PUB._virtualAGV.CurrentNode, PUB._virtualAGV.TargetNode);
                if (PathResult2 != null && PathResult2.Success)
                {
                    //절반이상 경로가 짧을때에는 재계산을 하게한다
                    var halfcnt = (int)(PUB._virtualAGV.CurrentPath.DetailedPath.Count / 2.0);
                    if (PathResult2.DetailedPath.Count < halfcnt)
                    {
                        var msg = $"단축경로가 확인되었습니다. 경로를 삭제 합니다";
                        PUB.log.AddE(msg);
                        Console.WriteLine(msg);
                        PUB._virtualAGV.SetPath(null);
                        return false;
                    }

                }
            }

            //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 == AGVNavigationCore.Models.MotorCommand.Stop)
            {
                if (PUB.AGV.system1.agv_run)
                {

                    // 완료(Complete) 상태라면 MarkStop 전송
                    if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.MarkStop)
                    {
                        if (PUB.AGV.data.Speed != 'S')
                        {
                            // [쿨타임 적용] 정지 명령은 별도의 타이머(LastStopCommandTime)를 사용하여
                            // 이동 직후라도 즉시 반응할 수 있도록 한다. (이미 정지신호를 보냈다면 2초 대기)
                            var tsCmd = VAR.TIME.RUN(eVarTime.LastStopCommandTime);
                            if (tsCmd.TotalSeconds >= 2.0)
                            {
                                PUB.log.Add("다음행동예측에서 MARK STOP이 확인되었습니다 (자동정지시퀀스)");
                                PUB.AGV.AGVMoveStop(nextAction.Message, arDev.Narumi.eStopOpt.MarkStop);

                                // 정지 타이머 갱신 (연속 정지 방지)
                                VAR.TIME.Update(eVarTime.LastStopCommandTime);
                                // 일반 타이머도 갱신 (정지 직후 불필요한 이동 방지)
                                LastCommandTime = DateTime.Now;
                            }
                        }
                    }
                    else
                    {
                        // [쿨타임 적용] 정지 명령은 별도의 타이머(LastStopCommandTime)를 사용하여
                        // 이동 직후라도 즉시 반응할 수 있도록 한다. (이미 정지신호를 보냈다면 2초 대기)
                        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;
                        }
                    }



                }

                // 목적지 도착 여부 확인
                // .. (생략) ..
                if (PUB._virtualAGV.IsPositionConfirmed)
                {
                    if (PUB.AGV.system1.agv_run == false)
                    {
                        // 경로가 존재한다면...
                        if (PUB._virtualAGV.CurrentPath != null && PUB._virtualAGV.CurrentPath.DetailedPath.Any())
                        {
                            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
                            {
                                // [DEBUG] 도착했으나 조건 불일치
                                // PUB.log.Add($"[DEBUG] Arrived but condition mismatch. CurNode:{PUB._virtualAGV.CurrentNode.Id}, Target:{lastInfo.NodeId}, CurDir:{PUB._virtualAGV.CurrentDirection}, TargetDir:{lastInfo.MotorDirection}");
                            }
                        }
                        else
                        {
                            // ...
                            if (PUB._virtualAGV.CurrentNode.Id == PUB._virtualAGV.TargetNode.Id)
                            {
                                // ...
                                PUB.log.AddI($"목표 도착 및 정지 확인됨(MarkStop 완료, No Path Info) Node:...");
                                return true;
                            }
                        }
                    }
                }

                return false;
            }
            else
            {
                // 이동 명령 변환 (AGVNavigationCore -> arDev.Narumi)
                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;

                // 방향 전환 시 정지 로직 추가
                // 이동 중인데 방향이 다르면 먼저 정지시킨다.
                if (PUB.AGV.system1.agv_run)
                {
                    if (PUB.AGV.data.Direction != dir.ToString()[0])
                    {
                        // 2초 쿨타임 (정지 명령도 빈번한 전송 방지)
                        var tsCmd = DateTime.Now - LastCommandTime;
                        if (tsCmd.TotalSeconds >= 2.0)
                        {
                            PUB.log.Add($"방향 전환을 위해 정지 명령을 전송합니다 Current:{PUB.AGV.data.Direction} Target:{dir}");
                            PUB.AGV.AGVMoveStop("Direction Change");
                            LastCommandTime = DateTime.Now;
                        }
                        return false;
                    }
                }

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

                        if (ret == arDev.eNarumiCommandResult.Success)
                            PUB.log.Add($"Predict Run Setting = bunki:{bunki},dir:{dir},pbs:1,spd:{spd}");
                        else
                            PUB.log.AddE($"Predict Run Setting = bunki:{bunki},dir:{dir},pbs:1,spd:{spd}");

                        LastCommandTime = DateTime.Now;
                    }
                }

                // AGV가 정지 상태라면 구동 시작 (라이다가켜져있을때에만 사용한다)
                if (PUB.AGV.system1.agv_run == false && PUB.AGV.PBSSensor ==  arDev.eNarmiPBSSensor.on)
                {
                    // 2초 쿨타임 적용 (AGVMoveSet과 동일한 타이머 사용)
                    var tsCmd = DateTime.Now - LastCommandTime;
                    if (tsCmd.TotalSeconds >= 2.0)
                    {
                        var runOpt = (dir == arDev.Narumi.eMoveDir.Forward) ? arDev.Narumi.eRunOpt.Forward : arDev.Narumi.eRunOpt.Backward;
                        PUB.AGV.AGVMoveRun(runOpt);
                        LastCommandTime = DateTime.Now;
                    }
                }

                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
}