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

#include <memory>
#include <vector>

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

#include "SGBuildHacks/SGInclude_Core_BasicHandModel.h"
#include "SGBuildHacks/SGInclude_Core_BuzzCommand.h"
#include "SGBuildHacks/SGInclude_Core_ForceFeedbackCommand.h"
#include "SGBuildHacks/SGInclude_Core_HandPose.h"
#include "SGBuildHacks/SGInclude_Core_HandProfile.h"
#include "SGBuildHacks/SGInclude_Core_HapticGlove.h"
#include "SGBuildHacks/SGInclude_Core_Quat.h"
#include "SGBuildHacks/SGInclude_Core_ThumperCommand.h"
#include "SGBuildHacks/SGInclude_Core_Vect3D.h"
#include "SGCoreImpl/SGBasicHandModelImpl.h"
#include "SGCoreImpl/SGBuzzCommandImpl.h"
#include "SGCoreImpl/SGCoreEnumCast.h"
#include "SGCoreImpl/SGForceFeedbackCommandImpl.h"
#include "SGCoreImpl/SGHandPoseImpl.h"
#include "SGCoreImpl/SGHandProfileImpl.h"
#include "SGCoreImpl/SGQuatImpl.h"
#include "SGCoreImpl/SGThumperCommandImpl.h"
#include "SGCoreImpl/SGVect3DImpl.h"
#include "SGUtils/SGArrayUtils.h"

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

    FSGHapticGloveImpl* Owner;

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

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

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

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

TArray<TSharedRef<FSGHapticGloveImpl>> FSGHapticGloveImpl::GetHapticGloves()
{
    const FHapticGlovePtrVectorType HapticGloves{FHapticGloveType::GetHapticGloves()};
    TArray<TSharedRef<FSGHapticGloveImpl>> HapticGloveImpls;

    for (FHapticGlovePtrType HapticGlove: HapticGloves)
    {
        TSharedRef<FSGHapticGloveImpl> HapticGloveImpl{MakeShared<FSGHapticGloveImpl>(MoveTemp(HapticGlove))};
        HapticGloveImpls.Push(MoveTemp(HapticGloveImpl));
    }

    return MoveTemp(HapticGloveImpls);
}

bool FSGHapticGloveImpl::GetGlove(TSharedPtr<FSGHapticGloveImpl>& OutGlove)
{
    FHapticGlovePtrType Glove;
    const bool bReturn = FHapticGloveType::GetGlove(Glove);

    if (bReturn)
    {
        OutGlove = MakeShared<FSGHapticGloveImpl>(MoveTemp(Glove));
    }

    return bReturn;
}

bool FSGHapticGloveImpl::GetGlove(const bool bRightHanded, TSharedPtr<FSGHapticGloveImpl>& OutGlove)
{
    FHapticGlovePtrType Glove;
    const bool bReturn = FHapticGloveType::GetGlove(bRightHanded, Glove);

    if (bReturn)
    {
        OutGlove = MakeShared<FSGHapticGloveImpl>(MoveTemp(Glove));
    }

    return bReturn;
}

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

FSGHapticGloveImpl::FSGHapticGloveImpl(const FHapticGlovePtrType HapticGlove)
    : FSGDeviceImpl(std::dynamic_pointer_cast<FSGDeviceType>(HapticGlove)),
      Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
}

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

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

FSGHapticGloveImpl::~FSGHapticGloveImpl() = default;

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

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

FSGHapticGloveImpl::FHapticGlovePtrType FSGHapticGloveImpl::ToHapticGlovePtr() const
{
    return std::dynamic_pointer_cast<FHapticGloveType>(ToSGDevicePtr());
}

FSGHapticGloveImpl::FHapticGlovePtrType FSGHapticGloveImpl::ToHapticGlovePtrChecked() const
{
    FHapticGlovePtrType HapticGlove = std::dynamic_pointer_cast<FHapticGloveType>(ToSGDevicePtr());
    ensureAlwaysMsgf(HapticGlove.get(), TEXT("Invalid HapticGlove"));
    return HapticGlove;
}

ESGDeviceType FSGHapticGloveImpl::GetDeviceType() const
{
    return FSGCoreEnumCast::ToESGDeviceType(ToHapticGlovePtrChecked()->GetDeviceType());
}

FString FSGHapticGloveImpl::GetDeviceId() const
{
    const FString Id{UTF8_TO_TCHAR(ToHapticGlovePtrChecked()->GetDeviceId().c_str())};
    return Id;
}

FString FSGHapticGloveImpl::GetHardwareVersion() const
{
    const FString Version{UTF8_TO_TCHAR(ToHapticGlovePtrChecked()->GetHardwareVersion().c_str())};
    return Version;
}

int32 FSGHapticGloveImpl::GetFirmwareVersion() const
{
    return ToHapticGlovePtrChecked()->GetFirmwareVersion();
}

int32 FSGHapticGloveImpl::GetSubFirmwareVersion() const
{
    return ToHapticGlovePtrChecked()->GetSubFirmwareVersion();
}

bool FSGHapticGloveImpl::IsRight() const
{
    return ToHapticGlovePtrChecked()->IsRight();
}

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

bool FSGHapticGloveImpl::GetHandPose(const FSGBasicHandModelImpl& HandGeometry, const FSGHandProfileImpl& HandProfile,
                                     FSGHandPoseImpl& OutHandPose)
{
    FSGHandPoseImpl::FHandPoseType HandPose;
    const bool bResult = ToHapticGlovePtrChecked()->GetHandPose(HandGeometry.ToBasicHandModel(),
                                                                HandProfile.ToHandProfile(), HandPose);
    OutHandPose = FSGHandPoseImpl{MoveTemp(HandPose)};
    return bResult;
}

bool FSGHapticGloveImpl::GetHandPose(const FSGHandProfileImpl& HandProfile, FSGHandPoseImpl& OutHandPose)
{
    FSGHandPoseImpl::FHandPoseType HandPose;
    const bool bResult = ToHapticGlovePtrChecked()->GetHandPose(HandProfile.ToHandProfile(), HandPose);
    OutHandPose = FSGHandPoseImpl{MoveTemp(HandPose)};
    return bResult;
}

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

bool FSGHapticGloveImpl::GetHandPose(FSGHandPoseImpl& OutHandPose)
{
    FSGHandPoseImpl::FHandPoseType HandPose;
    const bool bResult = ToHapticGlovePtrChecked()->GetHandPose(HandPose);
    OutHandPose = FSGHandPoseImpl{MoveTemp(HandPose)};
    return bResult;
}

#if PLATFORM_ANDROID
bool FSGHapticGloveImpl::GetHandPose(const float DeltaTime, FSGHandPoseImpl& OutHandPose)
{
    FSGHandPoseImpl::FHandPoseType HandPose;
    const bool bResult = ToHapticGlovePtrChecked()->GetHandPose(DeltaTime, HandPose);
    OutHandPose = FSGHandPoseImpl{MoveTemp(HandPose)};
    return bResult;
}
#endif  /* PLATFORM_ANDROID */

void FSGHapticGloveImpl::StopHaptics()
{
    ToHapticGlovePtrChecked()->StopHaptics();
}

void FSGHapticGloveImpl::StopVibrations()
{
    ToHapticGlovePtrChecked()->StopVibrations();
}

void FSGHapticGloveImpl::SendHaptics(const FSGForceFeedbackCommandImpl& ForceFeedbackCommand)
{
    ToHapticGlovePtrChecked()->SendHaptics(ForceFeedbackCommand.ToForceFeedbackCommand());
}

void FSGHapticGloveImpl::SendHaptics(const FSGBuzzCommandImpl& BuzzCommand)
{
    ToHapticGlovePtrChecked()->SendHaptics(BuzzCommand.ToBuzzCommand());
}

void FSGHapticGloveImpl::SendHaptics(const FSGThumperCommandImpl& ThumperCommand)
{
    ToHapticGlovePtrChecked()->SendHaptics(ThumperCommand.ToThumperCommand());
}

void FSGHapticGloveImpl::SendHaptics(const FSGForceFeedbackCommandImpl& ForceFeedbackCommand,
                                     const FSGBuzzCommandImpl& BuzzCommand)
{
    ToHapticGlovePtrChecked()->SendHaptics(ForceFeedbackCommand.ToForceFeedbackCommand(),
                                           BuzzCommand.ToBuzzCommand());
}

void FSGHapticGloveImpl::SendHaptics(const FSGForceFeedbackCommandImpl& ForceFeedbackCommand,
                                     const FSGBuzzCommandImpl& BuzzCommand, const FSGThumperCommandImpl& ThumperCommand)
{
    ToHapticGlovePtrChecked()->SendHaptics(ForceFeedbackCommand.ToForceFeedbackCommand(),
                                           BuzzCommand.ToBuzzCommand(),
                                           ThumperCommand.ToThumperCommand());
}

bool FSGHapticGloveImpl::GetCalibrationValues(TArray<FVector>& OutValues)
{
    std::vector<FSGVect3DImpl::FVect3DType> Values;
    const bool bResult = ToHapticGlovePtrChecked()->GetCalibrationValues(Values);
    OutValues = FSGArrayUtils::FromStdVector<FSGVect3DImpl::FVect3DType, FVector,
                                             FSGVect3DImpl, &FSGVect3DImpl::ToFVector>(MoveTemp(Values));
    return bResult;
}

void FSGHapticGloveImpl::ResetCalibrationRange()
{
    ToHapticGlovePtrChecked()->ResetCalibrationRange();
}

void FSGHapticGloveImpl::UpdateCalibrationRange()
{
    ToHapticGlovePtrChecked()->UpdateCalibrationRange();
}

void FSGHapticGloveImpl::UpdateCalibrationRange(const TArray<FVector>& CalibrationValues)
{
    std::vector<FSGVect3DImpl::FVect3DType> CalibrationValuesVector{
        FSGArrayUtils::ToStdVector<FVector, FSGVect3DImpl::FVect3DType, FSGVect3DImpl, &FSGVect3DImpl::ToVect3D>(
            CalibrationValues)
    };
    ToHapticGlovePtrChecked()->UpdateCalibrationRange(MoveTemp(CalibrationValuesVector));
}

void FSGHapticGloveImpl::GetCalibrationRange(TArray<FVector>& OutMinValues, TArray<FVector>& OutMaxValues)
{
    std::vector<FSGVect3DImpl::FVect3DType> MinValues;
    std::vector<FSGVect3DImpl::FVect3DType> MaxValues;

    ToHapticGlovePtrChecked()->GetCalibrationRange(MinValues, MaxValues);

    OutMinValues = FSGArrayUtils::FromStdVector<FSGVect3DImpl::FVect3DType, FVector,
                                                FSGVect3DImpl, &FSGVect3DImpl::ToFVector>(MoveTemp(MinValues));
    OutMaxValues = FSGArrayUtils::FromStdVector<FSGVect3DImpl::FVect3DType, FVector,
                                                FSGVect3DImpl, &FSGVect3DImpl::ToFVector>(MoveTemp(MaxValues));
}

void FSGHapticGloveImpl::UpdateCalibration(FSGHandProfileImpl& OutProfile)
{
    FSGHandProfileImpl::FHandProfileType Profile;
    ToHapticGlovePtrChecked()->UpdateCalibration(Profile);
    OutProfile = FSGHandProfileImpl(MoveTemp(Profile));
}

void FSGHapticGloveImpl::ApplyCalibration(FSGHandProfileImpl& OutProfile) const
{
    FSGHandProfileImpl::FHandProfileType Profile;
    ToHapticGlovePtrChecked()->ApplyCalibration(Profile);
    OutProfile = FSGHandProfileImpl(MoveTemp(Profile));
}

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

    ToHapticGlovePtrChecked()->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 FSGHapticGloveImpl::GetGloveLocation(const FVector& ReferencePosition, const FQuat& ReferenceRotation,
                                          ESGPositionalTrackingHardware TrackingHardware, FVector& OutGlovePosition,
                                          FQuat& OutGloveRotation) const
{
    FSGVect3DImpl::FVect3DType GlovePosition;
    FSGQuatImpl::FQuatType GloveRotation;

    ToHapticGlovePtrChecked()->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();
}

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

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

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