// © 2024 Long Ly. All rights reserved. Any unauthorized use, reproduction, or distribution of this trademark is strictly prohibited and may result in legal action.


#include "Libraries/OLSLocomotionBPLibrary.h"

FVector UOLSLocomotionBPLibrary::PredictGroundMovementStopLocation(const FVector& velocity,
                                                                   bool shouldUseSeparateBrakingFriction,
                                                                   float brakingFriction, float groundFriction,
                                                                   float brakingFrictionFactor,
                                                                   float brakingDecelerationWalking)
{
	FVector predictedStopLocation = FVector::ZeroVector;

	// Determine the actual braking friction
	float actualBrakingFriction = shouldUseSeparateBrakingFriction ? brakingFriction : groundFriction;
	actualBrakingFriction = FMath::Max(0.f, actualBrakingFriction * FMath::Max(0.f, brakingFrictionFactor));

	// Calculate 2D velocity and speed
	const FVector velocity2D = FVector(velocity.X, velocity.Y, 0.f);
	const float speed2D = velocity2D.Size();

	// Check if there's movement to stop
	if (speed2D > 0.f)
	{
		// Calculate braking divisor
		const float divisor = actualBrakingFriction * speed2D + FMath::Max(0.f, brakingDecelerationWalking);

		// Check if stopping is possible
		if (divisor > 0.f)
		{
			// Calculate time to stop
			const float timeToStop = speed2D / divisor;

			// Calculate predicted stop location
			predictedStopLocation = velocity2D * timeToStop + 0.5f * ((-actualBrakingFriction) * velocity2D -
				brakingDecelerationWalking * velocity2D.GetSafeNormal()) * FMath::Square(timeToStop);
		}
	}

	return predictedStopLocation;
}

FVector UOLSLocomotionBPLibrary::PredictGroundMovementPivotLocation(const FVector& acceleration,
                                                                    const FVector& velocity, float groundFriction)
{
	FVector predictedPivotLocation = FVector::ZeroVector;

	const FVector acceleration2D = acceleration * FVector(1.f, 1.f, 0.f);

	const float accelerationSize2D = acceleration2D.Size();

	// Check if velocity is along the opposite direction of acceleration
	if ((velocity | acceleration2D) < 0.0f)
	{
		const float speedAlongAcceleration = -(velocity | acceleration2D);
		const float divisor = accelerationSize2D + 2.f * speedAlongAcceleration * groundFriction;

		// Check if stopping is possible
		if (divisor > 0.f)
		{
			const float timeToDirectionChange = speedAlongAcceleration / divisor;

			// Calculate the acceleration force
			const FVector accelerationForce = acceleration - (velocity - acceleration2D * velocity.Size2D()) * groundFriction;

			// Calculate the predicted pivot location
			predictedPivotLocation = velocity * timeToDirectionChange + 0.5f * accelerationForce * timeToDirectionChange * timeToDirectionChange;
		}
	}

	return predictedPivotLocation;
}

EOLSCardinalDirection UOLSLocomotionBPLibrary::SelectCardinalDirectionFromAngle(float angle,
                                                                                float deadZone,
                                                                                EOLSCardinalDirection currentDirection,
                                                                                bool useCurrentDirection /* = false */)
{

	const float absAngle = FMath::Abs(angle);
	float fwdDeadZone = deadZone;
	float bwdDeadZone = deadZone;

	if (useCurrentDirection)
	{
		if (currentDirection == EOLSCardinalDirection::EForward)
		{
			fwdDeadZone *= 2.f;
		}
		else if (currentDirection == EOLSCardinalDirection::EBackward)
		{
			bwdDeadZone *= 2.f;
		}
	}

	if(absAngle <= (45 + fwdDeadZone))
	{
		return EOLSCardinalDirection::EForward;
	}
	else if (absAngle >= (135 - bwdDeadZone))
	{
		return EOLSCardinalDirection::EBackward;
	}
	else if (angle > 0)
	{
		return EOLSCardinalDirection::ERight;
	}

	return EOLSCardinalDirection::ELeft;
}

EOLSCardinalDirection UOLSLocomotionBPLibrary::GetOppositeCardinalDirectional(EOLSCardinalDirection currentDirection)
{
	switch (currentDirection)
	{
		case EOLSCardinalDirection::EForward: {return EOLSCardinalDirection::EBackward;}
		case EOLSCardinalDirection::EBackward: {return EOLSCardinalDirection::EForward;}
		case EOLSCardinalDirection::ELeft: {return EOLSCardinalDirection::ERight;}
		case EOLSCardinalDirection::ERight: {return EOLSCardinalDirection::ELeft;}
	}

	return EOLSCardinalDirection::EForward;
}

EOLSHipDirection UOLSLocomotionBPLibrary::GetOppositeHipDirection(EOLSHipDirection currentHipDirection)
{
	return (currentHipDirection == EOLSHipDirection::EForward ? EOLSHipDirection::EBackward : EOLSHipDirection::EForward);
}

void UOLSLocomotionBPLibrary::TryLinkAnimLayer(USkeletalMeshComponent* mesh,
                                               TSubclassOf<UAnimInstance> animClass,
                                               FName groupName,
                                               bool shouldUnlinkGroupIfInvalid)
{
	if (!animClass->IsValidLowLevelFast())
	{
		if (shouldUnlinkGroupIfInvalid)
		{
			if (const TObjectPtr<UAnimInstance> linkedAnimInstance = mesh->GetLinkedAnimLayerInstanceByGroup(groupName))
			{
				mesh->UnlinkAnimClassLayers(linkedAnimInstance.GetClass());
			}
		}
		
		return;
	}

	mesh->LinkAnimClassLayers(animClass);
}