using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using AGVNavigationCore.Models;
using AGVNavigationCore.Utils;
using AR;
using arDev;
using COMM;

namespace Project
{
    public partial class fMain
    {

        private void XBE_ProtocReceived(object sender, ENIG.EEProtocol.DataEventArgs e)
        {
            //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;

                switch (cmd)
                {

                    case ENIGProtocol.AGVCommandHE.SetCurrent: //Set Current Position

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

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

                    case ENIGProtocol.AGVCommandHE.Goto:  //move to tag
                        if (data.Length > 4)
                        {
                            var currTag = System.Text.Encoding.Default.GetString(data, 1, data.Length - 1);
                            var targetNode = PUB._mapNodes.FirstOrDefault(t => t.RfidId == currTag);


                            //자동상태가아니라면 처리하지 않는다.
                            if (VAR.BOOL[eVarBool.FLAG_AUTORUN] == false)
                            {
                                PUB.log.AddE($"[{logPrefix}-Goto] 자동실행상태가 아닙니다");
                                PUB.XBE.SendError(ENIGProtocol.AGVErrorCode.ManualMode, $"{currTag}");
                            }

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

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

                            if (startNode != null)
                            {
                                //시작위치가 존재한다면 경로를 예측한다.
                                var rltGoto = CalcPath(startNode, targetNode);
                                if (rltGoto.result == null)
                                {
                                    PUB.log.AddE($"[{logPrefix}-Goto] 경로예측실패 {rltGoto.message}");
                                }
                                else
                                {
                                    //경로예측을 화면에 표시해준다.
                                    PUB._virtualAGV.SetPath(rltGoto.result);
                                    var pathWithRfid = rltGoto.result.GetSimplePath().Select(nodeId => PUB._virtualAGV.GetRfidByNodeId(PUB._mapNodes, nodeId)).ToList();
                                    PUB.log.Add($"경로예측결과:{pathWithRfid}");
                                }
                            }

                            //대상이동으로 처리한다.
                            PUB.sm.SetNewRunStep(StateMachine.ERunStep.GOTO);

                            //Move to 
                            PUB.log.Add($"[{logPrefix}-Goto] {startNode.RfidId} -> {targetNode.RfidId}");

                        }
                        else PUB.log.AddE($"[{logPrefix}-Goto] TagString Lenght Errorr:{data.Length}");
                        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]");
                        PUB.AGV.AGVErrorReset();
                        break;

                    case ENIGProtocol.AGVCommandHE.Manual: //Manual Move (Direction, speed, runtime)
                        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:
                        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 bunkidata = new arDev.Narumi.BunkiData();

                        //speed;
                        if (AutSpeed == 1) bunkidata.Speed = arDev.Narumi.eMoveSpd.Middle;
                        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;

                        PUB.log.Add($"[{logPrefix}-AutoMove] DIR:{bunkidata.Direction}-{bunkidata.Bunki},SPD:{bunkidata.Speed}");
                        PUB.AGV.AGVMoveSet(bunkidata);
                        PUB.AGV.AGVMoveRun();
                        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 = data[1];    //0=stop, 1=up, 2=down

                        arDev.Narumi.LiftCommand LCmd = arDev.Narumi.LiftCommand.STP;
                        if (LiftCommand == 1) LCmd = arDev.Narumi.LiftCommand.UP;
                        else if (LiftCommand == 2) LCmd = arDev.Narumi.LiftCommand.DN;

                        PUB.log.Add($"[{logPrefix}-LiftControl] {LCmd}");
                        PUB.AGV.LiftControl(LCmd);  //리프트제어
                        break;

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

                        //충전시퀀스가 진행되지 않았다면 진행한다
                        if (PUB.sm.RunStep == StateMachine.ERunStep.GOCHARGE && PUB.sm.RunStepNew != StateMachine.ERunStep.GOCHARGE)
                        {
                            PUB.sm.SetNewRunStep(StateMachine.ERunStep.GOCHARGE);
                            PUB.log.AddI($"충전을 시작합니다");
                        }
                        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);
        }

        AGVNavigationCore.PathFinding.Planning.AGVPathfinder _advancedPathfinder = null;
        (AGVNavigationCore.PathFinding.Core.AGVPathResult result, string message) CalcPath(MapNode startNode, MapNode targetNode)
        {
            var _mapNodes = PUB._mapNodes;
            // 시작 RFID가 없으면 AGV 현재 위치로 설정
            if (startNode == null || targetNode == null)
                return (null, "시작 RFID와 목표 RFID를 선택해주세요.");

            //경로계산기확인
            if (_advancedPathfinder == null)
                _advancedPathfinder = new AGVNavigationCore.PathFinding.Planning.AGVPathfinder(_mapNodes);

            // 현재 AGV 방향 가져오기
            var selectedAGV = PUB._virtualAGV;
            var currentDirection = selectedAGV.CurrentDirection;

            // AGV의 이전 위치에서 가장 가까운 노드 찾기
            var prevNode = selectedAGV.PrevNode;
            var prevDir = selectedAGV.PrevDirection;

            // 고급 경로 계획 사용 (노드 객체 직접 전달)
            var advancedResult = _advancedPathfinder.FindPath(startNode, targetNode, prevNode, prevDir, currentDirection);

            var _simulatorCanvas = PUB._mapCanvas;

            _simulatorCanvas.FitToNodes();
            string Message = string.Empty;
            if (advancedResult.Success)
            {
                // 도킹 검증이 없는 경우 추가 검증 수행
                if (advancedResult.DockingValidation == null || !advancedResult.DockingValidation.IsValidationRequired)
                    advancedResult.DockingValidation = DockingValidator.ValidateDockingDirection(advancedResult, _mapNodes);

                _simulatorCanvas.CurrentPath = advancedResult;
                //_pathLengthLabel.Text = $"경로 길이: {advancedResult.TotalDistance:F1}";
                //_statusLabel.Text = $"경로 계산 완료 ({advancedResult.CalculationTimeMs}ms)";

                // 🔥 VirtualAGV에도 경로 설정 (Predict()가 동작하려면 필요)
                selectedAGV.SetPath(advancedResult);

                // 도킹 검증 결과 확인 및 UI 표시
                //CheckAndDisplayDockingValidation(advancedResult);

                // 고급 경로 디버깅 정보 표시
                //UpdateAdvancedPathDebugInfo(advancedResult);

            }
            else
            {
                // 경로 실패시 디버깅 정보 초기화
                //_pathDebugLabel.Text = $"경로: 실패 - {advancedResult.ErrorMessage}";
                advancedResult = null;
                Message = $"경로를 찾을 수 없습니다:\n{advancedResult.ErrorMessage}";
            }

            return (advancedResult, Message);
        }


    }
}