/**
 * A spring-based movement component that uses half-life values to control easing.
 * Many functions here are taken from the default CharacterMovementComponent and modified to use spring dynamics.
 * Next release of this plugin will aim to duplicate less of the CharacterMovementComponent functionality, for future-proofing.
 * Uses concepts introduced in "Spring-It-On: The Game Developer's Spring-Roll-Call": https://theorangeduck.com/page/spring-roll-call
 */

#include "SpringMovementComponent.h"
#include "EngineStats.h"
#include "Kismet/KismetMathLibrary.h"
#include "KismetAnimationLibrary.h"
#include "GameFramework/Character.h"
#include "PoseSearch/PoseSearchTrajectoryTypes.h"
#include "Engine/World.h"
#include "GameFramework/Actor.h"
#include "GameFramework/Controller.h"
#include "Components/SkeletalMeshComponent.h"
#include "DrawDebugHelpers.h"
#include "Misc/EngineVersionComparison.h"

/**
 * Character stats
 */
DECLARE_CYCLE_STAT(TEXT("Char Update Acceleration"), STAT_CharUpdateAcceleration, STATGROUP_Character);

// CVars
namespace CharacterMovementCVars
{
	static int32 bPreventNonVerticalOrientationBlock = 1;
	FAutoConsoleVariableRef CVarPreventNonVerticalOrientationBlock(
		TEXT("p.PreventNonVerticalOrientationBlock"),
		bPreventNonVerticalOrientationBlock,
		TEXT("When enabled, this allows a character that's supposed to remain vertical to snap to a vertical orientation even if RotationRate settings would block it. See @ShouldRemainVertical and @RotationRate."),
		ECVF_Default);
}

USpringMovementComponent::USpringMovementComponent()
{
	SetIsReplicatedByDefault(true);
}

void USpringMovementComponent::GetLifetimeReplicatedProps(TArray<FLifetimeProperty> &OutLifetimeProps) const
{
	Super::GetLifetimeReplicatedProps(OutLifetimeProps);

	DOREPLIFETIME_CONDITION(USpringMovementComponent, ReplicatedInputVector, COND_SkipOwner);
	DOREPLIFETIME_CONDITION(USpringMovementComponent, ReplicatedControlRotation, COND_SkipOwner);
	DOREPLIFETIME_CONDITION(USpringMovementComponent, ReplicatedSmoothedYawRate, COND_SkipOwner);

	DOREPLIFETIME(USpringMovementComponent, VelocityHalflife);
	DOREPLIFETIME(USpringMovementComponent, RotationHalflife);
	DOREPLIFETIME(USpringMovementComponent, BrakingHalflifeScalar);
}

void USpringMovementComponent::ServerUpdateIntent_Implementation(FVector_NetQuantize InInputVector, FRotator InControlRotation, float InSmoothedYawRate)
{
	// Runs on server: store values into replicated properties, which then replicate to simulated proxies
	/*UE_LOG(LogTemp, Verbose, TEXT("[SpringMC] ServerUpdateIntent received — Input: %s, Yaw: %.1f, SmoothedYawRate: %.1f"),
	 *InInputVector.ToString(), InControlRotation.Yaw, InSmoothedYawRate);*/

	ReplicatedInputVector = InInputVector;
	ReplicatedControlRotation = InControlRotation;
	ReplicatedSmoothedYawRate = InSmoothedYawRate;

	// Set TargetVelocity on the server so CalcVelocity has the correct goal during move replay.
	// (ControlledCharacterMove doesn't run on the server for client-owned characters — the server
	// replays client moves via MoveAutonomous, and without this, TargetVelocity stays at zero.)
	SmoothedYawRate = InSmoothedYawRate;
	ApplyMovementIntent(FVector(InInputVector));
}

void USpringMovementComponent::OnRep_ReplicatedIntent()
{
	if (!bHasReceivedReplicatedIntent)
	{
		UE_LOG(LogTemp, Log, TEXT("[SpringMC] OnRep_ReplicatedIntent — first intent received on simulated proxy!"));
	}

	bHasReceivedReplicatedIntent = true;
}

void USpringMovementComponent::TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction *ThisTickFunction)
{
	LastDeltaSeconds = DeltaTime;
	Super::TickComponent(DeltaTime, TickType, ThisTickFunction);

	// Record the current yaw rate
	if (GetCharacterOwner() && GetCharacterOwner()->GetController())
	{
		ControlRotation = GetCharacterOwner()->GetController()->GetControlRotation();
		if (bUseArcDamping || bUseYawRateForPrediction)
		{
			CurrentYawRate = (UKismetMathLibrary::NormalizedDeltaRotator(ControlRotation, LastControlRotation)).Yaw / DeltaTime;
			if (bSmoothYawRate)
			{
				SmoothYawRate(DeltaTime);
			}
			else
			{
				SmoothedYawRate = CurrentYawRate;
			}
		}
		LastControlRotation = ControlRotation;
	}

#if UE_BUILD_DEBUG || UE_BUILD_DEVELOPMENT
	if (bDrawDebugTrajectory)
	{
		DrawDebugTrajectory();
	}
#endif
}

void USpringMovementComponent::BeginPlay()
{
	Super::BeginPlay();
	FutureSamples.SetNum(TrajectorySampleTimes.Num());

	if (!GetCharacterOwner())
	{
		return;
	}

	// Initialise values for trajectory prediction
	if (bUseControllerDesiredRotation && GetCharacterOwner()->GetController())
	{
		LastDesiredRotation = GetCharacterOwner()->GetController()->GetControlRotation();
	}
	else
	{
		LastDesiredRotation = GetCharacterOwner()->GetActorRotation();
	}

	// Initialize control rotations for yaw rate tracking
	ControlRotation = LastDesiredRotation;
	LastControlRotation = LastDesiredRotation;
}

void USpringMovementComponent::Predict(FTransformTrajectory &InOutTrajectory, int32 NumPredictionSamples, float SecondsPerPredictionSample, int32 NumHistorySamples)
{
	/* In 5.7, NumPredictionSamples only includes future samples, but we still need to write the current sample, so must add 1 to the total*/
#if UE_VERSION_NEWER_THAN(5, 7, 0)
	const int TotalPredictionSamples = NumPredictionSamples + 1;
#else
	const int TotalPredictionSamples = NumPredictionSamples;
#endif

	if (!GetOwner())
	{
		return;
	}

	// First, resize the local array to match the number of prediction samples
	if (TrajectorySampleTimes.Num() != TotalPredictionSamples)
	{
		TrajectorySampleTimes.SetNum(TotalPredictionSamples);
		FutureSamples.SetNum(TotalPredictionSamples);
	}

	// Then, resize & populate the history samples array to match the number of history samples (purely for debugging)
	if (bDrawDebugTrajectory)
	{
		if (HistorySamples.Num() != NumHistorySamples)
		{
			HistorySamples.SetNum(NumHistorySamples);
		}

		for (int i = 0; i < NumHistorySamples; ++i)
		{
			HistorySamples[i].MeshPosition = InOutTrajectory.Samples[i].Position;
			HistorySamples[i].MeshRotation = InOutTrajectory.Samples[i].Facing.Rotator();
			HistorySamples[i].Time = InOutTrajectory.Samples[i].TimeInSeconds;
			HistorySamples[i].Position = HistorySamples[i].MeshPosition - (GetCharacterOwner() ? GetCharacterOwner()->GetMesh()->GetRelativeLocation() : FVector::ZeroVector);
			HistorySamples[i].Rotation = HistorySamples[i].MeshRotation - (GetCharacterOwner() ? GetCharacterOwner()->GetMesh()->GetRelativeRotation() : FRotator::ZeroRotator);
		}
	}

	// Now, populate the sample times array
	for (int32 i = 0; i < TotalPredictionSamples; ++i)
	{
		/* Have to add Last Delta seconds onto the prediction time, because a time of 0.0s is counted as history. See PoseSearchTrajectoryLibrary.cpp:
		   "Note we add current frame at delta time in the future since t = 0 should be the previous frame" */
		TrajectorySampleTimes[i] = (i * SecondsPerPredictionSample) + LastDeltaSeconds;
	}

	// Then, run our internal prediction logic
	PredictTrajectory(TrajectorySampleTimes, FutureSamples);

	if (InOutTrajectory.Samples.Num() < (NumHistorySamples + FutureSamples.Num()))
	{
		UE_LOG(LogTemp, Warning, TEXT("InOutTrajectory Samples array does not have enough space for %i predicted samples"), FutureSamples.Num());
	}
	else
	{
		for (int32 i = 0; i < TotalPredictionSamples; ++i)
		{
			const int PoseSampleIdx = i + NumHistorySamples;
			InOutTrajectory.Samples[PoseSampleIdx].Position = FutureSamples[i].MeshPosition;
			InOutTrajectory.Samples[PoseSampleIdx].Facing = FutureSamples[i].MeshRotation.Quaternion();
			InOutTrajectory.Samples[PoseSampleIdx].TimeInSeconds = TrajectorySampleTimes[i];
		}
	}
}

void USpringMovementComponent::GetGravity(FVector &OutGravityAccel)
{
	if (!GetOwner())
	{
		UE_LOG(LogTemp, Log, TEXT("Calling GetGravity without a Spring Movement Component. Return value will be defaulted."));
		OutGravityAccel = FVector::ZeroVector;
		return;
	}

	OutGravityAccel = GravityScale * GetGravityDirection();
}

void USpringMovementComponent::GetCurrentState(FVector &OutPosition, FQuat &OutFacing, FVector &OutVelocity)
{
	if (!GetCharacterOwner())
	{
		// UE_LOG(LogTemp, Log, TEXT("Calling GetCurrentState without an Owning Character. Return values will be defaulted."));
		OutPosition = OutVelocity = FVector::ZeroVector;
		OutFacing = FQuat::Identity;
		return;
	}

	if (const USceneComponent *VisualComp = GetCharacterOwner()->GetMesh())
	{
		OutPosition = VisualComp->GetComponentLocation();
		OutFacing = VisualComp->GetComponentRotation().Quaternion();
	}
	else
	{
		UE_LOG(LogTemp, Warning, TEXT("Calling GetCurrentState without a valid Mesh Component."));
		OutPosition = GetOwner()->GetActorLocation();
		OutFacing = GetOwner()->GetActorRotation().Quaternion();
	}
}

void USpringMovementComponent::GetVelocity(FVector &OutVelocity)
{
	OutVelocity = Velocity;
}

float USpringMovementComponent::GetMaxSpeed() const
{
	if (!GetCharacterOwner() || !GetCharacterOwner()->GetController())
	{
		return Super::GetMaxSpeed();
	}

	return ComputeSpeedScalingFactor() * Super::GetMaxSpeed();
}

float USpringMovementComponent::GetMinAnalogSpeed() const
{
	if (!GetCharacterOwner() || !GetCharacterOwner()->GetController())
	{
		return Super::GetMinAnalogSpeed();
	}

	return ComputeSpeedScalingFactor() * Super::GetMinAnalogSpeed();
}

// Copied from UCharacterMovementComponent and slightly altered
void USpringMovementComponent::CalcVelocity(float DeltaTime, float Friction, bool bFluid, float BrakingDeceleration)
{
	const float MaxAccel = GetMaxAcceleration();

	// Do not update velocity when using root motion or when SimulatedProxy and not simulating root motion - SimulatedProxy are repped their Velocity
	if (!HasValidData() || HasAnimRootMotion() || DeltaTime < MIN_TICK_TIME || (CharacterOwner && CharacterOwner->GetLocalRole() == ROLE_SimulatedProxy && !bWasSimulatingRootMotion))
	{
		return;
	}

	SpringVelocityUpdate(Position, Velocity, Acceleration, TargetVelocity, GetCurrentVelocityHalflife(GetLastInputVector()), DeltaTime);
}

void USpringMovementComponent::PhysicsRotation(float DeltaTime)
{
	if (!(bOrientRotationToMovement || bUseControllerDesiredRotation))
	{
		return;
	}

	const bool bIsSimulatedProxy = CharacterOwner && CharacterOwner->GetLocalRole() == ROLE_SimulatedProxy;
	if (!HasValidData() || (!bIsSimulatedProxy && !CharacterOwner->Controller && !bRunPhysicsWithNoController))
	{
		return;
	}

	FQuat GravityToWorldTransform_Local = GetGravityToWorldTransform();
	FQuat WorldToGravityTransform_Local = GetWorldToGravityTransform();

	FRotator CurrentRotation = UpdatedComponent->GetComponentRotation(); // Normalized
	CurrentRotation.DiagnosticCheckNaN(TEXT("CharacterMovementComponent::PhysicsRotation(): CurrentRotation"));

	// Determine equality tolerance
	const float AngleTolerance = 1e-3f;

	FRotator EmptyRot = FRotator::ZeroRotator;

	DesiredRotation = CurrentRotation;

	if (bOrientRotationToMovement)
	{
		DesiredRotation = ComputeOrientToMovementRotation(CurrentRotation, DeltaTime, EmptyRot);
	}
	else if (bUseControllerDesiredRotation)
	{
		// Resolve the desired controller rotation.
		// For simulated proxies (no controller), fall back to the replicated controller rotation.
		FRotator ControllerDesiredRot = FRotator::ZeroRotator;
		bool bHasControllerRotation = false;

		AController *TargetController = CharacterOwner->Controller;
		if (!TargetController && bRunPhysicsWithNoController)
		{
			TargetController = Cast<AController>(CharacterOwner->GetOwner());
		}

		if (TargetController)
		{
			ControllerDesiredRot = TargetController->GetDesiredRotation();
			bHasControllerRotation = true;
		}
		else if (bIsSimulatedProxy && bHasReceivedReplicatedIntent)
		{
			ControllerDesiredRot = ReplicatedControlRotation;
			bHasControllerRotation = true;
		}

		if (bHasControllerRotation)
		{
			const float AngleDiff = FMath::FindDeltaAngleDegrees(CurrentRotation.Yaw, ControllerDesiredRot.Yaw);

			// If we're grounded and stationary, & the rotation has reached the last desired target, then only update desired rotation if we are over the turn-in-place threshold
			if (!IsGroundedAndStationary() || !CurrentRotation.Equals(LastDesiredRotation, 0.1f) || FMath::Abs(AngleDiff) > TurnInPlaceAngleThreshold)
			{
				DesiredRotation = ControllerDesiredRot;
			}
		}
	}
	else
	{
		return;
	}

	const bool bWantsToBeVertical = ShouldRemainVertical();

	if (bWantsToBeVertical)
	{
		if (HasCustomGravity())
		{
			FRotator GravityRelativeDesiredRotation = (GravityToWorldTransform_Local * DesiredRotation.Quaternion()).Rotator();
			GravityRelativeDesiredRotation.Pitch = 0.f;
			GravityRelativeDesiredRotation.Yaw = FRotator::NormalizeAxis(GravityRelativeDesiredRotation.Yaw);
			GravityRelativeDesiredRotation.Roll = 0.f;
			DesiredRotation = (WorldToGravityTransform_Local * GravityRelativeDesiredRotation.Quaternion()).Rotator();
		}
		else
		{
			DesiredRotation.Pitch = 0.f;
			DesiredRotation.Yaw = FRotator::NormalizeAxis(DesiredRotation.Yaw);
			DesiredRotation.Roll = 0.f;
		}
	}
	else
	{
		DesiredRotation.Normalize();
	}

	// Compute new rotation using spring damping
	if (!CurrentRotation.Equals(DesiredRotation, AngleTolerance))
	{
		LastDesiredRotation = DesiredRotation;
		SpringRotationUpdate(DesiredRotation, RotationHalflife, DeltaTime);
		FRotator DeltaRot = GetDeltaRotation(DeltaTime);
		DeltaRot.DiagnosticCheckNaN(TEXT("CharacterMovementComponent::PhysicsRotation(): GetDeltaRotation"));

		// If we'd be prevented from becoming vertical, override the non-yaw rotation rates to allow the character to snap upright
		if (CharacterMovementCVars::bPreventNonVerticalOrientationBlock && bWantsToBeVertical)
		{
			if (FMath::IsNearlyZero(DeltaRot.Pitch))
			{
				DeltaRot.Pitch = 360.0;
			}
			if (FMath::IsNearlyZero(DeltaRot.Roll))
			{
				DeltaRot.Roll = 360.0;
			}
		}

		if (HasCustomGravity())
		{
			FRotator GravityRelativeCurrentRotation = (GravityToWorldTransform_Local * CurrentRotation.Quaternion()).Rotator();
			FRotator GravityRelativeDesiredRotation = (GravityToWorldTransform_Local * DesiredRotation.Quaternion()).Rotator();

			// PITCH
			if (!FMath::IsNearlyEqual(GravityRelativeCurrentRotation.Pitch, GravityRelativeDesiredRotation.Pitch, AngleTolerance))
			{
				GravityRelativeDesiredRotation.Pitch = FMath::FixedTurn(GravityRelativeCurrentRotation.Pitch, GravityRelativeDesiredRotation.Pitch, DeltaRot.Pitch);
			}

			// YAW
			if (!FMath::IsNearlyEqual(GravityRelativeCurrentRotation.Yaw, GravityRelativeDesiredRotation.Yaw, AngleTolerance))
			{
				GravityRelativeDesiredRotation.Yaw = FMath::FixedTurn(GravityRelativeCurrentRotation.Yaw, GravityRelativeDesiredRotation.Yaw, DeltaRot.Yaw);
			}

			// ROLL
			if (!FMath::IsNearlyEqual(GravityRelativeCurrentRotation.Roll, GravityRelativeDesiredRotation.Roll, AngleTolerance))
			{
				GravityRelativeDesiredRotation.Roll = FMath::FixedTurn(GravityRelativeCurrentRotation.Roll, GravityRelativeDesiredRotation.Roll, DeltaRot.Roll);
			}

			DesiredRotation = (WorldToGravityTransform_Local * GravityRelativeDesiredRotation.Quaternion()).Rotator();
		}
		else
		{
			// PITCH
			if (!FMath::IsNearlyEqual(CurrentRotation.Pitch, DesiredRotation.Pitch, AngleTolerance))
			{
				DesiredRotation.Pitch = FMath::FixedTurn(CurrentRotation.Pitch, DesiredRotation.Pitch, DeltaRot.Pitch);
			}

			// YAW
			if (!FMath::IsNearlyEqual(CurrentRotation.Yaw, DesiredRotation.Yaw, AngleTolerance))
			{
				DesiredRotation.Yaw = FMath::FixedTurn(CurrentRotation.Yaw, DesiredRotation.Yaw, DeltaRot.Yaw);
			}

			// ROLL
			if (!FMath::IsNearlyEqual(CurrentRotation.Roll, DesiredRotation.Roll, AngleTolerance))
			{
				DesiredRotation.Roll = FMath::FixedTurn(CurrentRotation.Roll, DesiredRotation.Roll, DeltaRot.Roll);
			}
		}

		// Set the new rotation.
		DesiredRotation.DiagnosticCheckNaN(TEXT("CharacterMovementComponent::PhysicsRotation(): DesiredRotation"));
		MoveUpdatedComponent(FVector::ZeroVector, DesiredRotation, /*bSweep*/ false);
	}
	else
	{
		AngVel = FRotator::ZeroRotator;
	}
}

FRotator USpringMovementComponent::ComputeOrientToMovementRotation(const FRotator &CurrentRotation, float DeltaTime, FRotator &DeltaRotation) const
{
	if (TargetVelocity.SizeSquared() < UE_KINDA_SMALL_NUMBER)
	{
		// AI path following request can orient us in that direction (it's effectively an acceleration)
		if (bHasRequestedVelocity && RequestedVelocity.SizeSquared() > UE_KINDA_SMALL_NUMBER)
		{
			return RequestedVelocity.GetSafeNormal().Rotation();
		}

		// If bOrientToMovementWhenStationary is true, then face the last input vector if there's no acceleration
		if (bOrientToMovementWhenStationary && !LastNonzeroInputVector.IsNearlyZero())
		{
			return LastNonzeroInputVector.Rotation();
		}
		else
		{
			return CurrentRotation;
		}
	}

	// Rotate toward direction of acceleration.
	return TargetVelocity.GetSafeNormal().Rotation();
}

void USpringMovementComponent::ControlledCharacterMove(const FVector &InputVector, float DeltaSeconds)
{
	{
		SCOPE_CYCLE_COUNTER(STAT_CharUpdateAcceleration);

		// We need to check the jump state before adjusting input acceleration, to minimize latency
		// and to make sure acceleration respects our potentially new falling state.
		CharacterOwner->CheckJumpInput(DeltaSeconds);

		ApplyMovementIntent(InputVector);
	}

	if (CharacterOwner->GetLocalRole() == ROLE_Authority)
	{
		// Store intent for replication to simulated proxies on other clients (listen server host path)
		ReplicatedInputVector = FVector_NetQuantize(InputVector);
		ReplicatedControlRotation = ControlRotation;
		ReplicatedSmoothedYawRate = SmoothedYawRate;

		PerformMovement(DeltaSeconds);
	}
	else if (CharacterOwner->GetLocalRole() == ROLE_AutonomousProxy && IsNetMode(NM_Client))
	{
		ReplicateMoveToServer(DeltaSeconds, Acceleration);

		// Send intent to server for replication to simulated proxies
		UE_LOG(LogTemp, Verbose, TEXT("[SpringMC] ControlledCharacterMove — sending ServerUpdateIntent, Input: %s"),
			   *InputVector.ToString());
		ServerUpdateIntent(FVector_NetQuantize(InputVector), ControlRotation, SmoothedYawRate);
	}
}

void USpringMovementComponent::SimulateMovement(float DeltaSeconds)
{
	if (!CharacterOwner || !bHasReceivedReplicatedIntent)
	{
		UE_LOG(LogTemp, Verbose, TEXT("[SpringMC] SimulateMovement — falling back to Super (no intent yet)"));
		Super::SimulateMovement(DeltaSeconds);
		return;
	}

	UE_LOG(LogTemp, Verbose, TEXT("[SpringMC] SimulateMovement — running spring sim, TargetVel: %s, Vel: %s"),
		   *TargetVelocity.ToString(), *Velocity.ToString());

	// --- Set up intent from replicated data ---
	SmoothedYawRate = ReplicatedSmoothedYawRate;
	ControlRotation = ReplicatedControlRotation;

	const FVector InputVec = FVector(ReplicatedInputVector);
	ApplyMovementIntent(InputVec);

	// Update LastDesiredRotation for trajectory prediction
	LastDesiredRotation = ReplicatedControlRotation;

	// --- Run spring velocity update ---
	SpringVelocityUpdate(Position, Velocity, Acceleration, TargetVelocity, GetCurrentVelocityHalflife(InputVec), DeltaSeconds);

	// --- Run spring rotation ---
	// PhysicsRotation is not called for simulated proxies by default,
	// so we call it explicitly. The PhysicsRotation override handles the
	// no-controller case by falling back to ReplicatedControlRotation.
	PhysicsRotation(DeltaSeconds);

	// --- Let base class handle floor/collision/movement application ---
	// Super will use our spring-computed Velocity for movement.
	// If a server position correction arrives, Super smoothly corrects toward it.
	Super::SimulateMovement(DeltaSeconds);
}

void USpringMovementComponent::PredictTrajectory(TArray<float> &SampleTimes, TArray<FSpringTrajectorySample> &Samples)
{
	const FRotator CurrentRotation = GetOwner()->GetActorRotation();

	// March forward (carry state across samples) so the yaw goal ramps gradually, like the real sim.
	// Evaluating each sample against the full yaw delta held constant instead curves the path ~2x too tight.
	FVector PredPos = GetActorLocation();
	FVector PredVel = Velocity;
	FVector PredAccel = Acceleration;
	FRotator PredRot = FRotator::ZeroRotator; // delta from CurrentRotation
	FRotator PredAngVel = AngVel;
	float PrevTime = 0.f;

	const float VelHalflife = GetCurrentVelocityHalflife(GetLastInputVector());

	// Cumulative predicted yaw by future time t. With decay the rate ramps linearly to zero over
	// YawRateDecayTime (arc stops tightening once the player likely eased off); else it's a constant rate.
	const auto CumulativeYaw = [&](float t) -> float
	{
		if (!bUseYawRateForPrediction)
		{
			return 0.f;
		}
		const float Rate = FMath::Clamp(SmoothedYawRate, -MaxYawRateForPrediction, MaxYawRateForPrediction);
		if (bEnableYawRateDecay && YawRateDecayTime > UE_KINDA_SMALL_NUMBER)
		{
			const float tc = FMath::Min(t, YawRateDecayTime);
			return Rate * (tc - tc * tc / (2.f * YawRateDecayTime));
		}
		return Rate * t;
	};

	for (int32 i = 0; i < SampleTimes.Num(); i++)
	{
		const float StepTime = SampleTimes[i] - PrevTime;
		// Midpoint = time-average of the ramping yaw over this step (most accurate constant goal).
		const float MidTime = (PrevTime + SampleTimes[i]) * 0.5f;
		PrevTime = SampleTimes[i];

		const float dYaw = CumulativeYaw(MidTime);

		// Target velocity turns with the controller's yaw; step the velocity/position spring.
		const FVector PredictedTargetVel = FRotator(0.f, dYaw, 0.f).RotateVector(TargetVelocity);
		SpringVelocityUpdate(PredPos, PredVel, PredAccel, PredictedTargetVel, VelHalflife, StepTime);

		// Facing goal advances at the same rate; step the rotation spring (delta space).
		if (bUseControllerDesiredRotation || !FMath::IsNearlyZero(TargetVelocity.Length()))
		{
			const FRotator SampleGoal(LastDesiredRotation.Pitch, LastDesiredRotation.Yaw + dYaw, LastDesiredRotation.Roll);
			const FRotator DeltaRotation = UKismetMathLibrary::NormalizedDeltaRotator(SampleGoal, CurrentRotation);
			simple_spring_damper_exact(PredRot.Pitch, PredAngVel.Pitch, DeltaRotation.Pitch, RotationHalflife, StepTime);
			simple_spring_damper_exact(PredRot.Yaw, PredAngVel.Yaw, DeltaRotation.Yaw, RotationHalflife, StepTime);
			simple_spring_damper_exact(PredRot.Roll, PredAngVel.Roll, DeltaRotation.Roll, RotationHalflife, StepTime);
		}

		Samples[i].Position = PredPos;
		Samples[i].Velocity = PredVel;
		Samples[i].Acceleration = PredAccel;

		// Add base rotation onto delta to get absolute rotation
		Samples[i].Rotation = PredRot;
		Samples[i].Rotation.Add(CurrentRotation.Pitch, CurrentRotation.Yaw, CurrentRotation.Roll);
		Samples[i].Rotation.Normalize();

		if (GetCharacterOwner())
		{
			Samples[i].MeshPosition = Samples[i].Position + GetCharacterOwner()->GetMesh()->GetRelativeLocation();
			Samples[i].MeshRotation = Samples[i].Rotation + GetCharacterOwner()->GetMesh()->GetRelativeRotation();
			Samples[i].MeshRotation.Normalize();
		}
	}
}

float USpringMovementComponent::ComputeAnalogInputModifier() const
{
	const float MaxSpeed = GetMaxSpeed();
	if (TargetVelocity.SizeSquared() > 0.f && MaxSpeed > UE_SMALL_NUMBER)
	{
		return FMath::Clamp<FVector::FReal>(TargetVelocity.Size() / MaxSpeed, 0.f, 1.f);
	}

	return 0.f;
}

FVector USpringMovementComponent::ConstrainTargetVelocity(const FVector &InputVelocity) const
{
	/*// walking or falling pawns ignore up/down sliding
	const double InputAccelerationDotGravityNormal = FVector::DotProduct(InputVelocity, -GetGravityDirection());
	if (!FMath::IsNearlyZero(InputAccelerationDotGravityNormal) && (IsMovingOnGround() || IsFalling()))
	{
		return FVector::VectorPlaneProject(InputVelocity, -GetGravityDirection());
	}*/

	return InputVelocity.GetSafeNormal2D();
}

void USpringMovementComponent::SmoothYawRate(float DeltaTime)
{
	const float SmoothingSpeed = FMath::IsNearlyZero(CurrentYawRate, 0.1f) ? YawRateSmoothingZeroSpeed : YawRateSmoothingSpeed;
	SmoothedYawRate = UKismetMathLibrary::FInterpTo(SmoothedYawRate, CurrentYawRate, DeltaTime, SmoothingSpeed);
}

bool USpringMovementComponent::IsGroundedAndStationary() const
{
	return (
		TargetVelocity.SizeSquared2D() < UE_KINDA_SMALL_NUMBER &&
		Velocity.SizeSquared2D() < UE_KINDA_SMALL_NUMBER &&
		IsMovingOnGround());
}

void USpringMovementComponent::ApplyMovementIntent(const FVector &InputVector)
{
	AnalogInputModifier = FMath::Clamp(InputVector.Length(), 0.0f, 1.0f);
	const float moveSpeed = FMath::Lerp(GetMinAnalogSpeed(), GetMaxSpeed(), AnalogInputModifier);

	if (!InputVector.IsNearlyZero())
	{
		LastNonzeroInputVector = InputVector.GetSafeNormal();
	}

	TargetVelocity = moveSpeed * ConstrainTargetVelocity(InputVector);
}

void USpringMovementComponent::DrawDebugTrajectory()
{
	for (int i = 0; i < FutureSamples.Num(); i++)
	{
		DrawDebugSphere(GetWorld(), FutureSamples[i].MeshPosition, 4.0f, 8, FColor::Blue, false, -1.0f, 0, 2.0f);
		if (i > 0)
		{
			DrawDebugLine(GetWorld(), FutureSamples[i - 1].MeshPosition, FutureSamples[i].MeshPosition, FColor::Blue, false, -1.0f, 0, 1.0f);
		}
		else if (i == 0 && !HistorySamples.IsEmpty())
		{
			DrawDebugLine(GetWorld(), HistorySamples.Last().MeshPosition, FutureSamples[i].MeshPosition, FColor::Red, false, -1.0f, 0, 1.0f);
		}
	}

	for (int i = 0; i < HistorySamples.Num(); i++)
	{
		DrawDebugSphere(GetWorld(), HistorySamples[i].MeshPosition, 4.0f, 8, FColor::Red, false, -1.0f, 0, 2.0f);
		if (i > 0)
		{
			DrawDebugLine(GetWorld(), HistorySamples[i - 1].MeshPosition, HistorySamples[i].MeshPosition, FColor::Red, false, -1.0f, 0, 1.0f);
		}
	}
}

float GetAxisDeltaRotation(float InAxisRotationRate, float DeltaTime)
{
	// Values over 360 don't do anything, see FMath::FixedTurn. However we are trying to avoid giant floats from overflowing other calculations.
	return (InAxisRotationRate >= 0.f) ? FMath::Min(InAxisRotationRate * DeltaTime, 360.f) : 360.f;
}

FRotator USpringMovementComponent::GetDeltaRotation(float DeltaTime) const
{
	return FRotator(
		GetAxisDeltaRotation(FMath::Abs(AngVel.Pitch), DeltaTime),
		GetAxisDeltaRotation(FMath::Abs(AngVel.Yaw), DeltaTime),
		GetAxisDeltaRotation(FMath::Abs(AngVel.Roll), DeltaTime));
}

void USpringMovementComponent::SpringVelocityUpdate(FVector &Pos, FVector &Vel, FVector &Accel, const FVector &VelGoal, float Halflife, float DeltaTime)
{
	// Damping coefficient
	float y = HalflifeToDamping(Halflife) / 2.0f;

	FVector j0 = Vel - VelGoal;
	FVector j1 = Accel + j0 * y;

	float eydt = FastNegExp(y * DeltaTime);

	Pos = eydt * (((-j1) / (y * y)) + ((-j0 - j1 * DeltaTime) / y)) +
		  (j1 / (y * y)) + j0 / y + VelGoal * DeltaTime + Pos;

	Vel = eydt * (j0 + j1 * DeltaTime) + VelGoal;
	Accel = eydt * (Accel - j1 * y * DeltaTime);
}

void USpringMovementComponent::SpringRotationUpdate(FRotator &RotGoal, float Halflife, float DeltaTime)
{
	// Assuming this is part of your class, and you're using some kind of current rotation like CurrentRotation.
	// If not, you can directly use the Rotator argument as needed.

	FRotator CurrentRotation = GetOwner()->GetActorRotation(); // or any current rotation reference.

	// Decompose the current rotation and goal rotation
	/*float CurrentPitch = CurrentRotation.Pitch;
	float CurrentYaw = CurrentRotation.Yaw;
	float CurrentRoll = CurrentRotation.Roll;*/

	double CurrentPitch = 0.0f;
	double CurrentYaw = 0.0f;
	double CurrentRoll = 0.0f;

	float GoalPitch = RotGoal.Pitch;
	float GoalYaw = RotGoal.Yaw;
	float GoalRoll = RotGoal.Roll;

	FRotator DeltaRotation = UKismetMathLibrary::NormalizedDeltaRotator(RotGoal, CurrentRotation);

	// Spring damping for each component (Pitch, Yaw, Roll)
	simple_spring_damper_exact(CurrentPitch, AngVel.Pitch, DeltaRotation.Pitch, Halflife, DeltaTime);
	simple_spring_damper_exact(CurrentYaw, AngVel.Yaw, DeltaRotation.Yaw, Halflife, DeltaTime);
	simple_spring_damper_exact(CurrentRoll, AngVel.Roll, DeltaRotation.Roll, Halflife, DeltaTime);
}

float USpringMovementComponent::GetDirectionalSpeedScalingFactor() const
{
	if (!bUseSpeedScaling)
	{
		return 1.0f;
	}

	if (bOrientRotationToMovement || !GetCharacterOwner())
	{
		return 1.0f;
	}
	if (!GetCharacterOwner()->GetController())
	{
		return 1.0f;
	}

	float direction = FMath::Abs(UKismetAnimationLibrary::CalculateDirection(TargetVelocity, ControlRotation));

	// TODO: Maybe use a curve based system for more control? But for now, use thresholds
	if (direction > BackwardsSpeedScaleThreshold)
	{
		return BackwardsSpeedScale;
	}
	else if (direction > StrafeSpeedScaleThreshold)
	{
		return UKismetMathLibrary::MapRangeClamped(
			direction,
			StrafeSpeedScaleThreshold,
			BackwardsSpeedScaleThreshold,
			StrafeSpeedScale,
			BackwardsSpeedScale);
	}
	// Speed scaling starts at 45 degrees
	else if (direction > 45.0f)
	{
		return UKismetMathLibrary::MapRangeClamped(
			direction,
			45.0f,
			StrafeSpeedScaleThreshold,
			1.0f,
			StrafeSpeedScale);
	}
	else
	{
		return 1.0f;
	}
}

float USpringMovementComponent::GetArcDampingFactor() const
{
	if (!bUseArcDamping)
	{
		return 1.0f;
	}

	return UKismetMathLibrary::MapRangeClamped(
		FMath::Abs(SmoothedYawRate),
		ArcDampingRange.GetLowerBoundValue(),
		ArcDampingRange.GetUpperBoundValue(),
		1.0f,
		MaxArcDampingScalar);
}

float USpringMovementComponent::ComputeSpeedScalingFactor() const
{
	return GetDirectionalSpeedScalingFactor() * GetArcDampingFactor();
}
