using agxROS2;
using AGXUnity;
using AGXUnity.Utils;
using UnityEngine;

public class ROS2CameraSubscriber : ScriptComponent
{
  SubscriberSensorMsgsImage m_cameraSubscriber;
  Material m_recievedMaterial;
  Texture2D m_recievedTexture;
  byte[] m_data;

  // Start is called before the first frame update
  protected override bool Initialize()
  {
    m_cameraSubscriber = new SubscriberSensorMsgsImage( "AGXUnity.Examples.Camera" );
    return true;
  }

  // Update is called once per frame
  void Update()
  {
    if ( m_recievedMaterial == null ) {
      m_recievedMaterial = RenderingUtils.CreateDefaultMaterial();
      GetComponent<MeshRenderer>().material = m_recievedMaterial;
    }

    var msg = new SensorMsgsImage();
    while ( m_cameraSubscriber.receiveMessage( msg ) ) {
      if ( m_recievedTexture == null || m_recievedTexture.width != msg.width ) {
        m_recievedTexture = new Texture2D( (int)msg.width, (int)msg.height );
        m_data = new byte[ msg.data.Count ];
        RenderingUtils.SetMainTexture( m_recievedMaterial, m_recievedTexture );
      }
      msg.data.Get( m_data );
      m_recievedTexture.SetPixelData( m_data, 0 );
      m_recievedTexture.Apply();
    }
  }
}
