using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using Project.StateMachine;
using COMM;
using AR;
using System.Windows.Media.Animation;
using System.Net.Security;
using AGVNavigationCore.Models;
using static arDev.Narumi;

namespace Project
{
    public partial class fMain
    {
        DateTime _lastLidarOffTime_IN = DateTime.MinValue;
        public Boolean _SM_RUN_BUFFER_IN(bool isFirst, TimeSpan seqtime)
        {
            var funcname = "_SM_RUN_BUFFER_IN";
            var idx = 1;

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

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

            // [Global Safety] 버퍼 작업 중에는 무조건 라이다를 끄도록 강제한다 (재시작 시 멈춤 방지)
            if (PUB.AGV.PBSSensor != arDev.eNarmiPBSSensor.off || PUB.AGV.data.Distance > 0)
            {
                if ((DateTime.Now - _lastLidarOffTime_IN).TotalSeconds > 2)
                {
                    PUB.log.Add($"[{funcname}] 버퍼 작업 중 라이다 강제 OFF 시도");
                    PUB.logdebug.Add($"[AI_TRACE] {funcname} LIDAR/Dist Safety Override: Sensor={PUB.AGV.PBSSensor}, Dist={PUB.AGV.data.Distance}");
                    PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
                    {
                        Bunki = arDev.Narumi.eBunki.Strate,
                        Direction = arDev.Narumi.eMoveDir.Backward, 
                        PBSSensor = 0,
                        Speed = arDev.Narumi.eMoveSpd.Low,
                    });
                    _lastLidarOffTime_IN = DateTime.Now;
                }
                return false; // 라이다가 꺼질 때까지 진행 스톱 (명령이 적용될 때까지 대기)
            }

            // [AI_TRACE] Log Step Progress
            PUB.logdebug.Add($"[AI_TRACE] {funcname} Step {PUB.sm.RunStepSeq} execution");

            // [State Save] 현재 진행 단계를 저장한다
            SaveBufferStep(PUB._virtualAGV.CurrentNode, PUB.sm.RunStepSeq, PUB.NextWorkCmd);

            /*
             * 버퍼IN시퀀스
             * 1. 회전이 진행되지 않았다면 회전을 진행한다.
             * 2. LIFT DOWN
             * 3. 후진-저속-마크다운 실행
            */

            if (PUB.sm.RunStepSeq == idx++)
            {
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 버퍼진입시작({PUB.NextWorkCmd}) Turn:{PUB._virtualAGV.Turn}");
                PUB.Speak(Lang.진입을작합니다);
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //버퍼 노드 여부 확인
                if (PUB._virtualAGV.CurrentNode.StationType != AGVNavigationCore.Models.Station.Buffer)
                {
                    SetRunStepError(ENIGProtocol.AGVErrorCode.NOT_BUFFERPOINT, $"[{funcname}-{PUB.sm.RunStepSeq}] 현재 위치가 버퍼가 아닙니다({PUB._virtualAGV.CurrentNode.StationType})");
                    return false;
                }
                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}-{PUB.sm.RunStepSeq}] 이미 카트가 감지되어 진입할 수 없습니다");
                        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,
                });
                PUB.log.AddAT("라이다를 끈다, 재시도 코드 필요함");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //턴확인
                if (PUB._virtualAGV.Turn != AGVNavigationCore.Models.AGVTurn.L90)
                {
                    //동작중이면 동작을 멈춘다
                    if (PUB.AGV.system1.agv_run == true)
                    {
                        var ts = VAR.TIME.RUN(eVarTime.LastStopCommandTime);
                        if (ts.TotalSeconds > 3)
                        {
                            PUB.AGV.AGVMoveStop(funcname);
                            VAR.TIME.Update(eVarTime.LastStopCommandTime);
                        }
                    }
                    else PUB.sm.UpdateRunStepSeq(); //agv가 멈춰있으므로 턴을 진행해야 한다
                }
                else PUB.sm.UpdateRunStepSeq();    //이미완료된상태이므로 다음으로 진행한다.
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //이미 턴이 되어있는지 확인한다 (재진입 시 중복 실행 방지)
                if (PUB._virtualAGV.Turn == AGVNavigationCore.Models.AGVTurn.L90)
                {
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 이미 Left Turn 완료 상태입니다. 턴 명령을 건너뜁니다.");
                }
                else
                {
                    PUB.AGV.AGVMoveLeft180Turn();
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] AGV Left Turn");
                    VAR.TIME.Update(eVarTime.LastTurnCommandTime);
                    PUB._mapCanvas.SetInfoMessage($"턴 실행");
                }
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //이미 완료된 상태라면 대기 과정을 건너뛴다.
                if (PUB._virtualAGV.Turn == AGVNavigationCore.Models.AGVTurn.L90)
                {
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 이미 턴이완료된 상태입니다");
                }
                else
                {
                    //왼쪽턴이 완료되지 않은경우
                    if (PUB.AGV.TurnInformation.State != arDev.eNarumiTurn.Left)
                    {
                        //움직임 확인을 위해 3초간은 검증을 유예한다
                        if (PUB.AGV.TurnInformation.Runtime.TotalSeconds < 3) return false;

                        //턴 이동 상태가 확인되어야 한다.
                        var overtime = 30;
                        if (PUB.AGV.TurnInformation.Runtime.TotalSeconds > overtime)
                        {
                            //30초동안 AGV까 움직이지 않았다면 오류 처리한다.
                            SetRunStepError(ENIGProtocol.AGVErrorCode.TURN_FAIL, $"[{funcname}] {overtime}초이내 턴 감지 안됨");
                            return false;
                        }
                        else PUB._mapCanvas.SetInfoMessage($"턴 진행 중({PUB.AGV.TurnInformation.Runtime.TotalSeconds:N0}/{overtime})");
                        return false;
                    }
                    PUB._virtualAGV.Turn = AGVNavigationCore.Models.AGVTurn.L90; //턴완료
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] Turn(left) 완료");
                }

                PUB.sm.UpdateRunStepSeq();    //이미완료된상태이므로 다음으로 진행한다.
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //버퍼에들어갈때에는 가져다 놓을때도 가지러 갈때에도 리프트는 내려서 들어간다
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 리프트를 내립니다");
                VAR.I32[eVarInt32.RetryLift] = 0;
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //리프트 센서 확인 
                if (PUB.AGV.signal1.lift_down == false)
                {
                    VAR.I32[eVarInt32.RetryLift] += 1;
                    var rlt = PUB.AGV.LiftControl(LiftCommand.DN);
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 리프트 하강 실행:{rlt}");
                }
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //리프트 센서 확인 
                if (PUB.AGV.signal1.lift_down == false)
                {
                    if (seqtime.TotalSeconds > 20)
                    {
                        if (VAR.I32[eVarInt32.RetryLift] < 3)
                        {
                            PUB.log.AddAT($"[{funcname}-{PUB.sm.RunStepSeq}] 리프트가 내려가지 않아 재시도 합니다");
                            PUB.sm.UpdateRunStepSeq(-1);
                        }
                        else
                        {
                            SetRunStepError(ENIGProtocol.AGVErrorCode.LIFT_ERROR,
                                $"[{funcname}-{PUB.sm.RunStepSeq}] 리프트가 내려가지 않습니다");
                            VAR.I32[eVarInt32.RetryLift] = 0;
                        }
                    }
                    else PUB._mapCanvas.SetInfoMessage($"리프트 하강 확인 중({seqtime.TotalSeconds:N0}/20)");
                    return false;
                }
                VAR.I32[eVarInt32.RetryManget] = 0;
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 리프트 하강 완료");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOffEnter)
                {
                    VAR.I32[eVarInt32.RetryManget] += 1;
                    PUB.AGV.LiftControl(LiftCommand.ON);
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 마그넷ON");
                }

                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //마그넷센서가 들어오는지 5초간 확인한다.
                if (PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOffEnter &&
                    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++)
            {
                //저속이동 (후진 진입)

                // [Smart Restart] 재시작 시 안전 검사
                // 이미 턴을 완료했고(L90), 현재 마크 센서가 감지된다면(ON),
                // 이미 목적지(Mark 2)에 도착한 것으로 간주하여 후진을 생략한다.
                if (PUB._virtualAGV.Turn == AGVNavigationCore.Models.AGVTurn.L90 && PUB.AGV.signal1.mark_sensor == true)
                {
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 이미 목적지 도착 확인됨(Turn:L90, Sensor:ON). 후진 이동을 생략합니다.");
                    PUB.sm.UpdateRunStepSeq();
                    return false;
                }

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

                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 도킹을 위해 후진 이동 시작 (Dir:Backward, Spd:Low)");
                PUB._mapCanvas.SetInfoMessage($"도킹을 위해 후진 이동 시작");
                VAR.I32[eVarInt32.RetryMove] = 0;
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                VAR.I32[eVarInt32.RetryMove] += 1;
                var rlt = PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 이동명령 결과:{rlt}");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //이동확인을 한다.
                if (PUB.AGV.system1.agv_run == false)
                {
                    if (seqtime.TotalSeconds > 10)
                    {
                        if (VAR.I32[eVarInt32.RetryMove] < 3)
                        {
                            PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 이동명령 재전송");
                            PUB.sm.UpdateRunStepSeq(-1);
                        }
                        else
                        {
                            SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_RUN_FAIL);
                            VAR.I32[eVarInt32.RetryMove] = 0;
                        }
                    }
                    else PUB._mapCanvas.SetInfoMessage($"AGV이동 확인 중");
                    return false;
                }

                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] AGV 이동 확인 완료");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //마크스탑으로 이동
                if (seqtime.TotalSeconds < 1.5) //일부시간을 기다려줘야한다.
                {
                    return false;
                }
                PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] MarkStop 명령 전송 (도킹 위치 정렬)");
                PUB._mapCanvas.SetInfoMessage($"도킹을 위해 후진 이동 시작");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //마크스탑신호가 3초이내로 들어와야 한다
                if (PUB.AGV.data.Speed != 'S')
                {
                    if (seqtime.TotalSeconds > 3)
                    {
                        SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_TIMEOUT);
                        return false;
                    }
                }
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] MarkStop 신호 확인 완료");
                VAR.TIME.Update(eVarTime.LastTurnCommandTime);
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //AGV가 멈출때까지 기다린다.
                if (PUB.AGV.system1.agv_run == true)
                {
                    var ts = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
                    if (ts.TotalSeconds > 10)
                    {
                        SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_STOP_FAIL);
                        return false;
                    }
                    return false;
                }
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] AGV멈춤확인");
                VAR.TIME.Update(eVarTime.LastTurnCommandTime);
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //마크센서확인
                if (PUB.AGV.signal1.mark_sensor == false)
                {
                    if (seqtime.TotalSeconds > 3)
                    {
                        SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_SENSOR_FAIL);
                        return false;
                    }
                    return false;
                }
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 마크센서 확인완료");
                VAR.I32[eVarInt32.RetryManget] = 0;
                VAR.TIME.Update(eVarTime.LastTurnCommandTime);
                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;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                if (seqtime.TotalSeconds < 2) return false;
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 작업({PUB.NextWorkCmd}) 완료. 대기 상태로 전환 (퇴출 명령 대기)");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //완료되었다.
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 버퍼 진입 완료");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }

            // 작업을 마치고 설비 안에 멈춰있는 상태.
            // ACS가 이 상태를 확인하고 NextWorkCmd로 퇴출(Out) 명령을 보내야 함.
            //PUB.AddEEDB($"[{funcname}] 버퍼작업완료({PUB.Result.TargetPos})");

            // [State Clear] 작업 완료 시 저장된 상태를 지운다
            ClearBufferStep(PUB._virtualAGV.CurrentNode);
            PUB.logdebug.Add($"[AI_TRACE] {funcname} Completed Successfully.");
            return true;
        }
    }
}
