RoveCommTCPWrapper.h

#pragma once
 
#include "CoreMinimal.h"
#include <shared_mutex>
#include <functional>
#include "RoveCommPacketWrapper.h"
#include "UObject/NoExportTypes.h"
#include "Async/Async.h"
#include "RoveCommTCPWrapper.generated.h"
 
// Forward declaration of your RoveCommTCP class from the RoveComm library
namespace rovecomm {
  class RoveCommTCP;
}
 
// Create events for when a packet is received.
DECLARE_DYNAMIC_MULTICAST_DELEGATE(FDriveDataReceivedTCP);
DECLARE_DYNAMIC_MULTICAST_DELEGATE(FLEDPanelDataReceivedTCP);
 
/**
 * @brief Blueprintable wrapper class for RoveCommTCP to allow usage in Unreal Engine Blueprints.
 */
UCLASS(Blueprintable, BlueprintType)
class ROVESOSIMULATOR_API URoveCommTCPWrapper : public UObject
{
  GENERATED_BODY()
 
public:
  // Constructor
  URoveCommTCPWrapper();
 
  // Destructor
  virtual ~URoveCommTCPWrapper();
 
  // Initialize the RoveComm TCP connection on a specified port
  UFUNCTION(BlueprintCallable, Category = "RoveComm TCP")
  bool InitTCPSocket(const FString& IPAddress, int32 Port);
 
  // Send a TCP packet
  UFUNCTION(BlueprintCallable, Category = "RoveComm TCP")
  int SendTCPPacket(URoveCommPacketWrapper* Packet, const FString& IPAddress, int32 Port);
 
  // Close the TCP socket
  UFUNCTION(BlueprintCallable, Category = "RoveComm TCP")
  void CloseTCPSocket();
 
  // Getter methods with shared lock
  UFUNCTION(BlueprintCallable, Category = "RoveComm TCP")
  TArray<float> GetDrivePowersCopyTCP();
  UFUNCTION(BlueprintCallable, Category = "RoveComm TCP")
  TArray<uint8> GetLEDPanelRGBColorsCopyTCP();
 
  // Getter method for RoveComm FPS.
  UFUNCTION(BlueprintCallable, Category = "RoveComm TCP")
  int32 GetRoveCommTCPFPS();
 
  // Delegates.
  UPROPERTY(BlueprintAssignable, Category = "RoveComm TCP")
  FDriveDataReceivedTCP OnDriveDataReceived;
  UPROPERTY(BlueprintAssignable, Category = "RoveComm TCP")
  FLEDPanelDataReceivedTCP OnLEDPanelDataReceived;
 
protected:
  // Instance of the RoveCommTCP class
  rovecomm::RoveCommTCP* RoveCommTCPInstance;
 
private:
    // Declare member variables for drive powers and LED panel.
    TArray<float> DrivePowers;
    TArray<uint8> LEDPanelRGBColors;
 
    // Mutexes for the member variables. These are needed because the RoveComm callbacks run on a separate thread.
    std::shared_mutex DrivePowersMutex;
    std::shared_mutex LEDPanelRGBColorsMutex;
 
    /////////////////////////////////////////
    // RoveComm Callbacks.
    /////////////////////////////////////////
 
    /******************************************************************************
     * @brief Callback function that is called whenever RoveComm receives new Accuracy data.
     *
     *
     * @author clayjay3 (claytonraycowen@gmail.com)
     * @date 2024-03-03
     ******************************************************************************/
    const std::function<void(const rovecomm::RoveCommPacket<float>&)> ProcessDriveDataTCP =
      [this](const rovecomm::RoveCommPacket<float>& stPacket)
    {
      // Assuming stPacket.vData contains two elements: left and right drive power
      if (stPacket.vData.size() >= 2)
      {
        // Lock the mutex.
        std::unique_lock<std::shared_mutex> Lock(DrivePowersMutex);
        // Clear the drive powers.
        DrivePowers.Empty();
        // Add the new drive powers.
        DrivePowers.Add(stPacket.vData[0]);
        DrivePowers.Add(stPacket.vData[1]);
 
        // Unlock the mutex.
        Lock.unlock();
      }
 
      // Trigger the OnDriveDataReceived event on the game thread.
      AsyncTask(ENamedThreads::GameThread, [this]()
      {
        OnDriveDataReceived.Broadcast();
      });
    };
 
    /******************************************************************************
     * @brief Callback function that is called whenever RoveComm receives new RGB data.
     *
     *
     * @author clayjay3 (claytonraycowen@gmail.com)
     * @date 2024-03-03
     ******************************************************************************/
    const std::function<void(const rovecomm::RoveCommPacket<uint8_t>&)> ProcessRGBDataTCP =
      [this](const rovecomm::RoveCommPacket<uint8_t>& stPacket)
    {
      // Lock the mutex.
      std::unique_lock<std::shared_mutex> Lock(LEDPanelRGBColorsMutex);
 
      // Clear the LED panel colors.
      LEDPanelRGBColors.Empty();
      // Store the first three indexes from vData in the LEDPanelRGBColors.
      LEDPanelRGBColors.Add(stPacket.vData[0]);
      LEDPanelRGBColors.Add(stPacket.vData[1]);
      LEDPanelRGBColors.Add(stPacket.vData[2]);
 
      // Unlock the mutex.
      Lock.unlock();
 
      // Trigger the OnLEDPanelDataReceived event on the game thread.
      AsyncTask(ENamedThreads::GameThread, [this]()
      {
        OnLEDPanelDataReceived.Broadcast();
      });
    };
 
    /******************************************************************************
     * @brief Callback function that is called whenever RoveComm receives new StateDisplay data.
     *
     *
     * @author clayjay3 (claytonraycowen@gmail.com)
     * @date 2024-03-03
     ******************************************************************************/
    const std::function<void(const rovecomm::RoveCommPacket<uint8_t>&)> ProcessStateDisplayDataTCP =
      [this](const rovecomm::RoveCommPacket<uint8_t>& stPacket)
    {
      // Determine display state.
      switch (stPacket.vData[0])
      {
        case 0:
        {
          // Teleop Blue
          std::unique_lock<std::shared_mutex> Lock(LEDPanelRGBColorsMutex);
          LEDPanelRGBColors.Empty();
          LEDPanelRGBColors.Add(0);
          LEDPanelRGBColors.Add(0);
          LEDPanelRGBColors.Add(255); // RGB for Blue
          // Unlock the mutex.
          Lock.unlock();
          break;
        }
        case 1:
        {
          // Autonomy Red
          std::unique_lock<std::shared_mutex> Lock(LEDPanelRGBColorsMutex);
          LEDPanelRGBColors.Empty();
          LEDPanelRGBColors.Add(255);
          LEDPanelRGBColors.Add(0);
          LEDPanelRGBColors.Add(0); // RGB for Red
          // Unlock the mutex.
          Lock.unlock();
          break;
        }
        case 2:
        {
          // Goal Green
          std::unique_lock<std::shared_mutex> Lock(LEDPanelRGBColorsMutex);
          LEDPanelRGBColors.Empty();
          LEDPanelRGBColors.Add(0);
          LEDPanelRGBColors.Add(255);
          LEDPanelRGBColors.Add(0); // RGB for Green
          // Unlock the mutex.
          Lock.unlock();
          break;
        }
        default:
          break;
      }
 
      // Trigger the OnLEDPanelDataReceived event on the game thread.
      AsyncTask(ENamedThreads::GameThread, [this]()
      {
        OnLEDPanelDataReceived.Broadcast();
      });
    };
};