using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.Rendering;
using Unity.Collections;
using ROS2;

/*
 * This class is the same as CameraJpegPub only the rendertexture is already defined and assigned to the Camera 
 * rendertexture MUST be R8G8B8_UNORM
 */
[RequireComponent(typeof(Camera))]
public class CameraJpegPubExistingTexture : MonoBehaviour
{
    private Queue<AsyncGPUReadbackRequest> requests = new Queue<AsyncGPUReadbackRequest>();

    public float timeBetweenRequests = 0.2f;
    public string TopicName = "rt/jaja/image_raw/compressed";
    public string frame_id = null;
    [Range(0, 100)]
    public int JpgQuality = 50;

    private Camera MyCamera;
    //private CompressedImagePublisher ImagePub;
    private Texture2D tex;
    private RenderTexture m_RenderTexture;
    private float timer = 0.0f;
    //private CompressedImage_ myimage = new CompressedImage_ { format_ = "jpeg" };
    private float[] depth_result;

    private ROS2UnityComponent ros2Unity;
    private ROS2Node ros2Node;
    private IPublisher<std_msgs.msg.Float32> chatter_pub;

    void Start()
    {
        //ImagePub = new CompressedImagePublisher(topicname);
        ros2Unity = GetComponent<ROS2UnityComponent>();
        if (ros2Unity.Ok())
        {
            if (ros2Node == null)
            {
                ros2Node = ros2Unity.CreateNode("ROS2LFRcamera");
                chatter_pub = ros2Node.CreatePublisher<std_msgs.msg.Float32>(TopicName);
            }
        }

        MyCamera = GetComponent<Camera>();
        MyCamera.rect = new Rect(0f, 0f, 1f, 1f);
        // create RenderTexture and assign it to the main camera
        m_RenderTexture = MyCamera.targetTexture; // get it instead of create it
        MyCamera.pixelRect = new Rect(0, 0, m_RenderTexture.width, m_RenderTexture.height);
        MyCamera.Render();

        tex = new Texture2D(MyCamera.pixelWidth, MyCamera.pixelHeight, TextureFormat.BGRA32, false);
        Debug.Log("In start, camsize width:" + MyCamera.pixelWidth.ToString() + " height:" + MyCamera.pixelHeight.ToString());
    }

    void Update()
    {
        // Handle Request Queue
        while (requests.Count > 0)
        {
            // Get the first Request in the Queue
            AsyncGPUReadbackRequest request = requests.Peek();
            if (request.hasError)
            {
                // Error!
                Debug.LogWarning("AsyncGPUReadbackRequest Error! :(");
                requests.Dequeue(); // Remove from Queue
            }
            else if (request.done)
            {
                ReadbackData(request);
                requests.Dequeue(); // Remove from Queue
            }
            else
            {
                // Request is still processing.
                break;
            }
            // Note : We have to Dequeue items or break,
            // or we'll be caught in an infinite loop!
        }

        // Handle Request Timer
        timer += Time.deltaTime;
        if (timer > timeBetweenRequests)
        {
            timer = timer - timeBetweenRequests;
            AsyncGPUReadbackRequest rq = AsyncGPUReadback.Request(m_RenderTexture);
            requests.Enqueue(rq);
        }
    }

    void ReadbackData(AsyncGPUReadbackRequest request)
    {
        int len = request.GetData<Color32>().ToArray().Length;

        tex.SetPixels32(request.GetData<Color32>().ToArray());
        tex.Apply();

        // TODO: JPG file format!!!
        std_msgs.msg.Float32 msg = new std_msgs.msg.Float32();
        msg.Header = 9;
        msg.Data = 99.0F;
        chatter_pub.Publish(msg);

        //myimage.data_ = tex.EncodeToJPG(JpgQuality);
        //myimage.header_.stamp_ = ros_helper.RosHelper.GetUnixTimestamp();
        //myimage.header_.frame_id_ = frame_id;
        //ImagePub.Publish(myimage); // dit duurt waarschijnlijk zo lang
    }
}
