Plugin/Source/SenseGloveCore/Private/SGCore/SGSenseGlove.cpp

/**
 * @file
 *
 * @author  Mamadou Babaei <mamadou@senseglove.com>
 *
 * @section LICENSE
 *
 * (The MIT License)
 *
 * Copyright (c) 2020 - 2024 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 "SGCore/SGSenseGlove.h"

#include "SGCore/SGDeviceImplCast.h"
#include "SGCore/SGHandInterpolator.h"
#include "SGCore/SGHandPose.h"
#include "SGCore/SGSenseGloveSensorNormalizer.h"
#include "SGCoreImpl/SGDeviceImpl.h"
#include "SGCoreImpl/SGHandInterpolatorImpl.h"
#include "SGCoreImpl/SGHandPoseImpl.h"
#include "SGCoreImpl/SGHapticGloveImpl.h"
#include "SGCoreImpl/SGHapticGloveImpl.h"
#include "SGCoreImpl/SGSenseGloveSensorNormalizerImpl.h"
#include "SGCoreImpl/SGExportedFunctions.h"
#include "SGCoreImpl/SGSenseGloveImpl.h"
#include "SGInterop/SGIC_ESGDeviceType.h"
#include "SGInterop/SGIC_ESGFinger.h"
#include "SGInterop/SGIC_ESGHapticGloveCalibrationState.h"
#include "SGInterop/SGIC_ESGHapticLocation.h"
#include "SGInterop/SGIC_ESGPositionalTrackingHardware.h"
#include "SGInterop/SGIC_ESGSenseGloveThumperCommand.h"
#include "SGInterop/SGIC_FQuat.h"
#include "SGInterop/SGIC_FSGBasicHandModelImpl.h"
#include "SGInterop/SGIC_FSGHandInterpolatorImpl.h"
#include "SGInterop/SGIC_FSGHandPoseImpl.h"
#include "SGInterop/SGIC_FSGSenseGloveImpl.h"
#include "SGInterop/SGIC_FSGSenseGloveInfoImpl.h"
#include "SGInterop/SGIC_FSGSenseGlovePoseImpl.h"
#include "SGInterop/SGIC_FSGSenseGloveSensorDataImpl.h"
#include "SGInterop/SGIC_FSGSenseGloveSensorNormalizerImpl.h"
#include "SGInterop/SGIC_FString.h"
#include "SGInterop/SGIC_FVector.h"
#include "SGInterop/SGIC_TArray_float.h"
#include "SGInterop/SGIC_TArray_FVector.h"
#include "SGInterop/SGIC_TArray_FSGSenseGloveImpl.h"
#include "SGInterop/SGIC_TArray_TArray_FVector.h"
#include "SGInterop/SGIC_TSharedPtr_FSGSenseGloveImpl.h"

USGSenseGlove* USGSenseGlove::NewSenseGlove(UObject* Outer, const FSGSenseGloveImpl& SenseGloveImpl)
{
    USGSenseGlove* Instance = NewObject<USGSenseGlove>(Outer);
    Instance->SetSGDeviceImpl(SenseGloveImpl);
    return Instance;
}

USGSenseGlove* USGSenseGlove::NewSenseGlove(UObject* Outer, TSharedRef<FSGSenseGloveImpl> SenseGloveImpl)
{
    USGSenseGlove* Instance = NewObject<USGSenseGlove>(Outer);
    Instance->SetSGDeviceImpl(MoveTemp(SenseGloveImpl));
    return Instance;
}

USGSenseGlove* USGSenseGlove::NewSenseGlove(UObject* Outer)
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl OutSharedPtrContainer;
    SGCoreImpl_FSGSenseGloveImpl_ctor_MakeShared(&OutSharedPtrContainer);

    return NewSenseGlove(Outer, OutSharedPtrContainer.Ptr.ToSharedRef());
}

USGSenseGlove* USGSenseGlove::NewSenseGlove(UObject* Outer, const USGSenseGloveInfo& Model)
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl OutSharedPtrContainer;
    FSGIC_FSGSenseGloveInfoImpl ModelContainer{Model.ToSenseGloveInfoImpl()};
    SGCoreImpl_FSGSenseGloveImpl_ctor_Model_MakeShared(&OutSharedPtrContainer, &ModelContainer);

    return NewSenseGlove(Outer, OutSharedPtrContainer.Ptr.ToSharedRef());
}

USGDevice* USGSenseGlove::Parse(UObject* Outer, const FString& ConstantsString)
{
    FSGIC_FSGSenseGloveImpl OutSenseGloveImplContainer;
    FSGIC_FString ConstantsStringContainer{ConstantsString};
    SGCoreImpl_FSGSenseGloveImpl_Parse(&OutSenseGloveImplContainer, &ConstantsStringContainer);

    return NewSenseGlove(Outer, MoveTemp(OutSenseGloveImplContainer.SenseGloveImpl));
}

TArray<USGSenseGlove*> USGSenseGlove::GetSenseGloves(UObject* Outer)
{
    FSGIC_TArray_FSGSenseGloveImpl OutGlovesContainer;
    SGCoreImpl_FSGSenseGloveImpl_GetSenseGloves(&OutGlovesContainer);
    TArray<USGSenseGlove*> Gloves;
    for (const FSGSenseGloveImpl& SenseGloveImpl: OutGlovesContainer.Array)
    {
        USGSenseGlove* SenseGlove = USGSenseGlove::NewSenseGlove(Outer, SenseGloveImpl);
        Gloves.Add(SenseGlove);
    }
    return Gloves;
}

bool USGSenseGlove::GetSenseGlove(UObject* Outer, USGSenseGlove*& OutGlove)
{
    FSGIC_FSGSenseGloveImpl OutGloveContainer;
    const bool bResult = SGCoreImpl_FSGSenseGloveImpl_GetSenseGlove_OutGlove(&OutGloveContainer);

    if (bResult)
    {
        OutGlove = NewSenseGlove(Outer, MoveTemp(OutGloveContainer.SenseGloveImpl));
    }

    return bResult;
}

bool USGSenseGlove::GetSenseGlove(UObject* Outer, const bool bRightHanded, USGSenseGlove*& OutGlove)
{
    FSGIC_FSGSenseGloveImpl OutGloveContainer;
    const bool bResult = SGCoreImpl_FSGSenseGloveImpl_GetSenseGlove_bRightHanded_OutGlove(
        bRightHanded, &OutGloveContainer);

    if (bResult)
    {
        OutGlove = NewSenseGlove(Outer, MoveTemp(OutGloveContainer.SenseGloveImpl));
    }

    return bResult;
}

void USGSenseGlove::CalculateGloveLocation(const FVector& ReferencePosition, const FQuat& ReferenceRotation,
                                           const ESGPositionalTrackingHardware TrackingHardware,
                                           const bool bRightHanded, const ESGFinger MountedOn,
                                           FVector& OutGlovePosition, FQuat& OutGloveRotation)
{
    FSGIC_FVector ReferencePositionContainer{ReferencePosition};
    FSGIC_FQuat ReferenceRotationContainer{ReferenceRotation};
    FSGIC_ESGPositionalTrackingHardware TrackingHardwareContainer{TrackingHardware};
    FSGIC_ESGFinger MountedOnContainer{MountedOn};
    FSGIC_FVector OutGlovePositionContainer;
    FSGIC_FQuat OutGloveRotationContainer;

    SGCoreImpl_FSGSenseGloveImpl_CalculateGloveLocation(
        &ReferencePositionContainer, &ReferenceRotationContainer,
        &TrackingHardwareContainer,
        bRightHanded, &MountedOnContainer,
        &OutGlovePositionContainer, &OutGloveRotationContainer);

    OutGlovePosition = MoveTemp(OutGlovePositionContainer.Vector);
    OutGloveRotation = MoveTemp(OutGloveRotationContainer.Quat);
}

void USGSenseGlove::CalculateGloveLocation(const FVector& ReferencePosition, const FRotator& ReferenceRotation,
                                           const ESGPositionalTrackingHardware TrackingHardware,
                                           const bool bRightHanded, const ESGFinger MountedOn,
                                           FVector& OutGlovePosition, FRotator& OutGloveRotation)
{
    FSGIC_FVector ReferencePositionContainer{ReferencePosition};
    FSGIC_FQuat ReferenceRotationContainer{ReferenceRotation.Quaternion()};
    FSGIC_ESGPositionalTrackingHardware TrackingHardwareContainer{TrackingHardware};
    FSGIC_ESGFinger MountedOnContainer{MountedOn};
    FSGIC_FVector OutGlovePositionContainer;
    FSGIC_FQuat OutGloveRotationContainer;

    SGCoreImpl_FSGSenseGloveImpl_CalculateGloveLocation(
        &ReferencePositionContainer, &ReferenceRotationContainer,
        &TrackingHardwareContainer,
        bRightHanded, &MountedOnContainer,
        &OutGlovePositionContainer, &OutGloveRotationContainer);

    OutGlovePosition = MoveTemp(OutGlovePositionContainer.Vector);
    OutGloveRotation = OutGloveRotationContainer.Quat.Rotator();
}

void USGSenseGlove::CalculateWristLocation(const FVector& ReferencePosition, const FQuat& ReferenceRotation,
                                           const ESGPositionalTrackingHardware TrackingHardware,
                                           const bool bRightHanded, const ESGFinger MountedOn,
                                           const FVector& GloveWristPositionOffset,
                                           const FQuat& GloveWristRotationOffset,
                                           FVector& OutWristPosition, FQuat& OutWristRotation)
{
    FSGIC_FVector ReferencePositionContainer{ReferencePosition};
    FSGIC_FQuat ReferenceRotationContainer{ReferenceRotation};
    FSGIC_ESGPositionalTrackingHardware TrackingHardwareContainer{TrackingHardware};
    FSGIC_ESGFinger MountedOnContainer{MountedOn};
    FSGIC_FVector GloveWristPositionOffsetContainer{GloveWristPositionOffset};
    FSGIC_FQuat GloveWristRotationOffsetContainer{GloveWristRotationOffset};
    FSGIC_FVector OutWristPositionContainer;
    FSGIC_FQuat OutWristRotationContainer;

    SGCoreImpl_FSGSenseGloveImpl_CalculateWristLocation(
        &ReferencePositionContainer, &ReferenceRotationContainer,
        &TrackingHardwareContainer,
        bRightHanded, &MountedOnContainer,
        &GloveWristPositionOffsetContainer, &GloveWristRotationOffsetContainer,
        &OutWristPositionContainer, &OutWristRotationContainer);

    OutWristPosition = MoveTemp(OutWristPositionContainer.Vector);
    OutWristRotation = MoveTemp(OutWristRotationContainer.Quat);
}

void USGSenseGlove::CalculateWristLocation(const FVector& ReferencePosition, const FRotator& ReferenceRotation,
                                           const ESGPositionalTrackingHardware TrackingHardware,
                                           const bool bRightHanded, const ESGFinger MountedOn,
                                           const FVector& GloveWristPositionOffset,
                                           const FRotator& GloveWristRotationOffset,
                                           FVector& OutWristPosition, FRotator& OutWristRotation)
{
    FSGIC_FVector ReferencePositionContainer{ReferencePosition};
    FSGIC_FQuat ReferenceRotationContainer{ReferenceRotation.Quaternion()};
    FSGIC_ESGPositionalTrackingHardware TrackingHardwareContainer{TrackingHardware};
    FSGIC_ESGFinger MountedOnContainer{MountedOn};
    FSGIC_FVector GloveWristPositionOffsetContainer{GloveWristPositionOffset};
    FSGIC_FQuat GloveWristRotationOffsetContainer{GloveWristRotationOffset.Quaternion()};
    FSGIC_FVector OutWristPositionContainer;
    FSGIC_FQuat OutWristRotationContainer;

    SGCoreImpl_FSGSenseGloveImpl_CalculateWristLocation(
        &ReferencePositionContainer, &ReferenceRotationContainer,
        &TrackingHardwareContainer,
        bRightHanded, &MountedOnContainer,
        &GloveWristPositionOffsetContainer, &GloveWristRotationOffsetContainer,
        &OutWristPositionContainer, &OutWristRotationContainer);

    OutWristPosition = MoveTemp(OutWristPositionContainer.Vector);
    OutWristRotation = OutWristRotationContainer.Quat.Rotator();
}

ANSICHAR USGSenseGlove::GetHapticsByte()
{
    return SGCoreImpl_FSGSenseGloveImpl_GetHapticsByte();
}

ANSICHAR USGSenseGlove::GetThumperByte()
{
    return SGCoreImpl_FSGSenseGloveImpl_GetThumperByte();
}

FVector USGSenseGlove::GetDefaultThimbleOffset()
{
    FSGIC_FVector OutOffsetContainer;
    SGCoreImpl_FSGSenseGloveImpl_GetDefaultThimbleOffset(&OutOffsetContainer);
    return OutOffsetContainer.Vector;
}

USGSenseGlovePose* USGSenseGlove::CalculateGlovePose(UObject* Outer,
                                                     const USGSenseGloveSensorData* SensorData,
                                                     const USGSenseGloveInfo* GloveModel)
{
    FSGIC_FSGSenseGlovePoseImpl OutPoseContainer;
    FSGIC_FSGSenseGloveSensorDataImpl SensorDataContainer{SensorData->ToSenseGloveSensorDataImpl()};
    FSGIC_FSGSenseGloveInfoImpl GloveModelContainer{GloveModel->ToSenseGloveInfoImpl()};
    SGCoreImpl_FSGSenseGloveImpl_CalculateGlovePose_SensorData_GloveModel(
        &OutPoseContainer, &SensorDataContainer, &GloveModelContainer);

    return USGSenseGlovePose::NewSenseGlovePose(Outer, MoveTemp(OutPoseContainer.SenseGlovePoseImpl));
}

USGSenseGlovePose* USGSenseGlove::CalculateGlovePose(UObject* Outer,
                                                     const TArray<TArray<FVector>>& GloveAngles,
                                                     const USGSenseGloveInfo* GloveModel)
{
    FSGIC_FSGSenseGlovePoseImpl OutPoseContainer;
    FSGIC_TArray_TArray_FVector GloveAnglesContainer{GloveAngles};
    FSGIC_FSGSenseGloveInfoImpl GloveModelContainer{GloveModel->ToSenseGloveInfoImpl()};
    SGCoreImpl_FSGSenseGloveImpl_CalculateGlovePose_GloveAngles_GloveModel(
        &OutPoseContainer, &GloveAnglesContainer, &GloveModelContainer);

    return USGSenseGlovePose::NewSenseGlovePose(Outer, MoveTemp(OutPoseContainer.SenseGlovePoseImpl));
}

void USGSenseGlove::NormalizeValues(
    const TArray<FVector>& SumAngles, const USGSenseGloveSensorNormalizer* Normalizer,
    TArray<float>& OutFlexions, TArray<float>& OutAbductions)
{
    FSGIC_TArray_FVector SumAnglesContainer{SumAngles};
    FSGIC_FSGSenseGloveSensorNormalizerImpl NormalizerContainer{Normalizer->ToSenseGloveSensorNormalizerImpl()};
    FSGIC_TArray_float OutFlexionsContainer;
    FSGIC_TArray_float OutAbductionsContainer;
    SGCoreImpl_FSGSenseGloveImpl_NormalizeValues(
        &SumAnglesContainer, &NormalizerContainer, &OutFlexionsContainer, &OutAbductionsContainer);

    OutFlexions = MoveTemp(OutFlexionsContainer.Array);
    OutAbductions = MoveTemp(OutAbductionsContainer.Array);
}

void USGSenseGlove::CalculateHandAngles(
    const USGSenseGlovePose* GlovePose,
    const USGSenseGloveSensorNormalizer* Normalizer, const USGHandInterpolator* NormalizedToAngles,
    TArray<TArray<FVector>>& OutHandAngles)
{
    FSGIC_FSGSenseGlovePoseImpl GlovePoseContainer{GlovePose->ToSenseGlovePoseImpl()};
    FSGIC_FSGSenseGloveSensorNormalizerImpl NormalizerContainer{Normalizer->ToSenseGloveSensorNormalizerImpl()};
    FSGIC_FSGHandInterpolatorImpl NormalizedToAnglesContainer{NormalizedToAngles->ToHandInterpolatorImpl()};
    FSGIC_TArray_TArray_FVector OutHandAnglesContainer;
    SGCoreImpl_FSGSenseGloveImpl_CalculateHandAngles(
        &GlovePoseContainer, &NormalizerContainer, &NormalizedToAnglesContainer, &OutHandAnglesContainer);

    OutHandAngles = MoveTemp(OutHandAnglesContainer.Array);
}

void USGSenseGlove::CalculateHandPose(
    UObject* Outer,
    const USGSenseGlovePose* GlovePose, const USGBasicHandModel* HandModel,
    const USGSenseGloveSensorNormalizer* Normalizer, const USGHandInterpolator* NormalizedToAngles,
    USGHandPose*& OutHandPose)
{
    FSGIC_FSGSenseGlovePoseImpl GlovePoseContainer{GlovePose->ToSenseGlovePoseImpl()};
    FSGIC_FSGSenseGloveSensorNormalizerImpl NormalizerContainer{Normalizer->ToSenseGloveSensorNormalizerImpl()};
    FSGIC_FSGHandInterpolatorImpl NormalizedToAnglesContainer{NormalizedToAngles->ToHandInterpolatorImpl()};
    FSGIC_FSGHandPoseImpl OutHandPoseContainer;
    SGCoreImpl_FSGSenseGloveImpl_CalculateHandAngles(
        &GlovePoseContainer, &NormalizerContainer, &NormalizedToAnglesContainer, &OutHandPoseContainer);

    OutHandPose = USGHandPose::NewHandPose(Outer, MoveTemp(OutHandPoseContainer.HandPoseImpl));
}

USGSenseGlove::USGSenseGlove()
    : USGHapticGlove()
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl OutSharedPtrContainer;
    SGCoreImpl_FSGSenseGloveImpl_ctor_MakeShared(&OutSharedPtrContainer);

    SetSGDeviceImpl(OutSharedPtrContainer.Ptr.ToSharedRef());
}

USGSenseGlove::USGSenseGlove(const USGSenseGloveInfo& Model)
    : USGHapticGlove()
{
}

USGSenseGlove::USGSenseGlove(const FSGDeviceImpl& SGDeviceImpl)
    : USGHapticGlove(SGDeviceImpl)
{
}

USGSenseGlove::USGSenseGlove(const FSGHapticGloveImpl& HapticGloveImpl)
    : USGHapticGlove(HapticGloveImpl)
{
}

USGSenseGlove::USGSenseGlove(const FSGSenseGloveImpl& SenseGloveImpl)
    : USGHapticGlove(SenseGloveImpl)
{
}

USGSenseGlove::USGSenseGlove(TSharedRef<FSGDeviceImpl> SGDeviceImpl)
    : USGHapticGlove(MoveTemp(SGDeviceImpl))
{
}

USGSenseGlove::USGSenseGlove(TSharedRef<FSGHapticGloveImpl> HapticGloveImpl)
    : USGHapticGlove(MoveTemp(HapticGloveImpl))
{
}

USGSenseGlove::USGSenseGlove(TSharedRef<FSGSenseGloveImpl> SenseGloveImpl)
    : USGHapticGlove(MoveTemp(SenseGloveImpl))
{
}

USGSenseGlove::~USGSenseGlove() = default;

TSharedRef<FSGSenseGloveImpl> USGSenseGlove::ToSenseGloveImpl() const
{
    return FSGDeviceImplCast::ToSenseGloveImpl(ToSGDeviceImpl());
}

ESGDeviceType USGSenseGlove::GetDeviceType() const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_ESGDeviceType OutTypeContainer;
    SGCoreImpl_FSGSenseGloveImpl_GetDeviceType(&InstanceContainer, &OutTypeContainer);
    return OutTypeContainer.Type;
}

FString USGSenseGlove::GetDeviceId() const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FString OutIdContainer;
    SGCoreImpl_FSGSenseGloveImpl_GetDeviceId(&InstanceContainer, &OutIdContainer);
    return OutIdContainer.String;
}

FString USGSenseGlove::GetHardwareVersion() const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FString OutVersionContainer;
    SGCoreImpl_FSGSenseGloveImpl_GetHardwareVersion(&InstanceContainer, &OutVersionContainer);
    return OutVersionContainer.String;
}

int32 USGSenseGlove::GetFirmwareVersion() const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    return SGCoreImpl_FSGSenseGloveImpl_GetFirmwareVersion(&InstanceContainer);
}

int32 USGSenseGlove::GetSubFirmwareVersion() const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    return SGCoreImpl_FSGSenseGloveImpl_GetSubFirmwareVersion(&InstanceContainer);
}

bool USGSenseGlove::IsRight() const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    return SGCoreImpl_FSGSenseGloveImpl_IsRight(&InstanceContainer);
}

bool USGSenseGlove::GetImuRotation(FQuat& OutImu)
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FQuat OutImuContainer;
    const bool bResult = SGCoreImpl_FSGSenseGloveImpl_GetImuRotation(&InstanceContainer, &OutImuContainer);

    if (bResult)
    {
        OutImu = MoveTemp(OutImuContainer.Quat);
    }

    return bResult;
}

bool USGSenseGlove::GetImuRotation(FRotator& OutImu)
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FQuat OutImuContainer;
    const bool bResult = SGCoreImpl_FSGSenseGloveImpl_GetImuRotation(&InstanceContainer, &OutImuContainer);

    if (bResult)
    {
        OutImu = OutImuContainer.Quat.Rotator();
    }

    return bResult;
}

bool USGSenseGlove::GetHandPose(UObject* Outer, const USGBasicHandModel* HandGeometry, USGHandPose*& OutHandPose) const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FSGBasicHandModelImpl HandModelContainer{HandGeometry->ToBasicHandModelImpl()};
    FSGIC_FSGHandPoseImpl OutHandPoseContainer;
    const bool bResult = SGCoreImpl_FSGSenseGloveImpl_GetHandPose(
        &InstanceContainer, &HandModelContainer, &OutHandPoseContainer);

    if (bResult)
    {
        OutHandPose = USGHandPose::NewHandPose(Outer, MoveTemp(OutHandPoseContainer.HandPoseImpl));
    }

    return bResult;
}

bool USGSenseGlove::GetHandAngles(TArray<TArray<FVector>>& OutHandAngles) const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_TArray_TArray_FVector OutHandAnglesContainer;
    const bool bResult = SGCoreImpl_FSGSenseGloveImpl_GetHandAngles(&InstanceContainer, &OutHandAnglesContainer);
    OutHandAngles = MoveTemp(OutHandAnglesContainer.Array);
    return bResult;
}

ESGHapticGloveCalibrationState USGSenseGlove::GetCalibrationState() const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_ESGHapticGloveCalibrationState OutHapticGloveCalibrationStateContainer;
    SGCoreImpl_FSGSenseGloveImpl_GetCalibrationState(&InstanceContainer, &OutHapticGloveCalibrationStateContainer);
    return OutHapticGloveCalibrationStateContainer.State;
}

void USGSenseGlove::EndCalibration()
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    SGCoreImpl_FSGSenseGloveImpl_EndCalibration(&InstanceContainer);
}

void USGSenseGlove::ResetCalibration()
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    SGCoreImpl_FSGSenseGloveImpl_ResetCalibration(&InstanceContainer);
}

void USGSenseGlove::StopHaptics()
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    SGCoreImpl_FSGSenseGloveImpl_StopHaptics(&InstanceContainer);
}

void USGSenseGlove::StopVibrations()
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    SGCoreImpl_FSGSenseGloveImpl_StopVibrations(&InstanceContainer);
}

bool USGSenseGlove::SendHaptics()
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    return SGCoreImpl_FSGSenseGloveImpl_SendHaptics(&InstanceContainer);
}

bool USGSenseGlove::QueueForceFeedbackLevels(const TArray<float>& Levels01)
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_TArray_float Levels01Container{Levels01};
    return SGCoreImpl_FSGSenseGloveImpl_QueueForceFeedbackLevels(&InstanceContainer, &Levels01Container);
}

bool USGSenseGlove::QueueForceFeedbackLevel(const int32 Finger, const float Level01)
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    return SGCoreImpl_FSGSenseGloveImpl_QueueForceFeedbackLevel(&InstanceContainer, Finger, Level01);
}

bool USGSenseGlove::QueueVibroLevels(const TArray<float>& Amplitudes)
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_TArray_float AmplitudesContainer{Amplitudes};
    return SGCoreImpl_FSGSenseGloveImpl_QueueVibroLevels(&InstanceContainer, &AmplitudesContainer);
}

bool USGSenseGlove::QueueVibroLevel(const ESGHapticLocation Location, const float Level01)
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_ESGHapticLocation LocationContainer{Location};
    return SGCoreImpl_FSGSenseGloveImpl_QueueVibroLevel_Location_Level01(
        &InstanceContainer, &LocationContainer, Level01);
}

void USGSenseGlove::GetGloveLocation(const FVector& ReferencePosition, const FQuat& ReferenceRotation,
                                     const ESGPositionalTrackingHardware TrackingHardware,
                                     FVector& OutGlovePosition, FQuat& OutGloveRotation) const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FVector ReferencePositionContainer{ReferencePosition};
    FSGIC_FQuat ReferenceRotationContainer{ReferenceRotation};
    FSGIC_ESGPositionalTrackingHardware TrackingHardwareContainer{TrackingHardware};
    FSGIC_FVector OutGlovePositionContainer;
    FSGIC_FQuat OutGloveRotationContainer;

    SGCoreImpl_FSGSenseGloveImpl_GetGloveLocation_ReferencePosition_ReferenceRotation_TrackingHardware_OutGlovePosition_OutGloveRotation(
        &InstanceContainer,
        &ReferencePositionContainer, &ReferenceRotationContainer,
        &TrackingHardwareContainer,
        &OutGlovePositionContainer, &OutGloveRotationContainer);

    OutGlovePosition = MoveTemp(OutGlovePositionContainer.Vector);
    OutGloveRotation = MoveTemp(OutGloveRotationContainer.Quat);
}

void USGSenseGlove::GetGloveLocation(const FVector& ReferencePosition, const FRotator& ReferenceRotation,
                                     const ESGPositionalTrackingHardware TrackingHardware,
                                     FVector& OutGlovePosition, FRotator& OutGloveRotation) const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FVector ReferencePositionContainer{ReferencePosition};
    FSGIC_FQuat ReferenceRotationContainer{ReferenceRotation.Quaternion()};
    FSGIC_ESGPositionalTrackingHardware TrackingHardwareContainer{TrackingHardware};
    FSGIC_FVector OutGlovePositionContainer;
    FSGIC_FQuat OutGloveRotationContainer;

    SGCoreImpl_FSGSenseGloveImpl_GetGloveLocation_ReferencePosition_ReferenceRotation_TrackingHardware_OutGlovePosition_OutGloveRotation(
        &InstanceContainer,
        &ReferencePositionContainer, &ReferenceRotationContainer,
        &TrackingHardwareContainer,
        &OutGlovePositionContainer, &OutGloveRotationContainer);

    OutGlovePosition = MoveTemp(OutGlovePositionContainer.Vector);
    OutGloveRotation = OutGloveRotationContainer.Quat.Rotator();
}

void USGSenseGlove::GetGloveLocation(const FVector& ReferencePosition, const FQuat& ReferenceRotation,
                                     const ESGPositionalTrackingHardware TrackingHardware, const ESGFinger MountedOn,
                                     FVector& OutGlovePosition, FQuat& OutGloveRotation) const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FVector ReferencePositionContainer{ReferencePosition};
    FSGIC_FQuat ReferenceRotationContainer{ReferenceRotation};
    FSGIC_ESGPositionalTrackingHardware TrackingHardwareContainer{TrackingHardware};
    FSGIC_ESGFinger MountedOnContainer{MountedOn};
    FSGIC_FVector OutGlovePositionContainer;
    FSGIC_FQuat OutGloveRotationContainer;

    SGCoreImpl_FSGSenseGloveImpl_GetGloveLocation_ReferencePosition_ReferenceRotation_TrackingHardware_MountedOn_OutGlovePosition_OutGloveRotation(
        &InstanceContainer,
        &ReferencePositionContainer, &ReferenceRotationContainer,
        &TrackingHardwareContainer, &MountedOnContainer,
        &OutGlovePositionContainer, &OutGloveRotationContainer);

    OutGlovePosition = MoveTemp(OutGlovePositionContainer.Vector);
    OutGloveRotation = MoveTemp(OutGloveRotationContainer.Quat);
}

void USGSenseGlove::GetGloveLocation(const FVector& ReferencePosition, const FRotator& ReferenceRotation,
                                     const ESGPositionalTrackingHardware TrackingHardware, const ESGFinger MountedOn,
                                     FVector& OutGlovePosition, FRotator& OutGloveRotation) const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FVector ReferencePositionContainer{ReferencePosition};
    FSGIC_FQuat ReferenceRotationContainer{ReferenceRotation.Quaternion()};
    FSGIC_ESGPositionalTrackingHardware TrackingHardwareContainer{TrackingHardware};
    FSGIC_ESGFinger MountedOnContainer{MountedOn};
    FSGIC_FVector OutGlovePositionContainer;
    FSGIC_FQuat OutGloveRotationContainer;

    SGCoreImpl_FSGSenseGloveImpl_GetGloveLocation_ReferencePosition_ReferenceRotation_TrackingHardware_MountedOn_OutGlovePosition_OutGloveRotation(
        &InstanceContainer,
        &ReferencePositionContainer, &ReferenceRotationContainer,
        &TrackingHardwareContainer, &MountedOnContainer,
        &OutGlovePositionContainer, &OutGloveRotationContainer);

    OutGlovePosition = MoveTemp(OutGlovePositionContainer.Vector);
    OutGloveRotation = OutGloveRotationContainer.Quat.Rotator();
}

void USGSenseGlove::GetWristLocation(const FVector& ReferencePosition, const FQuat& ReferenceRotation,
                                     const ESGPositionalTrackingHardware TrackingHardware,
                                     FVector& OutWristPosition, FQuat& OutWristRotation) const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FVector ReferencePositionContainer{ReferencePosition};
    FSGIC_FQuat ReferenceRotationContainer{ReferenceRotation};
    FSGIC_ESGPositionalTrackingHardware TrackingHardwareContainer{TrackingHardware};
    FSGIC_FVector OutWristPositionContainer;
    FSGIC_FQuat OutWristRotationContainer;
    SGCoreImpl_FSGSenseGloveImpl_GetWristLocation_ReferencePosition_ReferenceRotation_TrackingHardware_OutWristPosition_OutWristRotation(
        &InstanceContainer,
        &ReferencePositionContainer, &ReferenceRotationContainer,
        &TrackingHardwareContainer,
        &OutWristPositionContainer, &OutWristRotationContainer);

    OutWristPosition = MoveTemp(OutWristPositionContainer.Vector);
    OutWristRotation = MoveTemp(OutWristRotationContainer.Quat);
}

void USGSenseGlove::GetWristLocation(const FVector& ReferencePosition, const FRotator& ReferenceRotation,
                                     const ESGPositionalTrackingHardware TrackingHardware,
                                     FVector& OutWristPosition, FRotator& OutWristRotation) const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FVector ReferencePositionContainer{ReferencePosition};
    FSGIC_FQuat ReferenceRotationContainer{ReferenceRotation.Quaternion()};
    FSGIC_ESGPositionalTrackingHardware TrackingHardwareContainer{TrackingHardware};
    FSGIC_FVector OutWristPositionContainer;
    FSGIC_FQuat OutWristRotationContainer;
    SGCoreImpl_FSGSenseGloveImpl_GetWristLocation_ReferencePosition_ReferenceRotation_TrackingHardware_OutWristPosition_OutWristRotation(
        &InstanceContainer,
        &ReferencePositionContainer, &ReferenceRotationContainer,
        &TrackingHardwareContainer,
        &OutWristPositionContainer, &OutWristRotationContainer);

    OutWristPosition = MoveTemp(OutWristPositionContainer.Vector);
    OutWristRotation = OutWristRotationContainer.Quat.Rotator();
}

void USGSenseGlove::GetWristLocation(const FVector& ReferencePosition, const FQuat& ReferenceRotation,
                                     const ESGPositionalTrackingHardware TrackingHardware,
                                     const FVector& GloveWristPositionOffset,
                                     const FQuat& GloveWristRotationOffset,
                                     FVector& OutWristPosition, FQuat& OutWristRotation) const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FVector ReferencePositionContainer{ReferencePosition};
    FSGIC_FQuat ReferenceRotationContainer{ReferenceRotation};
    FSGIC_ESGPositionalTrackingHardware TrackingHardwareContainer{TrackingHardware};
    FSGIC_FVector GloveWristPositionOffsetContainer{GloveWristPositionOffset};
    FSGIC_FQuat GloveWristRotationOffsetContainer{GloveWristRotationOffset};
    FSGIC_FVector OutWristPositionContainer;
    FSGIC_FQuat OutWristRotationContainer;
    SGCoreImpl_FSGSenseGloveImpl_GetWristLocation_ReferencePosition_ReferenceRotation_TrackingHardware_GloveWristPositionOffset_GloveWristRotationOffset_OutWristPosition_OutWristRotation(
        &InstanceContainer,
        &ReferencePositionContainer, &ReferenceRotationContainer,
        &TrackingHardwareContainer,
        &GloveWristPositionOffsetContainer, &GloveWristRotationOffsetContainer,
        &OutWristPositionContainer, &OutWristRotationContainer);

    OutWristPosition = MoveTemp(OutWristPositionContainer.Vector);
    OutWristRotation = MoveTemp(OutWristRotationContainer.Quat);
}

void USGSenseGlove::GetWristLocation(const FVector& ReferencePosition, const FRotator& ReferenceRotation,
                                     const ESGPositionalTrackingHardware TrackingHardware,
                                     const FVector& GloveWristPositionOffset,
                                     const FRotator& GloveWristRotationOffset,
                                     FVector& OutWristPosition, FRotator& OutWristRotation) const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FVector ReferencePositionContainer{ReferencePosition};
    FSGIC_FQuat ReferenceRotationContainer{ReferenceRotation.Quaternion()};
    FSGIC_ESGPositionalTrackingHardware TrackingHardwareContainer{TrackingHardware};
    FSGIC_FVector GloveWristPositionOffsetContainer{GloveWristPositionOffset};
    FSGIC_FQuat GloveWristRotationOffsetContainer{GloveWristRotationOffset.Quaternion()};
    FSGIC_FVector OutWristPositionContainer;
    FSGIC_FQuat OutWristRotationContainer;
    SGCoreImpl_FSGSenseGloveImpl_GetWristLocation_ReferencePosition_ReferenceRotation_TrackingHardware_GloveWristPositionOffset_GloveWristRotationOffset_OutWristPosition_OutWristRotation(
        &InstanceContainer,
        &ReferencePositionContainer, &ReferenceRotationContainer,
        &TrackingHardwareContainer,
        &GloveWristPositionOffsetContainer, &GloveWristRotationOffsetContainer,
        &OutWristPositionContainer, &OutWristRotationContainer);

    OutWristPosition = MoveTemp(OutWristPositionContainer.Vector);
    OutWristRotation = OutWristRotationContainer.Quat.Rotator();
}

USGSenseGloveInfo* USGSenseGlove::GetGloveInfo(UObject* Outer) const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FSGSenseGloveInfoImpl OutGloveInfoContainer;
    SGCoreImpl_FSGSenseGloveImpl_GetGloveInfo(&InstanceContainer, &OutGloveInfoContainer);

    return USGSenseGloveInfo::NewSenseGloveInfo(Outer, MoveTemp(OutGloveInfoContainer.SenseGloveInfoImpl));
}

TArray<FVector> USGSenseGlove::GetFingerThimbleOffsets() const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_TArray_FVector OutFingerThimbleOffsetsContainer;
    SGCoreImpl_FSGSenseGloveImpl_GetFingerThimbleOffsets(&InstanceContainer, &OutFingerThimbleOffsetsContainer);
    return OutFingerThimbleOffsetsContainer.Array;
}

void USGSenseGlove::SetFingerThimbleOffset(const int32 Finger, const FVector& Offset)
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FVector OffsetContainer{Offset};
    SGCoreImpl_FSGSenseGloveImpl_SetFingerThimbleOffset(&InstanceContainer, Finger, &OffsetContainer);
}

bool USGSenseGlove::SupportsWristActuator() const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    return SGCoreImpl_FSGSenseGloveImpl_SupportsWristActuator(&InstanceContainer);
}

bool USGSenseGlove::GetSensorData(UObject* Outer, USGSenseGloveSensorData*& OutSensorData) const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FSGSenseGloveSensorDataImpl OutSensorDataContainer;
    const bool bResult = SGCoreImpl_FSGSenseGloveImpl_GetSensorData(&InstanceContainer, &OutSensorDataContainer);

    if (bResult)
    {
        OutSensorData = USGSenseGloveSensorData::NewSenseGloveSensorData(
            Outer, MoveTemp(OutSensorDataContainer.SenseGloveSensorDataImpl));
    }

    return bResult;
}

bool USGSenseGlove::GetGlovePose(UObject* Outer, USGSenseGlovePose*& OutGlovePose)
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FSGSenseGlovePoseImpl OutGlovePoseContainer;
    const bool bResult = SGCoreImpl_FSGSenseGloveImpl_GetGlovePose_OutGlovePose(
        &InstanceContainer, &OutGlovePoseContainer);

    if (bResult)
    {
        OutGlovePose = USGSenseGlovePose::NewSenseGlovePose(
            Outer, MoveTemp(OutGlovePoseContainer.SenseGlovePoseImpl));
    }

    return bResult;
}

bool USGSenseGlove::GetGlovePose(
    UObject* Outer, const USGSenseGloveSensorData* SensorData, USGSenseGlovePose*& OutGlovePose)
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FSGSenseGloveSensorDataImpl SensorDataContainer{SensorData->ToSenseGloveSensorDataImpl()};
    FSGIC_FSGSenseGlovePoseImpl OutGlovePoseContainer;
    const bool bResult = SGCoreImpl_FSGSenseGloveImpl_GetGlovePose_SensorData_OutGlovePose(
        &InstanceContainer, &SensorDataContainer, &OutGlovePoseContainer);

    if (bResult)
    {
        OutGlovePose = USGSenseGlovePose::NewSenseGlovePose(
            Outer, MoveTemp(OutGlovePoseContainer.SenseGlovePoseImpl));
    }

    return bResult;
}

bool USGSenseGlove::QueueVibroLevel(const int32 Finger, const float Amplitude)
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    return SGCoreImpl_FSGSenseGloveImpl_QueueVibroLevel_Finger_Amplitude(&InstanceContainer, Finger, Amplitude);
}

bool USGSenseGlove::QueueThumperCommand(ESGSenseGloveThumperCommand Command)
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_ESGSenseGloveThumperCommand CommandContainer{Command};
    return SGCoreImpl_FSGSenseGloveImpl_QueueThumperCommand(&InstanceContainer, &CommandContainer);
}

FString USGSenseGlove::ToString() const
{
    FSGIC_TSharedPtr_FSGSenseGloveImpl InstanceContainer{ToSenseGloveImpl()};
    FSGIC_FString OutStringContainer;
    SGCoreImpl_FSGSenseGloveImpl_ToString(&InstanceContainer, &OutStringContainer);
    return OutStringContainer.String;;
}