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
    {
        bool SpeedSetRetry = false;
        DateTime _lastLidarOffTime_OUT = DateTime.MinValue;
        public Boolean _SM_RUN_BUFFER_OUT(bool isFirst, TimeSpan seqtime)
        {
            var funcname = "_SM_RUN_BUFFER_OUT";
            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_OUT).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.Forward,
                        PBSSensor = 0,
                        Speed = arDev.Narumi.eMoveSpd.Low,
                    });
                    _lastLidarOffTime_OUT = 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);

            /*
             * 버퍼 OUT 시퀀스
             * 1-1. PickOn 라면 리프트를 Up 한다
             * 1-2. PickOff 라면 리프트를 Down 한다 
             * 2.전진-저속-마크다운
             * 3.Turn-Right-180
            */

            if (PUB.sm.RunStepSeq == idx++)
            {
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 버퍼진출시작({PUB.NextWorkCmd}) Turn:{PUB._virtualAGV.Turn}");
                PUB.Speak(Lang.진출을시작합니다);
                PUB.sm.UpdateRunStepSeq();
                SpeedSetRetry = false;
                VAR.I32[eVarInt32.RetryLift] = 0;
                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._virtualAGV.Turn != AGVNavigationCore.Models.AGVTurn.L90)
                {
                    SetRunStepError(ENIGProtocol.AGVErrorCode.BUFFER_NOT_COMPLETE, $"[{funcname}-{PUB.sm.RunStepSeq}] 버퍼에서 나오려면 버퍼진입작업이 완료되어있어야 합니다");
                    return false;
                }
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                LiftCommand lift = PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOnExit ? LiftCommand.UP : LiftCommand.DN;
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 리프트제어 {lift}");
                VAR.I32[eVarInt32.RetryLift] += 1;
                PUB.AGV.LiftControl(lift);
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //리프트 센서 확인 
                var checksensor = PUB.NextWorkCmd == ENIGProtocol.AGVCommandHE.PickOnExit ? PUB.AGV.signal1.lift_up : PUB.AGV.signal1.lift_down;
                if (checksensor == false)
                {
                    if (seqtime.TotalSeconds > 20)
                    {
                        if (VAR.I32[eVarInt32.RetryLift] < 3)
                        {
                            PUB.log.AddAT($"[{funcname}-{PUB.sm.RunStepSeq}] 리프트가 완료되지 않아 재시도 합니다");
                            PUB.sm.UpdateRunStepSeq(-1);
                        }
                        else
                        {
                            VAR.I32[eVarInt32.RetryLift] = 0;
                            SetRunStepError(ENIGProtocol.AGVErrorCode.LIFT_ERROR, $"[{funcname}-{PUB.sm.RunStepSeq}] 리프트가 동작하지 않습니다");
                        }
                    }
                    else return false;
                }
                PUB.sm.UpdateRunStepSeq();
                return false;
            }

            else if (PUB.sm.RunStepSeq == idx++)
            {
                // [Smart Restart] 재시작 시 안전 검사
                // 이미 마크센서가 켜져서 도착한 상태라면 전진 구동을 생략하고 턴으로 넘어간다.
                if (PUB.AGV.signal1.mark_sensor == true && PUB._virtualAGV.Turn == AGVNavigationCore.Models.AGVTurn.L90)
                {
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 이미 마크센서 감지됨. 전진 이동을 생략합니다.");
                    PUB.logdebug.Add($"[AI_TRACE] {funcname} Smart Restart Skip Movement: MarkSensor=true, Turn=L90");
                    PUB.sm.SetStepSeq((byte)(idx + 4)); // Turn 스텝으로 직행 (현재 idx부터 4단계 뒤)
                    return false;
                }

                //빈 상태로 아웃해야한다.
                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 = 0,
                    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.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] AGV 이동 설정 완료 (Dir:Forward, Spd:Low)");


                VAR.I32[eVarInt32.RetryMove] = 0;
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //전진이동
                //if (PUB.AGV.signal1.mark_sensor == false)
                {
                    VAR.I32[eVarInt32.RetryMove] += 1;
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] AGV 전진 구동 시작");
                    PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
                }
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //AGV구동을 확인하고 마크스탑을 설정한다.
                //if (PUB.AGV.signal1.mark_sensor == false)
                {
                    if (PUB.AGV.system1.agv_run == false)
                    {
                        if (seqtime.TotalSeconds > 3)
                        {
                            if (VAR.I32[eVarInt32.RetryMove] < 3)
                            {
                                //재시도를 하게한다.
                                PUB.sm.UpdateRunStepSeq(-1);
                            }
                            else
                            {
                                //구동이확인되지 않으면 오류처리를 한다.
                                SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_RUN_FAIL);
                                VAR.I32[eVarInt32.RetryMove] = 0;
                            }

                        }
                        return false;
                    }
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] AGV 구동 확인 완료");
                }
                VAR.I32[eVarInt32.RetryMarkStop] = 0;
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                VAR.I32[eVarInt32.RetryMarkStop] += 1;
                PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] MarkStop 명령 전송 (진출 정지 장소)");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //if (PUB.AGV.signal1.mark_sensor == false)
                {
                    //마크스탑신호가 3초이내로 들어와야 한다
                    if (PUB.AGV.data.Speed != 'S')
                    {
                        if (seqtime.TotalSeconds > 3)
                        {
                            if (VAR.I32[eVarInt32.RetryMarkStop] < 3)
                            {
                                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] MarkStop 재시도");
                                PUB.sm.UpdateRunStepSeq(-1);
                            }
                            else
                            {
                                SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_TIMEOUT);
                                VAR.I32[eVarInt32.RetryMarkStop] = 0;
                            }
                            return false;
                        }
                        return false;
                    }
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] MarkStop 신구 확인 완료");
                }
                //else PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}]마크스탑이 미리 들어와있어. 이동및 스탑을 하지 않습니다");

                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //AGV가 멈출때까지 기다린다.
                if (PUB.AGV.system1.agv_run == true)
                {
                    if (seqtime.TotalSeconds > 15)
                    {
                        SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_STOP_FAIL);
                        return false;
                    }
                    return false;
                }

                // [Resume Safety] 재시작 직후 여기에 진입했는데 마크 센서가 꺼져있다면, 
                // 중간에 멈췄을 가능성이 크므로 다시 구동 스텝으로 되돌아간다.
                // (단, seqtime이 매우 짧은 경우 = 방금 Resume된 경우를 필터링)
                if (PUB.AGV.signal1.mark_sensor == false && seqtime.TotalSeconds < 1.0)
                {
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 재시작 감지됨. 하지만 목적지(마크) 도착 불확실. 이동을 재시도합니다.");
                    PUB.logdebug.Add($"[AI_TRACE] {funcname} Resume Safety Check Failed (Mark OFF at Stop Step). Rolling back to Step 6.");
                    PUB.sm.SetStepSeq(6); // Speed Set 스텝(이동 시작 단계)으로 복귀 
                    return false;
                }

                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] AGV 멈춤 확인 완료");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //우측으로 180도 턴
                //이미 턴이 되어있는지 확인한다 (재진입 시 중복 실행 방지)
                if (PUB._virtualAGV.Turn == AGVNavigationCore.Models.AGVTurn.R90)
                {
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 이미 Right Turn 완료 상태입니다. 턴 명령을 건너뜁니다.");
                }
                else
                {
                    PUB.AGV.AGVMoveRight180Turn();
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] AGV Right 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.R90)
                {
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 이미 턴이완료된 상태입니다");
                }
                else
                {
                    //왼쪽턴이 완료되지 않은경우
                    if (PUB.AGV.TurnInformation.State != arDev.eNarumiTurn.Right)
                    {
                        //움직임 확인을 위해 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.None; //턴완료
                    PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] Turn(right) 완료 - 위치복귀됨");
                }

                PUB.sm.UpdateRunStepSeq();    //이미완료된상태이므로 다음으로 진행한다.
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] BufferOut Complete");

                //라이더를 켜기위해서 기본 값을 설정한다.
                var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
                {
                    Bunki = arDev.Narumi.eBunki.Strate,
                    Direction = arDev.Narumi.eMoveDir.Backward,
                    PBSSensor = 2,
                    Speed = arDev.Narumi.eMoveSpd.Low,
                });

                if (ret != arDev.eNarumiCommandResult.Success)
                    PUB.log.AddE($"진출완료 후 라이더ON명령이 실패되었습니다(AGVMoveSet)");

                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //1.전진으로 3초간 이동한다 - 방향/속도 설정
                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)
                {
                    PUB.log.AddE($"[{funcname}-{PUB.sm.RunStepSeq}] 얼라인 전진속도 설정 실패(AGVMoveSet)");
                    SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_SPEED_SET_FAIL);
                    return false;
                }

                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 장비 얼라인 전진 이동 설정 (Dir:Forward)");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //전진이동 시작
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 장비 얼라인 전진 구동 시작");
                PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Forward);
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //3초간 이동한다.
                if (seqtime.TotalSeconds < 3) return false;
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //2.멈춘다
                PUB.AGV.AGVMoveStop(funcname);
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 3초 전진 완료 후 정지 명령 전달");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //AGV 멈출때까지 기다림
                if (PUB.AGV.system1.agv_run == true)
                {
                    if (seqtime.TotalSeconds > 5)
                    {
                        PUB.AGV.AGVMoveStop(funcname + "[TIMEOUT]");
                        SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_STOP_FAIL);
                        return false;
                    }
                    return false;
                }
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] AGV 전진 정지 확인 완료");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //3. 후진으로 이동하면서 마크스탑을 설정한다 - 방향/속도 설정
                var ret = PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
                {
                    Bunki = arDev.Narumi.eBunki.Strate,
                    Direction = arDev.Narumi.eMoveDir.Backward,
                    PBSSensor = 2,
                    Speed = arDev.Narumi.eMoveSpd.Low,
                });

                if (ret != arDev.eNarumiCommandResult.Success)
                {
                    PUB.log.AddE($"[{funcname}-{PUB.sm.RunStepSeq}] 얼라인 후진속도 설정 실패(AGVMoveSet)");
                    SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_SPEED_SET_FAIL);
                    return false;
                }
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 장비 얼라인 후진 이동 설정 (Dir:Backward)");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //후진 이동 시작
                PUB.AGV.AGVMoveRun(arDev.Narumi.eRunOpt.Backward);
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 장비 얼라인 후진 구동 시작");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //마크스탑을 설정한다.
                if (seqtime.TotalSeconds < 1.0) return false; // 짧은 구동 후 마크스탑 설정
                PUB.AGV.AGVMoveStop(funcname, arDev.Narumi.eStopOpt.MarkStop);
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 얼라인 후진 MarkStop 명령 전송");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //마크스탑 신호 확인
                if (PUB.AGV.data.Speed != 'S')
                {
                    if (seqtime.TotalSeconds > 10)
                    {
                        SetRunStepError(ENIGProtocol.AGVErrorCode.MARK_TIMEOUT);
                        return false;
                    }
                    return false;
                }
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] MarkStop 신호 확인 완료");
                PUB.sm.UpdateRunStepSeq();
                return false;
            }
            else if (PUB.sm.RunStepSeq == idx++)
            {
                //4. agv정지가 확인되면 완료이다.
                if (PUB.AGV.system1.agv_run == true)
                {
                    if (seqtime.TotalSeconds > 10)
                    {
                        SetRunStepError(ENIGProtocol.AGVErrorCode.AGV_STOP_FAIL);
                        return false;
                    }
                    return false;
                }
                PUB.log.Add($"[{funcname}-{PUB.sm.RunStepSeq}] 장비 얼라인 완료 (AGV 완벽 정지 확인)");

                //완료후 라이더를 켜기위해서 기본 값을 복구한다
                PUB.AGV.AGVMoveSet(new arDev.Narumi.BunkiData
                {
                    Bunki = arDev.Narumi.eBunki.Strate,
                    Direction = arDev.Narumi.eMoveDir.Backward,
                    PBSSensor = 2,
                    Speed = arDev.Narumi.eMoveSpd.Low,
                });

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

            PUB.AddEEDB($"[{funcname}] bufferout 완료({PUB.Result.TargetPos})");

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