Plugin/Source/SenseGlove/Private/SenseGlove/Components/SGWristTrackerComponent.cpp

/**
 * @file
 *
 * @author  Mamadou Babaei <mamadou@senseglove.com>
 *
 * @section LICENSE
 *
 * (The MIT License)
 *
 * Copyright (c) 2020 - 2023 SenseGlove
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 * @section DESCRIPTION
 *
 *
 */


#include "SenseGlove/Components/SGWristTrackerComponent.h"

#include "Animation/Skeleton.h"
#include "Components/SkeletalMeshComponent.h"
#include "Engine/SkeletalMesh.h"
#include "UObject/ConstructorHelpers.h"

#include "SGConnect/SGConnect.h"
#include "SGCore/SGDeviceList.h"
#include "SGCore/SGHapticGlove.h"
#include "SGCore/SGHapticGloveHandProfiles.h"
#include "SGDebug/SGDebugGizmo.h"
#include "SGLog/SGLog.h"
#include "SGSettings/SGSettings.h"

struct USGWristTrackerComponent::FImpl
{
    /************************
     * Static methods
     ************************/

    /************************
     * Owner object
     ************************/

    USGWristTrackerComponent* Owner;

    /************************
     * Constructor / Destructor
     ************************/

    explicit FImpl(USGWristTrackerComponent* InOwner);
    ~FImpl();

    /************************
     * Default copy constructor & copy assignment operator
     ************************/

    FImpl(const FImpl& Rhs) = default;
    FImpl& operator=(const FImpl& Rhs) = default;

    /************************
     * Methods
     ************************/

    bool IsEditor() const;

    bool InitializeBackend() const;
    bool UninitializeBackend() const;
    bool CheckGlove() const;

    void UpdateWristTrackingData() const;
    void DrawDebugGizmo();
};

USGWristTrackerComponent::USGWristTrackerComponent(const FObjectInitializer& ObjectInitializer)
    : Super(ObjectInitializer),
      Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
    PrimaryComponentTick.bTickEvenWhenPaused = false;
    PrimaryComponentTick.bCanEverTick = true;
    PrimaryComponentTick.bStartWithTickEnabled = true;
    bTickInEditor = true;
    bNeverNeedsRenderUpdate = true;
    bAllowConcurrentTick = true;

    SetCastShadow(false);
    SetCastHiddenShadow(false);
    bCastDynamicShadow = false;
    SetOnlyOwnerSee(false);
    SetReceivesDecals(false);
    SetCanEverAffectNavigation(false);

    SetRight(true);

    bOverridePluginWristTrackingSettings = false;
    bDrawWristTrackerDebugGizmo = false;
    TrackingHardware = ESGPositionalTrackingHardware::None;
    TrackingHardwareLocationOffset = FVector::ZeroVector;
    TrackingHardwareLocationOffset = FVector::ZeroVector;

    bOverridePluginWristTrackingDebugSettings = false;
    bDrawWristTrackerDebugGizmo = false;
    WristTrackerDebugGizmoSettings = FSGDebugGizmoSettings(
        4.0f, 1.0f,
        FColor(1.0f, 0.0f, 0.0f, 1.0f),
        FColor(0.0f, 1.0f, 0.0f, 1.0f),
        FColor(0.0f, 0.0f, 1.0f, 1.0f),
        false, 1.1f, 0
        );
    bDrawWristTrackerDebugGizmo = false;

    bBackendInitialized = false;
    Glove = nullptr;
    WristLocation = FVector::ZeroVector;
    WristRotation = FRotator::ZeroRotator;
}

void USGWristTrackerComponent::SetRight(const bool bRight)
{
    SetTrackingSource(bRight ? EControllerHand::Right : EControllerHand::Left);
}

ESGPositionalTrackingHardware USGWristTrackerComponent::GetTrackingHardware() const
{
    if (OverridesPluginWristTrackingSettings())
    {
        return TrackingHardware;
    }

    return USGSettings::GetInstance()->GetTrackingHardware();
}

void USGWristTrackerComponent::SetTrackingHardware(const ESGPositionalTrackingHardware InTrackingHardware)
{
    ensureAlwaysMsgf(
        OverridesPluginWristTrackingSettings(),
        TEXT( "Setting TrackingHardware is only permitted when bOverridePluginWristTrackingSettings is enabled!"));

    TrackingHardware = InTrackingHardware;
}

const FVector& USGWristTrackerComponent::GetTrackingHardwareLocationOffset() const
{
    ensureAlwaysMsgf(
        GetTrackingHardware() == ESGPositionalTrackingHardware::Custom,
        TEXT( "Getting TrackingHardwareLocationOffset is only permitted when TrackingHardware is set to Custom!"));

    if (OverridesPluginWristTrackingSettings())
    {
        return TrackingHardwareLocationOffset;
    }

    return IsRight()
        ? USGSettings::GetInstance()->GetTrackingHardwareLocationOffsetRightHand()
        : USGSettings::GetInstance()->GetTrackingHardwareLocationOffsetLeftHand();
}

void USGWristTrackerComponent::SetTrackingHardwareLocationOffset(const FVector& InTrackingHardwareLocationOffset)
{
    ensureAlwaysMsgf(
        OverridesPluginWristTrackingSettings(),
        TEXT(
            "Setting TrackingHardwareLocationOffset is only permitted when bOverridePluginWristTrackingSettings is enabled!"
        ));

    ensureAlwaysMsgf(
        GetTrackingHardware() == ESGPositionalTrackingHardware::Custom,
        TEXT( "Setting TrackingHardwareLocationOffset is only permitted when TrackingHardware is set to Custom!"));

    TrackingHardwareLocationOffset = InTrackingHardwareLocationOffset;
}

const FRotator& USGWristTrackerComponent::GetTrackingHardwareRotationOffset() const
{
    ensureAlwaysMsgf(
        GetTrackingHardware() == ESGPositionalTrackingHardware::Custom,
        TEXT( "Getting TrackingHardwareRotationOffset is only permitted when TrackingHardware is set to Custom!"));

    if (OverridesPluginWristTrackingSettings())
    {
        return TrackingHardwareRotationOffset;
    }

    return IsRight()
        ? USGSettings::GetInstance()->GetTrackingHardwareRotationOffsetRightHand()
        : USGSettings::GetInstance()->GetTrackingHardwareRotationOffsetLeftHand();
}

void USGWristTrackerComponent::SetTrackingHardwareRotationOffset(const FRotator& InTrackingHardwareRotationOffset)
{
    ensureAlwaysMsgf(
        OverridesPluginWristTrackingSettings(),
        TEXT(
            "Setting TrackingHardwareRotationOffset is only permitted when bOverridePluginWristTrackingSettings is enabled!"
        ));

    ensureAlwaysMsgf(
        GetTrackingHardware() == ESGPositionalTrackingHardware::Custom,
        TEXT("Setting TrackingHardwareRotationOffsetRightHand is only permitted when TrackingHardware is set to Custom!"
        ));

    TrackingHardwareRotationOffset = InTrackingHardwareRotationOffset;
}

bool USGWristTrackerComponent::GetDrawWristTrackerDebugGizmo() const
{
    if (OverridesPluginWristTrackingDebugSettings())
    {
        return !!bDrawWristTrackerDebugGizmo;
    }

    return USGSettings::GetInstance()->GetDrawWristTrackerDebugGizmo();
}

void USGWristTrackerComponent::SetDrawWristTrackerDebugGizmo(const bool bInDrawWristTrackerDebugGizmo)
{
    ensureAlwaysMsgf(
        OverridesPluginWristTrackingDebugSettings(),
        TEXT( "Setting bDrawWristTrackerDebugGizmo is only permitted when bOverridePluginWristTrackingDebugSettings is"
            " enabled!"));

    bDrawWristTrackerDebugGizmo = bInDrawWristTrackerDebugGizmo;
}

const FSGDebugGizmoSettings& USGWristTrackerComponent::GetWristTrackerDebugGizmoSettings() const
{
    ensureAlwaysMsgf(
        GetDrawWristTrackerDebugGizmo(),
        TEXT("Getting WristTrackerDebugGizmoLength is only permitted when bDrawWristTrackerDebugGizmo is enabled!"));

    if (OverridesPluginWristTrackingDebugSettings())
    {
        return WristTrackerDebugGizmoSettings;
    }

    return USGSettings::GetInstance()->GetWristTrackerDebugGizmoSettings();
}

void USGWristTrackerComponent::SetWristTrackerDebugGizmoSettings(
    const FSGDebugGizmoSettings& InWristTrackerDebugGizmoSettings)
{
    ensureAlwaysMsgf(
        OverridesPluginWristTrackingDebugSettings(),
        TEXT("Setting WristTrackerDebugGizmoLength is only permitted when bOverridePluginWristTrackingDebugSettings is"
            " enabled!"));

    ensureAlwaysMsgf(
        GetDrawWristTrackerDebugGizmo(),
        TEXT("Setting WristTrackerDebugGizmoLength is only permitted when bDrawWristTrackerDebugGizmo is enabled!"));

    WristTrackerDebugGizmoSettings = InWristTrackerDebugGizmoSettings;
}

#if WITH_EDITOR
void USGWristTrackerComponent::PostEditChangeProperty(FPropertyChangedEvent& PropertyChangedEvent)
{
    Super::PostEditChangeProperty(PropertyChangedEvent);

    const FName PropertyName{
        PropertyChangedEvent.Property
        ? PropertyChangedEvent.Property->GetFName()
        : NAME_None
    };

    (void) PropertyName;
}
#endif  /* WITH_EDITOR */

void USGWristTrackerComponent::InitializeComponent()
{
    Super::InitializeComponent();

    if (Pimpl->InitializeBackend())
    {
        (void) Pimpl->CheckGlove();
    }
}

void USGWristTrackerComponent::UninitializeComponent()
{
    Super::UninitializeComponent();

    (void) Pimpl->UninitializeBackend();
}

void USGWristTrackerComponent::BeginPlay()
{
    Super::BeginPlay();

    if (Pimpl->InitializeBackend())
    {
        (void) Pimpl->CheckGlove();
    }
}

void USGWristTrackerComponent::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);

    (void) Pimpl->UninitializeBackend();
}

void USGWristTrackerComponent::TickComponent(
    const float DeltaTime, const ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction)
{
#if WITH_EDITOR
    if (Pimpl->IsEditor())
    {
        EditorTick(DeltaTime, TickType, ThisTickFunction);
        return;
    }
#endif  /* WITH_EDITOR */

    Super::TickComponent(DeltaTime, TickType, ThisTickFunction);

    if (GetDrawWristTrackerDebugGizmo())
    {
        Pimpl->DrawDebugGizmo();
    }

    (void) Pimpl->CheckGlove();
}


void USGWristTrackerComponent::EditorTick(
    const float DeltaTime, const ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction)
{
    (void) Pimpl->CheckGlove();
}

USGWristTrackerComponent::FImpl::FImpl(USGWristTrackerComponent* InOwner)
    : Owner(InOwner)
{
}

USGWristTrackerComponent::FImpl::~FImpl() = default;

bool USGWristTrackerComponent::FImpl::IsEditor() const
{
    const UWorld* World{Owner->GetWorld()};
    if (World && (World->WorldType == EWorldType::Editor || World->WorldType == EWorldType::EditorPreview))
    {
        return true;
    }

    return false;
}

bool USGWristTrackerComponent::FImpl::InitializeBackend() const
{
    if (!Owner->bBackendInitialized)
    {
#if PLATFORM_ANDROID
        const int32_t Result = FSGConnect::Init();
        switch (Result)
        {
        case -3:
            SGLOG_ERROR("An Unexpected error occurred. Please try again.");
            return false;
        case -2:
            SGLOG_ERROR("Device Scanning is already running within a different program.");
            return false;
        case -1:
            SGLOG_ERROR("Device Scanning already being initialized (function called twice in short succession).");
            return false;
        case 0:
            // Do not pollute the logs!
            //SGLOG_ERROR("Device Scanning is already running within this program.");
            // We must return here in case there are two virtual hand components are present in the scene.
            return true;
        case 1:
            SGLOG("Successfully started up DeviceScanning from the current program.");
            break;
        default:
            ensureAlwaysMsgf(false, TEXT("Unhandled Init return status code!"));
            return false;
        }
#endif  /* PLATFORM_ANDROID */

        // Ensure that the device list is empty before doing anything.
        FSGDeviceList::Dispose();
        FSGDeviceList::Reinitialize();

        // Load the latest hand profiles.
        FSGHapticGloveHandProfiles::TryLoadingFromDisk();

        Owner->bBackendInitialized = true;

        return true;
    }

    return false;
}

bool USGWristTrackerComponent::FImpl::UninitializeBackend() const
{
    // NOTE
    // In Editor builds we should be avoiding dispose, since stopping the game in PIE mode, causes the virtual-hand
    // to disappear in the editor as the plugin won't be able to retrieve the glove status anymore.
#if ! WITH_EDITOR
    if (Owner->bBackendInitialized)
    {
        FSGDeviceList::Dispose();

#if PLATFORM_ANDROID
        const int32_t Result = FSGConnect::Dispose();
        switch (Result)
        {
        case -3:
            SGLOG_ERROR("An Unexpected error occurred. Please try again.");
            return false;
        case -2:
            SGLOG_ERROR("Not allowed to dispose of Device Scanning because this is not the program which started it.");
            return false;
        case -1:
            SGLOG_ERROR("Device Scanning is currently being disposed off. This takes a second or two. (function called twice in short succession).");
            return false;
        case 0:
            SGLOG_ERROR("There is no deviceScanner running from any program, so disposing is skipped.");
            return false;
        case 1:
            SGLOG("Successfully disposed of DeviceScanner resources.");
            break;
        default:
            ensureAlwaysMsgf(false, TEXT("Unhandled Dispose return status code!"));
            return false;
        }
#endif  /* PLATFORM_ANDROID */

        Owner->bBackendInitialized = false;

        return true;
    }
#endif  /* ! WITH_EDITOR */

    return false;
}

bool USGWristTrackerComponent::FImpl::CheckGlove() const
{
    if (!Owner->bBackendInitialized)
    {
        const bool bSucceeded = InitializeBackend();
        if (!bSucceeded)
        {
            return false;
        }
    }

    const bool bGloveWasPresent = IsValid(Owner->Glove);
    const bool bFoundGlove = USGHapticGlove::GetGlove(Owner, Owner->IsRight(), Owner->Glove);

    if (!bFoundGlove && bGloveWasPresent)
    {
        SGLOG_ERROR(FString::Printf(TEXT("Could not find a %s glove!"),
            (Owner->IsRight() ? TEXT("right-handed") : TEXT("left-handed"))));
    }

    if (bFoundGlove)
    {
        UpdateWristTrackingData();
    }

    return bFoundGlove;
}

void USGWristTrackerComponent::FImpl::UpdateWristTrackingData() const
{
    ensureAlwaysMsgf(Owner->Glove, TEXT("Invalid Glove!"));

    FVector TrackerLocation{Owner->GetComponentLocation()};
    FRotator TrackerRotation{Owner->GetComponentRotation()};

    ESGPositionalTrackingHardware TrackerHardware = Owner->GetTrackingHardware();
    if (TrackerHardware == ESGPositionalTrackingHardware::None)
    {
        TrackerHardware = ESGPositionalTrackingHardware::Custom;
    }
    else if (TrackerHardware == ESGPositionalTrackingHardware::Custom)
    {
        TrackerLocation += Owner->GetTrackingHardwareLocationOffset();
        TrackerRotation += Owner->GetTrackingHardwareRotationOffset();
    }

    FVector NewWristLocation;
    FRotator NewWristRotation;
    Owner->Glove->GetWristLocation(TrackerLocation, TrackerRotation, TrackerHardware,
                                   NewWristLocation, NewWristRotation);

    const bool bLocationUpdated = !Owner->WristLocation.Equals(NewWristLocation, SMALL_NUMBER);
    const bool bRotationUpdated = !Owner->WristRotation.Equals(NewWristRotation, SMALL_NUMBER);

    if (bLocationUpdated)
    {
        Owner->WristLocation = MoveTemp(NewWristLocation);
    }

    if (bRotationUpdated)
    {
        Owner->WristRotation = MoveTemp(NewWristRotation);
    }

    if (bLocationUpdated && Owner->WristLocationChangedEvent.IsBound())
    {
        Owner->WristLocationChangedEvent.Broadcast(Owner->WristLocation);
    }

    if (bRotationUpdated && Owner->WristRotationChangedEvent.IsBound())
    {
        Owner->WristRotationChangedEvent.Broadcast(Owner->WristRotation);
    }

    if ((bLocationUpdated && bRotationUpdated) && Owner->WristLocationAndRotationChangedEvent.IsBound())
    {
        Owner->WristLocationAndRotationChangedEvent.Broadcast(Owner->WristLocation, Owner->WristRotation);
    }
}

void USGWristTrackerComponent::FImpl::DrawDebugGizmo()
{
    const UWorld* World{Owner->GetWorld()};
    const FVector& Location{Owner->GetComponentLocation()};
    const FRotator& Rotation{Owner->GetComponentRotation()};
    const FSGDebugGizmoSettings& DebugGizmoSettings{Owner->GetWristTrackerDebugGizmoSettings()};

    FSGDebugGizmo::Draw(World, Location, Rotation, DebugGizmoSettings);
}

void USGWristTrackerComponent::FImplDeleter::operator()(const FImpl* P) const
{
    delete P;
}