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
    {
        DateTime LastSpeakTime = DateTime.Now;
        DateTime CoverControlTime = DateTime.Now;
        DateTime LastCommandTime = DateTime.Now;
        DateTime LastCommandTimeNextStop = DateTime.Now;
        bool runStepisFirst = false;

        private void _SM_RUN(Boolean isFirst, TimeSpan stepTime)
        {
            //중단기능이 동작이라면 처리하지 않는다.
            if (PUB.sm.bPause)
            {
                System.Threading.Thread.Sleep(200);
                return;
            }

            //가동불가 조건 확인
            if (CheckStopCondition() == false)
            {
                PUB.sm.SetNewStep(eSMStep.IDLE);
                return;
            }

            //HW 연결오류
            if (PUB.AGV.IsOpen == false)
            {
                PUB.Result.SetResultMessage(eResult.Hardware, eECode.AGVCONN, eNextStep.ERROR);
                PUB.sm.SetNewStep(eSMStep.IDLE);
                return;
            }

            //이머전시상태라면 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;
            }


            //스텝이 변경되었다면?
            if (PUB.sm.RunStep != PUB.sm.RunStepNew)
            {
                runStepisFirst = true;
                PUB.sm.ApplyRunStep();
            }
            else runStepisFirst = false;

            //처음시작이라면 시작시간을 설정한다
            if (isFirst)
            {
                if (PUB.sm.RunStep == ERunStep.READY)
                    VAR.TIME.Update(eVarTime.ReadyStart);
                else
                    VAR.TIME.Update(eVarTime.RunStart);
            }

            //자동모드에서 대기상태 (추가동작없음)
            if (PUB.sm.RunStep == ERunStep.READY)
            {
                _SM_RUN_READY(runStepisFirst, PUB.sm.GetRunSteptime);
                return;
            }

            //#############################################
            //##  이 후에는 모든 동작루틴이 온다
            //#############################################

            //라이더 센서에서 멈춘경우 처리
            if (PUB.AGV.system1.stop_by_front_detect == true)
            {
                var tsSpeak = DateTime.Now - LastSpeakTime;
                if (tsSpeak.TotalSeconds >= PUB.setting.doorSoundTerm)
                {
                    PUB.Speak(Lang.전방에물체가감지되었습니다);
                    LastSpeakTime = DateTime.Now;
                }
                return;
            }

            //선로이탈감지
            if (PUB.AGV.error.runerror_by_no_magent_line == true)
            {
                var tsSpeak = DateTime.Now - LastSpeakTime;
                if (tsSpeak.TotalSeconds >= PUB.setting.doorSoundTerm)
                {
                    PUB.Speak(Lang.선로를이탈했습니다);
                    LastSpeakTime = DateTime.Now;
                }
                return;
            }

            //현재위치를 모르는 상태라면 이동하여 현재 위치를 찾는다
            if (PUB._virtualAGV.CurrentNodeId.isEmpty())// .cur.    . CurrentPos == ePosition.NONE)
            {
                //이동중이지 않거나 홈이동상태가 아니라면?
                if (PUB.AGV.system1.agv_run == false || PUB.sm.RunStep != ERunStep.GOHOME)
                {
                    //현재위치를 검색해야함
                    PUB.sm.ClearRunStep();
                    PUB.sm.SetNewRunStep(ERunStep.GOHOME);
                    PUB.AddEEDB($"READY상태에서 현재위치를 몰라 홈으로 이동 백업스텝:{PUB.sm.RunStep}");
                    PUB.sm.BackupRunStep.Push(PUB.sm.RunStep);  //현재 상태를 백업
                    return;
                }
                VAR.STR[eVarString.ChargeCheckMsg] = "현재 위치 모름";
            }

            //나머지 상황체크
            switch (PUB.sm.RunStep)
            {
                case ERunStep.GOHOME:
                    if (runStepisFirst)
                    {
                        PUB.Speak(Lang.FindCurrentPisition);
                    }
                    else if (_SM_RUN_GOHOME(runStepisFirst, PUB.sm.GetRunSteptime))
                    {
                        //230601
                        PUB.Speak(Lang.홈이동완료대기상태로전환합니다);
                        VAR.TIME.Update(eVarTime.ChargeTry);

                        ERunStep NextStep = ERunStep.READY;
                        if (PUB.sm.BackupRunStep.Count > 0)
                            NextStep = PUB.sm.BackupRunStep.Pop();

                        PUB.log.Add($"현재위치 검색완료로 {NextStep} 으로 RUNSTEP값을 변경 합니다");
                        PUB.sm.SetNewRunStep(NextStep); //대기상태로 전환한다
                        return;
                    }
                    break;

                case ERunStep.GOTO: //목적지까지 이동하는 경우
                    if (_SM_RUN_GOTO(runStepisFirst, PUB.sm.GetRunSteptime) == true)
                    {
                        //목적지가 BUFFER라면 버퍼투입대기위치까지 완료했다는 시그널을 보낸다.                        
                        var target = PUB._virtualAGV.TargetNode;
                        PUB.log.Add($"목적지({target.RfidId}) 도착완료 타입:{target.Type}, 출발지:{PUB._virtualAGV.StartNode.RfidId}");
                        if (target.Type == AGVNavigationCore.Models.NodeType.Buffer)
                        {
                            
                            //현재위치가 마지막경로의 NODEID와 일치해야한다
                            var lastPath = PUB._virtualAGV.CurrentPath.DetailedPath.LastOrDefault();
                            if(lastPath.NodeId.Equals(PUB._virtualAGV.CurrentNodeId))
                            {
                                //버퍼진입전 노드에 도착완료했따
                                PUB.XBE.BufferInReady = true;
                                PUB.XBE.BufferReadyError = false;                                
                            }
                            else
                            {
                                //마지막위치가 아닌 다른 위치에 있으니 버퍼 작업을 할 수없다
                                PUB.log.AddAT("목적지 버퍼이동완료 했지만 마지막 노드가 아닙니다");
                                PUB.XBE.BufferInReady = false;
                                PUB.XBE.BufferReadyError = true;                                
                            }
                            PUB.XBE.BufferInComplete = false;
                            PUB.XBE.BufferOutComplete = false;
                        }
                        
                        else if(target.Type == AGVNavigationCore.Models.NodeType.Charging)
                        {

                        }
                        else if (target.Type == AGVNavigationCore.Models.NodeType.Loader)
                        {

                        }
                        else if (target.Type == AGVNavigationCore.Models.NodeType.Clearner)
                        {

                        }
                        else if (target.Type == AGVNavigationCore.Models.NodeType.UnLoader)
                        {

                        }
                        else
                        {
                            //목적지다 다른 형태이다
                            
                        }
                        PUB._virtualAGV.Turn = AGVNavigationCore.Models.AGVTurn.None;
                        PUB.sm.SetNewRunStep(ERunStep.READY);
                    }
                    break;

                case ERunStep.MARKSTROPB:   //후진방향으로 마크스탑
                case ERunStep.MARKSTOPF:    //전진방향으로 마크스탑

                    //이동중이지 않다면 먼저 이동을 진행한다
                    var agvDir = PUB.sm.RunStep == ERunStep.MARKSTOPF ? arDev.Narumi.eRunOpt.Forward : arDev.Narumi.eRunOpt.Backward;
                    PUB.AGV.AGVMoveRun(agvDir);

                    //이동중이라면 마크스탑을 입력한다
                    PUB.AGV.AGVMoveStop("run-markstropb", arDev.Narumi.eStopOpt.MarkStop);

                    //마크스탑신호를 확인한다.(최대 5초)

                    //신호가 확인되지 않으면 오류로 정지한다
                    break;


                case ERunStep.GOCHARGE: //충전위치로 이동
                    if (runStepisFirst)
                    {
                        VAR.TIME[eVarTime.ChargeTry] = DateTime.Now;
                        PUB.sm.ResetRunStepSeq();
                        PUB.log.Add("충전 명령 시작");
                    }
                    else if (_SM_RUN_GOCHARGE(runStepisFirst, PUB.sm.GetRunSteptime))
                    {
                        //230601
                        //if (PUB.Result != null && PUB.sm != null)
                        //   EEMStatus.AddEEDBSQL(PUB.sm.Step, PUB.sm.RunStep.ToString(), PUB.Result.TargetPos.ToString());

                        PUB.Speak(Lang.충전을시작합니다);
                        PUB.sm.SetNewRunStep(ERunStep.CHARGECHECK);
                        return;
                    }
                    break;

                case ERunStep.CHARGECHECK: //충전중
                    if (runStepisFirst)
                    {
                        VAR.TIME.Update(eVarTime.ChargeStart);
                        LastCommandTime = DateTime.Now;
                    }
                    else if (_SM_RUN_GOCHARGECHECK(runStepisFirst, PUB.sm.GetRunSteptime))
                    {
                        //충전상태가 활성화되었으므로 대기상태로 전환한다
                        PUB.sm.SetNewRunStep(ERunStep.READY);
                        PUB.log.AddAT("충전상태 확인 완료로 인해 대기 합니다");
                    }
                    break;

                case ERunStep.CHARGEOFF:
                    if (runStepisFirst)
                    {
                        VAR.TIME.Update(eVarTime.ChargeEnd);
                        LastCommandTime = DateTime.Now;
                    }
                    else
                    {
                        //충전 상태가 OFF되어야 동작하게한다
                        if (_SM_RUN_CHGOFF(isFirst, stepTime) == true)
                        {
                            //충전상태가 활성화되었으므로 대기상태로 전환한다
                            PUB.sm.ClearRunStep();

                            //충전기위치에서 OFF를 한 경우이다
                            if (PUB.Result.CurrentPos != ePosition.CHARGE || PUB.Result.TargetPos != ePosition.CHARGE)
                            {
                                PUB.Result.CurrentPos = ePosition.NONE;
                            }
                            else
                            {
                                PUB.Result.TargetPos = ePosition.QC;
                            }
                            PUB.Result.CurrentPosCW = "1";
                            PUB.sm.SetNewRunStep(ERunStep.GOHOME);
                            PUB.log.AddAT("충전 해제로 홈으로 이동");
                        }
                    }
                    break;
                case ERunStep.BUFFER_OUT: //버퍼아웃
                    if (_SM_RUN_BUFFER_OUT(runStepisFirst, PUB.sm.GetRunSteptime))
                    {
                        PUB.Speak(Lang.버퍼도킹해제완료);

                        //도킹완료상태를 업데이트한다. 
                        PUB.XBE.BufferInComplete = false;
                        PUB.XBE.BufferOutComplete = true;

                        //대기상태로 전환
                        PUB.sm.SetNewRunStep(ERunStep.READY);
                        return;
                    }
                    break;

                case ERunStep.BUFFER_IN: //버퍼도킹
                    if (_SM_RUN_BUFFER_IN(runStepisFirst, PUB.sm.GetRunSteptime))
                    {
                        PUB.Speak(Lang.버퍼도킹이완료되었습니다);

                        //도킹완료상태를 업데이트한다. 
                        PUB.XBE.BufferInComplete = true;

                        //버퍼아웃으로 자동 진행 합니다
                        PUB.sm.ClearRunStep();
                        PUB.sm.SetNewRunStep(ERunStep.BUFFER_OUT);
                        return;
                    }
                    break;


            }



        }

        bool CheckStopCondition()
        {
            return true;
        }


        void CheckAGVMoveTo(eGoDir dir)
        {
            //계속내려간다
            if (dir == eGoDir.Down)
            {
                var tsCmd = DateTime.Now - LastCommandTime;
                if (tsCmd.TotalMilliseconds >= 1999)
                {
                    //현재 동작중인상태에서 방향이 맞지 않다면 일단 움직임을 멈춘다
                    if (PUB.AGV.system1.agv_run)
                    {
                        if (PUB.AGV.data.Direction == 'B')
                        {
                            PUB.log.Add($"방향이 맞지 않아 정지 합니다({dir})");
                            PUB.AGV.AGVMoveStop("CheckAGVMoveTo");
                        }
                        else if (PUB.AGV.data.Speed == 'S')
                        {
                            PUB.log.Add($"마크정지를 해제하기 위해 장비를 멈춥니다");
                        }
                    }
                    else
                    {
                        //움직이지 않으므로 전진하도록 한다
                        PUB.log.Add($"AGV 기동 방향(DOWN):{dir}");
                        PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
                    }
                    LastCommandTime = DateTime.Now;
                }
            }
            else
            {
                var tsCmd = DateTime.Now - LastCommandTime;
                if (tsCmd.TotalMilliseconds >= 1999)
                {
                    if (PUB.AGV.system1.agv_run)
                    {
                        if (PUB.AGV.data.Direction == 'F')
                        {
                            PUB.log.Add($"방향이 맞지 않아 정지 합니다({dir})");
                            PUB.AGV.AGVMoveStop("CheckAGVMoveTo");
                        }
                        else if (PUB.AGV.data.Speed == 'S')
                        {
                            PUB.log.Add($"마크정지를 해제하기 위해 장비를 멈춥니다");
                        }
                    }
                    else
                    {
                        PUB.log.Add($"AGV 기동 방향(UP):{dir}");
                        PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
                    }
                    LastCommandTime = DateTime.Now;
                }
            }
        }

        void CheckAGVStopbyMARK(string sender)
        {
            //계속내려간다
            var tsCmd = DateTime.Now - LastCommandTime;
            if (VAR.BOOL[eVarBool.NEXTSTOP_MARK] == false || tsCmd.TotalMilliseconds >= 1500)
            {
                PUB.AGV.AGVMoveStop("CheckAGVStopbyMARK", arDev.Narumi.eStopOpt.MarkStop);
                LastCommandTime = DateTime.Now;
                PUB.log.Add($"[{sender}] MARK신호에 멈춤 설정");
            }
        }

        Boolean UpdateMotionPositionForMark(string sender)
        {
            //이머전시상태에서는 처리하지 않는다.
            if (VAR.BOOL[eVarBool.EMERGENCY]) return false;

            //DOWN 작업
            // if (goDIR == eGoDir.Down)
            {
                //1. 현재위치 > 대상위치
                if (PUB.Result.CurrentPos > PUB.Result.TargetPos)
                {
                    //계속내려간다
                    if (PUB.setting.AGV_Direction_FVI_Backward)
                        CheckAGVMoveTo(eGoDir.Down);
                    else
                        CheckAGVMoveTo(eGoDir.Up);
                }
                //2. 현재위치 < 대상위치
                else if (PUB.Result.CurrentPos < PUB.Result.TargetPos)
                {
                    //올라가야한다
                    if (PUB.setting.AGV_Direction_FVI_Backward)
                        CheckAGVMoveTo(eGoDir.Up);
                    else
                        CheckAGVMoveTo(eGoDir.Down);
                }
                //3. 현재위치 = 대상위치
                else
                {
                    //현재위치가 확정되었는가?
                    var actpos = ctlPos1.GetPositionActive(PUB.Result.CurrentPos);
                    if (actpos == false && PUB.AGV.system1.agv_stop == true)
                    {
                        //위치확정이되지 않았다면 AGV멈춤시에 기동하게 한다.
                        var lastcom = DateTime.Now - LastCommandTime;
                        if (lastcom.TotalSeconds > 3)
                        {
                            if (PUB.Result.CurrentPosCW == "1")
                            {
                                if (PUB.setting.AGV_Direction_FVI_Backward)
                                    PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
                                else
                                    PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
                            }

                            else
                            {
                                if (PUB.setting.AGV_Direction_FVI_Backward)
                                    PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
                                else
                                    PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
                            }


                            LastCommandTime = DateTime.Now;
                            PUB.logagv.Add($"AGV가 멈춰있다 동작을 재개 합니다");
                        }
                    }
                    else
                    {
                        //마크센서가 들어와잇고, 위치가 act 되어있다면 해당 위치에 있는 것이다
                        if (PUB.AGV.error.Emergency == false &&
                            PUB.AGV.system1.agv_stop &&
                            PUB.AGV.signal.mark_sensor && actpos &&
                           PUB.Result.CurrentPos == PUB.Result.TargetPos)
                        {
                            //PUB.AGV.AGVMoveStop();
                            return true;
                        }

                        if (PUB.AGV.system1.agv_stop == true && PUB.AGV.system1.agv_run == false)
                        {
                            PUB.log.Add($"멈춰있으므로 이동을 시작 합니다");
                            if (PUB.Result.CurrentPosCW == "1")
                            {
                                if (PUB.setting.AGV_Direction_FVI_Backward)
                                    PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
                                else
                                    PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
                            }
                            else
                            {
                                if (PUB.setting.AGV_Direction_FVI_Backward)
                                    PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
                                else
                                    PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
                            }

                        }

                        //AGV는 아래로 FVI방향으로 내려가고 있다
                        if (VAR.BOOL[eVarBool.AGVDIR_UP] == false)
                        {
                            if (PUB.Result.CurrentPosCW == "0")
                            {
                                //장비가 마크센서에의해 멈췃다면 완료이다
                                if (PUB.AGV.error.Emergency == false && PUB.AGV.signal.mark_sensor)
                                {
                                    PUB.AGV.AGVMoveStop("UPdateMotionPositionForMark");
                                    return true;
                                }
                                else CheckAGVStopbyMARK(sender);
                            }
                            else if (PUB.Result.CurrentPosCW == "1")
                            {
                                //내려가는 작업이고  AGV는 올라가고 있는데 RFID는 위졲이 감지되었다면 아래로 내려가야한다
                                CheckAGVMoveTo(eGoDir.Up);
                            }
                            else
                            {
                                PUB.Result.CurrentPosCW = "1";
                                CheckAGVMoveTo(eGoDir.Up);
                            }
                        }
                        //AGV는 Qc방향으로 올라가고 있다
                        else
                        {
                            if (PUB.Result.CurrentPosCW == "1")
                            {
                                //네려가는 방향에서 내려가는 위치가 인식되었다면 마크에서 멈춰야 한다
                                if (PUB.AGV.error.Emergency == false && PUB.AGV.signal.mark_sensor)
                                {
                                    PUB.AGV.AGVMoveStop("UPdateMotionPositionForMark");
                                    return true;
                                }
                                else CheckAGVStopbyMARK(sender);
                            }
                            else if (PUB.Result.CurrentPosCW == "0")
                            {
                                //내려가는 작업이고  AGV는 올라가고 있는데 RFID는 위졲이 감지되었다면 아래로 내려가야한다
                                CheckAGVMoveTo(eGoDir.Down);
                            }
                            else
                            {
                                PUB.Result.CurrentPosCW = "0";
                                CheckAGVMoveTo(eGoDir.Down);
                            }
                        }
                    }


                }
            }
            //UP 작업
            //else
            //{
            //    //1. 현재위치 > 대상위치
            //    if (PUB.Result.CurrentPos > PUB.Result.TargetPos)
            //    {
            //        //계속내려간다
            //        CheckAGVMoveTo(eGoDir.Down);
            //    }
            //    //2. 현재위치 < 대상위치
            //    else if (PUB.Result.CurrentPos < PUB.Result.TargetPos)
            //    {
            //        //올라가야한다
            //        CheckAGVMoveTo(eGoDir.Up);
            //    }
            //    //3. 현재위치 = 대상위치
            //    else
            //    {
            //        //AGV는 위로가고 있다
            //        if (VAR.BOOL[eVarBool.AGVDIR_UP] == true)
            //        {
            //            if (PUB.Result.CurrentPosCW == "0")
            //            {
            //                //장비가 마크센서에의해 멈췃다면 완료이다
            //                if (PUB.AGV.system1.agv_stop && PUB.AGV.system1.stop_by_front_detect == false && PUB.AGV.error.Emergency == false && PUB.AGV.signal.mark_sensor)
            //                {
            //                    return true;
            //                }
            //                else CheckAGVStopbyMARK();
            //            }
            //            else if (PUB.Result.CurrentPosCW == "1")
            //            {
            //                //내려가는 작업이고  AGV는 올라가고 있는데 RFID는 위졲이 감지되었다면 아래로 내려가야한다
            //                CheckAGVMoveTo(eGoDir.Down);
            //            }
            //        }
            //        //AGV는 내려가고 있다
            //        else if (VAR.BOOL[eVarBool.AGVDIR_UP] == false)
            //        {
            //            if (PUB.Result.CurrentPosCW == "1")
            //            {
            //                //네려가는 방향에서 내려가는 위치가 인식되었다면 마크에서 멈춰야 한다
            //                if (PUB.AGV.system1.agv_stop && PUB.AGV.system1.stop_by_front_detect == false && PUB.AGV.error.Emergency == false && PUB.AGV.signal.mark_sensor)
            //                {
            //                    return true;
            //                }
            //                else CheckAGVStopbyMARK();
            //            }
            //            else if (PUB.Result.CurrentPosCW == "0")
            //            {
            //                //내려가는 작업이고  AGV는 올라가고 있는데 RFID는 위졲이 감지되었다면 아래로 내려가야한다
            //                CheckAGVMoveTo(eGoDir.Up);
            //            }
            //        }
            //    }
            //}






            return false;
        }
        Boolean UpdateMotionPositionForCharger(string sender)
        {
            if (VAR.BOOL[eVarBool.AGVDIR_UP] == 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.PLC.Move(Device.PLC.Rundirection.Backward, "UpdateMotionPosition(" + sender + ")");
                    PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);//
                    tm_gocharge_command = DateTime.Now;
                }
            }
            else
            {
                //CCW (역방향 )
                //if (Pub.Result.RFIDPos < ePosition.CHARGE || Pub.Result.RFIDPos > ePosition.QC)
                //{
                //    //정상적이라면 RFID값은 QC + 혹은  QC - 에 있어야 하므로 항상 차저보다 높이 있다
                //    //그렇지 않다면 초기화해서 QC검색부터 다시 실행하게 한다
                //    //여기는 비정상 위치 값이다.
                //    Pub.sm.SetStepSeq(1);
                //}
                //else 
                //{
                //현재위치가 충전위치이고, 움직이지 않았다면 완료된 경우라 할수 있따
                if (PUB.Result.CurrentPos == ePosition.CHARGE &&
                      PUB.AGV.signal.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.PLC.Move(Device.PLC.Rundirection.Backward, "UpdateMotionPosition #1(" + sender + ")");
                        PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);//
                        LastCommandTime = DateTime.Now;
                    }
                }
                //}
            }

            return false;
        }



    }//cvass
}