/*
* All or portions of this file Copyright (c) Amazon.com, Inc. or its affiliates or
* its licensors.
*
* For complete copyright and license terms please see the LICENSE at the root of this
* distribution (the "License"). All use of this software is governed by the License,
* or, if provided, by the license below or the license accompanying this file. Do not
* remove or modify any license notices. This file is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*
*/

#include <PhysXCharacters_precompiled.h>

#include "RagdollTestData.h"
#include "TestEnvironment.h"

#include <API/Utils.h>
#include <Components/RagdollComponent.h>
#include <PhysXCharacters/NativeTypeIdentifiers.h>

namespace PhysXCharacters
{
    TEST_F(PhysXCharactersTest, RagdollComponentSerialization_SharedPointerVersion1_NotRegisteredErrorDoesNotOccur)
    {
        // A stream buffer corresponding to a ragdoll component that was serialized before the "PhysXRagdoll" element
        // was changed from a shared pointer to a unique pointer.  Without a valid converter, deserializing this will
        // cause an error.
        const char* objectStreamBuffer =
            R"DELIMITER(<ObjectStream version="1">
            <Class name="RagdollComponent" field="m_template" version="1" type="{B89498F8-4718-42FE-A457-A377DD0D61A0}">
                <Class name="AZ::Component" field="BaseClass1" type="{EDFCB2CF-F75D-43BE-B26B-F35821B29247}">
                    <Class name="AZ::u64" field="Id" value="0" type="{D6597933-47CD-4FC8-B911-63F3E2B0993A}"/>
                </Class>
                <Class name="AZStd::shared_ptr" field="PhysXRagdoll" type="{A3E470C6-D6E0-5A32-9E83-96C379D9E7FA}"/>
            </Class>
            </ObjectStream>)DELIMITER";

        Physics::ErrorHandler errorHandler("not registered with the serializer");
        AZ::Utils::LoadObjectFromBuffer<RagdollComponent>(objectStreamBuffer, strlen(objectStreamBuffer) + 1);

        // Check that there were no errors during deserialization.
        EXPECT_EQ(errorHandler.GetErrorCount(), 0);
    }

    Physics::RagdollState GetTPose(Physics::SimulationType simulationType = Physics::SimulationType::Dynamic)
    {
        Physics::RagdollState ragdollState;
        for (int nodeIndex = 0; nodeIndex < RagdollTestData::NumNodes; nodeIndex++)
        {
            Physics::RagdollNodeState nodeState;
            nodeState.m_position = RagdollTestData::NodePositions[nodeIndex];
            nodeState.m_orientation = RagdollTestData::NodeOrientations[nodeIndex];
            nodeState.m_simulationType = simulationType;
            ragdollState.push_back(nodeState);
        }

        return ragdollState;
    }

    AZStd::unique_ptr<PhysXCharacters::Ragdoll> CreateRagdoll()
    {
        Physics::RagdollConfiguration* configuration =
            AZ::Utils::LoadObjectFromFile<Physics::RagdollConfiguration>("../Gems/PhysXCharacters/Code/Tests/RagdollConfiguration.xml");

        Physics::RagdollState initialState = GetTPose();
        ParentIndices parentIndices;
        for (int i = 0; i < configuration->m_nodes.size(); i++)
        {
            parentIndices.push_back(RagdollTestData::ParentIndices[i]);
        }

        return PhysXCharacters::Utils::CreateRagdoll(*configuration, initialState, parentIndices);
    }

    TEST_F(PhysXCharactersTest, Ragdoll_GetNativeType_CorrectType)
    {
        auto ragdoll = CreateRagdoll();
        EXPECT_EQ(ragdoll->GetNativeType(), PhysXCharacters::NativeTypeIdentifiers::Ragdoll);

        const size_t numNodes = ragdoll->GetNumNodes();
        for (size_t nodeIndex = 0; nodeIndex < numNodes; nodeIndex++)
        {
            EXPECT_EQ(ragdoll->GetNode(nodeIndex)->GetNativeType(), PhysXCharacters::NativeTypeIdentifiers::RagdollNode);
        }
    }

    TEST_F(PhysXCharactersTest, RagdollNode_GetNativePointer_CorrectType)
    {
        auto ragdoll = CreateRagdoll();

        const size_t numNodes = ragdoll->GetNumNodes();
        for (size_t nodeIndex = 0; nodeIndex < numNodes; nodeIndex++)
        {
            auto nativePointer = static_cast<physx::PxBase*>(ragdoll->GetNode(nodeIndex)->GetNativePointer());
            EXPECT_EQ(nativePointer->getConcreteType(), physx::PxConcreteType::eRIGID_DYNAMIC);
        }
    }

    TEST_F(PhysXCharactersTest, RagdollNode_GetTransform_MatchesTestSetup)
    {
        auto ragdoll = CreateRagdoll();
        ragdoll->EnableSimulation(GetTPose());

        for (size_t nodeIndex = 0; nodeIndex < RagdollTestData::NumNodes; nodeIndex++)
        {
            auto orientation = ragdoll->GetNode(nodeIndex)->GetOrientation();
            auto position = ragdoll->GetNode(nodeIndex)->GetPosition();
            auto transform = ragdoll->GetNode(nodeIndex)->GetTransform();
            EXPECT_TRUE(orientation.IsClose(RagdollTestData::NodeOrientations[nodeIndex]));
            EXPECT_TRUE(position.IsClose(RagdollTestData::NodePositions[nodeIndex]));
            EXPECT_TRUE(transform.IsClose(AZ::Transform::CreateFromQuaternionAndTranslation(
                RagdollTestData::NodeOrientations[nodeIndex], RagdollTestData::NodePositions[nodeIndex])));
        }
    }

    TEST_F(PhysXCharactersTest, Ragdoll_GetTransform_MatchesTestSetup)
    {
        auto ragdoll = CreateRagdoll();

        auto orientation = ragdoll->GetOrientation();
        auto position = ragdoll->GetPosition();
        auto transform = ragdoll->GetTransform();
        EXPECT_TRUE(orientation.IsClose(RagdollTestData::NodeOrientations[0]));
        EXPECT_TRUE(position.IsClose(RagdollTestData::NodePositions[0]));
        EXPECT_TRUE(transform.IsClose(AZ::Transform::CreateFromQuaternionAndTranslation(
            RagdollTestData::NodeOrientations[0], RagdollTestData::NodePositions[0])));
    }

    TEST_F(PhysXCharactersTest, Ragdoll_GetWorld_CorrectWorld)
    {
        auto ragdoll = CreateRagdoll();

        // the ragdoll isn't enabled yet, so it shouldn't be in a world
        EXPECT_EQ(ragdoll->GetWorld(), nullptr);
        const size_t numNodes = ragdoll->GetNumNodes();
        for (size_t nodeIndex = 0; nodeIndex < numNodes; nodeIndex++)
        {
            EXPECT_EQ(ragdoll->GetNode(nodeIndex)->GetWorld(), nullptr);
        }

        ragdoll->EnableSimulation(GetTPose());
        EXPECT_EQ(ragdoll->GetWorld()->GetWorldId(), Physics::DefaultPhysicsWorldId);
        EXPECT_EQ(ragdoll->GetWorldId(), Physics::DefaultPhysicsWorldId);
        for (size_t nodeIndex = 0; nodeIndex < numNodes; nodeIndex++)
        {
            EXPECT_EQ(ragdoll->GetNode(nodeIndex)->GetWorld()->GetWorldId(), Physics::DefaultPhysicsWorldId);
        }
    }

    TEST_F(PhysXCharactersTest, Ragdoll_GetNumNodes_EqualsNumInTestPose)
    {
        auto ragdoll = CreateRagdoll();
        EXPECT_EQ(ragdoll->GetNumNodes(), RagdollTestData::NumNodes);
    }

    TEST_F(PhysXCharactersTest, Ragdoll_GetJoint_MatchesTestDataJointStructure)
    {
        auto ragdoll = CreateRagdoll();
        const size_t numNodes = RagdollTestData::NumNodes;
        for (size_t nodeIndex = 0; nodeIndex < numNodes; nodeIndex++)
        {
            auto node = ragdoll->GetNode(nodeIndex);
            auto joint = node->GetJoint();

            size_t parentIndex = RagdollTestData::ParentIndices[nodeIndex];
            if (parentIndex >= numNodes)
            {
                // the root node shouldn't have a parent or a joint
                EXPECT_EQ(joint, nullptr);
            }
            else
            {
                EXPECT_EQ(joint->GetChildBody(), &node->GetRigidBody());
                EXPECT_EQ(joint->GetParentBody(), &ragdoll->GetNode(parentIndex)->GetRigidBody());
            }
        }
    }

    TEST_F(PhysXCharactersTest, Ragdoll_GetAabb_MatchesTestPoseAabb)
    {
        auto ragdoll = CreateRagdoll();
        auto aabb = ragdoll->GetAabb();
        EXPECT_TRUE(aabb.GetMin().IsClose(AZ::Vector3(-0.623f, -0.145f, -0.005f), 1e-3f));
        EXPECT_TRUE(aabb.GetMax().IsClose(AZ::Vector3(0.623f, 0.166f, 1.724f), 1e-3f));
    }

    TEST_F(PhysXCharactersTest, Ragdoll_GetNodeOutsideRange_GeneratesError)
    {
        auto ragdoll = CreateRagdoll();
        Physics::ErrorHandler errorHandler("Invalid node index");

        // this node index should be valid
        ragdoll->GetNode(RagdollTestData::NumNodes - 1);
        EXPECT_EQ(errorHandler.GetErrorCount(), 0);

        // this node index should be out of range
        ragdoll->GetNode(RagdollTestData::NumNodes);
        EXPECT_EQ(errorHandler.GetErrorCount(), 1);
    }

    TEST_F(PhysXCharactersTest, Ragdoll_GetNodeStateOutsideRange_GeneratesError)
    {
        auto ragdoll = CreateRagdoll();
        Physics::ErrorHandler errorHandler("Invalid node index");

        // this node index should be valid
        Physics::RagdollNodeState nodeState;
        ragdoll->GetNodeState(RagdollTestData::NumNodes - 1, nodeState);
        EXPECT_EQ(errorHandler.GetErrorCount(), 0);

        // this node index should be out of range
        ragdoll->GetNodeState(RagdollTestData::NumNodes, nodeState);
        EXPECT_EQ(errorHandler.GetErrorCount(), 1);
    }

    TEST_F(PhysXCharactersTest, Ragdoll_SetNodeStateOutsideRange_GeneratesError)
    {
        auto ragdoll = CreateRagdoll();
        Physics::ErrorHandler errorHandler("Invalid node index");

        auto ragdollState = GetTPose();
        auto& nodeState = ragdollState.back();

        // this node index should be valid
        ragdoll->SetNodeState(RagdollTestData::NumNodes - 1, nodeState);
        EXPECT_EQ(errorHandler.GetErrorCount(), 0);

        // this node index should be out of range
        ragdoll->SetNodeState(RagdollTestData::NumNodes, nodeState);
        EXPECT_EQ(errorHandler.GetErrorCount(), 1);
    }

    TEST_F(PhysXCharactersTest, Ragdoll_SimulateWithKinematicState_AabbDoesNotChange)
    {
        AZStd::shared_ptr<Physics::World> world;
        Physics::DefaultWorldBus::BroadcastResult(world, &Physics::DefaultWorldRequests::GetDefaultWorld);

        auto ragdoll = CreateRagdoll();
        auto initialAabb = ragdoll->GetAabb();
        auto kinematicTPose = GetTPose(Physics::SimulationType::Kinematic);

        ragdoll->EnableSimulation(kinematicTPose);
        ragdoll->SetState(kinematicTPose);

        for (int timeStep = 0; timeStep < 10; timeStep++)
        {
            world->Update(1.0f / 60.0f);
            EXPECT_TRUE(ragdoll->GetAabb().GetMax().IsClose(initialAabb.GetMax()));
            EXPECT_TRUE(ragdoll->GetAabb().GetMin().IsClose(initialAabb.GetMin()));
        }
    }

    AZ::u32 GetNumRigidDynamicActors(physx::PxScene* scene)
    {
        PHYSX_SCENE_READ_LOCK(scene);
        return scene->getNbActors(physx::PxActorTypeFlag::eRIGID_DYNAMIC);
    }

    TEST_F(PhysXCharactersTest, Ragdoll_EnableDisableSimulation_NumActorsInSceneCorrect)
    {
        auto ragdoll = CreateRagdoll();

        AZStd::shared_ptr<Physics::World> world;
        Physics::DefaultWorldBus::BroadcastResult(world, &Physics::DefaultWorldRequests::GetDefaultWorld);

        auto pxScene = static_cast<physx::PxScene*>(world->GetNativePointer());
        EXPECT_EQ(GetNumRigidDynamicActors(pxScene), 0);
        EXPECT_FALSE(ragdoll->IsSimulated());

        ragdoll->EnableSimulation(GetTPose());
        EXPECT_EQ(GetNumRigidDynamicActors(pxScene), RagdollTestData::NumNodes);
        EXPECT_TRUE(ragdoll->IsSimulated());

        ragdoll->DisableSimulation();
        EXPECT_EQ(GetNumRigidDynamicActors(pxScene), 0);
        EXPECT_FALSE(ragdoll->IsSimulated());
    }

    TEST_F(PhysXCharactersTest, Ragdoll_NoOtherGeometry_FallsUnderGravity)
    {
        auto ragdoll = CreateRagdoll();

        ragdoll->EnableSimulation(GetTPose());

        AZStd::shared_ptr<Physics::World> world;
        Physics::DefaultWorldBus::BroadcastResult(world, &Physics::DefaultWorldRequests::GetDefaultWorld);

        float z = ragdoll->GetPosition().GetZ();
        float expectedInitialZ = RagdollTestData::NodePositions[0].GetZ();
        EXPECT_NEAR(z, expectedInitialZ, 0.01f);
        for (int timeStep = 0; timeStep < 60; timeStep++)
        {
            world->Update(1.0f / 60.0f);
        }

        // after falling for 1 second, should have fallen about 1 / 2 * 9.8 * 1 * 1 = 4.9m
        // but allow plenty of leeway for effects of ragdoll pose changing, damping etc.
        z = ragdoll->GetPosition().GetZ();
        EXPECT_NEAR(z, expectedInitialZ - 4.9f, 0.5f);
    }

    TEST_F(PhysXCharactersTest, Ragdoll_AboveStaticFloor_SettlesOnFloor)
    {
        AZStd::shared_ptr<Physics::World> world;
        Physics::DefaultWorldBus::BroadcastResult(world, &Physics::DefaultWorldRequests::GetDefaultWorld);

        auto floor = Physics::AddStaticFloorToWorld(world.get(), DefaultFloorTransform);
        auto ragdoll = CreateRagdoll();
        ragdoll->EnableSimulation(GetTPose());

        for (int timeStep = 0; timeStep < 500; timeStep++)
        {
            world->Update(1.0f / 60.0f);
        }

        // the AABB min z should be close to 0
        // allow a little leeway because there might be a little ground penetration
        float minZ = ragdoll->GetAabb().GetMin().GetZ();
        EXPECT_NEAR(minZ, 0.0f, 0.05f);
    }
} // namespace PhysXCharacters