using System;
using System.Runtime.InteropServices;
using ROS2;
using Unity.Collections;
using Unity.Collections.LowLevel.Unsafe;
using UnityEngine;

namespace ZC
{
    [RequireComponent(typeof(ROS2UnityComponent))]
    public class PointsPublisher : MonoBehaviour
    {
        public static PointsPublisher pointPublisher;
        private ROS2UnityComponent ros2Unity;
        private ROS2Node ros2Node;
        private IPublisher<sensor_msgs.msg.PointCloud2> chatter_pub;
        private NativeArray<ZPointInfo> _pointInfos;

        private void Awake()
        {
            pointPublisher = this;
        }

        void Start()
        {
            ros2Unity = GetComponent<ROS2UnityComponent>();
        }

        void Update()
        {
            if (ros2Unity.Ok())
            {
                if (ros2Node == null)
                {
                    ros2Node = ros2Unity.CreateNode("ROS2UnityTalkerPointCloudNode");
                    chatter_pub = ros2Node.CreatePublisher<sensor_msgs.msg.PointCloud2>("chatter_PointCloud");
                }
            }
        }

        public void PushData(NativeArray<ZPointInfo> pointInfos, int hitCount, Vector3 position, Quaternion rotation, Bounds bounds)
        {
            this._pointInfos = pointInfos;
            unsafe
            {
                Debug.Log(111);
                sensor_msgs.msg.PointCloud2 msg = new sensor_msgs.msg.PointCloud2();
                msg.Point_step = 16;
                msg.Data = new byte[this._pointInfos.Length * msg.Point_step + sizeof(MessageHeader)];
                msg.Width = 1;
                msg.Row_step = 16;
                msg.Height = (uint)(this._pointInfos.Length + sizeof(MessageHeader)/msg.Point_step);
                var arrayPtr = UnsafeUtility.PinGCArrayAndGetDataAddress(msg.Data, out var handle);
                try
                {
                    MessageHeader messageHeader = new MessageHeader() { hitCount = hitCount, position = position, bounds = bounds, rotation = rotation };
                    void* ptr = &messageHeader;
                    UnsafeUtility.MemCpy(arrayPtr, ptr, sizeof(MessageHeader));
                    UnsafeUtility.MemCpy((void*)((nint)arrayPtr + sizeof(MessageHeader)), NativeArrayUnsafeUtility.GetUnsafeBufferPointerWithoutChecks(this._pointInfos), 150000 * 16);
                }
                finally
                {
                    UnsafeUtility.ReleaseGCObject(handle);
                }

                this.chatter_pub.Publish(msg);
            }
        }
    }

    [StructLayout(LayoutKind.Explicit)]
    public struct MessageHeader
    {
        [FieldOffset(0)] public int hitCount;
        [FieldOffset(4)] public Vector3 position;
        [FieldOffset(16)] public Bounds bounds;
        [FieldOffset(48)] public Quaternion rotation;
    }
}