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 stepTime)
        {
            ///명령어 재전송 간격(기본 2초)
            var CommandInterval = 2;

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

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

            //목적지가 설정되었는지 체크한다.
          //Z  if (PUB.mapctl.Manager.agv.TargetRFID.IsEmpty)
          //  {
          //      //최대 5초간 설정여부를 확인하고
          //      if (VAR.TIME.RUN(eVarTime.CheckGotoTargetSet).TotalSeconds > 5)
          //      {
          //          //실패시에는 READY로 전환한다.
          //          PUB.sm.SetNewRunStep(ERunStep.READY);
          //          PUB.Speak(Lang.목적지가없어대기상태로전환합니다);
          //      }
          //      return false;
          //  }

            //var idx = 1;
            //var BeforePredictIdx = -1;
            //var predict = PUB.mapctl.Manager.PredictResult;
            //if (PUB.sm.RunStepSeq == idx++)
            //{
            //    PUB.Speak(Lang.위치로이동합니다);
            //    PUB.log.Add($"목적지 위치 이동시작({PUB.mapctl.Manager.agv.TargetRFID.Value})");
            //    VAR.TIME.Update(eVarTime.CheckGotoTargetSet);
            //    VAR.TIME.Set(eVarTime.SendGotoCommand, DateTime.Now.AddDays(-1));
            //    PUB.sm.UpdateRunStepSeq();
            //    return false;
            //}
            //else if (PUB.sm.RunStepSeq == idx++)
            //{
            //    //멈춰야하는경우
            //    if (predict.MoveState == AGVControl.AGVMoveState.Stop)
            //    {
            //        if (PUB.AGV.system1.agv_run)
            //        {
            //            if (VAR.TIME.RUN(eVarTime.SendGotoCommand).TotalSeconds > 2)
            //            {
            //                PUB.Speak("AGV Stop");
            //                PUB.AGV.AGVMoveStop("Predict", arDev.Narumi.eStopOpt.Stop);
            //                VAR.TIME.Update(eVarTime.SendGotoCommand);
            //            }
            //            return false;
            //        }
            //        else
            //        {
            //            //완료되었거나 턴을진행해야한다
            //            if (predict.ReasonCode == AGVControl.AGVActionReasonCode.Arrived ||
            //                predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
            //                predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
            //            {
            //                GotoTurnStep = 0;
            //                GotoTurnSetTime = DateTime.Now.AddDays(-1);
            //                PUB.sm.UpdateRunStepSeq();
            //            }
            //            return false;
            //        }
            //    }
            //    else //이동해야하는 경우
            //    {
            //        //속도와 방향이 불일치하는 경우 다시 설정한다 (속도: H,L,M,[S]
            //        AGVControl.AgvDir AGV_Direction = (AGVControl.AgvDir)PUB.AGV.data.Direction;
            //        AGVControl.AgvSpeed AGV_Speed = (AGVControl.AgvSpeed)PUB.AGV.data.Speed;
            //        AGVControl.AgvSts AGV_Sts = (AGVControl.AgvSts)PUB.AGV.data.Sts;

            //        //상태값이 바뀌었다면 전송을 해야한다
            //        if (predict.Direction != AGV_Direction || predict.MoveSpeed != AGV_Speed || predict.MoveDiv != AGV_Sts)
            //        {
            //            if (VAR.TIME.RUN(eVarTime.SendGotoCommand).TotalSeconds > CommandInterval)
            //            {
            //                arDev.Narumi.eBunki v_bunki = arDev.Narumi.eBunki.Strate;
            //                if (predict.MoveDiv == AGVControl.AgvSts.Straight) v_bunki = arDev.Narumi.eBunki.Strate;
            //                else if (predict.MoveDiv == AGVControl.AgvSts.Left) v_bunki = arDev.Narumi.eBunki.Left;
            //                else if (predict.MoveDiv == AGVControl.AgvSts.Right) v_bunki = arDev.Narumi.eBunki.Right;

            //                arDev.Narumi.eMoveDir v_dir = arDev.Narumi.eMoveDir.Backward;
            //                if (predict.Direction == AGVControl.AgvDir.Forward) v_dir = arDev.Narumi.eMoveDir.Forward;
            //                else if (predict.Direction == AGVControl.AgvDir.Backward) v_dir = arDev.Narumi.eMoveDir.Backward;

            //                arDev.Narumi.eMoveSpd v_spd = arDev.Narumi.eMoveSpd.Low;
            //                if (predict.MoveSpeed == AGVControl.AgvSpeed.Middle) v_spd = arDev.Narumi.eMoveSpd.Middle;
            //                else if (predict.MoveSpeed == AGVControl.AgvSpeed.High) v_spd = arDev.Narumi.eMoveSpd.High;
            //                else if (predict.MoveSpeed == AGVControl.AgvSpeed.Low) v_spd = arDev.Narumi.eMoveSpd.Low;

            //                //이동셋팅을 해준다
            //                PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
            //                {
            //                    Bunki = v_bunki,
            //                    Direction = v_dir,
            //                    PBSSensor = 1,
            //                    Speed = v_spd,
            //                });

            //                if (predict.MoveSpeed == AGVControl.AgvSpeed.MarkStop)
            //                {
            //                    PUB.AGV.AGVMoveStop("Predict", arDev.Narumi.eStopOpt.Stop);
            //                }
            //                VAR.TIME.Update(eVarTime.SendGotoCommand);
            //            }
            //            return false;
            //        }

            //        //정지상태라면 이동 명령을 전달한다
            //        if (PUB.AGV.system1.agv_run == false)
            //        {
            //            if (VAR.TIME.RUN(eVarTime.SendGotoCommand).TotalSeconds > CommandInterval)
            //            {
            //                PUB.Speak("AGV Start");

            //                arDev.Narumi.eRunOpt v_dir = arDev.Narumi.eRunOpt.Backward;
            //                if (predict.Direction == AGVControl.AgvDir.Forward) v_dir = arDev.Narumi.eRunOpt.Forward;
            //                else if (predict.Direction == AGVControl.AgvDir.Backward) v_dir = arDev.Narumi.eRunOpt.Backward;

            //                PUB.AGV.AGVMoveRun(v_dir);
            //                VAR.TIME.Update(eVarTime.SendGotoCommand);
            //            }
            //            return false;
            //        }
            //    }

            //    //예측이 업데이트되지 않으면 오류 처리해야한다
            //    if (BeforePredictIdx == -1) BeforePredictIdx = (int)predict.Idx;
            //    else if (BeforePredictIdx != predict.Idx)   //이전사용한 IDX와  다르다면 예측이 실행된 경우이다
            //        BeforePredictIdx = (int)predict.Idx;
            //    else
            //    {
            //        //5초이상 예측값이 업데이트되지 않으면 오류 처리한다.
            //        var tsPredict = DateTime.Now - predict.CreateTime;
            //        if (tsPredict.TotalSeconds > 5)
            //        {
            //            PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.PredictFix, Lang.예측값이계산되지않아이동을중단합니다);
            //            PUB.Speak(Lang.예측값이계산되지않아이동을중단합니다);
            //            PUB.sm.SetNewRunStep(ERunStep.READY);
            //        }
            //    }

            //    return false;
            //}
            //else if (PUB.sm.RunStepSeq == idx++)
            //{
            //    if (predict.ReasonCode == AGVControl.AGVActionReasonCode.Arrived)
            //    {
            //        PUB.Speak(Lang.목적지이동이완료되었습니다);
            //        PUB.sm.SetNewRunStep(ERunStep.READY);
            //        PUB.sm.UpdateRunStepSeq();
            //    }
            //    else if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
            //        predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
            //    {
            //        //턴을 해야하는 경우이다
            //        //좌턴을 기본으로 진행하며, 좌턴이동 후 마크스탑을 입력한다
            //        if (GotoTurnStep == 0)
            //        {
            //            //턴을 한적이 없으므로 턴을 먼저 진행한다
            //            arDev.Narumi.eMoveDir moveDir = arDev.Narumi.eMoveDir.Backward;
            //            if (predict.Direction == AGVControl.AgvDir.Forward) moveDir = arDev.Narumi.eMoveDir.Forward;
            //            if (PUB.AGV.data.Sts != 'L' || PUB.AGV.data.Speed != 'L' || PUB.AGV.data.Direction != moveDir.ToString()[0])
            //            {
            //                //셋팅이 다르다면 3초간격으로 전송한다
            //                var tsTurnSet = DateTime.Now - GotoTurnSetTime;
            //                if (tsTurnSet.TotalSeconds > 3)
            //                {
            //                    PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
            //                    {
            //                        Bunki = arDev.Narumi.eBunki.Left,
            //                        Direction = moveDir,
            //                        PBSSensor = 1,
            //                        Speed = arDev.Narumi.eMoveSpd.Low,
            //                    });
            //                    GotoTurnSetTime = DateTime.Now;
            //                    PUB.log.Add("Turn Bunki Set");
            //                }
            //            }
            //            else
            //            {
            //                PUB.mapctl.Manager.agv.CurrentRFID.TurnOK = false;
            //                PUB.mapctl.Manager.agv.CurrentRFID.TurnStart = DateTime.Now;
            //                PUB.sm.UpdateRunStepSeq(); //셋팅이 맞으니 다음스텝으로 진행한다
            //                GotoTurnStep += 1;
            //            }
            //        }
            //    }
            //    else PUB.sm.UpdateRunStepSeq();
            //    return false;
            //}
            //else if (PUB.sm.RunStepSeq == idx++)
            //{
            //    //턴이완료되길 기다린다.
            //    if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
            //       predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
            //    {
            //        //(최소5초는 기다리고 판단한다)
            //        if (stepTime.TotalSeconds < 5) return false;

            //        //최대30초는 기다려준다
            //        if (stepTime.TotalSeconds > 30)
            //        {
            //            var ermsg = "Turn Timeout(30sec)";
            //            PUB.log.AddE(ermsg);
            //            PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.TurnTimeout, ermsg);
            //            PUB.sm.SetNewRunStep(ERunStep.READY);
            //        }

            //        //모션이 멈추었다면 턴이완료된것이다.
            //        if (PUB.AGV.system1.agv_stop)
            //        {
            //            if (PUB.AGV.system1.Mark1_check == false && PUB.AGV.system1.Mark2_check == false)
            //            {
            //                PUB.log.AddE($"Turn 완료이나 Mark 센서가 확인되지 않았습니다");
            //            }
            //            GotoTurnStep += 1;
            //            PUB.sm.UpdateRunStepSeq();
            //        }
            //        else
            //        {
            //            //아직 이동중이므로 대기한다
            //        }
            //    }
            //    else PUB.sm.UpdateRunStepSeq(); //기타사항은 다음으로 넘어간다
            //    return false;
            //}
            //else if (PUB.sm.RunStepSeq == idx++)
            //{
            //    if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove ||
            //        predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnPoint)
            //    {
            //        if (GotoTurnStep < 2)
            //        {
            //            PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.TurnError, "턴시퀀스 완료 실패");
            //            PUB.log.AddE($"턴완료시퀀스가 2가아닙니다. 대기 상태로 강제 전환합니다");
            //            PUB.sm.SetNewRunStep(ERunStep.READY);
            //        }
            //        else
            //        {
            //            PUB.log.AddI("Turn Complete");
            //        }

            //        //방향전환용 턴이라면 이동기록을 추가해서 방향이 맞도록 처리해주자
            //        if (predict.ReasonCode == AGVControl.AGVActionReasonCode.NeedTurnMove)
            //        {
            //            var rfid = PUB.mapctl.Manager.agv.CurrentRFID;
            //            var lastHistory = PUB.mapctl.Manager.agv.MovementHistory.Last();
            //            //원래방향에서 반대로 처리한다
            //            var revDir = lastHistory.Direction == AGVControl.AgvDir.Backward ? AGVControl.AgvDir.Forward : AGVControl.AgvDir.Backward;
            //            PUB.mapctl.Manager.agv.AddToMovementHistory(rfid.Value, rfid.Location, revDir);
            //        }
            //        else
            //        {
            //            //이동용 RFID에서 턴명령이 들어있는경우였다
            //            PUB.mapctl.Manager.agv.CurrentRFID.TurnEnd = DateTime.Now;
            //            PUB.mapctl.Manager.agv.CurrentRFID.TurnOK = true;
            //        }

            //        PUB.sm.UpdateRunStepSeq();
            //    }
            //    else PUB.sm.UpdateRunStepSeq();
            //    return false;
            //}


            //좌턴이동명령 전송

            //마크스탑전송

            //마크스탑이 확인되면 나머지는 경로예측에 맡긴다

            return true;
        }
    }
}
