using agxROS2;
using AGXUnity;
using UnityEngine;
using UnityEngine.Rendering;

public class ROS2CameraPublisher : ScriptComponent
{
  RenderTexture m_rt = null;
  PublisherSensorMsgsImage imagePublisher = null;
  byte[] m_data;

  public int Size = 250;

  // Start is called before the first frame update
  protected override bool Initialize()
  {
    m_rt = new RenderTexture( Size, Size, 16, RenderTextureFormat.ARGB32 );
    var cam = GetComponent<Camera>();
    cam.targetTexture = m_rt;

    imagePublisher = new PublisherSensorMsgsImage( "AGXUnity.Examples.Camera" );

    return true;
  }

  // Update is called once per frame
  void Update()
  {
    if ( m_rt != null )
      AsyncGPUReadback.Request( m_rt, 0, SendImage );
  }

  void SendImage( AsyncGPUReadbackRequest request )
  {
    var msg = new SensorMsgsImage();
    if ( m_data == null )
      m_data = new byte[ request.layerDataSize ];
    request.GetData<byte>().CopyTo( m_data );
    msg.data.Set( m_data );
    msg.height = (uint)Size;
    msg.width  = (uint)Size;
    imagePublisher.sendMessage( msg );
  }
}
