//using CommonUsage.Protocols.VDA5050.Messages;
//using System;
//using System.Collections.Generic;
//using System.Net.Http.Headers;
//using System.Runtime.CompilerServices;
//using System.Text;
//using System.Threading;
//using System.Threading.Tasks;
//using CommonUsage.Protocols.VDA5050.Objects;
//using ClumsyCore.Utilities;
//using System.Linq;
//using ClumsyCore.Pilot;
//using System.Numerics;

//namespace CommonUsage.Protocols.VDA5050
//{
//    public abstract class VDA5050Basic
//    {
//        protected static IVDACommunicationProtocol _communicationProtocol;

//        public void Enable(IVDACommunicationProtocol protocol)
//        {
//            _communicationProtocol = protocol;
//            Task.Run(() => ManageConnection());
//            StartVisualizationLoop();
//        }
//        private void StartVisualizationLoop()
//        {
//            Console.WriteLine("Visualization in CommonUsage");
//            new Thread(async () =>
//                {
//                    while (true)
//                    {
//                        var position = GetAGVPosition();
//                        if (position != null)
//                        {
//                            var msg = new stateMessage()
//                            {
//                                serialNumber = "test-01",
//                                agvPosition = new()
//                                {
//                                    x = position.Value.X,
//                                    y = position.Value.Y,
//                                    theta = position.Value.Theta,
//                                    positionInitialized = true
//                                }

//                            };
//                            await _communicationProtocol.SendMessageAsync(msg, "vda5050/frldAGV/visualization");
//                        }

//                        Thread.Sleep(100);
//                    }
//                })
//            { Name = "VDA5050TopicVisualization" }.Start();
//        }

//        private void ManageConnection()
//        {
//            while (true)
//            {
//                var status = CheckConnectionStatus() ? "ONLINE" : "OFFLINE";
//                _communicationProtocol.PublishConnectionStatus(status);
//                Thread.Sleep(1000);
//            }
//        }

//        protected List<sequenceItem> OrganizeReceivedSequence(orderMessage order)
//        {
//            List<sequenceItem> receivedSequence = new();

//            int ii = 0, jj = 0;
//            while (true)
//            {
//                var edge = order.edges[ii];
//                var node = order.nodes[jj];
//                var takeEdge = edge.sequenceId < node.sequenceId;

//                if (takeEdge)
//                {
//                    receivedSequence.Add(edge);
//                    ii++;
//                    if (ii == order.edges.Length) break;
//                }
//                else
//                {
//                    receivedSequence.Add(node);
//                    jj++;
//                    if (jj == order.nodes.Length) break;
//                }
//            }
//            for (var i = ii; i < order.edges.Length; ++i) receivedSequence.Add(order.edges[i]);
//            for (var j = jj; j < order.nodes.Length; ++j) receivedSequence.Add(order.nodes[j]);

//            for (var i = 1; i < receivedSequence.Count; i++)
//            {
//                if (receivedSequence[i - 1].sequenceId + 1 != receivedSequence[i].sequenceId)
//                    throw new Exception("stateMessage not continuous!");
//            }

//            return receivedSequence;
//        }

//        public virtual bool CheckConnectionStatus()
//        {
//            return false;
//        }

//        protected abstract Vector3? GetAGVPosition();
//        public struct Vector3
//        {
//            public double X;
//            public double Y;
//            public double Theta;
//        }
//    }

//}