using Sandbox;
using System;
using System.Linq;

public sealed class GrabPoint : Component
{
	[RequireComponent] public SphereCollider GrabCollider { get; private set; }

	/// <summary>True for foregrip / support-hand grab points on two-handed weapons.</summary>
	[Property] public bool IsSecondaryGrip { get; set; } = false;

	/// <summary>Local rotation applied to the holding hand for this grip (one-handed holds). Default matches the old 55° pitch.</summary>
	[Property] public Angles HandRotationOffset { get; set; } = new Angles( 55f, 0f, 0f );

	/// <summary>Seconds to blend the weapon's rotation from the one-hand pose into the two-hand pose (swung toward the support hand) when it grabs. The primary grip stays pinned to the primary hand throughout. 0 = instant.</summary>
	[Property, Group( "Two Hand" )]
	public float TwoHandBlendTime { get; set; } = 0.08f;

	[Property] public Action OnGrab { get; set; }

	/// <summary>Fires every frame the trigger is held (e.g. full-auto fire).</summary>
	[Property] public Action OnTrigger { get; set; }

	/// <summary>Fires once on the frame the trigger is first pressed (e.g. toggles, semi-auto).</summary>
	[Property] public Action OnTriggerPressed { get; set; }

	/// <summary>Fires once on the frame the trigger is released.</summary>
	[Property] public Action OnTriggerReleased { get; set; }

	/// <summary>
	/// Used by desktop users
	/// </summary>
	[Property] public Action OnReload { get; set; }

	[Property] public Action OnDrop { get; set; }

	/// <summary>Multiplier applied to the hand's velocity when throwing. 1 = realistic; higher makes throws feel stronger.</summary>
	[Property] public float ThrowVelocityScale { get; set; } = 5f;

	[Sync] public bool IsGrabbed { get; private set; }

	private bool IsLeftHand { get; set; }
	private bool _triggerDown;

	// Ring buffer of recent hand transforms, used to compute throw velocity on release
	private const int ThrowSampleCount = 6;
	private readonly (Vector3 Position, Rotation Rotation, float Time)[] _throwSamples = new (Vector3, Rotation, float)[ThrowSampleCount];
	private int _throwSampleHead;
	private int _throwSampleTotal;

	// 1H → 2H blend state (owned by the primary grip writer)
	private int _lastActiveGripCount;
	private float _twoHandBlendStartTime = -1f;
	private Rotation _twoHandBlendFromRotation;

	// Secondary-only hold: keep the weapon's existing pose relative to the hand (no snap)
	private bool _secondaryOnlyOffsetValid;
	private Vector3 _secondaryOnlyLocalPos;
	private Rotation _secondaryOnlyLocalRot;

	public Transform HandTransform => IsLeftHand ? Input.VR.LeftHand.Transform : Input.VR.RightHand.Transform;

	/// <summary>Rumble the VR controller currently holding this grip. No-ops if this point isn't grabbed.</summary>
	public void TriggerHaptic( HapticEffect effect, float lengthScale = 1f, float frequencyScale = 1f, float amplitudeScale = 1f )
	{
		if ( !IsGrabbed || !Game.IsRunningInVR )
			return;

		var controller = IsLeftHand ? Input.VR.LeftHand : Input.VR.RightHand;
		controller.TriggerHaptics( effect, lengthScale, frequencyScale, amplitudeScale );
	}

	/// <summary>
	/// The item's own root object. GameObject.Root can't be used directly because a stowed
	/// item is parented to an inventory slot on the player, making Root resolve to the player.
	/// </summary>
	public GameObject ItemRoot => Components.GetInAncestorsOrSelf<InventoryItem>()?.GameObject ?? GameObject.Root;

	/// <summary>
	/// Returns true if any GrabPoint on this entire weapon tree is currently held.
	/// </summary>
	public bool IsAnythingHeld => ItemRoot.Components.GetAll<GrabPoint>( FindMode.InDescendants ).Any( g => g.IsGrabbed );

	protected override void OnUpdate()
	{
		if ( !IsGrabbed || !Game.IsRunningInVR || IsProxy )
			return;

		var hand = HandTransform;
		_throwSamples[_throwSampleHead] = (hand.Position, hand.Rotation, Time.Now);
		_throwSampleHead = (_throwSampleHead + 1) % ThrowSampleCount;
		if ( _throwSampleTotal < ThrowSampleCount )
			_throwSampleTotal++;
	}

	protected override void OnPreRender()
	{
		if ( !IsGrabbed || !Game.IsRunningInVR ) return;

		var itemRoot = ItemRoot;
		var activeGrips = itemRoot.Components.GetAll<GrabPoint>( FindMode.InDescendants )
			.Where( g => g.IsGrabbed )
			.ToList();

		var gun = itemRoot.Components.Get<Gun>( FindMode.EnabledInSelfAndDescendants );

		// ---- CASE 1: ONE HANDED CONTROL ----
		if ( activeGrips.Count == 1 )
		{
			// Only the holding grip writes the weapon transform
			if ( this != activeGrips[0] )
				return;

			_lastActiveGripCount = 1;
			_twoHandBlendStartTime = -1f;

			// Foregrip alone: preserve current weapon pose relative to the hand so
			// releasing the primary after a two-hand hold doesn't snap to the secondary.
			if ( IsSecondaryGrip )
			{
				ApplySecondaryOnlyHold( itemRoot, gun );
				return;
			}

			_secondaryOnlyOffsetValid = false;

			Rotation targetRotation = HandTransform.Rotation * Rotation.From( HandRotationOffset );

			if ( gun is not null )
				targetRotation *= gun.VrRecoilRotation;

			Vector3 localGrabAnchorOffset = GetGrabAnchorLocalOffset( itemRoot.WorldTransform );
			Vector3 targetRootPosition = HandTransform.Position - (targetRotation * localGrabAnchorOffset);

			itemRoot.WorldTransform = new Transform( targetRootPosition, targetRotation );
		}
		// ---- CASE 2: TWO HANDED CONTROL ----
		else if ( activeGrips.Count >= 2 )
		{
			_secondaryOnlyOffsetValid = false;

			var primaryGrip = activeGrips.FirstOrDefault( g => !g.IsSecondaryGrip ) ?? activeGrips[0];
			var secondaryGrip = activeGrips.FirstOrDefault( g => g.IsSecondaryGrip ) ?? activeGrips[1];

			if ( this == primaryGrip )
			{
				var primaryHand = primaryGrip.HandTransform;
				Vector3 directionToFrontHand = (secondaryGrip.HandTransform.Position - primaryHand.Position).Normal;

				var primaryLocal = primaryGrip.GetGrabAnchorLocalOffset( itemRoot.WorldTransform );
				var secondaryLocal = secondaryGrip.GetGrabAnchorLocalOffset( itemRoot.WorldTransform );

				// Start from exactly the pose the primary hand would give one-handed so its
				// roll (and the grip's pitch offset) carry over into the two-hand hold.
				Rotation oneHandRotation = primaryHand.Rotation * Rotation.From( primaryGrip.HandRotationOffset );

				// Swing that pose by the smallest arc that points the actual grip-to-grip
				// axis at the support hand. FromToRotation adds no twist, so only pitch/yaw
				// change; when the support hand sits at the natural foregrip spot the
				// result equals the one-hand pose and nothing snaps.
				Rotation targetRotation = oneHandRotation;
				Vector3 localGripAxis = secondaryLocal - primaryLocal;
				if ( !localGripAxis.IsNearZeroLength )
				{
					Vector3 worldGripAxis = oneHandRotation * localGripAxis.Normal;
					targetRotation = Rotation.FromToRotation( worldGripAxis, directionToFrontHand ) * oneHandRotation;
				}

				if ( gun is not null )
					targetRotation *= gun.VrRecoilRotation;

				// Blend from the last one-hand pose when the support hand first grabs
				if ( _lastActiveGripCount < 2 )
				{
					_twoHandBlendFromRotation = itemRoot.WorldRotation;
					_twoHandBlendStartTime = Time.Now;
				}

				float blendTime = Math.Max( TwoHandBlendTime, 0f );
				if ( blendTime > 0f && _twoHandBlendStartTime >= 0f )
				{
					float t = Math.Clamp( (Time.Now - _twoHandBlendStartTime) / blendTime, 0f, 1f );
					if ( t < 1f )
						targetRotation = Rotation.Slerp( _twoHandBlendFromRotation, targetRotation, t );
					else
						_twoHandBlendStartTime = -1f;
				}

				// Position is always derived from the (possibly blended) rotation so the
				// primary grip stays pinned to the primary hand, including mid-blend.
				Vector3 targetRootPosition = primaryHand.Position - (targetRotation * primaryLocal);

				itemRoot.WorldTransform = new Transform( targetRootPosition, targetRotation );
			}

			_lastActiveGripCount = activeGrips.Count;
		}
	}

	public void Grab( bool isLeftHand )
	{
		if ( IsGrabbed )
			return;

		if ( IsAnythingHeld && !Network.IsOwner )
			return;

		// Use ItemRoot, not GameObject.Root: a stowed item is parented to the
		// player, so Root would resolve to the player object.
		var rootObject = ItemRoot;
		var inventoryItem = rootObject.Components.Get<InventoryItem>();

		rootObject.Network.TakeOwnership();

		IsGrabbed = true;
		IsLeftHand = isLeftHand;

		// Discard stale motion samples from a previous hold
		_throwSampleHead = 0;
		_throwSampleTotal = 0;
		_lastActiveGripCount = 0;
		_twoHandBlendStartTime = -1f;
		_secondaryOnlyOffsetValid = false;

		inventoryItem.IsPhysicsEnabled = false;
		if ( inventoryItem.IsStowed )
		{
			rootObject.SetParent( null, true );
			inventoryItem.Unstow();
		}

		OnGrab?.Invoke();
	}

	/// <summary>
	/// Releases this grab point. Pass <paramref name="throwVelocity"/> to launch the
	/// item (used by desktop throws); VR throws derive velocity from hand motion instead.
	/// </summary>
	public void Drop( Vector3? throwVelocity = null )
	{
		if ( !IsGrabbed ) return;

		IsGrabbed = false;
		_lastActiveGripCount = 0;
		_twoHandBlendStartTime = -1f;
		_secondaryOnlyOffsetValid = false;

		// Make sure a held trigger doesn't carry over to the next grab
		SetTriggerState( false );

		if ( !IsAnythingHeld )
		{
			var rootObject = ItemRoot;
			var inventoryItem = rootObject.Components.Get<InventoryItem>();
			bool isStowed = inventoryItem is not null && inventoryItem.IsStowed;

			// A stowed item stays attached to the player: keep ownership and the
			if ( !isStowed )
			{
				inventoryItem.IsPhysicsEnabled = true;

				// Apply throw velocity while we still own the object so the
				// physics state carries over the network before ownership drops
				if ( throwVelocity.HasValue )
				{
					var rigidbody = rootObject.GetComponent<Rigidbody>( includeDisabled: true );
					if ( rigidbody is not null )
						rigidbody.Velocity = throwVelocity.Value;
				}
				else if ( Game.IsRunningInVR )
				{
					ApplyThrowVelocity( rootObject );
				}
			}
		}

		OnDrop?.Invoke();
	}

	public void Reload()
	{
		OnReload?.Invoke();
	}

	/// <summary>
	/// Reports the current trigger state every frame. Drives the held/pressed/released
	/// events via edge detection so callers don't need to track press state themselves.
	/// </summary>
	public void SetTriggerState( bool down )
	{
		if ( down && !_triggerDown ) OnTriggerPressed?.Invoke();
		if ( !down && _triggerDown ) OnTriggerReleased?.Invoke();
		if ( down ) OnTrigger?.Invoke();

		_triggerDown = down;
	}

	/// <summary>
	/// Transfers the hand's recent motion onto the item's rigidbody so releasing
	/// mid-swing throws the object instead of dropping it straight down.
	/// </summary>
	private void ApplyThrowVelocity( GameObject rootObject )
	{
		if ( _throwSampleTotal < 2 )
			return;

		var rigidbody = rootObject.GetComponent<Rigidbody>( includeDisabled: true );
		if ( rigidbody is null )
			return;

		// Best average velocity over any span in the ring buffer so a late grip
		// release (after the swing slows) still keeps the throw impulse.
		var playerVelocity = PlayerCharacterController.Instance?.Velocity ?? Vector3.Zero;
		var bestRelative = Vector3.Zero;
		float bestSpeedSq = 0f;
		Rotation bestFromRotation = default;
		Rotation bestToRotation = default;
		float bestElapsed = 0f;

		int count = _throwSampleTotal;
		for ( var i = 0; i < count - 1; i++ )
		{
			for ( var j = i + 1; j < count; j++ )
			{
				var older = _throwSamples[ThrowSampleIndex( _throwSampleHead - count + i )];
				var newer = _throwSamples[ThrowSampleIndex( _throwSampleHead - count + j )];
				float elapsed = newer.Time - older.Time;
				if ( elapsed <= 1e-4f )
					continue;

				var relative = (newer.Position - older.Position) / elapsed - playerVelocity;
				float speedSq = relative.LengthSquared;
				if ( speedSq <= bestSpeedSq )
					continue;

				bestSpeedSq = speedSq;
				bestRelative = relative;
				bestFromRotation = older.Rotation;
				bestToRotation = newer.Rotation;
				bestElapsed = elapsed;
			}
		}

		if ( bestSpeedSq <= 0f || bestElapsed <= 0f )
			return;

		rigidbody.Velocity = playerVelocity + bestRelative * ThrowVelocityScale;
		rigidbody.AngularVelocity = ComputeAngularVelocity( bestFromRotation, bestToRotation, bestElapsed );
	}

	private static int ThrowSampleIndex( int index )
	{
		return ((index % ThrowSampleCount) + ThrowSampleCount) % ThrowSampleCount;
	}

	/// <summary>
	/// Converts the rotation change between two samples into an angular velocity
	/// vector (radians/s) suitable for Rigidbody.AngularVelocity.
	/// </summary>
	private static Vector3 ComputeAngularVelocity( Rotation from, Rotation to, float elapsed )
	{
		var delta = to * from.Inverse;

		// Flip to the shortest arc so the spin direction is correct
		if ( delta.w < 0f )
			delta = new Rotation( -delta.x, -delta.y, -delta.z, -delta.w );

		var axis = new Vector3( delta.x, delta.y, delta.z );
		if ( axis.LengthSquared < 1e-8f )
			return Vector3.Zero;

		float angle = 2f * MathF.Acos( Math.Clamp( delta.w, -1f, 1f ) );
		return axis.Normal * (angle / elapsed);
	}

	private Vector3 GetGrabAnchorLocalOffset( Transform rootWorldTransform )
	{
		return rootWorldTransform.PointToLocal( WorldPosition );
	}

	/// <summary>
	/// Follow the secondary hand using the weapon's pose at the moment it became
	/// secondary-only, instead of snapping the foregrip collider onto the controller.
	/// </summary>
	private void ApplySecondaryOnlyHold( GameObject itemRoot, Gun gun )
	{
		var hand = HandTransform;

		if ( !_secondaryOnlyOffsetValid )
		{
			var root = itemRoot.WorldTransform;
			_secondaryOnlyLocalPos = hand.Rotation.Inverse * (root.Position - hand.Position);
			_secondaryOnlyLocalRot = hand.Rotation.Inverse * root.Rotation;
			_secondaryOnlyOffsetValid = true;
		}

		var targetPosition = hand.Position + hand.Rotation * _secondaryOnlyLocalPos;
		var targetRotation = hand.Rotation * _secondaryOnlyLocalRot;

		if ( gun is not null )
			targetRotation *= gun.VrRecoilRotation;

		itemRoot.WorldTransform = new Transform( targetPosition, targetRotation );
	}
}