ozz::unique_ptr<ozz::animation::offline::RawAnimation> GLTFImport::RetargetAnimation(ozz::animation::offline::RawAnimation* anim, const std::vector<ozz::math::Transform>& sourceRestPose, const std::vector<ozz::math::Transform>& targetRestPose, const Animation::OzzSkeleton* skele)
{
    using namespace ozz::math;

    const auto NormalizeQuat = [](const Quaternion& q) {
        float magnitude = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
        if (magnitude > 0.0f) {
            float invMagnitude = 1.0f / magnitude;
            return Quaternion(q.x * invMagnitude, q.y * invMagnitude, q.z * invMagnitude, q.w * invMagnitude);
        }
        return q;
    };

    const auto InvertQuat = [&NormalizeQuat](const Quaternion& q) {
        Quaternion normalized = NormalizeQuat(q);
        return Quaternion(-normalized.x, -normalized.y, -normalized.z, normalized.w);
    };

    const auto RotateVector = [&InvertQuat](const Quaternion& q, const Float3& v) -> Float3 {
        using namespace ozz::math;
        Quaternion qv = { v.x, v.y, v.z, 0.0f };
        Quaternion conjugateQ = InvertQuat(q);
        Quaternion result = conjugateQ * (q * qv);
        return { result.x, result.y, result.z };
    };

    const auto GetGlobalTransform = [&RotateVector, &NormalizeQuat](const Transform& localTransform, const Transform& parentGlobalTransform) {
        Transform result;
        result.translation = parentGlobalTransform.translation + RotateVector(parentGlobalTransform.rotation, localTransform.translation);
        result.rotation = NormalizeQuat(parentGlobalTransform.rotation * localTransform.rotation);
        result.scale = { 1.0f, 1.0f, 1.0f };
        return result;
    };

    const auto GetLocalTransform = [&RotateVector, &InvertQuat, &NormalizeQuat](const Transform& globalTransform, const Transform& parentGlobalTransform) {
        Quaternion inverseParentRotation = InvertQuat(parentGlobalTransform.rotation);
        Transform result;
        result.translation = RotateVector(inverseParentRotation, globalTransform.translation - parentGlobalTransform.translation);
        result.rotation = NormalizeQuat(inverseParentRotation * globalTransform.rotation);
        result.scale = { 1.0f, 1.0f, 1.0f };
        return result;
    };

    const auto L2G = [&GetGlobalTransform](std::span<const Transform> locals, std::span<Transform> globals, std::span<const int16_t> parents, size_t root) {
        std::stack<size_t> stack;
        stack.push(root);

        while (!stack.empty()) {
            size_t current = stack.top();
            stack.pop();

            if (parents[current] == -1) {
                globals[current] = locals[current];
            } else {
                globals[current] = GetGlobalTransform(locals[current], globals[parents[current]]);
            }

            for (size_t i = locals.size(); i-- > 0;) {
                if (parents[i] == current) {
                    stack.push(i);
                }
            }
        }
    };

    const auto InitL2G = [&L2G](std::span<const Transform> locals, std::span<Transform> globals, std::span<const int16_t> parents) {
        for (size_t i = 0; i < parents.size(); i++) {
            if (parents[i] == -1) {
                L2G(locals, globals, parents, i);
            }
        }
    };

    const auto G2L = [&GetLocalTransform](std::span<Transform> locals, std::span<const Transform> globals, std::span<const int16_t> parents, size_t root) {
        std::stack<size_t> stack;
        stack.push(root);

        while (!stack.empty()) {
            size_t current = stack.top();
            stack.pop();

            if (parents[current] == -1) {
                locals[current] = globals[current];
            } else {
                locals[current] = GetLocalTransform(globals[current], globals[parents[current]]);
            }

            for (size_t i = locals.size(); i-- > 0;) {
                if (parents[i] == current) {
                    stack.push(i);
                }
            }
        }
    };

    const auto InitG2L = [&G2L](std::span<Transform> locals, std::span<const Transform> globals, std::span<const int16_t> parents) {
        for (size_t i = 0; i < parents.size(); i++) {
            if (parents[i] == -1) {
                G2L(locals, globals, parents, i);
            }
        }
    };

    const auto RetargetTransform = [&InvertQuat, &NormalizeQuat](const Transform& sourceBase, const Transform& sourceDest, const Transform& targetBase){
        Float3 positionDiff = sourceDest.translation - sourceBase.translation;
        Quaternion rotationDiff = NormalizeQuat(NormalizeQuat(sourceDest.rotation) * InvertQuat(sourceBase.rotation));

        Transform result;
        result.translation = targetBase.translation + positionDiff;
        result.rotation = NormalizeQuat(rotationDiff * NormalizeQuat(targetBase.rotation));
        result.scale = { 1.0f, 1.0f, 1.0f };
        return result;
    };

    auto parents = skele->data->joint_parents();

    if (sourceRestPose.size() != targetRestPose.size() || sourceRestPose.size() != anim->num_tracks())
        return nullptr;

    std::vector<Transform> sourceGlobals(anim->num_tracks(), Transform::identity());
    InitL2G(sourceRestPose, sourceGlobals, parents);

    std::vector<Transform> targetGlobals(anim->num_tracks(), Transform::identity());
    InitL2G(targetRestPose, targetGlobals, parents);

    std::vector<Transform> currentLocals(anim->num_tracks(), Transform::identity());
    std::vector<Transform> currentGlobals(anim->num_tracks(), Transform::identity());

    auto result = ozz::make_unique<ozz::animation::offline::RawAnimation>();
    result->tracks.resize(anim->num_tracks());

    auto localSpan = ozz::make_span(currentLocals);

    const auto DoSample = [&](float time) {
        ozz::animation::offline::SampleAnimation(*anim, time, localSpan);
        InitL2G(currentLocals, currentGlobals, parents);
        for (size_t i = 0; i < currentGlobals.size(); i++) {
            currentGlobals[i] = RetargetTransform(sourceGlobals[i], currentGlobals[i], targetGlobals[i]);
        }
        InitG2L(currentLocals, currentGlobals, parents);
        for (size_t i = 0; i < currentGlobals.size(); i++) {
            auto& rKey = result->tracks[i].rotations.emplace_back();
            auto& tKey = result->tracks[i].translations.emplace_back();
            rKey.time = time;
            tKey.time = time;
            rKey.value = currentLocals[i].rotation;
            tKey.value = currentLocals[i].translation;
        }
    };

    for (float i = 0; i < anim->duration; i += 0.0333333333f) {
        DoSample(i);
    }
    DoSample(anim->duration);

    return result;
}