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

#include <string>
#include <vector>

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

#include "SGBuildHacks/SGInclude_Core_BasicHandModel.h"
#include "SGBuildHacks/SGInclude_Core_Vect3D.h"
#include "SGCoreImpl/SGCoreEnumCast.h"
#include "SGCoreImpl/SGQuatImpl.h"
#include "SGCoreImpl/SGVect3DImpl.h"
#include "SGUtils/SGArrayUtils.h"
#include "SGUtils/SGUnits.h"

struct FSGBasicHandModelImpl::FImpl
{
    /************************
     * Member variables
     ************************/

    FBasicHandModelType BasicHandModel;

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

    FSGBasicHandModelImpl* Owner;

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

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

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

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

FSGBasicHandModelImpl FSGBasicHandModelImpl::Default(const bool bRightHanded)
{
    return FSGBasicHandModelImpl(FSGBasicHandModelImpl::FBasicHandModelType::Default(bRightHanded));
}

FSGBasicHandModelImpl FSGBasicHandModelImpl::Deserialize(const FString& SerializedString)
{
    std::string Serialized(TCHAR_TO_UTF8(*SerializedString));
    const FSGBasicHandModelImpl BasicHandModel(FBasicHandModelType::Deserialize(MoveTemp(Serialized)));
    return BasicHandModel;
}

const TArray<TArray<float>>& FSGBasicHandModelImpl::GetBaseFingerLengths()
{
    static const TArray<TArray<float>> BaseFingerLengths{
        FSGArrayUtils::FromMillimeters(
            FSGBasicHandModelImpl::FBasicHandModelType::GetBaseFingerLengths())
    };
    return BaseFingerLengths;
}

const TArray<FVector>& FSGBasicHandModelImpl::GetBaseJointPositions()
{
    static const TArray<FVector> BaseJointPositions{
        FSGArrayUtils::FromMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FBasicHandModelType::GetBaseJointPositions())
    };
    return BaseJointPositions;
}

const TArray<FVector>& FSGBasicHandModelImpl::GetBaseJointAngles()
{
    static const TArray<FVector> BaseJointAngles{
        FSGArrayUtils::FromStdVector<FSGVect3DImpl::FVect3DType, FVector, FSGVect3DImpl, &FSGVect3DImpl::ToFVector>(
            FBasicHandModelType::GetBaseJointAngles())
    };
    return BaseJointAngles;
}

FSGBasicHandModelImpl::FSGBasicHandModelImpl()
    :
#if SG_CPP17
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
#else
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}))
#endif  /* SG_CPP17 */
{
}

FSGBasicHandModelImpl::FSGBasicHandModelImpl(const bool bRightHanded, const TArray<TArray<float>>& Lengths,
                                             const TArray<FVector>& StartPositions)
    :
#if SG_CPP17
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
#else
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}))
#endif  /* SG_CPP17 */
{
    std::vector<std::vector<float>> LengthsVector{
        FSGArrayUtils::ToMillimeters(Lengths)
    };

    std::vector<FSGVect3DImpl::FVect3DType> StartPositionsVector{
        FSGArrayUtils::ToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(StartPositions)
    };

    Pimpl->BasicHandModel = FSGBasicHandModelImpl::FBasicHandModelType(bRightHanded, MoveTemp(LengthsVector),
                                                                       MoveTemp(StartPositionsVector));
}

FSGBasicHandModelImpl::FSGBasicHandModelImpl(const bool bRightHanded, const TArray<TArray<float>>& Lengths,
                                             const TArray<FVector>& StartPositions, const TArray<FQuat>& StartRotations)
    :
#if SG_CPP17
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
#else
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}))
#endif  /* SG_CPP17 */
{
    std::vector<std::vector<float>> LengthsVector{
        FSGArrayUtils::ToMillimeters(Lengths)
    };

    std::vector<FSGVect3DImpl::FVect3DType> StartPositionsVector{
        FSGArrayUtils::ToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(StartPositions)
    };

    std::vector<FSGQuatImpl::FQuatType> StartRotationsVector{
        FSGArrayUtils::ToStdVector<FQuat, FSGQuatImpl::FQuatType, FSGQuatImpl, &FSGQuatImpl::ToQuat>(StartRotations)
    };

    Pimpl->BasicHandModel = FSGBasicHandModelImpl::FBasicHandModelType(bRightHanded, MoveTemp(LengthsVector),
                                                                       MoveTemp(StartPositionsVector),
                                                                       MoveTemp(StartRotationsVector));
}

FSGBasicHandModelImpl::FSGBasicHandModelImpl(const FBasicHandModelType& BasicHandModel)
    :
#if SG_CPP17
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
#else
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}))
#endif  /* SG_CPP17 */
{
    Pimpl->BasicHandModel = BasicHandModel;
}

FSGBasicHandModelImpl::FSGBasicHandModelImpl(const FSGBasicHandModelImpl& Rhs)
    :
#if SG_CPP17
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{*Rhs.Pimpl}, PimplDeleter))
#else
    Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{*Rhs.Pimpl}))
#endif  /* SG_CPP17 */
{
    Pimpl->Owner = this;
}

FSGBasicHandModelImpl::FSGBasicHandModelImpl(FSGBasicHandModelImpl&& Rhs)
#if SG_CPP17
    noexcept
#endif  /* SG_CPP17 */
    = default;

FSGBasicHandModelImpl::~FSGBasicHandModelImpl() = default;

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

FSGBasicHandModelImpl& FSGBasicHandModelImpl::operator=(FSGBasicHandModelImpl&& Rhs)
#if SG_CPP17
    noexcept
#endif  /* SG_CPP17 */
    = default;

const FSGBasicHandModelImpl::FBasicHandModelType& FSGBasicHandModelImpl::ToBasicHandModel() const
{
    return Pimpl->BasicHandModel;
}

bool FSGBasicHandModelImpl::IsRight() const
{
    return Pimpl->BasicHandModel.IsRight();
}

void FSGBasicHandModelImpl::SetIsRight(const bool bRightHanded)
{
    Pimpl->BasicHandModel.SetIsRight(bRightHanded);
}

TArray<TArray<float>> FSGBasicHandModelImpl::GetFingerLengths() const
{
    TArray<TArray<float>> FingerLengths{
        FSGArrayUtils::FromMillimeters(Pimpl->BasicHandModel.GetFingerLengths())
    };
    return MoveTemp(FingerLengths);
}

void FSGBasicHandModelImpl::SetFingerLengths(const TArray<TArray<float>>& FingerLengths)
{
    std::vector<std::vector<float>> FingerLengthsVector{
        FSGArrayUtils::ToMillimeters(FingerLengths)
    };
    Pimpl->BasicHandModel.SetFingerLengths(MoveTemp(FingerLengthsVector));
}

TArray<float> FSGBasicHandModelImpl::GetFingerLengths(const ESGFinger Finger) const
{
    TArray<float> FingerLengths{
        FSGArrayUtils::FromMillimeters(Pimpl->BasicHandModel.GetFingerLengths(FSGCoreEnumCast::ToEFinger(Finger)))
    };
    return MoveTemp(FingerLengths);
}

void FSGBasicHandModelImpl::SetFingerLengths(const ESGFinger Finger, const TArray<float>& Lengths)
{
    std::vector<float> LengthsVector{
        FSGArrayUtils::ToMillimeters(Lengths)
    };
    Pimpl->BasicHandModel.SetFingerLengths(FSGCoreEnumCast::ToEFinger(Finger), MoveTemp(LengthsVector));
}

void FSGBasicHandModelImpl::SetFingerLengths(const uint8 Finger, const TArray<float>& Lengths)
{
    std::vector<float> LengthsVector{
        FSGArrayUtils::ToMillimeters(Lengths)
    };
    Pimpl->BasicHandModel.SetFingerLengths(Finger, MoveTemp(LengthsVector));
}

TArray<TArray<float>> FSGBasicHandModelImpl::GetFingerRatios() const
{
    TArray<TArray<float>> FingerRatios{
        FSGArrayUtils::FromStdVector<float>(Pimpl->BasicHandModel.GetFingerRatios())
    };
    return MoveTemp(FingerRatios);
}

TArray<float> FSGBasicHandModelImpl::GetFingerRatios(const ESGFinger Finger) const
{
    TArray<float> FingerRatios{
        FSGArrayUtils::FromStdVector<float>(Pimpl->BasicHandModel.GetFingerRatios(FSGCoreEnumCast::ToEFinger(Finger)))
    };
    return MoveTemp(FingerRatios);
}

TArray<FVector> FSGBasicHandModelImpl::GetStartJointPositions() const
{
    TArray<FVector> StartJointPositions{
        FSGArrayUtils::FromMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            Pimpl->BasicHandModel.GetStartJointPositions())
    };
    return MoveTemp(StartJointPositions);
}

void FSGBasicHandModelImpl::SetStartJointPositions(const TArray<FVector>& StartJointPositions)
{
    std::vector<FSGVect3DImpl::FVect3DType> StartJointPositionsVector{
        FSGArrayUtils::ToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(StartJointPositions)
    };
    Pimpl->BasicHandModel.SetStartJointPositions(MoveTemp(StartJointPositionsVector));
}

void FSGBasicHandModelImpl::SetStartJointPosition(const ESGFinger Finger, const FVector& Position)
{
    Pimpl->BasicHandModel.SetStartJointPosition(
        FSGCoreEnumCast::ToEFinger(Finger),
        FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(Position));
}

void FSGBasicHandModelImpl::SetStartJointPosition(const uint8 Finger, const FVector& Position)
{
    Pimpl->BasicHandModel.SetStartJointPosition(
        Finger,
        FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(Position));
}

TArray<FQuat> FSGBasicHandModelImpl::GetStartJointRotations() const
{
    TArray<FQuat> StartJointRotations{
        FSGArrayUtils::FromStdVector<
            FSGQuatImpl::FQuatType, FQuat, FSGQuatImpl, &FSGQuatImpl::ToFQuat>(
            Pimpl->BasicHandModel.GetStartJointRotations())
    };
    return MoveTemp(StartJointRotations);
}

void FSGBasicHandModelImpl::SetStartJointRotations(const TArray<FQuat>& StartJointRotations)
{
    std::vector<FSGQuatImpl::FQuatType> StartJointRotationsVector{
        FSGArrayUtils::ToStdVector<
            FQuat, FSGQuatImpl::FQuatType, FSGQuatImpl, &FSGQuatImpl::ToQuat>(StartJointRotations)
    };
    Pimpl->BasicHandModel.SetStartJointRotations(MoveTemp(StartJointRotationsVector));
}

FVector FSGBasicHandModelImpl::GetJointPosition(const ESGFinger Finger) const
{
    return FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
        Pimpl->BasicHandModel.GetJointPosition(FSGCoreEnumCast::ToEFinger(Finger)));
}

void FSGBasicHandModelImpl::SetJointPosition(const FVector& NewPosition, const ESGFinger Finger)
{
    Pimpl->BasicHandModel.SetJointPosition(
        FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(NewPosition),
        FSGCoreEnumCast::ToEFinger(Finger));
}

FQuat FSGBasicHandModelImpl::GetJointRotation(const ESGFinger Finger) const
{
    const FSGQuatImpl Rotation(Pimpl->BasicHandModel.GetJointRotation(FSGCoreEnumCast::ToEFinger(Finger)));
    return Rotation.ToFQuat();
}

void FSGBasicHandModelImpl::SetJointRotation(const FQuat& NewRotation, const ESGFinger Finger)
{
    Pimpl->BasicHandModel.SetJointRotation(FSGQuatImpl(NewRotation).ToQuat(), FSGCoreEnumCast::ToEFinger(Finger));
}

TArray<float> FSGBasicHandModelImpl::GetTotalLengths() const
{
    const TArray<float> TotalLengths{
        FSGArrayUtils::FromMillimeters(Pimpl->BasicHandModel.GetTotalLengths())
    };
    return TotalLengths;
}

float FSGBasicHandModelImpl::GetTotalLength(const ESGFinger Finger) const
{
    return FSGUnits::MillimetersToCentimeters(
        Pimpl->BasicHandModel.GetTotalLength(FSGCoreEnumCast::ToEFinger(Finger)));
}

TArray<FVector> FSGBasicHandModelImpl::Get3DLengths(const ESGFinger Finger) const
{
    const TArray<FVector> Lengths{
        FSGArrayUtils::FromMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            Pimpl->BasicHandModel.Get3DLengths(FSGCoreEnumCast::ToEFinger(Finger)))
    };
    return Lengths;
}

bool FSGBasicHandModelImpl::Equals(const FSGBasicHandModelImpl& BasicHandModelImpl) const
{
    return Pimpl->BasicHandModel.Equals(BasicHandModelImpl.ToBasicHandModel());
}

FString FSGBasicHandModelImpl::ToString() const
{
    const FString String(UTF8_TO_TCHAR(Pimpl->BasicHandModel.ToString().c_str()));
    return String;
}

FString FSGBasicHandModelImpl::ToString(const bool bLengthsOnly) const
{
    const FString String(UTF8_TO_TCHAR(Pimpl->BasicHandModel.ToString(bLengthsOnly).c_str()));
    return String;
}

FString FSGBasicHandModelImpl::Serialize() const
{
    const FString Serialized(UTF8_TO_TCHAR(Pimpl->BasicHandModel.Serialize().c_str()));
    return Serialized;
}

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

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

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