void PhysxBone::Initiliaze(MeshFilter* pMeshFilter)
{
	//Map the bone
	MapToBone(pMeshFilter);
	//Create actor and set it on the correct position
	CreatePhysxBone(pMeshFilter);
}

void PhysxBone::MapToBone(MeshFilter* pMeshFilter)
{
	//Find the bone we want to map to
	for(auto bone : pMeshFilter->GetSkeleton())
	{
		if(m_boneLayout.name == bone.Name)
		{
			m_pBone = &bone;
			break;
		}
	}
	//Check if we found our bone, else there is a mistake with the input
	ASSERT(m_pBone!=nullptr, _T("PhysxBone NAME INCORRECT! PhysxBone can not be mapped to a Bone in the Model!"));

	if(m_pBone)
	{
		//Store the index
		m_iBoneIndex = m_pBone->Index;

		//Get all it's parent and calculate the total offset, or the position in modelspace
		int currentParentIndex = m_pBone->Parent;
		vector<D3DXMATRIX> vOffsets;

		//get all offsets first
		while(currentParentIndex > -1)
		{
			vOffsets.push_back(pMeshFilter->GetSkeleton().at(currentParentIndex).Offset);
			currentParentIndex = pMeshFilter->GetSkeleton().at(currentParentIndex).Parent;
		}

		//inverse the vector and calculate the total offset
		std::reverse(vOffsets.begin(), vOffsets.end());
		for(auto offset : vOffsets)
		{
			m_matTotalOffset *= offset;
		}
		//finally adding it's own offset to the calculation to get the final position on modelspace
		m_matTotalOffset *= m_pBone->Offset;

		//when we know the transform in modelspace calculate the transform in worldspace
		//FIXME: worldtransform of model is hardcoded atm!!
		D3DXMATRIX world;
		D3DXMatrixIdentity(&world);

		m_matWorldSpace = m_matTotalOffset * world;
	}
}

void PhysxBone::UpdateLeechMode(D3DXMATRIX matKeyTransform)
{
	//Calculate the final position
	//Orientation * TotalOffset * LocalTransformAnimation
	m_matWorldSpace = m_matTotalOffset * matKeyTransform * world;

	//Convert matrix and position the actor
	NxMat34 nPos;
	PhysicsManager::GetInstance()->DMatToNMat(nPos, m_matWorldSpace);
	m_pActor->setGlobalPose(nPos);
}

void PhysxBone::CreatePhysxBone(MeshFilter* pMeshFilter)
{
	//Creates the bone using the information we know when we mapped the bone
	//Also taking into account which shape we want
	if(m_boneLayout.shapeType == RagdollShapeType::capsule)
	{
		//Create the capsule shape desc
		NxCapsuleShapeDesc capsuleDesc;
		capsuleDesc.height = m_boneLayout.height;
		capsuleDesc.radius = m_boneLayout.radius;
		capsuleDesc.localPose.t = NxVec3(0, capsuleDesc.radius + 0.5f * capsuleDesc.height, 0);

		//Create body so the actor is dynamic
		NxBodyDesc bodyDesc;
		bodyDesc.setToDefault();
		bodyDesc.angularDamping = 0.75f;
		bodyDesc.linearVelocity = NxVec3(0,0,0);

		//Create the actor
		NxActorDesc actorDesc;
		actorDesc.shapes.pushBack(&capsuleDesc);
		actorDesc.body = &bodyDesc;
		actorDesc.density = 10.0f; 
		//Important to set a density if we have a body to succesfully create an actor with a body

		//Position the actor to the correct place
		NxMat34 nPos;
		PhysicsManager::GetInstance()->DMatToNMat(nPos, m_matWorldSpace);
		actorDesc.globalPose = nPos;

		//Create the actor
		m_pActor = m_pPhysicsScene->createActor(actorDesc);
		
		if(!m_pActor)
			Logger::Log(_T("Error creating actor"), LogLevel::Error);

		//Default set our actor to be kinematic
		m_pActor->raiseBodyFlag(NX_BF_KINEMATIC);
		m_pActor->raiseActorFlag(NX_AF_DISABLE_COLLISION);
	}
	else if(m_boneLayout.shapeType == RagdollShapeType::sphere)
	{	
		//Create the sphere shape desc
		NxSphereShapeDesc sphereDesc;
		sphereDesc.radius = m_boneLayout.radius;
		sphereDesc.localPose.t = NxVec3(0, m_boneLayout.radius, 0);

		//Create body so the actor is dynamic
		NxBodyDesc bodyDesc;
		bodyDesc.setToDefault();
		bodyDesc.angularDamping = 0.75f;
		bodyDesc.linearVelocity = NxVec3(0,0,0);

		NxActorDesc actorDesc;
		actorDesc.shapes.pushBack(&sphereDesc);
		actorDesc.body = &bodyDesc;
		actorDesc.density = 10.0f; 
		//Important to set a density if we have a body to succesfully create an actor with a body

		//Position the actor to the correct place
		NxMat34 nPos;
		PhysicsManager::GetInstance()->DMatToNMat(nPos, m_matWorldSpace);
		actorDesc.globalPose = nPos;

		//Create the actor
		m_pActor = m_pPhysicsScene->createActor(actorDesc);

		if(!m_pActor)
			Logger::Log(_T("Error creating actor"), LogLevel::Error);

		//Default set our actor to be kinematic
		m_pActor->raiseBodyFlag(NX_BF_KINEMATIC);
		m_pActor->raiseActorFlag(NX_AF_DISABLE_COLLISION);
	}
}