// Source/TengriPlatformer/Movement/TengriMovementComponent.cpp

#include "TengriMovementComponent.h"
#include "Components/CapsuleComponent.h"
#include "TengriPlatformer/Movement/Collision/TengriCollisionResolver.h"

DEFINE_LOG_CATEGORY_STATIC(LogTengriMovement, Log, All);

// ============================================================================
//                             CONSTANTS
// ============================================================================

namespace TengriMovement
{
	// Maximum physics iterations per frame (prevents infinite loop)
	constexpr int32 MaxPhysicsIterationsPerFrame = 5;

	// Ground snapping thresholds
	constexpr float FastFallThreshold = -200.0f;  // cm/s, skip snap when falling fast
	constexpr float JumpingThreshold = 10.0f;     // cm/s, skip snap when moving up
}

// ============================================================================
//                           CONSTRUCTOR
// ============================================================================

UTengriMovementComponent::UTengriMovementComponent()
{
	PrimaryComponentTick.bCanEverTick = true;
	Velocity = FVector::ZeroVector;
	bIsGrounded = false;
}

// ============================================================================
//                          INITIALIZATION
// ============================================================================

void UTengriMovementComponent::BeginPlay()
{
	Super::BeginPlay();
	InitializeSystem();
}

void UTengriMovementComponent::InitializeSystem()
{
	AActor* Owner = GetOwner();
	if (!Owner)
	{
		UE_LOG(LogTengriMovement, Error, TEXT("InitializeSystem failed: No owner"));
		SetComponentTickEnabled(false);
		return;
	}

	OwnerCapsule = Cast<UCapsuleComponent>(Owner->GetRootComponent());
	if (!OwnerCapsule)
	{
		UE_LOG(LogTengriMovement, Error,
			TEXT("InitializeSystem failed: Owner root component is not a CapsuleComponent"));
		SetComponentTickEnabled(false);
		return;
	}

	if (!MovementConfig)
	{
		UE_LOG(LogTengriMovement, Warning,
			TEXT("InitializeSystem: No MovementConfig assigned"));
		return;
	}

	// Cache thresholds
	CachedThresholds = MovementConfig->GetThresholds();

	// Initialize fixed timestep parameters from config
	FixedTimeStep = 1.0f / MovementConfig->PhysicsTickRate;
	MaxAccumulatorTime = MovementConfig->MaxAccumulatedTime;
	TimeAccumulator = 0.0f;

	// Initialize physics state from current actor transform
	PhysicsLocation = Owner->GetActorLocation();
	PhysicsRotation = Owner->GetActorRotation();
	PhysicsVelocity = FVector::ZeroVector;

	// Initialize render state to match physics (no interpolation on first frame)
	RenderLocation = PhysicsLocation;
	RenderRotation = PhysicsRotation;
	PreviousPhysicsLocation = PhysicsLocation;
	PreviousPhysicsRotation = PhysicsRotation;

	UE_LOG(LogTengriMovement, Log,
		TEXT("System initialized. WalkableZ: %.3f, PhysicsRate: %.0f Hz, FixedStep: %.4f s"),
		CachedThresholds.WalkableZ,
		MovementConfig->PhysicsTickRate,
		FixedTimeStep);
}

// ============================================================================
//                           BLUEPRINT API
// ============================================================================

void UTengriMovementComponent::SetInputVector(FVector NewInput)
{
	InputVector = NewInput.GetClampedToMaxSize(1.0f);
	InputVector.Z = 0.0f;
}

// ============================================================================
//                              TICK
// ============================================================================

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

	if (!MovementConfig || !OwnerCapsule)
	{
		return;
	}

	// ════════════════════════════════════════════════════════════════════
	//                  FIXED TIMESTEP ACCUMULATION
	// ════════════════════════════════════════════════════════════════════

	TimeAccumulator += DeltaTime;

	// Clamp accumulator to prevent "spiral of death"
	if (TimeAccumulator > MaxAccumulatorTime)
	{
		UE_LOG(LogTengriMovement, Warning,
			TEXT("TimeAccumulator clamped: %.3f -> %.3f (frame took too long)"),
			TimeAccumulator, MaxAccumulatorTime);
		TimeAccumulator = MaxAccumulatorTime;
	}

	// ════════════════════════════════════════════════════════════════════
	//                  DETERMINISTIC PHYSICS LOOP
	// ════════════════════════════════════════════════════════════════════

	int32 PhysicsIterations = 0;

	while (TimeAccumulator >= FixedTimeStep)
	{
		// Save state for interpolation BEFORE physics step
		SavePreviousPhysicsState();

		// Run deterministic physics at fixed rate
		TickPhysics(FixedTimeStep);

		// Consume fixed time from accumulator
		TimeAccumulator -= FixedTimeStep;
		PhysicsIterations++;

		// Safety: prevent runaway loop
		if (PhysicsIterations >= TengriMovement::MaxPhysicsIterationsPerFrame)
		{
			UE_LOG(LogTengriMovement, Warning,
				TEXT("Max physics iterations reached (%d), discarding remaining time"),
				TengriMovement::MaxPhysicsIterationsPerFrame);
			TimeAccumulator = 0.0f;
			break;
		}
	}

	// ════════════════════════════════════════════════════════════════════
	//                  INTERPOLATION & RENDERING
	// ════════════════════════════════════════════════════════════════════

	if (MovementConfig->bEnableInterpolation && PhysicsIterations > 0)
	{
		// Calculate interpolation factor [0..1]
		const float Alpha = TimeAccumulator / FixedTimeStep;
		InterpolateRenderState(Alpha);
	}
	else
	{
		// No interpolation: use physics state directly
		RenderLocation = PhysicsLocation;
		RenderRotation = PhysicsRotation;
	}

	// Apply visual transform to actor
	ApplyRenderState();

	// Sync public Velocity for Blueprint access
	Velocity = PhysicsVelocity;
}

// ============================================================================
//                          PHYSICS TICK
// ============================================================================

void UTengriMovementComponent::TickPhysics(float FixedDeltaTime)
{
	// ════════════════════════════════════════════════════════════════════
	//                  Phase 1: Acceleration & Friction
	// ════════════════════════════════════════════════════════════════════

	const float CurrentZ = PhysicsVelocity.Z;
	FVector HorizontalVelocity(PhysicsVelocity.X, PhysicsVelocity.Y, 0.f);

	if (!InputVector.IsNearlyZero())
	{
		const FVector TargetVelocity = InputVector * MovementConfig->MaxSpeed;
		HorizontalVelocity = FMath::VInterpTo(
			HorizontalVelocity,
			TargetVelocity,
			FixedDeltaTime,
			MovementConfig->Acceleration
		);
	}
	else
	{
		HorizontalVelocity = FMath::VInterpTo(
			HorizontalVelocity,
			FVector::ZeroVector,
			FixedDeltaTime,
			MovementConfig->Friction
		);
	}

	PhysicsVelocity = HorizontalVelocity;
	PhysicsVelocity.Z = CurrentZ;

	// ════════════════════════════════════════════════════════════════════
	//                  Phase 2: Rotation
	// ════════════════════════════════════════════════════════════════════

	const float MinSpeedSq = FMath::Square(MovementConfig->MinSpeedForRotation);

	if (PhysicsVelocity.SizeSquared2D() > MinSpeedSq)
	{
		FRotator TargetRot = PhysicsVelocity.ToOrientationRotator();
		TargetRot.Pitch = 0.0f;
		TargetRot.Roll = 0.0f;

		PhysicsRotation = FMath::RInterpConstantTo(
			PhysicsRotation,
			TargetRot,
			FixedDeltaTime,
			MovementConfig->RotationSpeed
		);
	}

	// ════════════════════════════════════════════════════════════════════
	//                  Phase 3: Gravity
	// ════════════════════════════════════════════════════════════════════

	if (!bIsGrounded)
	{
		PhysicsVelocity.Z -= MovementConfig->Gravity * FixedDeltaTime;
	}
	else
	{
		PhysicsVelocity.Z = 0.0f;
	}

	// ════════════════════════════════════════════════════════════════════
	//                  Phase 4: Collision Resolution
	// ════════════════════════════════════════════════════════════════════

	const FVector DesiredDelta = PhysicsVelocity * FixedDeltaTime;

	const FTengriSweepResult MoveResult = UTengriCollisionResolver::ResolveMovement(
		this,
		PhysicsLocation,
		DesiredDelta,
		OwnerCapsule,
		CachedThresholds,
		MovementConfig->MaxStepHeight,
		MovementConfig->MaxSlideIterations,
		false
	);

	PhysicsLocation = MoveResult.Location;

	// ════════════════════════════════════════════════════════════════════
	//                  Phase 5: Ground Snapping
	// ════════════════════════════════════════════════════════════════════

	FHitResult SnapHit;
	const bool bJustSnapped = PerformGroundSnapping(PhysicsLocation, SnapHit);

	if (bJustSnapped && !InputVector.IsNearlyZero())
	{
		// Preserve momentum along slope
		PhysicsVelocity = UTengriCollisionResolver::ProjectVelocity(
			PhysicsVelocity,
			SnapHit.ImpactNormal
		);
	}

	// ════════════════════════════════════════════════════════════════════
	//                  Phase 6: State Update
	// ════════════════════════════════════════════════════════════════════

	// Determine grounded state from snap or collision
	const bool bHitWalkable = MoveResult.bBlocked &&
		CachedThresholds.IsWalkable(MoveResult.Hit.ImpactNormal.Z);

	bIsGrounded = bJustSnapped || bHitWalkable;

	// Prevent Z velocity accumulation when grounded
	if (bIsGrounded && PhysicsVelocity.Z < 0.f)
	{
		PhysicsVelocity.Z = 0.f;
	}
}

// ============================================================================
//                       INTERPOLATION
// ============================================================================

void UTengriMovementComponent::SavePreviousPhysicsState()
{
	PreviousPhysicsLocation = PhysicsLocation;
	PreviousPhysicsRotation = PhysicsRotation;
}

void UTengriMovementComponent::InterpolateRenderState(float Alpha)
{
	Alpha = FMath::Clamp(Alpha, 0.0f, 1.0f);

	// Linear interpolation for location
	RenderLocation = FMath::Lerp(PreviousPhysicsLocation, PhysicsLocation, Alpha);

	// Lerp for rotation (FMath::Lerp handles FRotator correctly)
	RenderRotation = FMath::Lerp(PreviousPhysicsRotation, PhysicsRotation, Alpha);
}

void UTengriMovementComponent::ApplyRenderState() const
{
	if (AActor* Owner = GetOwner())
	{
		Owner->SetActorLocation(RenderLocation, false, nullptr, ETeleportType::None);
		Owner->SetActorRotation(RenderRotation, ETeleportType::None);
	}
}

// ============================================================================
//                       GROUND SNAPPING
// ============================================================================

bool UTengriMovementComponent::PerformGroundSnapping(
	FVector& InOutLocation,
	FHitResult& OutSnapHit) const
{
	// Skip snap when clearly airborne intentionally
	const bool bIsFallingFast = PhysicsVelocity.Z < TengriMovement::FastFallThreshold;

	if (const bool bIsJumping = PhysicsVelocity.Z > TengriMovement::JumpingThreshold; !bIsGrounded && (bIsFallingFast || bIsJumping))
	{
		return false;
	}

	// Use physics location as start point (NOT render position!)
	const FVector Start = InOutLocation;
	const FVector End = Start - FVector(0.f, 0.f, MovementConfig->GroundSnapDistance);

	const FTengriSweepResult Sweep = UTengriCollisionResolver::PerformSweep(
		this,
		Start,
		End,
		OwnerCapsule,
		false
	);

	if (Sweep.bBlocked && CachedThresholds.IsWalkable(Sweep.Hit.ImpactNormal.Z))
	{
		// Apply micro-offset to prevent floor penetration
		InOutLocation = Sweep.Location + FVector(0.f, 0.f, MovementConfig->GroundSnapOffset);
		OutSnapHit = Sweep.Hit;
		return true;
	}

	return false;
}