using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using Project.StateMachine;
using COMM;
using AR;
using AGVNavigationCore.Models;
using static arDev.Narumi;

namespace Project
{
    public partial class fMain
    {
        /// <summary>
        /// 장비로 진입한다. -
        /// </summary>
        public Boolean _SM_RUN_ENTER(bool isFirst, TimeSpan seqTime)
        {
            var idx = 1;
            var funcname = PUB.sm.RunStep.ToString();

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

            //라이더멈춤이 설정되어있다면 음성으로 알려준다
            if (CheckLiderStop() == false) return false;

            if (PUB.sm.RunStepSeq == idx++)
            {
                PUB.log.Add("IN작업-시작");
                PUB.Speak("작업을 시작합니다");
                PUB.AGV.AGVMoveStop(funcname);
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //장비 노드 여부 확인 (버퍼가 아닌 도킹 가능 노드여야 함)
                if (PUB._virtualAGV.CurrentNode.StationType != StationType.Loader &&
                    PUB._virtualAGV.CurrentNode.StationType != StationType.Plating &&
                    PUB._virtualAGV.CurrentNode.StationType != StationType.Cleaner)
                {
                    SetRunStepError(ENIGProtocol.AGVErrorCode.NOT_EQUIPMENTPOINT, $"[{funcname}-{PUB.sm.RunStepSeq}] 현재 위치가 장비 노드가 아닙니다({PUB._virtualAGV.CurrentNode.StationType})");
                    return false;
                }
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //마크센서가 감지된상태여야 완전한위치로 가정한다
                if (PUB.AGV.signal1.mark_sensor == false)
                {
                    SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_SENSOR_FAIL, $"[{funcname}] 마크센서가 감지되지 않습니다");
                }
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //작업상태에 따라서 카트감지여부를 확인한ㄷ.
                if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOnEnter)
                {
                    //가지러가야하는 경우이므로 카트가 없어야한다.
                    if (PUB.AGV.signal2.cart_detect1 || PUB.AGV.signal2.cart_detect2)
                    {
                        SetRunStepError(ENIGProtocol.AGVErrorCode.CART_EXIST, $"[{funcname}] 카트가 존재하여 진입을 할 수 없습니다");
                        return false;
                    }
                }
                else if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOffEnter)
                {
                    //가져다 놓아야하므로 카트가 존재해야 한다
                    if (PUB.AGV.signal2.cart_detect1 == false || PUB.AGV.signal2.cart_detect2 == false)
                    {
                        var errmsg = $"[{funcname}] 카트감지센서가 인식되지 않았습니다";
                        SetRunStepError(ENIGProtocol.AGVErrorCode.CART_EXIST, errmsg);
                        return false;
                    }
                }
                else
                {
                    var errmsg = $"[{funcname}] 알수없는 작업형태입니다({PUB.NextWorkCmd})";
                    SetRunStepError(ENIGProtocol.AGVErrorCode.UnknownCommand, errmsg);
                    return false;
                }

                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //pick off/on 상관없이 리프트는 내려서 이동한다
                var liftCmd = LiftCommand.DN;
                PUB.AGV.LiftControl(liftCmd);
                VAR.TIME.Update(eVarTime.LastTurnCommandTime);
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //리프트 센서 확인 
                if ((PUB.AGV.signal1.lift_down == true && PUB.AGV.signal1.lift_up == false) == false)
                {
                    var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
                    if (ts.TotalSeconds > 10)
                    {
                        var errmsg = $"[{funcname}] 리프트다운이 확인되지 않습니다";
                        SetRunStepError(ENIGProtocol.AGVErrorCode.LIFT_ERROR, errmsg);
                    }
                    return false;
                }

                VAR.I32[eVarInt32.RetryManget] = 0;
                PUB.log.Add("리프트 동작 확인 완료");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //pick off 조건이라면. 마그넷을 on 한후에 민다.
                if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOffEnter)
                {
                    VAR.I32[eVarInt32.RetryManget] += 1;
                    PUB.AGV.LiftControl(LiftCommand.ON);
                }
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //pick off 조건이라면. 마그넷을 on 한후에 민다.
                if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOffEnter)
                {
                    //마그넷센서가 들어오는지 5초간 확인한다.
                    if (PUB.AGV.signal1.magnet_on == false)
                    {
                        if (seqTime.TotalSeconds > 5)
                        {
                            if (VAR.I32[eVarInt32.RetryManget] < 3)
                            {
                                PUB.sm.UpdateRunStepSeq(-1);
                            }
                            else
                            {
                                var errmsg = $"[{funcname}] 마그넷이 ON 되지 않았습니다";
                                SetRunStepError(ENIGProtocol.AGVErrorCode.MAGNET_ON_ERROR, errmsg);
                                VAR.I32[eVarInt32.RetryManget] = 0;
                            }

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

            else if (PUB.sm.RunStepSeq == idx++)
            {
                //저속이동 (후진 진입)
                var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
                {
                    Bunki = arDev.Narumi.eBunki.Strate,
                    Direction = arDev.Narumi.eMoveDir.Backward,
                    PBSSensor = 0,
                    Speed = arDev.Narumi.eMoveSpd.Low,
                });

                //명령이 실패되었다면 재시도를 한다
                if (ret != arDev.eNarumiCommandResult.Success)
                {
                    if (ret >= arDev.eNarumiCommandResult.Error)
                    {
                        SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_SPEED_SET_FAIL);
                    }
                    return false;
                }

                VAR.TIME.Update(eVarTime.LastTurnCommandTime);
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //전진이동
                PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //AGV구동을 확인하고 마크스탑을 설정한다.
                if (PUB.AGV.system1.agv_run == false)
                {
                    if (seqTime.TotalMilliseconds > 1000)
                    {
                        //구동이확인되지 않으면 오류처리를 한다.
                        SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_RUN_FAIL);
                    }
                    return false;
                }
                //마크스탑설정
                PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //마크스탑신호가 3초이내로 들어와야 한다
                if (PUB.AGV.data.Speed != 'S')
                {
                    if (seqTime.TotalMilliseconds > 3000)
                    {
                        SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_TIMEOUT);
                    }
                    return false;
                }
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //AGV가 멈출때까지 기다린다.
                if (PUB.AGV.system1.agv_run == true)
                {
                    if (seqTime.TotalSeconds > 10)
                    {
                        SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_STOP_FAIL);
                    }
                    return false;
                }
                PUB.sm.UpdateRunStepSeq();
                return false;
            }

            else if (PUB.sm.RunStepSeq == idx++)
            {
                //마크센서입력을 확인한다.
                if (PUB.AGV.signal1.mark_sensor == false)
                {
                    if (seqTime.TotalSeconds > 5)
                    {
                        SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_SENSOR_FAIL);
                    }
                    return false;
                }
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //완료되었다.
                PUB.log.Add("진입 완료 마그넷을 off합니다");
                VAR.I32[eVarInt32.RetryManget] = 0;
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //pick off 조건이라면. 마그넷을 on 한후에 민다.
                if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOffEnter)
                {
                    VAR.I32[eVarInt32.RetryManget] += 1;
                    PUB.AGV.LiftControl(LiftCommand.OFF);
                }
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //pick off 조건이라면. 마그넷을 on 한후에 민다.
                if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOffEnter)
                {
                    //마그넷센서가 들어오는지 5초간 확인한다.
                    if (PUB.AGV.signal1.magnet_on == true)
                    {
                        if (seqTime.TotalSeconds > 5)
                        {
                            if (VAR.I32[eVarInt32.RetryManget] < 3)
                            {
                                PUB.sm.UpdateRunStepSeq(-1);
                            }
                            else
                            {
                                VAR.I32[eVarInt32.RetryManget] = 0;
                                var errmsg = $"[{funcname}] 마그넷이 OFF 되지 않았습니다";
                                SetRunStepError(ENIGProtocol.AGVErrorCode.MAGNET_OF_ERROR, errmsg);
                            }
                        }
                        return false;
                    }
                }
                PUB.sm.UpdateRunStepSeq();
                return false;
            }


            return true;
        }
    }
}