/**
 * @file
 *
 * @author  Mamadou Babaei <mamadou@senseglove.com>
 *
 * @section LICENSE
 *
 * (The MIT License)
 *
 * Copyright (c) 2020 - 2023 NovaGlove
 *
 * 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/SGNovaGloveImpl.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_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_NovaGlove.h"
#include "SGBuildHacks/SGInclude_Core_NovaGloveHandProfile.h"
#include "SGBuildHacks/SGInclude_Core_NovaGloveInfo.h"
#include "SGBuildHacks/SGInclude_Core_NovaGloveSensorData.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/SGNovaGloveHandProfileImpl.h"
#include "SGCoreImpl/SGNovaGloveInfoImpl.h"
#include "SGCoreImpl/SGNovaGloveSensorDataImpl.h"
#include "SGCoreImpl/SGQuatImpl.h"
#include "SGCoreImpl/SGThumperCommandImpl.h"
#include "SGCoreImpl/SGVect3DImpl.h"
#include "SGUtils/SGArrayUtils.h"

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

    FSGNovaGloveImpl* Owner;

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

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

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

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

ANSICHAR FSGNovaGloveImpl::GetHapticsByte()
{
    static const ANSICHAR Byte = FNovaGloveType::GetHapticsByte();
    return Byte;
}

const FVector& FSGNovaGloveImpl::GetMinCalibrationRange()
{
    static const FVector Range{FSGVect3DImpl{FNovaGloveType::GetMinCalibrationRange()}.ToFVector()};
    return Range;
}

FSGHandPoseImpl FSGNovaGloveImpl::CalculateHandPose(const FSGBasicHandModelImpl& HandModel,
                                                    const FSGNovaGloveSensorDataImpl& NovaData,
                                                    const FSGNovaGloveHandProfileImpl& Profile)
{
    FSGHandPoseImpl HandPose{FNovaGloveType::CalculateHandPose(
        HandModel.ToBasicHandModel(), NovaData.ToNovaGloveSensorData(), Profile.ToNovaGloveHandProfile())
    };
    return MoveTemp(HandPose);
}

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

TArray<FSGNovaGloveImpl> FSGNovaGloveImpl::GetNovaGloves()
{
    FNovaGloveVectorType Vector{FNovaGloveType::GetNovaGloves()};

    TArray<FSGNovaGloveImpl> Array{
        FSGArrayUtils::FromStdVector<FNovaGloveType, FSGNovaGloveImpl>(MoveTemp(Vector))
    };

    return MoveTemp(Array);
}


bool FSGNovaGloveImpl::GetNovaGlove(FSGNovaGloveImpl& OutGlove)
{
    FNovaGloveType Glove;
    const bool bResult = FNovaGloveType::GetNovaGlove(Glove);
    OutGlove = FSGNovaGloveImpl{MoveTemp(Glove)};
    return bResult;
}

bool FSGNovaGloveImpl::GetNovaGlove(const bool bRightHanded, FSGNovaGloveImpl& OutGlove)
{
    FNovaGloveType Glove;
    const bool bResult = FNovaGloveType::GetNovaGlove(bRightHanded, Glove);
    OutGlove = FSGNovaGloveImpl{MoveTemp(Glove)};
    return bResult;
}

TArray<FVector> FSGNovaGloveImpl::GetCalibrationValues(const FSGNovaGloveSensorDataImpl& SensorData)
{
    TArray<FVector> Values{
        FSGArrayUtils::FromStdVector<FSGVect3DImpl::FVect3DType, FVector, FSGVect3DImpl, &FSGVect3DImpl::ToFVector>(
            FNovaGloveType::GetCalibrationValues(SensorData.ToNovaGloveSensorData()))
    };
    return MoveTemp(Values);
}

void FSGNovaGloveImpl::CalibrateInterpolation(const TArray<FVector>& MinValues, const TArray<FVector>& MaxValues,
                                              FSGNovaGloveHandProfileImpl& OutProfile)
{
    std::vector<FSGVect3DImpl::FVect3DType> MinValuesVector{
        FSGArrayUtils::ToStdVector<FVector, FSGVect3DImpl::FVect3DType, FSGVect3DImpl, &FSGVect3DImpl::ToVect3D>(
            MinValues)
    };
    std::vector<FSGVect3DImpl::FVect3DType> MaxValuesVector{
        FSGArrayUtils::ToStdVector<FVector, FSGVect3DImpl::FVect3DType, FSGVect3DImpl, &FSGVect3DImpl::ToVect3D>(
            MaxValues)
    };

    FSGNovaGloveHandProfileImpl::FNovaGloveHandProfileType Profile;
    FNovaGloveType::CalibrateInterpolation(MoveTemp(MinValuesVector), MoveTemp(MaxValuesVector), Profile);
    OutProfile = FSGNovaGloveHandProfileImpl{MoveTemp(Profile)};
}

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

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

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

void FSGNovaGloveImpl::CalculateWristLocation(const FVector& ReferencePosition, const FQuat& ReferenceRotation,
                                              const ESGPositionalTrackingHardware TrackingHardware,
                                              const bool bRightHanded,
                                              FVector& OutWristPosition, FQuat& OutWristRotation)
{
    FSGVect3DImpl::FVect3DType WristPosition;
    FSGQuatImpl::FQuatType WristRotation;

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

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

FString FSGNovaGloveImpl::ToBytes(const FSGThumperCommandImpl& ThumperCommand)
{
    const FString Bytes{UTF8_TO_TCHAR(FNovaGloveType::ToBytes(ThumperCommand.ToThumperCommand()).c_str())};
    return Bytes;
}

FString FSGNovaGloveImpl::ToBytes(const FSGForceFeedbackCommandImpl& ForceFeedbackCommand)
{
    FString Bytes{UTF8_TO_TCHAR(FNovaGloveType::ToBytes(ForceFeedbackCommand.ToForceFeedbackCommand()).c_str())};
    return MoveTemp(Bytes);
}

FString FSGNovaGloveImpl::ToBytes(const FSGBuzzCommandImpl& BuzzCommand)
{
    FString Bytes{UTF8_TO_TCHAR(FNovaGloveType::ToBytes(BuzzCommand.ToBuzzCommand()).c_str())};
    return MoveTemp(Bytes);
}

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

FSGNovaGloveImpl::FSGNovaGloveImpl(const FSGNovaGloveInfoImpl& DeviceInfo)
    : FSGHapticGloveImpl(FSGNovaGloveImpl(std::make_shared<FNovaGloveType>((DeviceInfo.ToNovaGloveInfo())))),
      Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
}

FSGNovaGloveImpl::FSGNovaGloveImpl(const FNovaGloveType NovaGlove)
    : FSGHapticGloveImpl(std::make_shared<FNovaGloveType>(NovaGlove)),
      Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
}

FSGNovaGloveImpl::FSGNovaGloveImpl(const FNovaGlovePtrType NovaGlove)
    : FSGHapticGloveImpl(NovaGlove),
      Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
}

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

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

FSGNovaGloveImpl::~FSGNovaGloveImpl() = default;

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

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

const FSGNovaGloveImpl::FNovaGloveType& FSGNovaGloveImpl::ToNovaGlove() const
{
    return *ToNovaGlovePtrChecked();
}

FSGNovaGloveImpl::FNovaGlovePtrType FSGNovaGloveImpl::ToNovaGlovePtr() const
{
    return std::dynamic_pointer_cast<FNovaGloveType>(ToSGDevicePtr());
}

FSGNovaGloveImpl::FNovaGlovePtrType FSGNovaGloveImpl::ToNovaGlovePtrChecked() const
{
    FNovaGlovePtrType NovaGlove = std::dynamic_pointer_cast<FNovaGloveType>(ToSGDevicePtr());
    ensureAlwaysMsgf(NovaGlove.get(), TEXT("Invalid NovaGlove"));
    return NovaGlove;
}

ESGDeviceType FSGNovaGloveImpl::GetDeviceType() const
{
    return FSGCoreEnumCast::ToESGDeviceType(ToNovaGlovePtrChecked()->GetDeviceType());
}

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

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

int32 FSGNovaGloveImpl::GetFirmwareVersion() const
{
    return ToNovaGlovePtrChecked()->GetFirmwareVersion();
}

int32 FSGNovaGloveImpl::GetSubFirmwareVersion() const
{
    return ToNovaGlovePtrChecked()->GetSubFirmwareVersion();
}

bool FSGNovaGloveImpl::HasBattery() const
{
    return ToNovaGlovePtrChecked()->HasBattery();
}

bool FSGNovaGloveImpl::IsCharging()
{
    return ToNovaGlovePtrChecked()->IsCharging();
}

bool FSGNovaGloveImpl::GetBatteryLevel(float& OutBatteryLevel)
{
    return ToNovaGlovePtrChecked()->GetBatteryLevel(OutBatteryLevel);
}

bool FSGNovaGloveImpl::GetSensorData(FSGNovaGloveSensorDataImpl& OutSensorData)
{
    FSGNovaGloveSensorDataImpl::FNovaGloveSensorDataType SensorData;
    const bool bResult = ToNovaGlovePtrChecked()->GetSensorData(SensorData);
    OutSensorData = FSGNovaGloveSensorDataImpl{MoveTemp(SensorData)};
    return bResult;
}

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

bool FSGNovaGloveImpl::GetHandPose(const FSGBasicHandModelImpl& HandModel, const FSGNovaGloveHandProfileImpl& Profile,
                                   FSGHandPoseImpl& OutHandPose)
{
    FSGHandPoseImpl::FHandPoseType HandPose;
    const bool bResult = ToNovaGlovePtrChecked()->GetHandPose(
        HandModel.ToBasicHandModel(), Profile.ToNovaGloveHandProfile(), HandPose);
    OutHandPose = FSGHandPoseImpl{MoveTemp(HandPose)};
    return bResult;
}

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

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

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

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

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

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

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

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

void FSGNovaGloveImpl::ApplyCalibration(FSGNovaGloveHandProfileImpl& OutProfile) const
{
    FSGNovaGloveHandProfileImpl::FNovaGloveHandProfileType Profile;
    ToNovaGlovePtrChecked()->ApplyCalibration(Profile);
    OutProfile = FSGNovaGloveHandProfileImpl{MoveTemp(Profile)};
}

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

    ToNovaGlovePtrChecked()->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 FSGNovaGloveImpl::GetWristLocation(const FVector& ReferencePosition, const FQuat& ReferenceRotation,
                                        const ESGPositionalTrackingHardware TrackingHardware,
                                        FVector& OutWristPosition, FQuat& OutWristRotation) const
{
    FSGVect3DImpl::FVect3DType WristPosition;
    FSGQuatImpl::FQuatType WristRotation;

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

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


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

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

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