using System;
using System.Collections.Concurrent;
using System.Runtime.InteropServices;
using ROS2;
using sensor_msgs.msg;
using std_msgs.msg;
using Unity.Collections;
using Unity.Collections.LowLevel.Unsafe;
using Unity.Mathematics;
using UnityEngine;

namespace ZC
{
    [RequireComponent(typeof(ROS2UnityComponent))]
    public class PointsReceiver : MonoBehaviour
    {
        private ROS2UnityComponent _ros2UnityComponent;
        private ROS2Node _ros2Node;
        private Subscription<sensor_msgs.msg.PointCloud2> _chatterPub;
        private PointsDrawer _pointsDrawer;
        [SerializeField] private Mesh _mesh;
        [SerializeField] private Material _mat;
        [SerializeField] private Camera _camera;
        private readonly int rayCount = 150000;

        private NativeArray<ZPointInfo> _pointInfos;

        private MessageHeader? messageHeader;
        private PointCloud2 _data;
        private byte[] _cache;

        private void Start()
        {
            this._ros2UnityComponent = this.GetComponent<ROS2UnityComponent>();
            _pointsDrawer = new PointsDrawer();
            _pointsDrawer.Setup(_mesh, rayCount, 1000, _mat);
            _cache = new byte[this.rayCount * UnsafeUtility.SizeOf<ZPointInfo>() + UnsafeUtility.SizeOf<MessageHeader>()];
            this._pointInfos = new NativeArray<ZPointInfo>(rayCount, Allocator.Persistent);
        }

        private void OnDestroy()
        {
            _pointsDrawer.OnDispose();
            _pointInfos.Dispose();
        }

        void Update()
        {
            if (_ros2Node == null && _ros2UnityComponent.Ok())
            {
                _ros2Node = _ros2UnityComponent.CreateNode("ROS2UnityListenerPointCloudNode");
                _chatterPub = _ros2Node.CreateSubscription<sensor_msgs.msg.PointCloud2>("chatter_PointCloud", OnReceiveData);
            }

            if (this._data != null)
                Render(this._data);
        }

        private void Render(PointCloud2 obj1)
        {
            unsafe
            {
                var bytes = obj1.Data;
                if(bytes.Length==0)
                    return;
                bytes.AsSpan().CopyTo(_cache);
                
                MessageHeader messageHeader;
                var arrayPtr = UnsafeUtility.PinGCArrayAndGetDataAddress(_cache, out var handle);
                try
                {
                    UnsafeUtility.MemCpy(&messageHeader, arrayPtr, sizeof(MessageHeader));
                    UnsafeUtility.MemCpy(NativeArrayUnsafeUtility.GetUnsafeBufferPointerWithoutChecks(this._pointInfos), (void*)((nint)arrayPtr + sizeof(MessageHeader)), 150000 * 16);
                }
                finally
                {
                    UnsafeUtility.ReleaseGCObject(handle);
                }

                this._pointsDrawer.Render(_pointInfos, messageHeader.hitCount, new Bounds(default, Vector3.one * 1000), messageHeader.rotation, messageHeader.position);
            }
            
        }
        private void OnReceiveData(sensor_msgs.msg.PointCloud2 data)
        {
            unsafe
            {
                this._data = data;
            }
        }
    }
}