using Godot;

// Nimso's "soft grab" (NEXT GEN — No Jank): a grab never snaps and never welds across a gap — the
// grabbed thing is EASED into the hand first, then the weld locks the pose. Without it the weld
// froze whatever gap there was at the moment of the press: hands hovering next to bars and holds,
// props held a few centimetres off the palm (owner).
// The grip point is where the palm actually faces the object's surface (a short sphere cast along
// the palm normal, else toward the object's centre). Over EaseTime that surface point and the palm
// surface are brought together: a light prop flies into the hand, the hand goes to static world or
// anything too heavy to pull. Position only — the relative orientation is kept (no rotation snap).
public sealed class SoftGrab
{
    public const float EaseTime = 0.1f;          // s
    public const float SettleGap = 0.004f;       // m: close enough to weld
    public const float MaxEaseSpeed = 3f;        // m/s: a far grab takes a little longer instead of jerking
    public const float LightMass = 6f;           // kg: lighter props come to the hand, heavier things pull the hand

    public readonly RigidBody3D Hand;
    public readonly PhysicsBody3D Target;
    public readonly int Side;
    private readonly Vector3 _onTarget;          // grip point, target-local
    private float _t, _lastGap = float.MaxValue;

    public float Gap => (PalmSurface(Hand, Side) - Target.GlobalTransform * _onTarget).Length();

    private SoftGrab(RigidBody3D hand, int side, PhysicsBody3D target, Vector3 gripWorld)
    {
        Hand = hand;
        Side = side;
        Target = target;
        _onTarget = target.GlobalTransform.AffineInverse() * gripWorld;
    }

    // The palm faces +X on the left hand, −X on the right (PropGrabber.PalmPoint).
    public static Vector3 PalmNormal(RigidBody3D hand, int side) =>
        hand.GlobalTransform.Basis.Orthonormalized().X * (side == 0 ? 1f : -1f);

    public static Vector3 PalmSurface(RigidBody3D hand, int side) =>
        hand.GlobalPosition + PalmNormal(hand, side) * (PlayerRagdoll.HandSize.X * 0.5f);

    public static SoftGrab Begin(RigidBody3D hand, int side, PhysicsBody3D target)
    {
        Vector3 surface = PalmSurface(hand, side);
        Vector3 grip = CastTo(hand, target, hand.GlobalPosition, PalmNormal(hand, side) * 0.14f)
            ?? CastTo(hand, target, surface, target.GlobalPosition - surface)
            ?? surface;   // already touching/inside: weld where it is
        return new SoftGrab(hand, side, target, grip);
    }

    private static Vector3? CastTo(RigidBody3D hand, PhysicsBody3D target, Vector3 from, Vector3 motion)
    {
        if (motion.LengthSquared() < 1e-8f)
            return null;
        PhysicsShapeQueryParameters3D q = new()
        {
            Shape = new SphereShape3D { Radius = 0.01f },
            Transform = new Transform3D(Basis.Identity, from),
            Motion = motion,
            CollisionMask = 1,
        };
        q.Exclude = new Godot.Collections.Array<Rid> { hand.GetRid() };
        Godot.Collections.Dictionary hit = hand.GetWorld3D().DirectSpaceState.GetRestInfo(q);
        if (hit.Count == 0 || (ulong)hit["collider_id"] != target.GetInstanceId())
            return null;
        return (Vector3)hit["point"];
    }

    // Every physics tick until it returns true (then weld). Velocities are set, not forces: the ease
    // is a short, exact move, and the solver still resolves contacts along the way.
    public bool Step(float dt)
    {
        _t += dt;
        Vector3 gap = PalmSurface(Hand, Side) - Target.GlobalTransform * _onTarget;
        // Done when touching, or when it stopped closing (the palm is against the surface — contact
        // slop leaves ~1 cm on curved holds — or the arm can't get there).
        bool stalled = _t > EaseTime && _lastGap - gap.Length() < 0.0005f;
        _lastGap = gap.Length();
        if (gap.Length() < SettleGap || stalled || _t > EaseTime * 3f)
            return true;
        float remaining = Mathf.Max(EaseTime - _t, dt);
        Vector3 close = (gap / remaining).LimitLength(MaxEaseSpeed);
        if (Target is RigidBody3D body && !body.Freeze && body.Mass <= LightMass)
            body.LinearVelocity = Hand.LinearVelocity + close;
        else
            Hand.LinearVelocity = (Target is RigidBody3D moving ? moving.LinearVelocity : Vector3.Zero) - close;
        return false;
    }
}