/** Sets the current ball possessor */
void AMagicBattleSoccerBall::SetPossessor(AMagicBattleSoccerCharacter* Player)
{
    if (Role < ROLE_Authority)
    {
        // Safety check. Only authority entities should drive the ball.
    }
    else
    {
        // We only allow a possession change if the ball is being unpossessed or the player is not one we're ignoring
        if (nullptr == Player || (CanPossessBall(Player) && (PossessorToIgnore != Player)))
        {
            AMagicBattleSoccerCharacter *OldPossessor = Possessor;

            // Assign the new possessor
            Possessor = Player;

            // Handle cases when the ball had a possessor in the previous frame
            if (nullptr != OldPossessor)
            {
                // Assign the last possessor
                LastPossessor = OldPossessor;
                // Assign the possessor to ignore
                PossessorToIgnore = OldPossessor;
            }

            // Toggle physics
            UPrimitiveComponent *Root = Cast<UPrimitiveComponent>(GetRootComponent());
            if (nullptr != Possessor)
            {
                Possessor->StopWeaponFire(Possessor->PrimaryWeapon);
                Possessor->StopWeaponFire(Possessor->SecondaryWeapon);
                Root->PutRigidBodyToSleep();
                Root->SetSimulatePhysics(false);
                Root->SetEnableGravity(false);
                SetActorEnableCollision(false);
                MoveWithPossessor();
            }
            else
            {
                Root->SetSimulatePhysics(true);
                Root->SetEnableGravity(true);
                SetActorEnableCollision(true);
                Root->PutRigidBodyToSleep();
            }
        }

        // Force the orientation to be replicated at the same time the possessor is replicated
        UPrimitiveComponent *Root = Cast<UPrimitiveComponent>(GetRootComponent());
        ServerPhysicsState.pos = GetActorLocation();
        ServerPhysicsState.rot = GetActorRotation();
        ServerPhysicsState.vel = Root->GetComponentVelocity();
        ServerPhysicsState.timestamp = AMagicBattleSoccerPlayerController::GetLocalTime();
    }
}
/** This occurs when play begins */
void AMagicBattleSoccerBall::BeginPlay()
{
    Super::BeginPlay();

    if (Role < ROLE_Authority)
    {
        // The server manages the game state; the soccer ball will be replicated to us.

        // Physics however are not replicated. We will need to have the ball orientation
        // replicated to us. We need to turn off physics simulation and collision detection.
        UPrimitiveComponent *Root = Cast<UPrimitiveComponent>(GetRootComponent());
        Root->PutRigidBodyToSleep();
        Root->SetSimulatePhysics(false);
        Root->SetEnableGravity(false);
        SetActorEnableCollision(false);
    }
    else
    {
        // Servers should add this soccer ball to the game mode cache.
        // It will get replicated to clients for when they need to access
        // the ball itself to get information such as who possesses it.
        AMagicBattleSoccerGameState* GameState = GetGameState();
        GameState->SoccerBall = this;
    }
}
void UGripMotionControllerComponent::NotifyDrop_Implementation(const FBPActorGripInformation &NewDrop, bool bSimulate)
{
	if (bIsServer)
	{
		if (NewDrop.Actor)
		{
			NewDrop.Actor->RemoveTickPrerequisiteComponent(this);
			this->IgnoreActorWhenMoving(NewDrop.Actor, false);
			NewDrop.Actor->SetReplicateMovement(NewDrop.bOriginalReplicatesMovement);
		}
		else if (NewDrop.Component)
		{
			NewDrop.Component->RemoveTickPrerequisiteComponent(this);
			
			if (NewDrop.Component->GetOwner())
			{
				this->IgnoreActorWhenMoving(NewDrop.Component->GetOwner(), false);
				NewDrop.Component->GetOwner()->SetReplicateMovement(NewDrop.bOriginalReplicatesMovement);
			}
		}
	}

	DestroyPhysicsHandle(NewDrop);

	UPrimitiveComponent *root = NewDrop.Component;

	if (!root && NewDrop.Actor)
	{
		root = Cast<UPrimitiveComponent>(NewDrop.Actor->GetRootComponent());

		if (root)
		{
			root->IgnoreActorWhenMoving(this->GetOwner(), false);
			root->SetSimulatePhysics(bSimulate);
			root->WakeAllRigidBodies();
			root->SetEnableGravity(true);
		}
	}
	else if (root)
	{
		root->IgnoreActorWhenMoving(this->GetOwner(), false);
		root->SetSimulatePhysics(bSimulate);
		root->WakeAllRigidBodies();
		root->SetEnableGravity(true);
	}
}
void UGripMotionControllerComponent::TickGrip()
{
	if (GrippedActors.Num())
	{
		FTransform WorldTransform;
		FTransform InverseTransform = this->GetComponentTransform().Inverse();

		for (int i = GrippedActors.Num() - 1; i >= 0; --i)
		{
			if (GrippedActors[i].Actor || GrippedActors[i].Component)
			{
				// GetRelativeTransformReverse had some serious f*****g floating point errors associated with it that was f*****g everything up
				// Not sure whats wrong with the function but I might want to push a patch out eventually
				WorldTransform = GrippedActors[i].RelativeTransform.GetRelativeTransform(InverseTransform);

				UPrimitiveComponent *root = GrippedActors[i].Component;
				AActor *actor = GrippedActors[i].Actor;

				if (!root)
					root = Cast<UPrimitiveComponent>(GrippedActors[i].Actor->GetRootComponent());

				if (!root)
					continue;

				if (!actor)
					actor = root->GetOwner();

				if (!actor)
					continue;

				if (bIsServer)
				{
					// Handle collision intersection detection, this is used to intelligently manage some of the networking and late update features.
					switch (GrippedActors[i].GripCollisionType.GetValue())
					{
					case EGripCollisionType::InteractiveCollisionWithPhysics:
					case EGripCollisionType::InteractiveHybridCollisionWithSweep:
					{
						TArray<FOverlapResult> Hits;
						FComponentQueryParams Params(NAME_None, this->GetOwner());
						Params.bTraceAsyncScene = root->bCheckAsyncSceneOnMove;
						Params.AddIgnoredActor(actor);
						Params.AddIgnoredActors(root->MoveIgnoreActors);

						if(GetWorld()->ComponentOverlapMulti(Hits, root, root->GetComponentLocation(), root->GetComponentQuat(), Params))
						{
							GrippedActors[i].bColliding = true;
						}
						else
						{
							GrippedActors[i].bColliding = false;
						}
					}

					// Skip collision intersects with these types, they dont need it
					case EGripCollisionType::PhysicsOnly:
					case EGripCollisionType::SweepWithPhysics:
					case EGripCollisionType::InteractiveCollisionWithSweep:
					default:break;
					}
				}

				// Need to figure out best default behavior
				/*if (GrippedActors[i].bHasSecondaryAttachment && GrippedActors[i].SecondaryAttachment)
				{
				WorldTransform.SetRotation((WorldTransform.GetLocation() - GrippedActors[i].SecondaryAttachment->GetComponentLocation()).ToOrientationRotator().Quaternion());
				}*/

				if (GrippedActors[i].GripCollisionType == EGripCollisionType::InteractiveCollisionWithPhysics)
				{
					UpdatePhysicsHandleTransform(GrippedActors[i], WorldTransform);
				}
				else if (GrippedActors[i].GripCollisionType == EGripCollisionType::InteractiveCollisionWithSweep)
				{
					if (bIsServer)
					{
						FHitResult OutHit;

						// Need to use without teleport so that the physics velocity is updated for when the actor is released to throw
						root->SetWorldTransform(WorldTransform, true, &OutHit);

						if (OutHit.bBlockingHit)
						{
							GrippedActors[i].bColliding = true;

							if (!actor->bReplicateMovement)
								actor->SetReplicateMovement(true);
						}
						else
						{
							GrippedActors[i].bColliding = false;

							if (actor->bReplicateMovement)
								actor->SetReplicateMovement(false);
						}
					}
					else
					{
						if (!GrippedActors[i].bColliding)
						{
							root->SetWorldTransform(WorldTransform, true);
						}
					}
				}
				else if (GrippedActors[i].GripCollisionType == EGripCollisionType::InteractiveHybridCollisionWithSweep)
				{
					// Need to use without teleport so that the physics velocity is updated for when the actor is released to throw
					//if (bIsServer)
					//{
					FBPActorPhysicsHandleInformation * GripHandle = GetPhysicsGrip(GrippedActors[i]);
					if (!GrippedActors[i].bColliding)
					{
						// Make sure that there is no collision on course before turning off collision and snapping to controller
						if (bIsServer && actor && actor->bReplicateMovement)
						{
							FCollisionQueryParams QueryParams(NAME_None, false, this->GetOwner());
							FCollisionResponseParams ResponseParam;
							root->InitSweepCollisionParams(QueryParams, ResponseParam);
							QueryParams.AddIgnoredActor(actor);
							QueryParams.AddIgnoredActors(root->MoveIgnoreActors);

							if (GetWorld()->SweepTestByChannel(root->GetComponentLocation(), WorldTransform.GetLocation(), root->GetComponentQuat(), ECollisionChannel::ECC_Visibility, root->GetCollisionShape(), QueryParams, ResponseParam))
							{
								GrippedActors[i].bColliding = true;
							}
							else
							{
								GrippedActors[i].bColliding = false;
							}
						}
						
						// If still not colliding
						if (!GrippedActors[i].bColliding)
						{
							if (bIsServer && actor->bReplicateMovement) // So we don't call on on change event over and over locally
								actor->SetReplicateMovement(false);

							if (bIsServer && GripHandle)
							{
								DestroyPhysicsHandle(GrippedActors[i]);

								if(GrippedActors[i].Actor)
									GrippedActors[i].Actor->DisableComponentsSimulatePhysics();
								else
									root->SetSimulatePhysics(false);
							}

							root->SetWorldTransform(WorldTransform, false);
						}
						else
						{
							if (bIsServer && GrippedActors[i].bColliding && GripHandle)
								UpdatePhysicsHandleTransform(GrippedActors[i], WorldTransform);
						}

					}
					else if (GrippedActors[i].bColliding && !GripHandle)
					{
						if (bIsServer && !actor->bReplicateMovement)
							actor->SetReplicateMovement(true);

						root->SetSimulatePhysics(true);

						if (bIsServer)
							SetUpPhysicsHandle(GrippedActors[i]);
					}
					else
					{
						if (bIsServer && GrippedActors[i].bColliding && GripHandle)
							UpdatePhysicsHandleTransform(GrippedActors[i], WorldTransform);
					}
				}
				else if (GrippedActors[i].GripCollisionType == EGripCollisionType::SweepWithPhysics)
				{
					if (bIsServer)
					{
						
						FVector OriginalPosition(root->GetComponentLocation());
						FRotator OriginalOrientation(root->GetComponentRotation());

						FVector NewPosition(WorldTransform.GetTranslation());
						FRotator NewOrientation(WorldTransform.GetRotation());

						// Now sweep collision separately so we can get hits but not have the location altered
						if (bUseWithoutTracking || NewPosition != OriginalPosition || NewOrientation != OriginalOrientation)
						{
							FVector move = NewPosition - OriginalPosition;

							// ComponentSweepMulti does nothing if moving < KINDA_SMALL_NUMBER in distance, so it's important to not try to sweep distances smaller than that. 
							const float MinMovementDistSq = (FMath::Square(4.f*KINDA_SMALL_NUMBER));

							if (bUseWithoutTracking || move.SizeSquared() > MinMovementDistSq || NewOrientation != OriginalOrientation)
							{
								if (CheckComponentWithSweep(root, move, NewOrientation, false))
								{

								}
							}
						}

						// Move the actor, we are not offsetting by the hit result anyway
						root->SetWorldTransform(WorldTransform, false);
					}
					else
					{
						// Move the actor, we are not offsetting by the hit result anyway
						root->SetWorldTransform(WorldTransform, false);
					}
				}
				else if (GrippedActors[i].GripCollisionType == EGripCollisionType::PhysicsOnly)
				{
					// Move the actor, we are not offsetting by the hit result anyway
					root->SetWorldTransform(WorldTransform, false);
				}
			}
			else
			{
				if (bIsServer)
				{
					DestroyPhysicsHandle(GrippedActors[i]);

					GrippedActors.RemoveAt(i); // If it got garbage collected then just remove the pointer, won't happen with new uproperty use, but keeping it here anyway
				}
			}

		}
	}

}
bool UGripMotionControllerComponent::SetUpPhysicsHandle(const FBPActorGripInformation &NewGrip)
{
	UPrimitiveComponent *root = NewGrip.Component;
	if(!root)
		root = Cast<UPrimitiveComponent>(NewGrip.Actor->GetRootComponent());
	
	if (!root)
		return false;

	// Needs to be simulating in order to run physics
	root->SetSimulatePhysics(true);
	root->SetEnableGravity(false);

	FBPActorPhysicsHandleInformation * HandleInfo = CreatePhysicsGrip(NewGrip);

#if WITH_PHYSX
	// Get the PxRigidDynamic that we want to grab.
	FBodyInstance* BodyInstance = root->GetBodyInstance(NAME_None/*InBoneName*/);
	if (!BodyInstance)
	{
		return false;
	}

	ExecuteOnPxRigidDynamicReadWrite(BodyInstance, [&](PxRigidDynamic* Actor)
	{
		PxScene* Scene = Actor->getScene();
	
		// Get transform of actor we are grabbing

		FTransform WorldTransform;
		FTransform InverseTransform = this->GetComponentTransform().Inverse();
		WorldTransform = NewGrip.RelativeTransform.GetRelativeTransform(InverseTransform);

		PxVec3 KinLocation = U2PVector(WorldTransform.GetLocation() - (WorldTransform.GetLocation() - root->GetComponentLocation()));
		PxTransform GrabbedActorPose = Actor->getGlobalPose();
		PxTransform KinPose(KinLocation, GrabbedActorPose.q);

		// set target and current, so we don't need another "Tick" call to have it right
		//TargetTransform = CurrentTransform = P2UTransform(KinPose);

		// If we don't already have a handle - make one now.
		if (!HandleInfo->HandleData)
		{
			// Create kinematic actor we are going to create joint with. This will be moved around with calls to SetLocation/SetRotation.
			PxRigidDynamic* KinActor = Scene->getPhysics().createRigidDynamic(KinPose);
			KinActor->setRigidDynamicFlag(PxRigidDynamicFlag::eKINEMATIC, true);
			KinActor->setMass(0.0f); // 1.0f;
			KinActor->setMassSpaceInertiaTensor(PxVec3(0.0f, 0.0f, 0.0f));// PxVec3(1.0f, 1.0f, 1.0f));
			KinActor->setMaxDepenetrationVelocity(PX_MAX_F32);

			// No bodyinstance
			KinActor->userData = NULL;
			
			// Add to Scene
			Scene->addActor(*KinActor);

			// Save reference to the kinematic actor.
			HandleInfo->KinActorData = KinActor;

			// Create the joint
			PxVec3 LocalHandlePos = GrabbedActorPose.transformInv(KinLocation);
			PxD6Joint* NewJoint = PxD6JointCreate(Scene->getPhysics(), KinActor, PxTransform::createIdentity(), Actor, PxTransform(LocalHandlePos));
			
			if (!NewJoint)
			{
				HandleInfo->HandleData = 0;
			}
			else
			{
				// No constraint instance
				NewJoint->userData = NULL;
				HandleInfo->HandleData = NewJoint;

				// Remember the scene index that the handle joint/actor are in.
				FPhysScene* RBScene = FPhysxUserData::Get<FPhysScene>(Scene->userData);
				const uint32 SceneType = root->BodyInstance.UseAsyncScene(RBScene) ? PST_Async : PST_Sync;
				HandleInfo->SceneIndex = RBScene->PhysXSceneIndex[SceneType];
				
				// Setting up the joint
				NewJoint->setMotion(PxD6Axis::eX, PxD6Motion::eFREE);
				NewJoint->setMotion(PxD6Axis::eY, PxD6Motion::eFREE);
				NewJoint->setMotion(PxD6Axis::eZ, PxD6Motion::eFREE);
				NewJoint->setDrivePosition(PxTransform(PxVec3(0, 0, 0)));

				NewJoint->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
				NewJoint->setMotion(PxD6Axis::eSWING1, PxD6Motion::eFREE);
				NewJoint->setMotion(PxD6Axis::eSWING2, PxD6Motion::eFREE);

				//UpdateDriveSettings();
				if (HandleInfo->HandleData != nullptr)
				{
					HandleInfo->HandleData->setDrive(PxD6Drive::eX, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
					HandleInfo->HandleData->setDrive(PxD6Drive::eY, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
					HandleInfo->HandleData->setDrive(PxD6Drive::eZ, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));

					HandleInfo->HandleData->setDrive(PxD6Drive::eSLERP, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));

						//HandleData->setDrive(PxD6Drive::eTWIST, PxD6JointDrive(Stiffness, Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
						//HandleData->setDrive(PxD6Drive::eSWING, PxD6JointDrive(Stiffness, Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
				}
			}
		}
	});
#else
	return false;
#endif // WITH_PHYSX

	return true;
}