using System;

namespace Sandbox;

[Title( "Head Bobbing Effect" )]
[Category( "Camera" )]
[Description( "Polished first-person camera motion driven by player velocity." )]
[Icon( "visibility" )]
public sealed class FirstPersonHeadBob : Component
{
	const float MeanAbsoluteCosine = 0.6366198f;

	[Property, Group( "Motion" ), Range( 0f, 2f )]
	public float Amount { get; set; } = 1f;

	[Property, Group( "Motion" ), Range( 0f, 3f )]
	public float HorizontalAmount { get; set; } = 0.85f;

	[Property, Group( "Motion" ), Range( 0f, 3f )]
	public float VerticalAmount { get; set; } = 1.2f;

	[Property, Group( "Motion" ), Range( 0.5f, 4f )]
	public float StepFrequency { get; set; } = 1.9f;

	[Property, Group( "Motion" ), Range( 0f, 0.5f )]
	public float GaitDetail { get; set; } = 0.12f;

	[Property, Group( "Motion" ), Range( 0.25f, 1f )]
	public float CrouchMultiplier { get; set; } = 0.72f;

	[Property, Group( "Idle" ), Range( 0f, 1f )]
	public float IdleAmount { get; set; } = 0.5f;

	[Property, Group( "Idle" ), Range( 0.05f, 1f )]
	public float BreathingFrequency { get; set; } = 0.23f;

	[Property, Group( "Idle" ), Range( 0f, 1f )]
	public float IdlePitchAmount { get; set; } = 0.32f;

	[Property, Group( "Rotation" ), Range( 0f, 2f )]
	public float PitchAmount { get; set; } = 0.38f;

	[Property, Group( "Rotation" ), Range( 0f, 2f )]
	public float RollAmount { get; set; } = 0.62f;

	[Property, Group( "Velocity" ), Range( 0f, 2f )]
	public float VelocityPitchAmount { get; set; } = 0.32f;

	[Property, Group( "Velocity" ), Range( 0f, 3f )]
	public float VelocityRollAmount { get; set; } = 0.75f;

	[Property, Group( "Velocity" ), Range( 0f, 3f )]
	public float InertiaAmount { get; set; } = 0.85f;

	[Property, Group( "Velocity" ), Range( 1f, 30f )]
	public float VelocitySmoothing { get; set; } = 6.5f;

	[Property, Group( "Velocity" ), Range( 1f, 30f )]
	public float InertiaSmoothing { get; set; } = 3.5f;

	[Property, Group( "Response" ), Range( 1f, 30f )]
	public float BlendSpeed { get; set; } = 6.5f;

	[Property, Group( "Response" ), Range( 1f, 30f )]
	public float CadenceSmoothing { get; set; } = 5.5f;

	[Property, Group( "Response" ), Range( 1f, 30f )]
	public float AirborneBlendSpeed { get; set; } = 5f;

	[Property, Group( "Landing" ), Range( 0f, 4f )]
	public float LandingDip { get; set; } = 1.6f;

	[Property, Group( "Landing" ), Range( 1f, 30f )]
	public float LandingSpring { get; set; } = 15f;

	[Property, Group( "Landing" ), Range( 0.1f, 2f )]
	public float LandingDamping { get; set; } = 0.82f;

	[Property, Group( "Speed" ), Range( 0f, 100f )]
	public float MinimumSpeed { get; set; } = 1.5f;

	[Property, Group( "Speed" ), Range( 1f, 500f )]
	public float ReferenceSpeed { get; set; } = 110f;

	[Property, Group( "Speed" ), Range( 1f, 3f )]
	public float MaximumSpeedScale { get; set; } = 1.65f;

	[Property, Group( "Safety" ), Range( 0.5f, 8f )]
	public float MaximumPositionOffset { get; set; } = 3.5f;

	[Property, Group( "Safety" ), Range( 0.5f, 5f )]
	public float MaximumRotationOffset { get; set; } = 2.5f;

	PlayerController Controller { get; set; }

	float _gaitPhase;
	float _idlePhase;
	float _movementStrength;
	float _cadence;
	float _landingOffset;
	float _landingVelocity;
	float _lastVerticalVelocity;
	Vector3 _velocity;
	Vector3 _smoothedVelocity;
	Vector3 _inertiaVelocity;
	bool _wasGrounded;
	bool _isGrounded;
	bool _initialized;

	protected override void OnStart()
	{
		Controller = Components.Get<PlayerController>();
		InitializeState();
	}

	protected override void OnDisabled()
	{
		_initialized = false;
		_movementStrength = 0f;
		_cadence = 0f;
		_landingOffset = 0f;
		_landingVelocity = 0f;
	}

	protected override void OnUpdate()
	{
		Controller ??= Components.Get<PlayerController>();

		if ( !Controller.IsValid() || IsProxy || Controller.ThirdPerson )
		{
			_initialized = false;
			return;
		}

		if ( !_initialized )
			InitializeState();

		var delta = MathF.Min( Time.Delta, 0.05f );
		if ( delta <= 0f )
			return;

		_velocity = Controller.Velocity;
		var planarVelocity = new Vector3( _velocity.x, _velocity.y, 0f );
		var planarSpeed = planarVelocity.Length;
		var grounded = Controller.IsOnGround;
		var moving = grounded && planarSpeed >= MinimumSpeed;
		var referenceSpeed = MathF.Max( ReferenceSpeed, 1f );
		var speedScale = (planarSpeed / referenceSpeed).Clamp( 0f, MaximumSpeedScale );
		var crouchScale = Controller.IsDucking ? CrouchMultiplier : 1f;
		var targetStrength = moving ? speedScale * crouchScale : 0f;
		var targetCadence = moving ? speedScale * MathF.Sqrt( crouchScale ) : 0f;
		var movementBlendSpeed = grounded ? BlendSpeed : AirborneBlendSpeed;

		_movementStrength = MathX.LerpTo(
			_movementStrength,
			targetStrength,
			ExponentialBlend( movementBlendSpeed, delta )
		);
		_cadence = MathX.LerpTo(
			_cadence,
			targetCadence,
			ExponentialBlend( CadenceSmoothing, delta )
		);
		_smoothedVelocity = Vector3.Lerp(
			_smoothedVelocity,
			planarVelocity,
			ExponentialBlend( VelocitySmoothing, delta )
		);
		_inertiaVelocity = Vector3.Lerp(
			_inertiaVelocity,
			_smoothedVelocity,
			ExponentialBlend( InertiaSmoothing, delta )
		);

		_idlePhase = (_idlePhase + MathF.Tau * BreathingFrequency * delta) % (MathF.Tau * 2f);

		if ( grounded && _cadence > 0.001f )
		{
			_gaitPhase += MathF.PI * StepFrequency * _cadence * delta;
			_gaitPhase %= MathF.Tau * 2f;
		}

		if ( grounded && !_wasGrounded )
		{
			var impact = (-_lastVerticalVelocity / 600f).Clamp( 0f, 1f );
			_landingVelocity -= impact * LandingDip * LandingSpring;
		}

		UpdateLandingSpring( delta );

		if ( !grounded )
			_lastVerticalVelocity = _velocity.z;

		_wasGrounded = grounded;
		_isGrounded = grounded;
	}

	protected override void OnPreRender()
	{
		if ( !_initialized || !Controller.IsValid() || IsProxy || Controller.ThirdPerson )
			return;

		var camera = Scene.Camera;
		if ( !camera.IsValid() )
			return;

		var rotation = camera.WorldRotation;
		var flatForward = new Vector3( rotation.Forward.x, rotation.Forward.y, 0f );
		var flatLength = flatForward.Length;
		flatForward = flatLength > 0.001f ? flatForward / flatLength : Vector3.Forward;
		var flatRight = new Vector3( -flatForward.y, flatForward.x, 0f );
		var referenceSpeed = MathF.Max( ReferenceSpeed, 1f );
		var forwardSpeed = (_smoothedVelocity.Dot( flatForward ) / referenceSpeed)
			.Clamp( -MaximumSpeedScale, MaximumSpeedScale );
		var sideSpeed = (_smoothedVelocity.Dot( flatRight ) / referenceSpeed)
			.Clamp( -MaximumSpeedScale, MaximumSpeedScale );
		var velocityLag = (_smoothedVelocity - _inertiaVelocity) / referenceSpeed;
		var lagForward = velocityLag.Dot( flatForward ).Clamp( -1f, 1f );
		var lagSide = velocityLag.Dot( flatRight ).Clamp( -1f, 1f );

		var sideWave = MathF.Sin( _gaitPhase * 0.5f );
		var sideDetail = MathF.Sin( _gaitPhase * 1.5f + 0.35f ) * GaitDetail;
		var footPlant = MathF.Abs( MathF.Cos( _gaitPhase ) ) - MeanAbsoluteCosine;
		var stepDetail = MathF.Sin( _gaitPhase * 2f + 0.7f ) * GaitDetail;
		var movementAmount = Amount * _movementStrength;
		var idleBlend = _isGrounded
			? 1f - (_movementStrength / 0.45f).Clamp( 0f, 1f )
			: 0f;
		var breathWave = MathF.Sin( _idlePhase );
		var weightShift = MathF.Sin( _idlePhase * 0.5f );

		var sideOffset = (sideWave + sideDetail) * HorizontalAmount * movementAmount;
		sideOffset += weightShift * IdleAmount * 0.22f * idleBlend * Amount;
		sideOffset -= lagSide * InertiaAmount * Amount;

		var verticalOffset = -(footPlant + stepDetail) * VerticalAmount * movementAmount;
		verticalOffset += breathWave * IdleAmount * idleBlend * Amount;
		verticalOffset += _landingOffset * Amount;

		var forwardOffset = -lagForward * InertiaAmount * Amount;
		var pitch = (footPlant + stepDetail * 0.5f) * PitchAmount * movementAmount;
		pitch -= breathWave * IdlePitchAmount * idleBlend * Amount;
		pitch -= forwardSpeed * VelocityPitchAmount * Amount;
		pitch -= lagForward * VelocityPitchAmount * Amount;

		var roll = -(sideWave + sideDetail * 0.5f) * RollAmount * movementAmount;
		roll += weightShift * IdlePitchAmount * 0.22f * idleBlend * Amount;
		roll += sideSpeed * VelocityRollAmount * Amount;
		roll += lagSide * VelocityRollAmount * Amount;

		var localOffset = new Vector3( forwardOffset, sideOffset, verticalOffset );
		var offsetLength = localOffset.Length;
		if ( offsetLength > MaximumPositionOffset )
			localOffset *= MaximumPositionOffset / offsetLength;

		pitch = pitch.Clamp( -MaximumRotationOffset, MaximumRotationOffset );
		roll = roll.Clamp( -MaximumRotationOffset, MaximumRotationOffset );

		camera.WorldPosition += rotation.Forward * localOffset.x;
		camera.WorldPosition += rotation.Right * localOffset.y;
		camera.WorldPosition += rotation.Up * localOffset.z;
		camera.WorldRotation *= Rotation.From( pitch, 0f, roll );
	}

	void InitializeState()
	{
		if ( !Controller.IsValid() )
			return;

		_gaitPhase = 0f;
		_movementStrength = 0f;
		_cadence = 0f;
		_velocity = Controller.Velocity;
		var planarVelocity = new Vector3( _velocity.x, _velocity.y, 0f );
		_smoothedVelocity = planarVelocity;
		_inertiaVelocity = planarVelocity;
		_wasGrounded = Controller.IsOnGround;
		_isGrounded = _wasGrounded;
		_lastVerticalVelocity = _velocity.z;
		_landingOffset = 0f;
		_landingVelocity = 0f;
		_initialized = true;
	}

	void UpdateLandingSpring( float delta )
	{
		var remaining = delta;
		var spring = MathF.Max( LandingSpring, 1f );
		var damping = MathF.Max( LandingDamping, 0.1f );

		while ( remaining > 0f )
		{
			var step = MathF.Min( remaining, 1f / 120f );
			var acceleration = -spring * spring * _landingOffset
				- 2f * damping * spring * _landingVelocity;
			_landingVelocity += acceleration * step;
			_landingOffset += _landingVelocity * step;
			remaining -= step;
		}

		_landingOffset = _landingOffset.Clamp( -LandingDip * 1.25f, LandingDip * 0.35f );
	}

	static float ExponentialBlend( float speed, float delta )
	{
		return 1f - MathF.Exp( -MathF.Max( speed, 0.01f ) * delta );
	}
}