using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using Project.StateMachine;
using COMM;
using AR;

namespace Project
{
    public partial class fMain
    {
        byte GotoTurnStep = 0;
        DateTime GotoTurnSetTime = DateTime.Now;
        public Boolean _SM_RUN_GOTO(bool isFirst, TimeSpan seqtime)
        {
            ///명령어 재전송 간격(기본 2초)
            var CommandInterval = 2;
            var funcname = "_SM_RUN_GOTO";

            //충전 상태가 OFF되어야 동작하게한다
            if (_SM_RUN_CHARGE_OFF(isFirst, seqtime) == false)
                return false;

            //최초시작이라면 시간변수 초기화
            if (isFirst)
            {
                PUB.log.Add($"[>>] _SM_RUN_GOTO");
                VAR.TIME.Update(eVarTime.CheckGotoTargetSet);
            }

            //라이더멈춤이 설정되어있다면 음성으로 알려준다 200409
            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;
            }

            var idx = 1;
            if (PUB.sm.RunStepSeq == idx++)
            {
                SpeedSetRetry = false;
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                ////빈 상태로 아웃해야한다.
                //if (SpeedSetRetry)
                //{
                //    if (seqtime.TotalSeconds < 2)
                //        return 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)
                //{
                //    if (SpeedSetRetry == true || ret >= arDev.eNarumiCommandResult.Error)
                //    {
                //        SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_SPEED_SET_FAIL);
                //    }
                //    else SpeedSetRetry = true;
                //    return false;
                //}


                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                if (PUB._virtualAGV.TargetNode == null)
                {
                    PUB.log.Add($"대상노드가 없어 이동을할 수 없습니다");
                    PUB.logdebug.Add($"[AI_TRACE] {funcname} REJECTED: TargetNode is NULL");
                    PUB.sm.SetNewRunStep(ERunStep.READY);
                    return false;
                }
                PUB.log.Add($"[GOTO] Step {idx - 1}: TargetNode Checked ({PUB._virtualAGV.TargetNode.ID2})");
                PUB.logdebug.Add($"[AI_TRACE] {funcname} TargetNode Confirmed: {PUB._virtualAGV.TargetNode.ID2} (RFID: {PUB._virtualAGV.TargetNode.RfidId})");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //모션 전후진 제어
                if (UpdateMotionPositionForMark(funcname))
                {
                    PUB.log.Add($"[GOTO] Step {idx - 1}: UpdateMotionPositionForMark Completed. Stopping AGV.");
                    PUB.AGV.AGVMoveStop(funcname);
                    PUB.sm.UpdateRunStepSeq();
                }
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //QC까지 모두 완료되었다.(완전히 정차할때까지 기다린다)
                PUB.log.Add($"[GOTO] Step {idx - 1}: Movement Finished. Waiting for full stop.");
                PUB.Speak(Lang.이동완료, true);
                PUB.AddEEDB($"이동완료({PUB._virtualAGV.TargetNode.ID2})");

                //ㅇ목적지 완료신호를 ACS에전송한다.
                PUB.XBE.SendMoveComplete(PUB._virtualAGV.TargetNode.RfidId.ToString("0000"));
                PUB.logdebug.Add($"[AI_TRACE] {funcname} Navigation Complete. ACS Notified. Node={PUB._virtualAGV.TargetNode.ID2}");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }

            return true;
        }
    }
}