Plugin/Source/SenseGloveCoreImpl/Private/SGCoreImpl/SGHapticGloveCalibrationSequenceImpl.cpp

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

#include <string>
#include <vector>

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

#include "SGBuildHacks/SGInclude_Core_CalibrationDataPoint.h"
#include "SGBuildHacks/SGInclude_Core_HandPose.h"
#include "SGBuildHacks/SGInclude_Core_HandProfile.h"
#include "SGBuildHacks/SGInclude_Core_HapticGlove.h"
#include "SGBuildHacks/SGInclude_Core_HapticGloveCalibrationSequence.h"
#include "SGBuildHacks/SGInclude_Core_HapticGloveQuickCalibration.h"
#include "SGBuildHacks/SGInclude_Core_SensorRange.h"
#include "SGBuildHacks/SGInclude_Core_Vect3D.h"
#include "SGCoreImpl/SGCalibrationDataPointImpl.h"
#include "SGCoreImpl/SGCoreEnumCast.h"
#include "SGCoreImpl/SGHandPoseImpl.h"
#include "SGCoreImpl/SGHandProfileImpl.h"
#include "SGCoreImpl/SGHapticGloveImpl.h"
#include "SGCoreImpl/SGHapticGloveQuickCalibrationImpl.h"
#include "SGCoreImpl/SGSensorRangeImpl.h"
#include "SGCoreImpl/SGVect3DImpl.h"
#include "SGUtils/SGArrayUtils.h"
#include "Templates/Casts.h"

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

    static TSharedPtr<FHapticGloveCalibrationSequenceType> MakeNewHapticGloveCalibrationSequence(
        TSharedPtr<FHapticGloveCalibrationSequenceType> Ptr);

    /************************
     * Member variables
     ************************/

    TSharedPtr<FHapticGloveCalibrationSequenceType> HapticGloveCalibrationSequence;

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

    FSGHapticGloveCalibrationSequenceImpl* Owner;

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

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

    /************************
     * The copy constructor & copy assignment operator
     ************************/

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

int32 FSGHapticGloveCalibrationSequenceImpl::GetMaxDataPoints()
{
    return FHapticGloveCalibrationSequenceType::GetMaxDataPoints();
}

bool FSGHapticGloveCalibrationSequenceImpl::CompileProfile(const FSGSensorRangeImpl& SensorRange,
                                                           const ESGDeviceType ForDevice, const bool bRightHanded,
                                                           FSGHandProfileImpl& OutProfile)
{
    FSGHandProfileImpl::FHandProfileType Profile;
    const bool bResult = FHapticGloveCalibrationSequenceType::CompileProfile(
        SensorRange.ToSensorRange(), FSGCoreEnumCast::ToEDeviceType(ForDevice), bRightHanded, Profile);
    OutProfile = FSGHandProfileImpl{Profile};
    return bResult;
}

FSGHapticGloveCalibrationSequenceImpl::FSGHapticGloveCalibrationSequenceImpl()
    :
#if ENGINE_MAJOR_VERSION == 5 || ( ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION > 22 )
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
#else
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}))
#endif  /* ENGINE_MAJOR_VERSION == 5 || ( ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION > 22 ) */
{
}

FSGHapticGloveCalibrationSequenceImpl::FSGHapticGloveCalibrationSequenceImpl(
    const TSharedRef<FSGHapticGloveImpl> LinkedGlove)
    :
#if ENGINE_MAJOR_VERSION == 5 || ( ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION > 22 )
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
#else
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}))
#endif  /* ENGINE_MAJOR_VERSION == 5 || ( ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION > 22 ) */
{
    Pimpl->HapticGloveCalibrationSequence = MakeShared<FHapticGloveCalibrationSequenceType>(
        LinkedGlove->ToHapticGlovePtrChecked());
}

FSGHapticGloveCalibrationSequenceImpl::FSGHapticGloveCalibrationSequenceImpl(
    const FHapticGloveCalibrationSequenceType& HapticGloveCalibrationSequence)
    :
#if ENGINE_MAJOR_VERSION == 5 || ( ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION > 22 )
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
#else
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}))
#endif  /* ENGINE_MAJOR_VERSION == 5 || ( ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION > 22 ) */
{
    Pimpl->HapticGloveCalibrationSequence = MakeShared<FHapticGloveCalibrationSequenceType>(
        HapticGloveCalibrationSequence);
}

FSGHapticGloveCalibrationSequenceImpl::FSGHapticGloveCalibrationSequenceImpl(
    TSharedRef<FHapticGloveCalibrationSequenceType> HapticGloveCalibrationSequence)
    :
#if ENGINE_MAJOR_VERSION == 5 || ( ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION > 22 )
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
#else
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}))
#endif  /* ENGINE_MAJOR_VERSION == 5 || ( ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION > 22 ) */
{
    Pimpl->HapticGloveCalibrationSequence =
        Pimpl->MakeNewHapticGloveCalibrationSequence(HapticGloveCalibrationSequence);
}

FSGHapticGloveCalibrationSequenceImpl::FSGHapticGloveCalibrationSequenceImpl(
    const FSGHapticGloveCalibrationSequenceImpl& Rhs)
    :
#if ENGINE_MAJOR_VERSION == 5 || ( ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION > 22 )
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{*Rhs.Pimpl}, PimplDeleter))
#else
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{*Rhs.Pimpl}))
#endif  /* ENGINE_MAJOR_VERSION == 5 || ( ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION > 22 ) */
{
    Pimpl->Owner = this;
}

FSGHapticGloveCalibrationSequenceImpl::FSGHapticGloveCalibrationSequenceImpl(
    FSGHapticGloveCalibrationSequenceImpl&& Rhs) noexcept = default;

FSGHapticGloveCalibrationSequenceImpl::~FSGHapticGloveCalibrationSequenceImpl() = default;

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

FSGHapticGloveCalibrationSequenceImpl& FSGHapticGloveCalibrationSequenceImpl::operator=(
    FSGHapticGloveCalibrationSequenceImpl&& Rhs) noexcept = default;

const FSGHapticGloveCalibrationSequenceImpl::FHapticGloveCalibrationSequenceType&
FSGHapticGloveCalibrationSequenceImpl::ToHapticGloveCalibrationSequence() const
{
    return *Pimpl->HapticGloveCalibrationSequence;
}

TSharedRef<FSGHapticGloveCalibrationSequenceImpl::FHapticGloveCalibrationSequenceType>
FSGHapticGloveCalibrationSequenceImpl::ToHapticGloveCalibrationSequenceRef() const
{
    return Pimpl->HapticGloveCalibrationSequence.ToSharedRef();
}

TSharedRef<FSGHapticGloveCalibrationSequenceImpl> FSGHapticGloveCalibrationSequenceImpl::
ToHapticGloveCalibrationSequenceImplRef()
{
    return SharedThis(this);
}

TSharedRef<FSGHapticGloveQuickCalibrationImpl> FSGHapticGloveCalibrationSequenceImpl::
ToHapticGloveQuickCalibrationImplRef()
{
    FSGHapticGloveQuickCalibrationImpl* HapticGloveQuickCalibration{
        dynamic_cast<FSGHapticGloveQuickCalibrationImpl*>(this)};
    ensureAlwaysMsgf(HapticGloveQuickCalibration, TEXT("Invalid HapticGloveQuickCalibration!"));
    return SharedThis(HapticGloveQuickCalibration);
}

TSharedRef<FSGHapticGloveImpl> FSGHapticGloveCalibrationSequenceImpl::GetLinkedGlove() const
{
    FSGHapticGloveImpl::FHapticGlovePtrType Glove{Pimpl->HapticGloveCalibrationSequence->GetLinkedGlove()};
    return MakeShared<FSGHapticGloveImpl>(MoveTemp(Glove));
}

void FSGHapticGloveCalibrationSequenceImpl::SetLinkedGlove(const TSharedRef<FSGHapticGloveImpl> LinkedGlove)
{
    Pimpl->HapticGloveCalibrationSequence->SetLinkedGlove(LinkedGlove->ToHapticGlovePtrChecked());
}

TArray<FSGCalibrationDataPointImpl> FSGHapticGloveCalibrationSequenceImpl::GetCalibrationPoints() const
{
    const TArray<FSGCalibrationDataPointImpl> CalibrationPoints{
        FSGArrayUtils::FromStdVector<FSGCalibrationDataPointImpl::FCalibrationDataPointType,
                                     FSGCalibrationDataPointImpl>(
            Pimpl->HapticGloveCalibrationSequence->GetCalibrationPoints())
    };
    return CalibrationPoints;
}

float FSGHapticGloveCalibrationSequenceImpl::GetElapsedTime() const
{
    return Pimpl->HapticGloveCalibrationSequence->GetElapsedTime();
}

bool FSGHapticGloveCalibrationSequenceImpl::IsManuallyCompleted() const
{
    return Pimpl->HapticGloveCalibrationSequence->IsManuallyCompleted();
}

int32 FSGHapticGloveCalibrationSequenceImpl::GetCurrentStage() const
{
    return Pimpl->HapticGloveCalibrationSequence->GetCurrentStage();
}

SIZE_T FSGHapticGloveCalibrationSequenceImpl::GetCalibrationPointsCount() const
{
    return Pimpl->HapticGloveCalibrationSequence->GetCalibrationPointsCount();
}

bool FSGHapticGloveCalibrationSequenceImpl::AutoCompleted() const
{
    return Pimpl->HapticGloveCalibrationSequence->AutoCompleted();
}

bool FSGHapticGloveCalibrationSequenceImpl::Completed() const
{
    return Pimpl->HapticGloveCalibrationSequence->Completed();
}

void FSGHapticGloveCalibrationSequenceImpl::Reset()
{
    Pimpl->HapticGloveCalibrationSequence.Reset();
}

void FSGHapticGloveCalibrationSequenceImpl::Update(const float DeltaSeconds)
{
    return Pimpl->HapticGloveCalibrationSequence->Update(DeltaSeconds);
}

void FSGHapticGloveCalibrationSequenceImpl::AddDataPoint(const TArray<FVector>& CalibrationData)
{
    std::vector<FSGVect3DImpl::FVect3DType> CalibrationDataVector{
        FSGArrayUtils::ToStdVector<FVector, FSGVect3DImpl::FVect3DType, FSGVect3DImpl, &FSGVect3DImpl::ToVect3D>(
            CalibrationData)
    };
    Pimpl->HapticGloveCalibrationSequence->AddDataPoint(MoveTemp(CalibrationDataVector));
}

void FSGHapticGloveCalibrationSequenceImpl::ConfirmCurrentStep()
{
    Pimpl->HapticGloveCalibrationSequence->ConfirmCurrentStep();
}

bool FSGHapticGloveCalibrationSequenceImpl::CompileRange(FSGSensorRangeImpl& OutSensorRange) const
{
    FSGSensorRangeImpl::FSensorRangeType SensorRange;
    const bool bResult = Pimpl->HapticGloveCalibrationSequence->CompileRange(SensorRange);
    OutSensorRange = FSGSensorRangeImpl{SensorRange};
    return bResult;
}

bool FSGHapticGloveCalibrationSequenceImpl::CompileProfile(const ESGDeviceType ForDevice, const bool bRightHanded,
                                                           FSGHandProfileImpl& OutProfile) const
{
    FSGHandProfileImpl::FHandProfileType Profile;
    const bool bResult = Pimpl->HapticGloveCalibrationSequence->CompileProfile(
        FSGCoreEnumCast::ToEDeviceType(ForDevice), bRightHanded, Profile);
    OutProfile = FSGHandProfileImpl{Profile};
    return bResult;
}

bool FSGHapticGloveCalibrationSequenceImpl::GetHandPose(FSGHandPoseImpl& OutCurrentHandPose) const
{
    FSGHandPoseImpl::FHandPoseType CurrentHandPose;
    const bool bResult = Pimpl->HapticGloveCalibrationSequence->GetHandPose(CurrentHandPose);
    OutCurrentHandPose = FSGHandPoseImpl{CurrentHandPose};
    return bResult;
}

bool FSGHapticGloveCalibrationSequenceImpl::GetHandPose(const bool bRightHanded,
                                                        FSGHandPoseImpl& OutCurrentHandPose) const
{
    FSGHandPoseImpl::FHandPoseType CurrentHandPose;
    const bool bResult = Pimpl->HapticGloveCalibrationSequence->GetHandPose(bRightHanded, CurrentHandPose);
    OutCurrentHandPose = FSGHandPoseImpl{CurrentHandPose};
    return bResult;
}

FString FSGHapticGloveCalibrationSequenceImpl::GetCurrentInstruction(const FString& NextStepKey) const
{
    std::string NextStepKeyString{TCHAR_TO_UTF8(*NextStepKey)};
    const FString CurrentInstruction{TCHAR_TO_UTF8(Pimpl->HapticGloveCalibrationSequence->GetCurrentInstruction(
        MoveTemp(NextStepKeyString)).c_str())};
    return CurrentInstruction;
}

TSharedPtr<FSGHapticGloveCalibrationSequenceImpl::FHapticGloveCalibrationSequenceType>
FSGHapticGloveCalibrationSequenceImpl::FImpl::MakeNewHapticGloveCalibrationSequence(
    const TSharedPtr<FHapticGloveCalibrationSequenceType> Ptr)
{
    {
        FSGHapticGloveQuickCalibrationImpl::FHapticGloveQuickCalibrationType* HapticGloveQuickCalibration{
            dynamic_cast<FSGHapticGloveQuickCalibrationImpl::FHapticGloveQuickCalibrationType*>(
                Ptr.Get())
        };
        if (HapticGloveQuickCalibration)
        {
            return MakeShared<FSGHapticGloveQuickCalibrationImpl::FHapticGloveQuickCalibrationType>(
                *HapticGloveQuickCalibration);
        }
    }

    return MakeShared<FHapticGloveCalibrationSequenceType>(*Ptr);
}

FSGHapticGloveCalibrationSequenceImpl::FImpl::FImpl(FSGHapticGloveCalibrationSequenceImpl* InOwner)
    : HapticGloveCalibrationSequence(MakeShared<FHapticGloveCalibrationSequenceType>()),
      Owner(InOwner)
{
}

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

FSGHapticGloveCalibrationSequenceImpl::FImpl::FImpl(const FImpl& Rhs)
    : HapticGloveCalibrationSequence(MakeNewHapticGloveCalibrationSequence(Rhs.HapticGloveCalibrationSequence))
{
}

FSGHapticGloveCalibrationSequenceImpl::FImpl& FSGHapticGloveCalibrationSequenceImpl::FImpl::operator=(const FImpl& Rhs)
{
    HapticGloveCalibrationSequence = MakeNewHapticGloveCalibrationSequence(Rhs.HapticGloveCalibrationSequence);
    return *this;
}

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