Plugin/Source/SenseGloveCoreImpl/Private/SGCoreImpl/SGSenseGloveImpl.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 "SGCoreImpl/SGSenseGloveImpl.h"

#include <memory>
#include <vector>

#include "Containers/StringConv.h"
#include "Misc/AssertionMacros.h"
#include "Runtime/Launch/Resources/Version.h"

#include "SGBuildHacks/SGInclude_Core_BasicHandModel.h"
#include "SGBuildHacks/SGInclude_Core_HandInterpolator.h"
#include "SGBuildHacks/SGInclude_Core_HandPose.h"
#include "SGBuildHacks/SGInclude_Core_HapticGlove.h"
#include "SGBuildHacks/SGInclude_Core_Quat.h"
#include "SGBuildHacks/SGInclude_Core_SenseGlove.h"
#include "SGBuildHacks/SGInclude_Core_SenseGloveInfo.h"
#include "SGBuildHacks/SGInclude_Core_SenseGlovePose.h"
#include "SGBuildHacks/SGInclude_Core_SenseGloveSensorData.h"
#include "SGBuildHacks/SGInclude_Core_SenseGloveSensorNormalizer.h"
#include "SGBuildHacks/SGInclude_Core_Vect3D.h"
#include "SGCoreImpl/SGBasicHandModelImpl.h"
#include "SGCoreImpl/SGCoreEnumCast.h"
#include "SGCoreImpl/SGHandInterpolatorImpl.h"
#include "SGCoreImpl/SGHandPoseImpl.h"
#include "SGCoreImpl/SGQuatImpl.h"
#include "SGCoreImpl/SGSenseGloveInfoImpl.h"
#include "SGCoreImpl/SGSenseGlovePoseImpl.h"
#include "SGCoreImpl/SGSenseGloveSensorDataImpl.h"
#include "SGCoreImpl/SGSenseGloveSensorNormalizerImpl.h"
#include "SGCoreImpl/SGVect3DImpl.h"
#include "SGUtils/SGArrayUtils.h"

struct FSGSenseGloveImpl::FImpl
{
    /************************
     * Owner object
     ************************/

    FSGSenseGloveImpl* Owner;

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

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

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

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

TSharedRef<FSGDeviceImpl> FSGSenseGloveImpl::Parse(const FString& ConstantsString)
{
    std::string ConstantsStdString{TCHAR_TO_UTF8(*ConstantsString)};
    FSGDevicePtrType SGDevicePtr{FSenseGloveType::Parse(MoveTemp(ConstantsStdString))};
    ensureAlwaysMsgf(SGDevicePtr, TEXT("Invalid SGDevice!"));
    TSharedRef<FSGSenseGloveImpl> SenseGloveImpl{MakeShared<FSGSenseGloveImpl>(
        std::dynamic_pointer_cast<FSenseGloveType>(MoveTemp(SGDevicePtr)))};
    return MoveTemp(SenseGloveImpl);
}

TArray<FSGSenseGloveImpl> FSGSenseGloveImpl::GetSenseGloves()
{
    FSenseGloveVectorType Vector{FSenseGloveType::GetSenseGloves()};

    TArray<FSGSenseGloveImpl> Array{
        FSGArrayUtils::FromStdVector<FSenseGloveType, FSGSenseGloveImpl>(MoveTemp(Vector))
    };

    return MoveTemp(Array);
}

bool FSGSenseGloveImpl::GetSenseGlove(FSGSenseGloveImpl& OutGlove)
{
    FSenseGloveType Glove;
    const bool bResult = FSenseGloveType::GetSenseGlove(Glove);
    OutGlove = FSGSenseGloveImpl{MoveTemp(Glove)};
    return bResult;
}

bool FSGSenseGloveImpl::GetSenseGlove(const bool bRightHanded, FSGSenseGloveImpl& OutGlove)
{
    FSenseGloveType Glove;
    const bool bResult = FSenseGloveType::GetSenseGlove(bRightHanded, Glove);
    OutGlove = FSGSenseGloveImpl{MoveTemp(Glove)};
    return bResult;
}

void FSGSenseGloveImpl::CalculateGloveLocation(const FVector& ReferencePosition, const FQuat& ReferenceRotation,
                                               const ESGPositionalTrackingHardware TrackingHardware,
                                               const bool bRightHanded, const ESGFinger MountedOn,
                                               FVector& OutGlovePosition, FQuat& OutGloveRotation)
{
    FSGVect3DImpl::FVect3DType GlovePosition;
    FSGQuatImpl::FQuatType GloveRotation;

    FSenseGloveType::CalculateGloveLocation(
        FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(ReferencePosition),
        FSGQuatImpl{ReferenceRotation}.ToQuat(),
        FSGCoreEnumCast::ToEPositionalTrackingHardware(TrackingHardware),
        bRightHanded, FSGCoreEnumCast::ToEFinger(MountedOn),
        GlovePosition, GloveRotation);

    OutGlovePosition = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
        MoveTemp(GlovePosition));;
    OutGloveRotation = FSGQuatImpl{MoveTemp(GloveRotation)}.ToFQuat();
}

void FSGSenseGloveImpl::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)
{
    FSGVect3DImpl::FVect3DType WristPosition;
    FSGQuatImpl::FQuatType WristRotation;

    FSenseGloveType::CalculateWristLocation(
        FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(ReferencePosition),
        FSGQuatImpl{ReferenceRotation}.ToQuat(),
        FSGCoreEnumCast::ToEPositionalTrackingHardware(TrackingHardware),
        bRightHanded, FSGCoreEnumCast::ToEFinger(MountedOn),
        FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(GloveWristPositionOffset),
        FSGQuatImpl(GloveWristRotationOffset).ToQuat(),
        WristPosition, WristRotation);

    OutWristPosition = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
        MoveTemp(WristPosition));
    OutWristRotation = FSGQuatImpl{MoveTemp(WristRotation)}.ToFQuat();
}

ANSICHAR FSGSenseGloveImpl::GetHapticsByte()
{
    static const ANSICHAR Byte = FSenseGloveType::GetHapticsByte();
    return Byte;
}

ANSICHAR FSGSenseGloveImpl::GetThumperByte()
{
    static const ANSICHAR Byte = FSenseGloveType::GetThumperByte();
    return Byte;
}

const FVector& FSGSenseGloveImpl::GetDefaultThimbleOffset()
{
    static const FVector Offset{FSGVect3DImpl{FSenseGloveType::GetDefaultThimbleOffset()}.ToFVector()};
    return Offset;
}

FSGSenseGlovePoseImpl FSGSenseGloveImpl::CalculateGlovePose(const FSGSenseGloveSensorDataImpl& SensorData,
                                                            const FSGSenseGloveInfoImpl& GloveModel)
{
    FSGSenseGlovePoseImpl Pose{
        FSenseGloveType::CalculateGlovePose(SensorData.ToSenseGloveSensorData(), GloveModel.ToSenseGloveInfo())
    };
    return MoveTemp(Pose);
}

FSGSenseGlovePoseImpl FSGSenseGloveImpl::CalculateGlovePose(const TArray<TArray<FVector>>& GloveAngles,
                                                            const FSGSenseGloveInfoImpl& GloveModel)
{
    std::vector<std::vector<FSGVect3DImpl::FVect3DType>> GloveAnglesVector{
        FSGArrayUtils::ToSenseGloveAngles<FSGVect3DImpl::FVect3DType>(GloveAngles)
    };

    FSGSenseGlovePoseImpl Pose{
        FSenseGloveType::CalculateGlovePose(MoveTemp(GloveAnglesVector), GloveModel.ToSenseGloveInfo())
    };

    return MoveTemp(Pose);
}

void FSGSenseGloveImpl::NormalizeValues(
    const TArray<FVector>& SumAngles, const FSGSenseGloveSensorNormalizerImpl& Normalizer,
    TArray<float>& OutFlexions, TArray<float>& OutAbductions)
{
    std::vector<FSGVect3DImpl::FVect3DType> SumAnglesVector{
        FSGArrayUtils::ToSenseGloveAngles<FSGVect3DImpl::FVect3DType>(SumAngles)
    };

    std::vector<float> Flexions;
    std::vector<float> Abductions;

    FSenseGloveType::NormalizeValues(MoveTemp(SumAnglesVector), Normalizer.ToSenseGloveSensorNormalizer(),
                                     Flexions, Abductions);

    OutFlexions = FSGArrayUtils::FromStdVector(MoveTemp(Flexions));
    OutAbductions = FSGArrayUtils::FromStdVector(MoveTemp(Abductions));
}

void FSGSenseGloveImpl::CalculateHandAngles(
    const FSGSenseGlovePoseImpl& GlovePose,
    const FSGSenseGloveSensorNormalizerImpl& Normalizer, const FSGHandInterpolatorImpl& NormalizedToAngles,
    TArray<TArray<FVector>>& OutHandAngles)
{
    std::vector<std::vector<FSGVect3DImpl::FVect3DType>> HandAngles;

    FSenseGloveType::CalculateHandAngles(
        GlovePose.ToSenseGlovePose(),
        Normalizer.ToSenseGloveSensorNormalizer(), NormalizedToAngles.ToHandInterpolator(),
        HandAngles);

    OutHandAngles = FSGArrayUtils::FromSenseGloveAngles<FSGVect3DImpl::FVect3DType>(MoveTemp(HandAngles));
}

void FSGSenseGloveImpl::CalculateHandPose(
    const FSGSenseGlovePoseImpl& GlovePose, const FSGBasicHandModelImpl& HandModel,
    const FSGSenseGloveSensorNormalizerImpl& Normalizer, const FSGHandInterpolatorImpl& NormalizedToAngles,
    FSGHandPoseImpl& OutHandPose)
{
    FSGHandPoseImpl::FHandPoseType HandPose;

    FSenseGloveType::CalculateHandPose(
        GlovePose.ToSenseGlovePose(), HandModel.ToBasicHandModel(),
        Normalizer.ToSenseGloveSensorNormalizer(), NormalizedToAngles.ToHandInterpolator(),
        HandPose);

    OutHandPose = FSGHandPoseImpl{MoveTemp(HandPose)};
}

FSGSenseGloveImpl::FSGSenseGloveImpl()
    : FSGHapticGloveImpl(),
      Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
}

FSGSenseGloveImpl::FSGSenseGloveImpl(const FSGSenseGloveInfoImpl& Model)
    : FSGHapticGloveImpl(std::make_shared<FSenseGloveType>(Model.ToSenseGloveInfo())),
      Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
}

FSGSenseGloveImpl::FSGSenseGloveImpl(const FSenseGloveType SenseGlove)
    : FSGHapticGloveImpl(std::make_shared<FSenseGloveType>(SenseGlove)),
      Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
}

FSGSenseGloveImpl::FSGSenseGloveImpl(const FSenseGlovePtrType SenseGlove)
    : FSGHapticGloveImpl(std::dynamic_pointer_cast<FHapticGloveType>(SenseGlove)),
      Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
}

FSGSenseGloveImpl::FSGSenseGloveImpl(const FSGSenseGloveImpl& Rhs)
    : FSGHapticGloveImpl(Rhs),
      Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{*Rhs.Pimpl}, PimplDeleter))
{
    Pimpl->Owner = this;
}

FSGSenseGloveImpl::FSGSenseGloveImpl(FSGSenseGloveImpl&& Rhs) SG_NOEXCEPT = default;

FSGSenseGloveImpl::~FSGSenseGloveImpl() = default;

FSGSenseGloveImpl& FSGSenseGloveImpl::operator=(const FSGSenseGloveImpl& Rhs)
{
    FSGHapticGloveImpl::operator=(Rhs);
    *Pimpl = *Rhs.Pimpl;
    Pimpl->Owner = this;
    return *this;
}

FSGSenseGloveImpl& FSGSenseGloveImpl::operator=(FSGSenseGloveImpl&& Rhs) SG_NOEXCEPT = default;

const FSGSenseGloveImpl::FSenseGloveType& FSGSenseGloveImpl::ToSenseGlove() const
{
    return *ToSenseGlovePtrChecked();
}

FSGSenseGloveImpl::FSenseGlovePtrType FSGSenseGloveImpl::ToSenseGlovePtr() const
{
    return std::dynamic_pointer_cast<FSenseGloveType>(ToSGDevicePtr());
}

FSGSenseGloveImpl::FSenseGlovePtrType FSGSenseGloveImpl::ToSenseGlovePtrChecked() const
{
    FSenseGlovePtrType SenseGlove = std::dynamic_pointer_cast<FSenseGloveType>(ToSGDevicePtr());
    ensureAlwaysMsgf(SenseGlove.get(), TEXT("Invalid SenseGlove"));
    return SenseGlove;
}

ESGDeviceType FSGSenseGloveImpl::GetDeviceType() const
{
    return FSGCoreEnumCast::ToESGDeviceType(ToSenseGlovePtrChecked()->GetDeviceType());
}

FString FSGSenseGloveImpl::GetDeviceId() const
{
    FString Id{UTF8_TO_TCHAR(ToSenseGlovePtrChecked()->GetDeviceId().c_str())};
    return MoveTemp(Id);
}

FString FSGSenseGloveImpl::GetHardwareVersion() const
{
    FString Version{UTF8_TO_TCHAR(ToSenseGlovePtrChecked()->GetHardwareVersion().c_str())};
    return MoveTemp(Version);
}

int32 FSGSenseGloveImpl::GetFirmwareVersion() const
{
    return ToSenseGlovePtrChecked()->GetFirmwareVersion();
}

int32 FSGSenseGloveImpl::GetSubFirmwareVersion() const
{
    return ToSenseGlovePtrChecked()->GetSubFirmwareVersion();
}

bool FSGSenseGloveImpl::IsRight() const
{
    return ToSenseGlovePtr()->IsRight();
}

bool FSGSenseGloveImpl::GetImuRotation(FQuat& OutImu)
{
    FSGQuatImpl::FQuatType Imu;
    const bool bResult = ToSenseGlovePtrChecked()->GetImuRotation(Imu);
    OutImu = FSGQuatImpl{MoveTemp(Imu)}.ToFQuat();
    return bResult;
}

bool FSGSenseGloveImpl::GetHandPose(const FSGBasicHandModelImpl& HandGeometry, FSGHandPoseImpl& OutHandPose) const
{
    FSGHandPoseImpl::FHandPoseType HandPose;
    const bool bResult = ToSenseGlovePtrChecked()->GetHandPose(HandGeometry.ToBasicHandModel(), HandPose);
    OutHandPose = FSGHandPoseImpl{MoveTemp(HandPose)};
    return bResult;
}

bool FSGSenseGloveImpl::GetHandAngles(TArray<TArray<FVector>>& OutHandAngles) const
{
    std::vector<std::vector<FSGVect3DImpl::FVect3DType>> HandAngles;
    const bool bResult = ToSenseGlovePtrChecked()->GetHandAngles(HandAngles);
    OutHandAngles =
        FSGArrayUtils::FromStdVector<FSGVect3DImpl::FVect3DType, FVector, FSGVect3DImpl, &FSGVect3DImpl::ToFVector>(
            MoveTemp(HandAngles));
    return bResult;
}

ESGHapticGloveCalibrationState FSGSenseGloveImpl::GetCalibrationState() const
{
    return FSGCoreEnumCast::ToESGHapticGloveCalibrationState(ToSenseGlovePtrChecked()->GetCalibrationState());
}

void FSGSenseGloveImpl::EndCalibration()
{
    return ToSenseGlovePtrChecked()->EndCalibration();
}

void FSGSenseGloveImpl::ResetCalibration()
{
    return ToSenseGlovePtrChecked()->EndCalibration();
}

void FSGSenseGloveImpl::StopHaptics()
{
    ToSenseGlovePtrChecked()->StopHaptics();
}

void FSGSenseGloveImpl::StopVibrations()
{
    ToSenseGlovePtrChecked()->StopVibrations();
}

bool FSGSenseGloveImpl::SendHaptics()
{
    return ToSenseGlovePtrChecked()->SendHaptics();
}

bool FSGSenseGloveImpl::QueueForceFeedbackLevels(const TArray<float>& Levels01)
{
    std::vector<float> Levels01Vector{FSGArrayUtils::ToStdVector(Levels01)};
    return ToSenseGlovePtrChecked()->QueueForceFeedbackLevels(MoveTemp(Levels01Vector));
}

bool FSGSenseGloveImpl::QueueForceFeedbackLevel(const int32 Finger, const float Level01)
{
    return ToSenseGlovePtrChecked()->QueueForceFeedbackLevel(Finger, Level01);
}

void FSGSenseGloveImpl::GetGloveLocation(const FVector& ReferencePosition, const FQuat& ReferenceRotation,
                                         const ESGPositionalTrackingHardware TrackingHardware,
                                         FVector& OutGlovePosition, FQuat& OutGloveRotation) const
{
    FSGVect3DImpl::FVect3DType GlovePosition;
    FSGQuatImpl::FQuatType GloveRotation;

    ToSenseGlovePtrChecked()->GetGloveLocation(
        FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(ReferencePosition),
        FSGQuatImpl{ReferenceRotation}.ToQuat(),
        FSGCoreEnumCast::ToEPositionalTrackingHardware(TrackingHardware),
        GlovePosition, GloveRotation);

    OutGlovePosition = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
        MoveTemp(GlovePosition));
    OutGloveRotation = FSGQuatImpl{MoveTemp(GloveRotation)}.ToFQuat();
}

void FSGSenseGloveImpl::GetGloveLocation(const FVector& ReferencePosition, const FQuat& ReferenceRotation,
                                         const ESGPositionalTrackingHardware TrackingHardware,
                                         const ESGFinger MountedOn,
                                         FVector& OutGlovePosition, FQuat& OutGloveRotation) const
{
    FSGVect3DImpl::FVect3DType GlovePosition;
    FSGQuatImpl::FQuatType GloveRotation;

    ToSenseGlovePtrChecked()->GetGloveLocation(
        FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(ReferencePosition),
        FSGQuatImpl{ReferenceRotation}.ToQuat(),
        FSGCoreEnumCast::ToEPositionalTrackingHardware(TrackingHardware),
        FSGCoreEnumCast::ToEFinger(MountedOn),
        GlovePosition, GloveRotation);

    OutGlovePosition = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
        MoveTemp(GlovePosition));
    OutGloveRotation = FSGQuatImpl{MoveTemp(GloveRotation)}.ToFQuat();
}

void FSGSenseGloveImpl::GetWristLocation(const FVector& ReferencePosition, const FQuat& ReferenceRotation,
                                         const ESGPositionalTrackingHardware TrackingHardware,
                                         FVector& OutWristPosition, FQuat& OutWristRotation) const
{
    FSGVect3DImpl::FVect3DType WristPosition;
    FSGQuatImpl::FQuatType WristRotation;

    ToSenseGlovePtrChecked()->GetWristLocation(
        FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(ReferencePosition),
        FSGQuatImpl{ReferenceRotation}.ToQuat(),
        FSGCoreEnumCast::ToEPositionalTrackingHardware(TrackingHardware),
        WristPosition, WristRotation);

    OutWristPosition = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
        MoveTemp(WristPosition));
    OutWristRotation = FSGQuatImpl{MoveTemp(WristRotation)}.ToFQuat();
}

void FSGSenseGloveImpl::GetWristLocation(
    const FVector& ReferencePosition, const FQuat& ReferenceRotation,
    const ESGPositionalTrackingHardware TrackingHardware,
    const FVector& GloveWristPositionOffset, const FQuat& GloveWristRotationOffset,
    FVector& OutWristPosition, FQuat& OutWristRotation) const
{
    FSGVect3DImpl::FVect3DType WristPosition;
    FSGQuatImpl::FQuatType WristRotation;

    ToSenseGlovePtrChecked()->GetWristLocation(
        FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(ReferencePosition),
        FSGQuatImpl{ReferenceRotation}.ToQuat(),
        FSGCoreEnumCast::ToEPositionalTrackingHardware(TrackingHardware),
        FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(GloveWristPositionOffset),
        FSGQuatImpl(GloveWristRotationOffset).ToQuat(),
        WristPosition, WristRotation);

    OutWristPosition = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
        MoveTemp(WristPosition));
    OutWristRotation = FSGQuatImpl{MoveTemp(WristRotation)}.ToFQuat();
}

FSGSenseGloveInfoImpl FSGSenseGloveImpl::GetGloveInfo() const
{
    FSGSenseGloveInfoImpl GloveInfo{ToSenseGlovePtr()->GetGloveInfo()};
    return MoveTemp(GloveInfo);
}

TArray<FVector> FSGSenseGloveImpl::GetFingerThimbleOffsets() const
{
    TArray<FVector> Offsets{
        FSGArrayUtils::FromStdVector<FSGVect3DImpl::FVect3DType, FVector, FSGVect3DImpl, &FSGVect3DImpl::ToFVector>(
            ToSenseGlovePtrChecked()->GetFingerThimbleOffsets())
    };
    return MoveTemp(Offsets);
}

void FSGSenseGloveImpl::SetFingerThimbleOffset(const int32 Finger, const FVector& Offset)
{
    ToSenseGlovePtrChecked()->SetFingerThimbleOffset(Finger, FSGVect3DImpl{Offset}.ToVect3D());
}

bool FSGSenseGloveImpl::SupportsWristActuator() const
{
    return ToSenseGlovePtrChecked()->SupportsWristActuator();
}

bool FSGSenseGloveImpl::GetSensorData(FSGSenseGloveSensorDataImpl& OutSensorData) const
{
    FSGSenseGloveSensorDataImpl::FSenseGloveSensorDataType SensorData;
    const bool bResult = ToSenseGlovePtrChecked()->GetSensorData(SensorData);
    OutSensorData = FSGSenseGloveSensorDataImpl{MoveTemp(SensorData)};
    return bResult;
}

bool FSGSenseGloveImpl::GetGlovePose(FSGSenseGlovePoseImpl& OutGlovePose) const
{
    FSGSenseGlovePoseImpl::FSenseGlovePoseType GlovePose;
    const bool bResult = ToSenseGlovePtr()->GetGlovePose(GlovePose);
    OutGlovePose = FSGSenseGlovePoseImpl{GlovePose};
    return bResult;
}

bool FSGSenseGloveImpl::GetGlovePose(const FSGSenseGloveSensorDataImpl& SensorData,
                                     FSGSenseGlovePoseImpl& OutGlovePose) const
{
    FSGSenseGlovePoseImpl::FSenseGlovePoseType GlovePose;
    const bool bResult = ToSenseGlovePtr()->GetGlovePose(SensorData.ToSenseGloveSensorData(), GlovePose);
    OutGlovePose = FSGSenseGlovePoseImpl{GlovePose};
    return bResult;
}

bool FSGSenseGloveImpl::QueueVibroLevels(const TArray<float>& Amplitudes)
{
    std::vector<float> AmplitudesVector{FSGArrayUtils::ToStdVector(Amplitudes)};
    return ToSenseGlovePtrChecked()->QueueVibroLevels(MoveTemp(AmplitudesVector));
}

bool FSGSenseGloveImpl::QueueVibroLevel(const int32 Finger, const float Amplitude)
{
    return ToSenseGlovePtrChecked()->QueueVibroLevel(Finger, Amplitude);
}

bool FSGSenseGloveImpl::QueueThumperCommand(const ESGSenseGloveThumperCommand Command)
{
    return ToSenseGlovePtrChecked()->QueueThumperCommand(FSGCoreEnumCast::ToESenseGloveThumperCommand(Command));
}

FString FSGSenseGloveImpl::ToString() const
{
    FString String{UTF8_TO_TCHAR(ToSenseGlovePtrChecked()->ToString().c_str())};
    return MoveTemp(String);
}

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

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

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