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;

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;
        }

        /// <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 (_SM_RUN_POSCHK(false, new TimeSpan()) == false) 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.CurrentPath == null ||
                PUB._virtualAGV.CurrentPath.DetailedPath.Any() == 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("경로재생성");
                }

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

                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.sm.SetNewRunStep(ERunStep.READY);
                return false;
            }

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

            var message = $"[다음 행동 예측]\n\n" +
                         $"모터: {nextAction.Motor}\n" +
                         $"마그넷: {nextAction.Magnet}\n" +
                         $"속도: {nextAction.Speed}\n" +
                         $"이유: {nextAction.Message}\n\n" +
                         $"---\n" +
                         $"현재 상태: {PUB._virtualAGV.CurrentState}\n" +
                         $"현재 방향: {PUB._virtualAGV.CurrentDirection}\n" +
                         $"위치 확정: {PUB._virtualAGV.IsPositionConfirmed} (RFID {PUB._virtualAGV.DetectedRfidCount}개)\n" +
                         $"현재 노드: {PUB._virtualAGV.CurrentNode.Id ?? "없음"}";

            //모터에서 정지를 요청했다
            if (nextAction.Motor == AGVNavigationCore.Models.MotorCommand.Stop)
            {
                if (PUB.AGV.system1.agv_run)
                {
                    // 완료(Complete) 상태라면 MarkStop 전송
                    if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.MarkStop)
                    {
                        PUB.log.Add("다음행동예측에서 MARK STOP이 확인되었습니다 (자동정지시퀀스)");
                        PUB.AGV.AGVMoveStop(nextAction.Message, arDev.Narumi.eStopOpt.MarkStop);
                    }
                    else
                    {
                        PUB.log.Add("다음행동예측에서 장비 멈춤이 확인되었습니다");
                        PUB.AGV.AGVMoveStop(nextAction.Message);
                    }
                }

                // 목적지 도착 여부 확인
                // 현재 노드가 타겟 노드와 같고, 위치가 확정된 상태라면 도착으로 간주
                // 단, AGV가 실제로 멈췄는지 확인 (agv_run == false)
                if (PUB._virtualAGV.IsPositionConfirmed &&
                    PUB._virtualAGV.CurrentNode.Id == PUB._virtualAGV.TargetNode.Id)
                {
                    if (PUB.AGV.system1.agv_run == false)
                    {
                        PUB.log.AddI($"목표 도착 및 정지 확인됨(MarkStop 완료). Node:{PUB._virtualAGV.CurrentNode.Id}");
                        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.data.Sts != bunki.ToString()[0] ||
                    PUB.AGV.data.Direction != dir.ToString()[0] ||
                    PUB.AGV.data.Speed != spd.ToString()[0])
                {
                    PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
                    {
                        Bunki = bunki,
                        Direction = dir,
                        PBSSensor = 1,
                        Speed = spd,
                    });
                    PUB.log.Add($"Predict Run Setting = bunki:{bunki},dir:{dir},pbs:1,spd:{spd}");
                }

                // AGV가 정지 상태라면 구동 시작
                if (PUB.AGV.system1.agv_run == false)
                {
                    var runOpt = (dir == arDev.Narumi.eMoveDir.Forward) ? arDev.Narumi.eRunOpt.Forward : arDev.Narumi.eRunOpt.Backward;
                    PUB.AGV.AGVMoveRun(runOpt);
                }

                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)
                {
                    PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
                    {
                        Bunki = arDev.Narumi.eBunki.Strate,
                        Direction = arDev.Narumi.eMoveDir.Forward,
                        PBSSensor = 1,
                        Speed = arDev.Narumi.eMoveSpd.Low,
                    });
                    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)
                    {
                        PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
                        {
                            Bunki = arDev.Narumi.eBunki.Strate,
                            Direction = arDev.Narumi.eMoveDir.Backward,
                            PBSSensor = 1,
                            Speed = arDev.Narumi.eMoveSpd.Low,
                        });
                        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
}