SensorsManagerComponent.cpp

Go to the documentation of this file.

// Copyright (c) 2023 FrostBit Software Lab at the Lapland University of Applied Sciences
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
 
#include "Agrarsense/Sensor/SensorsManagerComponent.h"
#include "Agrarsense/Sensor/SensorFactory.h"
#include "Agrarsense/Sensor/SensorTypes.h"
#include "Agrarsense/Sensor/Sensor.h"
#include "Agrarsense/Sensor/Camera/Camera.h"
#include "Agrarsense/Sensor/Camera/DepthCamera.h"
#include "Agrarsense/Sensor/Camera/DVSCamera.h"
#include "Agrarsense/Sensor/Camera/SemanticSegmentationCamera.h"
#include "Agrarsense/Sensor/Camera/InstanceSegmentationCamera.h"
#include "Agrarsense/Sensor/Camera/ThermalCamera.h"
#include "Agrarsense/Sensor/Lidar/Lidar.h"
#include "Agrarsense/Sensor/Radar/Radar.h"
 
const FString USensorsManagerComponent::ComponentHierarchySeparator = TEXT("_|_");
 
USensorsManagerComponent::USensorsManagerComponent()
{
    PrimaryComponentTick.bCanEverTick = false;
}
 
USensorsManagerComponent::~USensorsManagerComponent()
{
}
 
ALidar* USensorsManagerComponent::SpawnLidar(const FTransform& transform, bool isTransformRelativeToComponent, const FString sensorIdentifier, const FString sensorName, FLidarParameters lidarParameters, bool simulateSensor, ASensorModel*& createdModel, USceneComponent* attachToComponent, FName attachToBone)
{
    // Check if attachToComponent is not set but attachToBone is set
    if (!attachToComponent && !attachToBone.IsNone())
    {
        attachToComponent = GetOwner()->GetRootComponent();
    }
 
    FTransform sensorTransform = isTransformRelativeToComponent ? GetSensorAttachmentTransform(transform, attachToComponent, attachToBone) : transform;
 
    ALidar* lidar = USensorFactory::SpawnLidarSensor(sensorTransform, sensorIdentifier, sensorName, lidarParameters, simulateSensor, GetOwner());
 
    if (!lidar)
    {
        return nullptr;
    }
 
    createdModel = lidar->GetSensorModel();
 
    ASensor* CastedSensor = Cast<ASensor>(lidar);
 
    AddSensorDestroyListener(CastedSensor);
 
    AttachSensor(CastedSensor, attachToComponent, attachToBone);
 
    Lidars.Emplace(lidar);
    Sensors.Emplace(CastedSensor);
 
    OnLidarSpawned.Broadcast(lidar);
    OnSensorSpawned.Broadcast(CastedSensor);
 
    return lidar;
}
 
ACamera* USensorsManagerComponent::SpawnCamera(const FTransform& transform, bool isTransformRelativeToComponent, const FString sensorIdentifier, const FString sensorName, FCameraBaseParameters cameraParameters, bool simulateSensor, ASensorModel*& createdModel, USceneComponent* attachToComponent, FName attachToBone)
{
    // Check if attachToComponent is not set but attachToBone is set
    if (!attachToComponent && !attachToBone.IsNone())
    {
        attachToComponent = GetOwner()->GetRootComponent();
    }
 
    FTransform sensorTransform = isTransformRelativeToComponent ? GetSensorAttachmentTransform(transform, attachToComponent, attachToBone) : transform;
 
    ACamera* camera = USensorFactory::SpawnCamera(sensorTransform, sensorIdentifier, sensorName, cameraParameters, simulateSensor, GetOwner());
 
    if (!camera)
    {
        return nullptr;
    }
 
    createdModel = camera->GetSensorModel();
 
    ASensor* CastedSensor = Cast<ASensor>(camera);
 
    AttachSensor(CastedSensor, attachToComponent, attachToBone);
 
    AddSensorDestroyListener(CastedSensor);
 
    Cameras.Emplace(camera);
    Sensors.Emplace(CastedSensor);
 
    camera->OnCameraWindowClosed.AddDynamic(this, &USensorsManagerComponent::OnCameraWindowClosed);
 
    OnCameraSpawned.Broadcast(camera);
    OnSensorSpawned.Broadcast(CastedSensor);
 
    return camera;
}
 
AThermalCamera* USensorsManagerComponent::SpawnThermalCamera(const FTransform& transform, bool isTransformRelativeToComponent, const FString sensorIdentifier, const FString sensorName, FThermalCameraParameters thermalCameraParameters, bool simulateSensor, ASensorModel*& createdModel, USceneComponent* attachToComponent, FName attachToBone)
{
 
    // Check if attachToComponent is not set but attachToBone is set
    if (!attachToComponent && !attachToBone.IsNone())
    {
        attachToComponent = GetOwner()->GetRootComponent();
    }
 
    FTransform sensorTransform = isTransformRelativeToComponent ? GetSensorAttachmentTransform(transform, attachToComponent, attachToBone) : transform;
 
    AThermalCamera* thermalCamera = USensorFactory::SpawnThermalCamera(sensorTransform, sensorIdentifier, sensorName, thermalCameraParameters, simulateSensor, GetOwner());
 
    if (!thermalCamera)
    {
        return nullptr;
    }
 
    createdModel = thermalCamera->GetSensorModel();
 
    ASensor* CastedSensor = Cast<ASensor>(thermalCamera);
 
    AttachSensor(CastedSensor, attachToComponent, attachToBone);
 
    AddSensorDestroyListener(CastedSensor);
 
    ThermalCameras.Emplace(thermalCamera);
    Sensors.Emplace(CastedSensor);
 
    thermalCamera->OnCameraWindowClosed.AddDynamic(this, &USensorsManagerComponent::OnCameraWindowClosed);
 
    OnThermalCameraSpawned.Broadcast(thermalCamera);
    OnSensorSpawned.Broadcast(CastedSensor);
 
    return thermalCamera;
}
 
ADVSCamera* USensorsManagerComponent::SpawnDVSCamera(const FTransform& transform, bool isTransformRelativeToComponent, const FString sensorIdentifier, const FString sensorName, FDVSCameraParameters DVSCameraParameters, bool simulateSensor, ASensorModel*& createdModel, USceneComponent* attachToComponent, FName attachToBone)
{
    // Check if attachToComponent is not set but attachToBone is set
    if (!attachToComponent && !attachToBone.IsNone())
    {
        attachToComponent = GetOwner()->GetRootComponent();
    }
 
    FTransform sensorTransform = isTransformRelativeToComponent ? GetSensorAttachmentTransform(transform, attachToComponent, attachToBone) : transform;
 
    ADVSCamera* DVSCamera = USensorFactory::SpawnDVSCamera(sensorTransform, sensorIdentifier, sensorName, DVSCameraParameters, simulateSensor, GetOwner());
 
    if (!DVSCamera)
    {
        return nullptr;
    }
 
    createdModel = DVSCamera->GetSensorModel();
 
    ASensor* CastedSensor = Cast<ASensor>(DVSCamera);
 
    AttachSensor(CastedSensor, attachToComponent, attachToBone);
 
    AddSensorDestroyListener(CastedSensor);
 
    DVSCameras.Emplace(DVSCamera);
    Sensors.Emplace(CastedSensor);
 
    DVSCamera->OnCameraWindowClosed.AddDynamic(this, &USensorsManagerComponent::OnCameraWindowClosed);
 
    OnDVSCameraSpawned.Broadcast(DVSCamera);
    OnSensorSpawned.Broadcast(CastedSensor);
 
    return DVSCamera;
}
 
ASemanticSegmentationCamera* USensorsManagerComponent::SpawnSegmentationCamera(const FTransform& transform, bool isTransformRelativeToComponent, const FString sensorIdentifier, const FString sensorName, FCameraBaseParameters cameraParameters, bool simulateSensor, ASensorModel*& createdModel, USceneComponent* attachToComponent, FName attachToBone)
{
    // Check if attachToComponent is not set but attachToBone is set
    if (!attachToComponent && !attachToBone.IsNone())
    {
        attachToComponent = GetOwner()->GetRootComponent();
    }
 
    FTransform sensorTransform = isTransformRelativeToComponent ? GetSensorAttachmentTransform(transform, attachToComponent, attachToBone) : transform;
 
    ASemanticSegmentationCamera* segmentationCamera = USensorFactory::SpawnSegmentationCamera(sensorTransform, sensorIdentifier, sensorName, cameraParameters, simulateSensor, GetOwner());
 
    if (!segmentationCamera)
    {
        return nullptr;
    }
 
    createdModel = segmentationCamera->GetSensorModel();
 
    ASensor* CastedSensor = Cast<ASensor>(segmentationCamera);
 
    AttachSensor(CastedSensor, attachToComponent, attachToBone);
 
    AddSensorDestroyListener(CastedSensor);
 
    SegmentationCameras.Emplace(segmentationCamera);
    Sensors.Emplace(CastedSensor);
 
    segmentationCamera->OnCameraWindowClosed.AddDynamic(this, &USensorsManagerComponent::OnCameraWindowClosed);
 
    OnSegmentationCameraSpawned.Broadcast(segmentationCamera);
    OnSensorSpawned.Broadcast(CastedSensor);
 
    return segmentationCamera;
}
 
AInstanceSegmentationCamera* USensorsManagerComponent::SpawnInstanceSegmentationCamera(const FTransform& transform, bool isTransformRelativeToComponent, const FString sensorIdentifier, const FString sensorName, FCameraBaseParameters CameraParameters, bool simulateSensor, ASensorModel*& createdModel, USceneComponent* attachToComponent, FName attachToBone)
{
    // Check if attachToComponent is not set but attachToBone is set
    if (!attachToComponent && !attachToBone.IsNone())
    {
        attachToComponent = GetOwner()->GetRootComponent();
    }
 
    FTransform sensorTransform = isTransformRelativeToComponent ? GetSensorAttachmentTransform(transform, attachToComponent, attachToBone) : transform;
 
    AInstanceSegmentationCamera* InstanceSegmentationCamera = USensorFactory::SpawnInstanceSegmentationCamera(sensorTransform, sensorIdentifier, sensorName, CameraParameters, simulateSensor, GetOwner());
 
    if (!InstanceSegmentationCamera)
    {
        return nullptr;
    }
 
    createdModel = InstanceSegmentationCamera->GetSensorModel();
 
    ASensor* CastedSensor = Cast<ASensor>(InstanceSegmentationCamera);
 
    AttachSensor(CastedSensor, attachToComponent, attachToBone);
 
    AddSensorDestroyListener(CastedSensor);
 
    InstanceSegmentationCameras.Emplace(InstanceSegmentationCamera);
    Sensors.Emplace(CastedSensor);
 
    InstanceSegmentationCamera->OnCameraWindowClosed.AddDynamic(this, &USensorsManagerComponent::OnCameraWindowClosed);
 
    OnInstanceSegmentationCameraSpawned.Broadcast(InstanceSegmentationCamera);
    OnSensorSpawned.Broadcast(CastedSensor);
 
    return InstanceSegmentationCamera;
}
 
ADepthCamera* USensorsManagerComponent::SpawnDepthCamera(const FTransform& transform, bool isTransformRelativeToComponent, const FString sensorIdentifier, const FString sensorName, FDepthCameraParameters DepthCameraParameters, bool simulateSensor, ASensorModel*& createdModel, USceneComponent* attachToComponent, FName attachToBone)
{
    // Check if attachToComponent is not set but attachToBone is set
    if (!attachToComponent && !attachToBone.IsNone())
    {
        attachToComponent = GetOwner()->GetRootComponent();
    }
 
    FTransform sensorTransform = isTransformRelativeToComponent ? GetSensorAttachmentTransform(transform, attachToComponent, attachToBone) : transform;
 
    ADepthCamera* depthCamera = USensorFactory::SpawnDepthCamera(sensorTransform, sensorIdentifier, sensorName, DepthCameraParameters, simulateSensor, GetOwner());
 
    if (!depthCamera)
    {
        return nullptr;
    }
 
    createdModel = depthCamera->GetSensorModel();
 
    ASensor* CastedSensor = Cast<ASensor>(depthCamera);
 
    AttachSensor(CastedSensor, attachToComponent, attachToBone);
 
    AddSensorDestroyListener(CastedSensor);
 
    DepthCameras.Emplace(depthCamera);
    Sensors.Emplace(CastedSensor);
 
    depthCamera->OnCameraWindowClosed.AddDynamic(this, &USensorsManagerComponent::OnCameraWindowClosed);
 
    OnDepthCameraSpawned.Broadcast(depthCamera);
    OnSensorSpawned.Broadcast(CastedSensor);
 
    return depthCamera;
}
 
ARadar* USensorsManagerComponent::SpawnRadar(const FTransform& transform, bool isTransformRelativeToComponent, const FString sensorIdentifier, const FString sensorName, FRadarParameters radarParameters, bool simulateSensor, ASensorModel*& createdModel, USceneComponent* attachToComponent, FName attachToBone)
{
    // Check if attachToComponent is not set but attachToBone is set
    if (!attachToComponent && !attachToBone.IsNone())
    {
        attachToComponent = GetOwner()->GetRootComponent();
    }
 
    FTransform sensorTransform = isTransformRelativeToComponent ? GetSensorAttachmentTransform(transform, attachToComponent, attachToBone) : transform;
 
    ARadar* radar = USensorFactory::SpawnRadar(sensorTransform, sensorIdentifier, sensorName, radarParameters, simulateSensor, GetOwner());
 
    if (!radar)
    {
        return nullptr;
    }
 
    createdModel = radar->GetSensorModel();
 
    ASensor* CastedSensor = Cast<ASensor>(radar);
 
    AttachSensor(CastedSensor, attachToComponent, attachToBone);
 
    AddSensorDestroyListener(CastedSensor);
 
    Radars.Emplace(radar);
    Sensors.Emplace(CastedSensor);
 
    OnRadarSpawned.Broadcast(radar);
    OnSensorSpawned.Broadcast(CastedSensor);
 
    return radar;
}
 
ASensor* USensorsManagerComponent::SpawnSensor(const FTransform& transform, bool isTransformRelativeToComponent, const FString sensorIdentifier, const FString sensorName, ESensorTypes sensorType, bool simulateSensor, ASensorModel*& createdModel, USceneComponent* attachToComponent, FName attachToBone)
{
    ASensor* sensor = nullptr;
 
    switch (sensorType)
    {
        case ESensorTypes::Lidar:
            sensor = Cast<ASensor>(SpawnLidar(transform, isTransformRelativeToComponent, sensorIdentifier, sensorName, FLidarParameters(), simulateSensor, createdModel, attachToComponent, attachToBone));
            break;
        case ESensorTypes::RGBCamera:
            sensor = Cast<ASensor>(SpawnCamera(transform, isTransformRelativeToComponent, sensorIdentifier, sensorName, FCameraBaseParameters(), simulateSensor, createdModel, attachToComponent, attachToBone));
            break;
        case ESensorTypes::ThermalCamera:
            sensor = Cast<ASensor>(SpawnThermalCamera(transform, isTransformRelativeToComponent, sensorIdentifier, sensorName, FThermalCameraParameters(), simulateSensor, createdModel, attachToComponent, attachToBone));
            break;
        case ESensorTypes::DVSCamera:
            sensor = Cast<ASensor>(SpawnDVSCamera(transform, isTransformRelativeToComponent, sensorIdentifier, sensorName, FDVSCameraParameters(), simulateSensor, createdModel, attachToComponent, attachToBone));
            break;
        case ESensorTypes::SemanticSegmentationCamera:
            sensor = Cast<ASensor>(SpawnSegmentationCamera(transform, isTransformRelativeToComponent, sensorIdentifier, sensorName, FCameraBaseParameters(), simulateSensor, createdModel, attachToComponent, attachToBone));
            break;
        case ESensorTypes::DepthCamera:
            sensor = Cast<ASensor>(SpawnDepthCamera(transform, isTransformRelativeToComponent, sensorIdentifier, sensorName, FDepthCameraParameters(), simulateSensor, createdModel, attachToComponent, attachToBone));
            break;
        case ESensorTypes::Radar:
            sensor = Cast<ASensor>(SpawnRadar(transform, isTransformRelativeToComponent, sensorIdentifier, sensorName, FRadarParameters(), simulateSensor, createdModel, attachToComponent, attachToBone));
            break;
        default:
            break;
    }
 
    return sensor;
}
 
void USensorsManagerComponent::DestroyLidar(ALidar* lidarToDestroy)
{
    if (lidarToDestroy)
    {
        FString sensorIdentifier = lidarToDestroy->GetSensorIdentifier();
        Lidars.Remove(lidarToDestroy);
 
        ASensor* CastedSensor = Cast<ASensor>(lidarToDestroy);
 
        RemoveSensorDestroyListener(CastedSensor);
 
        Sensors.Remove(CastedSensor);
        DestroySensorActor(CastedSensor);
 
        OnLidarDestroyed.Broadcast(sensorIdentifier);
        OnSensorDestroyed.Broadcast(ESensorTypes::Lidar, sensorIdentifier);
    }
}
 
void USensorsManagerComponent::DestroyCamera(ACamera* cameraToDestroy)
{
    if (cameraToDestroy)
    {
        FString sensorIdentifier = cameraToDestroy->GetSensorIdentifier();
 
        cameraToDestroy->OnCameraWindowClosed.RemoveDynamic(this, &USensorsManagerComponent::OnCameraWindowClosed);
        Cameras.Remove(cameraToDestroy);
 
        ASensor* CastedSensor = Cast<ASensor>(cameraToDestroy);
 
        RemoveSensorDestroyListener(CastedSensor);
 
        Sensors.Remove(CastedSensor);
        DestroySensorActor(CastedSensor);
 
        OnCameraDestroyed.Broadcast(sensorIdentifier);
        OnSensorDestroyed.Broadcast(ESensorTypes::RGBCamera, sensorIdentifier);
    }
}
 
void USensorsManagerComponent::DestroyThermalCamera(AThermalCamera* thermalCameraToDestroy)
{
    if (thermalCameraToDestroy)
    {
        FString sensorIdentifier = thermalCameraToDestroy->GetSensorIdentifier();
 
        thermalCameraToDestroy->OnCameraWindowClosed.RemoveDynamic(this, &USensorsManagerComponent::OnCameraWindowClosed);
        ThermalCameras.Remove(thermalCameraToDestroy);
 
        ASensor* CastedSensor = Cast<ASensor>(thermalCameraToDestroy);
 
        RemoveSensorDestroyListener(CastedSensor);
 
        Sensors.Remove(CastedSensor);
        DestroySensorActor(CastedSensor);
 
        OnThermalCameraDestroyed.Broadcast(sensorIdentifier);
        OnSensorDestroyed.Broadcast(ESensorTypes::ThermalCamera, sensorIdentifier);
    }
}
 
void USensorsManagerComponent::DestroyDVSCamera(ADVSCamera* DVSCameraToDestroy)
{
    if (DVSCameraToDestroy)
    {
        FString sensorIdentifier = DVSCameraToDestroy->GetSensorIdentifier();
 
        DVSCameraToDestroy->OnCameraWindowClosed.RemoveDynamic(this, &USensorsManagerComponent::OnCameraWindowClosed);
        DVSCameras.Remove(DVSCameraToDestroy);
 
        ASensor* CastedSensor = Cast<ASensor>(DVSCameraToDestroy);
 
        RemoveSensorDestroyListener(CastedSensor);
 
        Sensors.Remove(CastedSensor);
        DestroySensorActor(CastedSensor);
 
        OnDVSCameraDestroyed.Broadcast(sensorIdentifier);
        OnSensorDestroyed.Broadcast(ESensorTypes::DVSCamera, sensorIdentifier);
    }
}
 
void USensorsManagerComponent::DestroySegmentationCamera(ASemanticSegmentationCamera* segmentationCameraToDestroy)
{
    if (segmentationCameraToDestroy)
    {
        FString sensorIdentifier = segmentationCameraToDestroy->GetSensorIdentifier();
 
        segmentationCameraToDestroy->OnCameraWindowClosed.RemoveDynamic(this, &USensorsManagerComponent::OnCameraWindowClosed);
        SegmentationCameras.Remove(segmentationCameraToDestroy);
 
        ASensor* CastedSensor = Cast<ASensor>(segmentationCameraToDestroy);
 
        RemoveSensorDestroyListener(CastedSensor);
 
        Sensors.Remove(CastedSensor);
        DestroySensorActor(CastedSensor);
 
        OnSegmentationCameraDestroyed.Broadcast(sensorIdentifier);
        OnSensorDestroyed.Broadcast(ESensorTypes::SemanticSegmentationCamera, sensorIdentifier);
    }
}
 
void USensorsManagerComponent::DestroyDepthCamera(ADepthCamera* depthCameraToDestroy)
{
    if (depthCameraToDestroy)
    {
        FString sensorIdentifier = depthCameraToDestroy->GetSensorIdentifier();
 
        depthCameraToDestroy->OnCameraWindowClosed.RemoveDynamic(this, &USensorsManagerComponent::OnCameraWindowClosed);
        DepthCameras.Remove(depthCameraToDestroy);
 
        ASensor* CastedSensor = Cast<ASensor>(depthCameraToDestroy);
 
        RemoveSensorDestroyListener(CastedSensor);
 
        Sensors.Remove(CastedSensor);
        DestroySensorActor(CastedSensor);
 
        OnDepthCameraDestroyed.Broadcast(sensorIdentifier);
        OnSensorDestroyed.Broadcast(ESensorTypes::DepthCamera, sensorIdentifier);
    }
}
 
void USensorsManagerComponent::DestroyRadar(ARadar* radarToDestroy)
{
    if (radarToDestroy)
    {
        FString sensorIdentifier = radarToDestroy->GetSensorIdentifier();
 
        Radars.Remove(radarToDestroy);
 
        ASensor* CastedSensor = Cast<ASensor>(radarToDestroy);
 
        RemoveSensorDestroyListener(CastedSensor);
 
        Sensors.Remove(CastedSensor);
        DestroySensorActor(CastedSensor);
 
        OnRadarDestroyed.Broadcast(sensorIdentifier);
        OnSensorDestroyed.Broadcast(ESensorTypes::Radar, sensorIdentifier);
    }
}
 
void USensorsManagerComponent::DestroySensor(ASensor* sensorToDestroy)
{
    if (sensorToDestroy)
    {
        ESensorTypes sensorType = sensorToDestroy->GetSensorType();
 
        switch (sensorType)
        {
            case ESensorTypes::Lidar:
                DestroyLidar(Cast<ALidar>(sensorToDestroy));
                break;
            case ESensorTypes::RGBCamera:
                DestroyCamera(Cast<ACamera>(sensorToDestroy));
                break;
            case ESensorTypes::ThermalCamera:
                DestroyThermalCamera(Cast<AThermalCamera>(sensorToDestroy));
                break;
            case ESensorTypes::DVSCamera:
                DestroyDVSCamera(Cast<ADVSCamera>(sensorToDestroy));
                break;
            case ESensorTypes::SemanticSegmentationCamera:
                DestroySegmentationCamera(Cast<ASemanticSegmentationCamera>(sensorToDestroy));
                break;
            case ESensorTypes::DepthCamera:
                DestroyDepthCamera(Cast<ADepthCamera>(sensorToDestroy));
                break;
            case ESensorTypes::Radar:
                DestroyRadar(Cast<ARadar>(sensorToDestroy));
                break;
            default:
                break;
        }
    }
}
 
void USensorsManagerComponent::DestroyAllSensors()
{
    TArray<ASensor*> SensorsTempArray = Sensors;
 
    for (const auto& sensor : SensorsTempArray)
    {
        DestroySensor(sensor);
    }
}
 
FTransform USensorsManagerComponent::SensorRelativeTransformToWorld(const FTransform& relativeTransform)
{
    AActor* owner = GetOwner();
    return relativeTransform * owner->GetActorTransform();
}
 
FTransform USensorsManagerComponent::SensorRelativeTransformToComponentWorld(const FTransform& relativeTransform, const FTransform& componentTransformWorld)
{
    return relativeTransform * componentTransformWorld;
}
 
FTransform USensorsManagerComponent::GetSensorAttachmentTransform(const FTransform& sensorRelativeTransform, USceneComponent* attachToComponent, const FName& attachToBone, bool revertScaleToVectorOne)
{
    FTransform worldTransform;
 
    // Calculate worldTransform based on attachToComponent availability
    if (attachToComponent)
    {
        worldTransform = SensorRelativeTransformToComponentWorld(sensorRelativeTransform, attachToComponent->GetSocketTransform(attachToBone));
    }
    else
    {
        worldTransform = SensorRelativeTransformToWorld(sensorRelativeTransform);
    }
 
    if (revertScaleToVectorOne)
    {
        worldTransform.SetScale3D(FVector(1, 1, 1));
    }
 
    return worldTransform;
}
 
USceneComponent* USensorsManagerComponent::GetComponentByHierarchyName(FString hierarchyName)
{
 
    // Array to hold the split parts
    TArray<FString> componentNames;
 
    AActor* owner = GetOwner();
 
    // Always fallback to owner root component
    USceneComponent* matchingComponent = owner->GetRootComponent();
 
    // Split the hierarchy
    hierarchyName.ParseIntoArray(componentNames, *ComponentHierarchySeparator, true);
 
    if (componentNames.Num() > 0)
    {
        // Search for the initial component using the first element in the split array
        FString initialComponentName = componentNames[0];
        USceneComponent* initialComponent = nullptr;
        bool skipFirstComponentName = true;
 
        TArray<USceneComponent*> ownerComponents;
        owner->GetComponents<USceneComponent>(ownerComponents);
 
        // Iterate over owner's components to find the initial component
        for (USceneComponent* childComponent : ownerComponents)
        {
            if (childComponent && childComponent->GetName() == initialComponentName)
            {
                initialComponent = childComponent;
                matchingComponent = initialComponent;
                break;
            }
        }
 
        if (!initialComponent)
        {
            // UE_LOG(LogTemp, Warning, TEXT("Initial component '%s' not found. Fallback to using owner root component"), *initialComponentName);
 
            initialComponent = owner->GetRootComponent();
 
            skipFirstComponentName = false;
        }
 
        USceneComponent* currentComponent = initialComponent;
 
        // Check if there is more components to search through.
        if (currentComponent && componentNames.Num() > (skipFirstComponentName ? 1 : 0))
        {
            // Flag to check if the component with the given name is found
            bool bComponentFound = false;
 
            // Iterate over each part of the hierarchy
            for (int32 i = (skipFirstComponentName ? 1 : 0); i < componentNames.Num(); ++i)
            {
                FString componentName = componentNames[i];
 
                bComponentFound = false;
 
                // Loop through the child components of the current component
                for (USceneComponent* childComponent : currentComponent->GetAttachChildren())
                {
                    FString childComponentName = childComponent->GetName();
 
                    // Check if the child component's name matches the current part of the split string
                    // UE_LOG(LogTemp, Warning, TEXT("Compare between %s and %s."), *childComponentName, *componentName);
 
                    if (childComponent && childComponent->GetName() == componentName)
                    {
                        // Match found, update the current component
                        currentComponent = childComponent;
                        bComponentFound = true;
                        // break out of this inner component search loop
                        break;
                    }
                }
 
                // If the component was not found, can't continue the search anymore
                if (!bComponentFound)
                {
                    // Component not found, break out of the loop or handle as needed
                    UE_LOG(LogTemp, Warning, TEXT("Component '%s' not found. Breaking out of the loop"), *componentName);
                    // break out of the whole search loop
                    break;
                }
            }
 
            // At this point, CurrentComponent should point to the component matching the hierarchy
            if (bComponentFound)
            {
                FString foundComponentName = currentComponent->GetName();
                matchingComponent = currentComponent;
            }
            else
            {
                // Component not found, handle as needed
                UE_LOG(LogTemp, Warning, TEXT("Component not found"));
            }
        }
    }
 
    return matchingComponent;
}
 
void USensorsManagerComponent::AttachSensorToOwner(ASensor* sensor)
{
    // Owner is probably a vehicle
    AActor* owner = GetOwner();
 
    // Parent sensor to the owner of this component (vehicle)
    AttachSensorToActor(sensor, owner);
}
 
void USensorsManagerComponent::AttachSensorToActor(ASensor* sensor, AActor* attachToActor)
{
    if (sensor && attachToActor)
    {
        sensor->AttachToActor(attachToActor, FAttachmentTransformRules::KeepWorldTransform);
        sensor->AttachedToComponent = FString();
        sensor->AttachedToBone = NAME_None;
    }
}
 
void USensorsManagerComponent::AttachSensorToBone(ASensor* sensor, USceneComponent* attachToComponent, const FName& boneName)
{
    if (!sensor || !attachToComponent)
    {
        return;
    }
 
    if (IsValid(attachToComponent))
    {
        sensor->AttachToComponent(attachToComponent, FAttachmentTransformRules::KeepWorldTransform, boneName);
 
        USceneComponent* currentParentComponent = attachToComponent->GetAttachParent();
 
        // FString fullComponentName = FString();
        FString fullComponentName = attachToComponent->GetName();
 
        // If this does not have a parent, this is the highest attach parent which we will not include
        if (currentParentComponent)
        {
            fullComponentName = attachToComponent->GetName();
 
            // Skip the highest attach parent which should be the owner itself. Having only the currentParentComponent-check would include the owner as well.
            while (currentParentComponent && currentParentComponent->GetAttachParent())
            {
                FString ParentComponentName = currentParentComponent->GetName();
                // Prepend the parent's name into the full component name like ParentComponent_|_ChildComponent.
                // This could have been a TArray<FString> because that can be also saved easily
                fullComponentName = FString::Format(TEXT("{0}{1}{2}"), { *ParentComponentName, *ComponentHierarchySeparator, *fullComponentName });
 
                // Move to the next parent
                currentParentComponent = currentParentComponent->GetAttachParent();
            }
        }
 
        sensor->AttachedToComponent = fullComponentName;
        sensor->AttachedToBone = boneName;
 
        //UE_LOG(LogTemp, Warning, TEXT("Full component name: %s"), *fullComponentName);
    }
}
 
void USensorsManagerComponent::AttachSensor(ASensor* sensor, USceneComponent* attachToComponent, const FName& boneName)
{
    // If attachToComponent is valid OR if the boneName is set.
    if (IsValid(attachToComponent) || !boneName.IsNone())
    {
        AttachSensorToBone(sensor, attachToComponent, boneName);
    }
    else
    {
        AttachSensorToOwner(sensor);
    }
}
 
void USensorsManagerComponent::DestroySensorActor(AActor* sensorActor)
{
    if (!sensorActor)
    {
        return;
    }
 
    TArray<AActor*> attachedActors;
    sensorActor->GetAttachedActors(attachedActors);
 
    for (AActor* CActor : attachedActors)
    {
        if (CActor)
        {
            CActor->Destroy();
        }
    }
 
    sensorActor->Destroy();
}
 
void USensorsManagerComponent::LoadPreset(FVehicleSensorsPreset preset, bool simulateSensors)
{
    DestroyAllSensors();
 
    for (const auto& LidarData : preset.LidarDatas)
    {
        FSensorPreset sensorData = LidarData.SensorData;
        FTransform sensorTransform(sensorData.Rotation, sensorData.Location);
 
        USceneComponent* attachToComponent = GetComponentByHierarchyName(sensorData.AttachedToComponent);
        FName attachToBone = sensorData.AttachedToBone;
 
        ASensorModel* createdModel = nullptr;
 
        SpawnLidar(sensorTransform, true, sensorData.SensorIdentifier, sensorData.SensorName, LidarData.LidarParameters, simulateSensors, createdModel, attachToComponent, attachToBone);
    }
 
    for (const auto& RadarData : preset.RadarDatas)
    {
        FSensorPreset sensorData = RadarData.SensorData;
        FTransform sensorTransform(sensorData.Rotation, sensorData.Location);
 
        USceneComponent* attachToComponent = GetComponentByHierarchyName(sensorData.AttachedToComponent);
        FName attachToBone = sensorData.AttachedToBone;
 
        ASensorModel* createdModel = nullptr;
 
        SpawnRadar(sensorTransform, true, sensorData.SensorIdentifier, sensorData.SensorName, RadarData.RadarParameters, simulateSensors, createdModel, attachToComponent, attachToBone);
    }
 
    for (const auto& CameraData : preset.CameraDatas)
    {
        FSensorPreset sensorData = CameraData.SensorData;
 
        FTransform sensorTransform(sensorData.Rotation, sensorData.Location);
 
        USceneComponent* attachToComponent = GetComponentByHierarchyName(sensorData.AttachedToComponent);
        FName attachToBone = sensorData.AttachedToBone;
 
        ASensorModel* createdModel = nullptr;
 
        SpawnCamera(sensorTransform, true, sensorData.SensorIdentifier, sensorData.SensorName, CameraData.CameraParameters, simulateSensors, createdModel, attachToComponent, attachToBone);
    }
 
    for (const auto& CameraData : preset.ThermalCameraDatas)
    {
        FSensorPreset sensorData = CameraData.SensorData;
 
        FTransform sensorTransform(sensorData.Rotation, sensorData.Location);
 
        USceneComponent* attachToComponent = GetComponentByHierarchyName(sensorData.AttachedToComponent);
        FName attachToBone = sensorData.AttachedToBone;
 
        ASensorModel* createdModel = nullptr;
 
        SpawnThermalCamera(sensorTransform, true, sensorData.SensorIdentifier, sensorData.SensorName, CameraData.CameraParameters, simulateSensors, createdModel, attachToComponent, attachToBone);
    }
 
    for (const auto& CameraData : preset.DVSCameraDatas)
    {
        FSensorPreset sensorData = CameraData.SensorData;
 
        FTransform sensorTransform(sensorData.Rotation, sensorData.Location);
 
        USceneComponent* attachToComponent = GetComponentByHierarchyName(sensorData.AttachedToComponent);
        FName attachToBone = sensorData.AttachedToBone;
 
        ASensorModel* createdModel = nullptr;
 
        SpawnDVSCamera(sensorTransform, true, sensorData.SensorIdentifier, sensorData.SensorName, CameraData.DVSCameraParameters, simulateSensors, createdModel, attachToComponent, attachToBone);
    }
 
    for (const auto& CameraData : preset.SegmentationCameraDatas)
    {
        FSensorPreset sensorData = CameraData.SensorData;
 
        FTransform sensorTransform(sensorData.Rotation, sensorData.Location);
 
        USceneComponent* attachToComponent = GetComponentByHierarchyName(sensorData.AttachedToComponent);
        FName attachToBone = sensorData.AttachedToBone;
 
        ASensorModel* createdModel = nullptr;
 
        SpawnSegmentationCamera(sensorTransform, true, sensorData.SensorIdentifier, sensorData.SensorName, CameraData.CameraParameters, simulateSensors, createdModel, attachToComponent, attachToBone);
    }
 
    for (const auto& CameraData : preset.DepthCameraDatas)
    {
        FSensorPreset sensorData = CameraData.SensorData;
 
        FTransform sensorTransform(sensorData.Rotation, sensorData.Location);
 
        USceneComponent* attachToComponent = GetComponentByHierarchyName(sensorData.AttachedToComponent);
        FName attachToBone = sensorData.AttachedToBone;
 
        ASensorModel* createdModel = nullptr;
 
        SpawnDepthCamera(sensorTransform, true, sensorData.SensorIdentifier, sensorData.SensorName, CameraData.CameraParameters, simulateSensors, createdModel, attachToComponent, attachToBone);
    }
}
 
FVehicleSensorsPreset USensorsManagerComponent::GetSensorsAsPreset()
{
    FVehicleSensorsPreset preset;
 
    for (const auto& Lidar : Lidars)
    {
        FSensorPreset sensorPreset = GetSensorPreset(Cast<ASensor>(Lidar));
        FLidarPreset lidarPreset;
        lidarPreset.SensorData = sensorPreset;
        lidarPreset.LidarParameters = Lidar->GetLidarParameters();
 
        preset.LidarDatas.Add(lidarPreset);
    }
 
    for (const auto& Radar : Radars)
    {
        FSensorPreset sensorPreset = GetSensorPreset(Cast<ASensor>(Radar));
        FRadarPreset radarPreset;
        radarPreset.SensorData = sensorPreset;
        radarPreset.RadarParameters = Radar->GetRadarParameters();
 
        preset.RadarDatas.Add(radarPreset);
    }
 
    for (const auto& Camera : Cameras)
    {
        FSensorPreset sensorPreset = GetSensorPreset(Cast<ASensor>(Camera));
        FCameraPreset cameraPreset;
        cameraPreset.SensorData = sensorPreset;
        cameraPreset.CameraParameters = Camera->GetCameraParameters();
        preset.CameraDatas.Add(cameraPreset);
    }
 
    for (const auto& ThermalCamera : ThermalCameras)
    {
        FSensorPreset sensorPreset = GetSensorPreset(Cast<ASensor>(ThermalCamera));
        FThermalCameraPreset cameraPreset;
        cameraPreset.SensorData = sensorPreset;
        cameraPreset.CameraParameters = ThermalCamera->GetThermalCameraParameters();
        preset.ThermalCameraDatas.Add(cameraPreset);
    }
 
    for (const auto& DVSCamera : DVSCameras)
    {
        FSensorPreset sensorPreset = GetSensorPreset(Cast<ASensor>(DVSCamera));
        FDVSCameraPreset cameraPreset;
        cameraPreset.SensorData = sensorPreset;
        cameraPreset.DVSCameraParameters = DVSCamera->GetDVSCameraParameters();
        preset.DVSCameraDatas.Add(cameraPreset);
    }
 
    for (const auto& SegmentationCamera : SegmentationCameras)
    {
        FSensorPreset sensorPreset = GetSensorPreset(Cast<ASensor>(SegmentationCamera));
        FSemanticSegmentationCameraPreset cameraPreset;
        cameraPreset.SensorData = sensorPreset;
        cameraPreset.CameraParameters = SegmentationCamera->GetCameraParameters();
        preset.SegmentationCameraDatas.Add(cameraPreset);
    }
 
    for (const auto& DepthCamera : DepthCameras)
    {
        FSensorPreset sensorPreset = GetSensorPreset(Cast<ASensor>(DepthCamera));
        FDepthCameraPreset cameraPreset;
        cameraPreset.SensorData = sensorPreset;
        cameraPreset.CameraParameters = DepthCamera->GetDepthCameraParameters();
        preset.DepthCameraDatas.Add(cameraPreset);
    }
 
    return preset;
}
 
FSensorPreset USensorsManagerComponent::GetSensorPreset(ASensor* sensor)
{
    FSensorPreset sensorPreset;
 
    FTransform relativeTransform = sensor->GetRootComponent()->GetRelativeTransform();
 
    sensorPreset.Location = relativeTransform.GetLocation();
    sensorPreset.Rotation = relativeTransform.Rotator();
    sensorPreset.SensorIdentifier = sensor->GetSensorIdentifier();
    sensorPreset.SensorName = sensor->GetSensorName();
    sensorPreset.SensorType = sensor->GetSensorType();
    sensorPreset.AttachedToComponent = sensor->AttachedToComponent;
    sensorPreset.AttachedToBone = sensor->AttachedToBone;
 
    return sensorPreset;
}
 
void USensorsManagerComponent::AddSensorDestroyListener(ASensor* sensor)
{
    if (sensor)
    {
        sensor->OnSensorDestroy.AddDynamic(this, &USensorsManagerComponent::OnSensorDestroy);
    }
}
 
void USensorsManagerComponent::RemoveSensorDestroyListener(ASensor* sensor)
{
    if (sensor)
    {
        sensor->OnSensorDestroy.RemoveDynamic(this, &USensorsManagerComponent::OnSensorDestroy);
    }
}
 
void USensorsManagerComponent::OnCameraWindowClosed(ACamera* camera)
{
    if (camera)
    {
        ASensor* CastedSensor = Cast<ASensor>(camera);
        DestroySensor(CastedSensor);
    }
}
 
void USensorsManagerComponent::OnSensorDestroy(ASensor* sensor)
{
    if (sensor)
    {
        // Calling this will actually try to remove the sensor again but I think that is OK
        DestroySensor(sensor);
    }
}