using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Core;
using AGVNavigationCore.PathFinding.Planning;
using AGVNavigationCore.Utils;
using AR;
using arDev;
using COMM;
using Project.StateMachine;

namespace Project
{
    public partial class fMain
    {

        private void XBE_ProtocReceived(object sender, ENIG.EEProtocol.DataEventArgs e)
        {
            PUB.logdebug.Add($"[AI_TRACE] XBEE Protoc Received: Cmd={e.ReceivedPacket.Command}, ID={e.ReceivedPacket.ID}, DataLen={e.ReceivedPacket.Data.Length}");
            //TODO : 기능 처리필요 (XBee 메세지 데이터처리)
            //PUB.CheckManualChargeMode() : 수동충전확인
            //VAR.BOOL[eVarBool.FLAG_AUTORUN]  : 자동실행
            //PUB.Speak("현재 위치는 QA 입니다.") : 음성출력

            //ACS 수신 데이터 처리(타 장비는 확인하지 않는다)
            if (e.ReceivedPacket.ID == 0)
            {
                var logPrefix = "ACS";
                var data = e.ReceivedPacket.Data;
                var cmd = (ENIGProtocol.AGVCommandHE)e.ReceivedPacket.Command;
                if (data.Length < 1)
                {
                    PUB.log.Add($"ACS 데이터에서 TARGET ID가 없습니다(data : first byte)");
                    return;
                }

                //대상디바이스
                var TargetID = data[0];

                //해당 패킷의 대상이 내가아니면 처리하지 않는다
                if (PUB.setting.XBE_ID != TargetID) return;


                //자동실행모드가 아니라면 무조건 에러를 반환한다
                if (VAR.BOOL[eVarBool.FLAG_AUTORUN] == false)
                {
                    SetRunStepError(ENIGProtocol.AGVErrorCode.ManualMode, "현재 자동실행 모드가 아닙니다");
                    return;
                }

                switch (cmd)
                {

                    case ENIGProtocol.AGVCommandHE.SetCurrent: //Set Current Position
                        Resultclear();

                        if (data.Length > 4)
                        {
                            var currTag = System.Text.Encoding.Default.GetString(data, 1, data.Length - 1);
                            if (ushort.TryParse(currTag, out ushort currtagValue))
                            {
                                var node = PUB._mapCanvas.Nodes.FirstOrDefault(t => t.RfidId == currtagValue);
                                if (node == null)
                                {
                                    var ermsg = ($"[{logPrefix}-SetCurrent] 노드정보를 찾을 수 없습니다 RFID:{currTag}");
                                    SetRunStepError(ENIGProtocol.AGVErrorCode.EmptyNode, ermsg);
                                    return;
                                }
                                else
                                {
                                    PUB.log.AddI($"XBEE:현재위치설정:[{node.RfidId}]{node.Id}");
                                }

                                PUB._mapCanvas.SetAGVPosition(PUB.setting.MCID, node, PUB._virtualAGV.CurrentDirection);
                                PUB._virtualAGV.SetPosition(node, PUB._virtualAGV.CurrentDirection);
                                PUB.SaveLastPosition(); //위치저장 260205
                            }
                            else PUB.log.AddE($"[{logPrefix}-SetCurrent] TagString Value Errorr:{data}");
                        }
                        else PUB.log.AddE($"[{logPrefix}-SetCurrent] TagString Lenght Errorr:{data.Length}");
                        break;

                    case ENIGProtocol.AGVCommandHE.PickOnEnter:  // 110
                    case ENIGProtocol.AGVCommandHE.PickOffEnter: // 111
                        {
                            Resultclear();
                            PUB.log.AddI($"XBEE:작업명령수신:{cmd}");
                            PUB.logdebug.Add($"[AI_TRACE] Processing WorkCommand: {cmd}");

                            // 현재 위치 확인 (TargetNode가 아닌 CurrentNode 기준)
                            var currNode = PUB._virtualAGV.CurrentNode;
                            if (currNode == null)
                            {
                                var msg = $"[{logPrefix}-{cmd}] 현재 노드를 알 수 없습니다";
                                SetRunStepError(ENIGProtocol.AGVErrorCode.EmptyNode, msg);
                                return;
                            }

                            var targetNode = PUB._virtualAGV.TargetNode;
                            if (targetNode == null)
                            {
                                var msg = $"[{logPrefix}-{cmd}] 목표 노드를 알 수 없습니다";
                                SetRunStepError(ENIGProtocol.AGVErrorCode.EmptyNode, msg);
                                return;
                            }

                            //버퍼의 경우 직전에 멈추기 때문에 스테이션종류를 보정해준다
                            var StationType = currNode.StationType;
                            if (StationType == Station.Normal && targetNode.StationType == Station.Buffer)
                                StationType = Station.Buffer;

                            ERunStep nextStep = ERunStep.READY;
                            switch (StationType)
                            {
                                case Station.Loder: nextStep = ERunStep.LOADER_IN; break;
                                case Station.Cleaner: nextStep = ERunStep.UNLOADER_IN; break;
                                case Station.Buffer: nextStep = ERunStep.BUFFER_IN; break;
                                case Station.Plating: nextStep = ERunStep.CLEANER_IN; break;
                                default:
                                    PUB.log.AddE($"[{logPrefix}-{cmd}] 해당 노드타입({StationType})은 작업을 지원하지 않습니다.");
                                    return;
                            }

                            //다음명령처리
                            PUB.NextWorkCmd = cmd;
                            PUB.log.AddI($"작업 시작: {nextStep} (Type: {cmd})");

                            // [Resume Logic]
                            if (nextStep == ERunStep.BUFFER_IN || nextStep == ERunStep.BUFFER_OUT)
                            {
                                int bufIdx = GetBufferIndex(targetNode);
                                if (bufIdx > 0)
                                {
                                    var stepVar = (eVarInt32)Enum.Parse(typeof(eVarInt32), $"Buffer{bufIdx}Step");
                                    var cmdVar = (eVarString)Enum.Parse(typeof(eVarString), $"Buffer{bufIdx}LastCmd");

                                    int savedStep = VAR.I32[stepVar];
                                    string savedCmd = VAR.STR[cmdVar];

                                    // Resume Condition: Saved step exists AND command matches AND node matches
                                    if (savedStep > 1 && savedCmd == cmd.ToString() && currNode.Id == targetNode.Id)
                                    {
                                        PUB.log.AddI($"[RESUME] 버퍼 {bufIdx} 작업 재개: Step {savedStep} (Cmd: {cmd})");
                                        VAR.I32[eVarInt32.RetryMove] = 0;
                                        VAR.I32[eVarInt32.RetryLift] = 0;
                                        VAR.I32[eVarInt32.RetryManget] = 0;
                                        VAR.I32[eVarInt32.RetryMarkStop] = 0;
                                        PUB.sm.SetNewRunStep(nextStep);
                                        PUB.sm.SetStepSeq((byte)savedStep);
                                        return; // SetNewRunStep + SetStepSeq 로직으로 바로 시작
                                    }
                                    else if (savedStep > 0)
                                    {
                                        PUB.log.AddI($"[RESET] 버퍼 {bufIdx} 작업 초기화 (SavedStep:{savedStep}, SavedCmd:{savedCmd}, NodeMatch:{currNode.Id == targetNode.Id})");
                                        VAR.I32[stepVar] = 0;
                                        VAR.STR[cmdVar] = "";
                                    }
                                }
                            }

                            PUB.sm.SetNewRunStep(nextStep);
                        }
                        break;
                    case ENIGProtocol.AGVCommandHE.PickOnExit:
                    case ENIGProtocol.AGVCommandHE.PickOffExit:
                        {
                            Resultclear();
                            PUB.log.AddI($"XBEE:작업명령수신:{cmd}");
                            PUB.logdebug.Add($"[AI_TRACE] Processing Exit WorkCommand: {cmd}");

                            // 현재 위치 확인 (TargetNode가 아닌 CurrentNode 기준)
                            var currNode = PUB._virtualAGV.CurrentNode;
                            if (currNode == null)
                            {
                                var msg = $"[{logPrefix}-{cmd}] 현재 노드를 알 수 없습니다";
                                SetRunStepError(ENIGProtocol.AGVErrorCode.EmptyNode, msg);
                                return;
                            }

                            var targetNode = PUB._virtualAGV.TargetNode;
                            if (targetNode == null)
                            {
                                var msg = $"[{logPrefix}-{cmd}] 목표 노드를 알 수 없습니다";
                                SetRunStepError(ENIGProtocol.AGVErrorCode.EmptyNode, msg);
                                return;
                            }

                            //버퍼의 경우 직전에 멈추기 때문에 스테이션종류를 보정해준다
                            var StationType = currNode.StationType;
                            if (StationType == Station.Normal && targetNode.StationType == Station.Buffer)
                                StationType = Station.Buffer;

                            ERunStep nextStep = ERunStep.READY;
                            switch (StationType)
                            {
                                case Station.Loder: nextStep = ERunStep.LOADER_OUT; break;
                                case Station.Cleaner: nextStep = ERunStep.UNLOADER_OUT; break;
                                case Station.Buffer: nextStep = ERunStep.BUFFER_OUT; break;
                                case Station.Plating: nextStep = ERunStep.CLEANER_OUT; break;
                                default:
                                    PUB.logdebug.Add($"[AI_TRACE] REJECTED: Unsupported station type {StationType} for command {cmd}");
                                    PUB.log.AddE($"[{logPrefix}-{cmd}] 해당 노드타입({StationType})은 작업을 지원하지 않습니다.");
                                    return;
                            }

                            //다음명령처리
                            PUB.NextWorkCmd = cmd;
                            // [Resume Logic]
                            if (nextStep == ERunStep.BUFFER_IN || nextStep == ERunStep.BUFFER_OUT)
                            {
                                int bufIdx = GetBufferIndex(targetNode);
                                if (bufIdx > 0)
                                {
                                    var stepVar = (eVarInt32)Enum.Parse(typeof(eVarInt32), $"Buffer{bufIdx}Step");
                                    var cmdVar = (eVarString)Enum.Parse(typeof(eVarString), $"Buffer{bufIdx}LastCmd");

                                    int savedStep = VAR.I32[stepVar];
                                    string savedCmd = cmd.ToString();

                                    // Resume Condition (Exit): Exit commands can resume if the AGV is still at the buffer
                                    if (savedStep > 1 && VAR.STR[cmdVar] == savedCmd && currNode.Id == targetNode.Id)
                                    {
                                        PUB.log.AddI($"[RESUME] 버퍼 {bufIdx} 퇴출 재개: Step {savedStep}");
                                        VAR.I32[eVarInt32.RetryMove] = 0;
                                        VAR.I32[eVarInt32.RetryLift] = 0;
                                        VAR.I32[eVarInt32.RetryManget] = 0;
                                        VAR.I32[eVarInt32.RetryMarkStop] = 0;
                                        PUB.sm.SetNewRunStep(nextStep);
                                        PUB.sm.SetStepSeq((byte)savedStep);
                                        return;
                                    }
                                }
                            }

                            PUB.sm.SetNewRunStep(nextStep);
                        }
                        break;
                    case ENIGProtocol.AGVCommandHE.Charger: // 112
                        {
                            Resultclear();
                            PUB.log.AddI($"XBEE:충전명령수신");
                            PUB.logdebug.Add($"[AI_TRACE] Processing Charger Command");

                            // 버퍼 작업 중일 경우 충전 명령 거부
                            if (PUB._virtualAGV.Turn != AGVTurn.None)
                            {
                                SetRunStepError(ENIGProtocol.AGVErrorCode.BUFFER_NOT_COMPLETE, $"[{logPrefix}-Charger] 버퍼 작업이 완료되지 않아 충전할 수 없습니다.");
                                return;
                            }

                            PUB.NextWorkCmd = ENIGProtocol.AGVCommandHE.Charger;
                            PUB.sm.SetNewRunStep(ERunStep.GOCHARGE);
                        }
                        break;

                    case ENIGProtocol.AGVCommandHE.GotoAlias:
                    case ENIGProtocol.AGVCommandHE.Goto:  //move to tag
                        Resultclear();
                        var datalength = cmd == ENIGProtocol.AGVCommandHE.GotoAlias ? 2 : 1;
                        if (data.Length > datalength)
                        {
                            var currTag = System.Text.Encoding.Default.GetString(data, 1, data.Length - 1).Trim();
                            MapNode targetNode = null;
                            if (cmd == ENIGProtocol.AGVCommandHE.GotoAlias)
                            {
                                targetNode = PUB._mapCanvas.Nodes.FirstOrDefault(t => t.AliasName == currTag);
                            }
                            else
                            {
                                if (ushort.TryParse(currTag, out ushort currtagvalue))
                                    targetNode = PUB._mapCanvas.Nodes.FirstOrDefault(t => t.RfidId == currtagvalue);
                                else PUB.log.Add($"targstring 이 숫자가 아니라서 대상을 설정할 수 없습니다 값:{currTag}");
                            }

                            if (targetNode != null)
                            {
                                //자동상태가아니라면 처리하지 않는다.
                                if (VAR.BOOL[eVarBool.FLAG_AUTORUN] == false)
                                {
                                    PUB.logdebug.Add($"[AI_TRACE] REJECTED: Manual mode (AutoRun=false) for Goto {currTag}");
                                    SetRunStepError(ENIGProtocol.AGVErrorCode.ManualMode, $"[{logPrefix}-Goto] 자동실행상태가 아닙니다");
                                    return;
                                }

                                //노드가 들어왔는데. 일반 노드라면.. 턴 정보를 제거한다.
                                if (PUB._virtualAGV.CurrentNode != null &&
                                    PUB._virtualAGV.CurrentNode.StationType != Station.Buffer &&
                                    PUB._virtualAGV.Turn != AGVTurn.None)
                                {
                                    PUB.log.AddAT($"[{logPrefix}-Goto] 현재노드위치가 버퍼가 아니라서 턴정보를 초기화합니다{PUB._virtualAGV.Turn}");
                                    PUB._virtualAGV.Turn = AGVTurn.None;
                                }

                                //s/w턴이 걸려있다면 이동 불가로한다.(버퍼 작업중이다)
                                if (PUB._virtualAGV.Turn != AGVTurn.None)
                                {
                                    SetRunStepError(ENIGProtocol.AGVErrorCode.BUFFER_NOT_COMPLETE, $"[{logPrefix}-Goto] 버퍼 작업이 완료되지 않았습니다({PUB._virtualAGV.Turn})");
                                    return;
                                }

                                //목적지
                                PUB._virtualAGV.TargetNode = targetNode;
                                if (targetNode == null)
                                {
                                    SetRunStepError(ENIGProtocol.AGVErrorCode.EmptyNode, $"[{logPrefix}-Goto] 노드정보를 찾을 수 없습니다 RFID:{currTag}");
                                    return;
                                }

                                ///출발지
                                var startNode = PUB._mapCanvas.Nodes.FirstOrDefault(t => t.RfidId == PUB._virtualAGV.CurrentNode.RfidId);
                                PUB._virtualAGV.StartNode = startNode;
                                if (startNode == null)
                                {
                                    PUB.log.AddE($"[{logPrefix}-Goto] 시작노드가 없습니다(현재위치 없음) NodeID:{PUB._virtualAGV.CurrentNode.Id}");
                                }

                                //대상이동으로 처리한다.
                                if (PUB.sm.RunStep != ERunStep.GOTO)
                                {
                                    PUB.sm.SetNewRunStep(StateMachine.ERunStep.GOTO);
                                    PUB.sm.ResetRunStepSeq();
                                }

                                PUB.SaveLastPosition();

                                //Move to 
                                PUB.log.Add($"[{logPrefix}-{cmd}] {startNode.RfidId} -> {targetNode.RfidId}");
                            }
                            else PUB.log.AddE($"[{logPrefix}-{cmd}] 대상노드가 없습니다 {data}");
                        }
                        else PUB.log.AddE($"[{logPrefix}-{cmd}] Length Error:{data.Length}");
                        break;

                    case ENIGProtocol.AGVCommandHE.LTurn180:
                    case ENIGProtocol.AGVCommandHE.RTurn180:
                    case ENIGProtocol.AGVCommandHE.LTurn:
                    case ENIGProtocol.AGVCommandHE.RTurn:
                        {
                            // 버퍼 작업 중일 경우 원격 턴 명령 거부
                            if (PUB._virtualAGV.Turn != AGVTurn.None)
                            {
                                SetRunStepError(ENIGProtocol.AGVErrorCode.BUFFER_NOT_COMPLETE, $"[{logPrefix}-{cmd}] 버퍼 작업 중에는 턴 명령을 수행할 수 없습니다.");
                                return;
                            }

                            if (cmd == ENIGProtocol.AGVCommandHE.LTurn180)
                            {
                                PUB.log.Add($"[{logPrefix}-LTurn180]");
                                PUB.sm.SetNewRunStep(ERunStep.REMOTECONTROL);
                                PUB.AGV.AGVMoveLeft180Turn();
                            }
                            else if (cmd == ENIGProtocol.AGVCommandHE.RTurn180)
                            {
                                PUB.log.Add($"[{logPrefix}-RTurn180]");
                                PUB.sm.SetNewRunStep(ERunStep.REMOTECONTROL);
                                PUB.AGV.AGVMoveRight180Turn();
                            }
                            else if (cmd == ENIGProtocol.AGVCommandHE.LTurn)
                            {
                                PUB.log.Add($"[{logPrefix}-LTurn]");
                                PUB.sm.SetNewRunStep(ERunStep.REMOTECONTROL);
                                PUB.AGV.AGVMoveManual(arDev.Narumi.ManulOpt.LT, arDev.Narumi.Speed.Low, arDev.Narumi.Sensor.AllOn);
                            }
                            else if (cmd == ENIGProtocol.AGVCommandHE.RTurn)
                            {
                                PUB.log.Add($"[{logPrefix}-RTurn]");
                                PUB.sm.SetNewRunStep(ERunStep.REMOTECONTROL);
                                PUB.AGV.AGVMoveManual(arDev.Narumi.ManulOpt.RT, arDev.Narumi.Speed.Low, arDev.Narumi.Sensor.AllOn);
                            }
                        }
                        break;
                    case ENIGProtocol.AGVCommandHE.Stop:  //stop
                        PUB.log.Add($"[{logPrefix}-Stop]");
                        PUB.AGV.AGVMoveStop("xbee");
                        break;
                    case ENIGProtocol.AGVCommandHE.Reset: //Error Reset
                        PUB.log.Add($"[{logPrefix}-Reset]");
                        ResetSystemError();
                        break;

                    case ENIGProtocol.AGVCommandHE.Manual: //Manual Move (Direction, speed, runtime)
                        Resultclear();

                        // 버퍼 작업 중일 경우 수동 이동 거부
                        if (PUB._virtualAGV.Turn != AGVTurn.None)
                        {
                            SetRunStepError(ENIGProtocol.AGVErrorCode.BUFFER_NOT_COMPLETE, $"[{logPrefix}-Manual] 버퍼 작업 중에는 수동 이동을 할 수 없습니다.");
                            return;
                        }

                        PUB.sm.SetNewRunStep(ERunStep.REMOTECONTROL);
                        var Direction = data[1];     //0=back, 1=forward, 2=left, 3=right
                        var Speed = data[2];         //0=slow, 1=normal, 2=fast

                        arDev.Narumi.ManulOpt opt = arDev.Narumi.ManulOpt.BS;
                        arDev.Narumi.Speed spd = arDev.Narumi.Speed.Low;
                        if (Speed == 1) spd = arDev.Narumi.Speed.Mid;
                        else if (Speed == 2) spd = arDev.Narumi.Speed.High;

                        //0.자동모드가 아니라면 실행하지 않는다
                        //1.입력된 파라미터로 AGV를 이동한다
                        if (Direction == 0) opt = arDev.Narumi.ManulOpt.BS;
                        else if (Direction == 1) opt = arDev.Narumi.ManulOpt.FS;
                        else if (Direction == 2) opt = arDev.Narumi.ManulOpt.LT;
                        else if (Direction == 3) opt = arDev.Narumi.ManulOpt.RT;

                        PUB.log.Add($"[{logPrefix}-Manual] DIR:{opt},SPD:{spd}");
                        PUB.AGV.AGVMoveManual(opt, spd, arDev.Narumi.Sensor.PBSOn);
                        break;

                    case ENIGProtocol.AGVCommandHE.AutoMove:
                        Resultclear();

                        // 버퍼 작업 중일 경우 자동 이동 거부
                        if (PUB._virtualAGV.Turn != AGVTurn.None)
                        {
                            SetRunStepError(ENIGProtocol.AGVErrorCode.BUFFER_NOT_COMPLETE, $"[{logPrefix}-AutoMove] 버퍼 작업 중에는 이동할 수 없습니다.");
                            return;
                        }

                        PUB.sm.SetNewRunStep(ERunStep.REMOTECONTROL);
                        var MotDirection = data[1];     //0=back, 1=forward
                        var MagDirection = data[2];     //0=straight, 1=left, 2=right
                        var AutSpeed = data[3];         //0=slow, 1=normal, 2=fast
                        var Lidar = data[4];            //0=off, 1=on

                        var bunkidata = new arDev.Narumi.BunkiData();

                        //speed;
                        if (AutSpeed == 1) bunkidata.Speed = arDev.Narumi.eMoveSpd.Mid;
                        else if (AutSpeed == 2) bunkidata.Speed = arDev.Narumi.eMoveSpd.High;
                        else bunkidata.Speed = arDev.Narumi.eMoveSpd.Low;

                        //motor direction
                        if (MotDirection == 0) bunkidata.Direction = arDev.Narumi.eMoveDir.Backward;
                        else bunkidata.Direction = arDev.Narumi.eMoveDir.Forward;

                        if (MagDirection == 2) bunkidata.Bunki = arDev.Narumi.eBunki.Right;
                        else if (MagDirection == 1) bunkidata.Bunki = arDev.Narumi.eBunki.Left;
                        else bunkidata.Bunki = arDev.Narumi.eBunki.Strate;

                        if (Lidar == 0) bunkidata.PBSSensor = 0;
                        else bunkidata.PBSSensor = 2;

                        PUB.log.Add($"[{logPrefix}-AutoMove] DIR:{bunkidata.Direction}-{bunkidata.Bunki},SPD:{bunkidata.Speed}");

                        if (PUB.AGV.AGVMoveSet(bunkidata) != eNarumiCommandResult.Success)
                            PUB.log.AddE($"AGV속도설정실패로 인해 자동전환이 실패되었습니다");
                        else
                        {
                            PUB.log.Add($"XBE 에서 자동모드로 전환합니다");
                            PUB.AGV.AGVMoveRun((MotDirection == 0 ? arDev.Narumi.eRunOpt.Backward : arDev.Narumi.eRunOpt.Forward));
                        }
                        break;

                    case ENIGProtocol.AGVCommandHE.MarkStop: //Set MarkStop
                        //마크센서에서 멈추게 한다
                        PUB.log.Add($"[{logPrefix}-MarkStop]");
                        PUB.AGV.AGVMoveStop("Xbee", arDev.Narumi.eStopOpt.MarkStop);
                        break;

                    case ENIGProtocol.AGVCommandHE.LiftControl: //Lift Control
                        var LiftCommand = ( arDev.Narumi.LiftCommand)data[1]; 
                        PUB.log.Add($"[{logPrefix}-LiftControl] {LiftCommand}");
                        PUB.AGV.LiftControl(LiftCommand);  //리프트제어
                        break;

                    case ENIGProtocol.AGVCommandHE.ChargeControl:   //충전을 제어한다
                        var chargeAction = data[1] == 1; //0= off, 1=on 

                        //충전시퀀스가 진행되지 않았다면 진행한다
                        if(chargeAction==true)
                        {
                            PUB.sm.SetNewRunStep(StateMachine.ERunStep.GOCHARGE);
                            PUB.log.AddI($"충전을 시작합니다");
                        }
                        else
                        {
                            PUB.sm.SetNewRunStep(ERunStep.CHARGEOFF);
                            PUB.log.AddI($"충전을 해제 합니다");
                        }
                        break;

                    default:
                        SetRunStepError(ENIGProtocol.AGVErrorCode.UnknownCommand, $"Unknown Command : {cmd} Sender:{e.ReceivedPacket.ID}, Target:{data[0]}");
                        break;
                }
            }
        }

        private void XBE_MessageReceived(object sender, Device.Xbee.MessageArgs e)
        {
            if (e.IsError) PUB.log.AddE(e.Message);
            else PUB.log.Add(e.Message);
        }

    }
}

