using Godot;

public partial class PhysicsTorso : Node
{
    private const float TorsoMass = 30f;
    private const float MaxForce = 2400f;
    private const float MaxSpeed = 6f;
    public const float SnapDistance = 2f;
    private const float TorsoBackOffset = 0.06f;
    private const float HeadBackOffset = 0.06f;
    public const float YawHandsWeight = 0.6f;
    public const float YawForwardness = 0.3f;
    public const float YawSmoothRate = 12f;
    private const float OrientKp = 20f;
    private const float OrientMaxSpin = 10f;
    private const float OrientMaxAlpha = 150f;

    private static readonly Vector3 TorsoSize = new(0.22f, 0.5f, 0.13f);

    private static readonly Color BodyColor = new(0.4f, 0.45f, 0.55f);

    public Node3D HeadSource;
    public RigidBody3D HandBodyLeft;
    public RigidBody3D HandBodyRight;
    public Vector3 ReachOffset;

    private XRCamera3D _camera;
    private XRController3D _leftController;
    private XRController3D _rightController;
    private RigidBody3D _handL;
    private RigidBody3D _handR;
    private RigidBody3D _body;

    private Basis _yawBasis = Basis.Identity;
    private Basis _torsoBasis = Basis.Identity;
    private float _torsoYaw;
    private bool _yawInitialized;
    private bool _arrived;
    private MeshInstance3D _mesh;

    public RigidBody3D Body => _body;
    public Basis TorsoBasis => _body != null ? _body.GlobalTransform.Basis : _torsoBasis;
    public Vector3 TargetPosition { get; private set; }
    // Head-derived chest target WITHOUT ReachOffset, and its yaw basis — the open-loop frame
    // PhysicsArms computes the lean from (reading the body pose instead made the lean hunt).
    public Vector3 NominalTarget { get; private set; }
    public Basis TargetBasis => _torsoBasis;

    public override void _Ready()
    {
        _body = new RigidBody3D
        {
            Name = "TorsoBody",
            Mass = TorsoMass,
            GravityScale = 0f,
            ContinuousCd = true,
            ContactMonitor = true,
            MaxContactsReported = 8,
            CollisionLayer = 0b100,  // layer 3 = player body
            CollisionMask = 0b101,   // world + player body; hands (layer 4) excluded both ways
        };
        _body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = TorsoSize } });
        _mesh = new MeshInstance3D { Mesh = new BoxMesh { Size = TorsoSize }, Visible = false };
        _mesh.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = BodyColor, Roughness = 0.6f });
        _body.AddChild(_mesh);
        AddChild(_body);

        if (HeadSource == null)
        {
            _camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
            _leftController = GetNode<XRController3D>("../XROrigin3D/LeftHand");
            _rightController = GetNode<XRController3D>("../XROrigin3D/RightHand");
            _handL = PhysicsHands.GetBody(_leftController);
            _handR = PhysicsHands.GetBody(_rightController);
        }
        GD.Print("PhysicsTorso: chase body spawned");
    }

    public override void _PhysicsProcess(double delta)
    {
        Node3D head = HeadSource != null ? HeadSource : (Node3D)_camera;
        if (head == null || _body == null)
            return;
        // Main scene: wait for the calibrated profile (the T-pose gate). Headless spikes inject
        // HeadSource and get the anthropometric estimate from their head height instead.
        BodyProfile profile = BodyProfile.Current ?? (HeadSource != null ? BodyProfile.Estimate(head.GlobalPosition.Y) : null);
        if (profile == null)
            return;
        // Chest centre sits so that its shoulder line (centre + ShoulderRise) is at the
        // measured shoulder height below the eyes.
        float torsoDrop = profile.ShoulderDrop + PhysicsArms.ShoulderRise;
        float dt = (float)delta;

        _yawBasis = FlatYawBasis(head.GlobalTransform.Basis);
        Vector3 headPos = head.GlobalPosition - _yawBasis * Vector3.Forward * HeadBackOffset;
        UpdateTorsoYaw(headPos, dt);
        NominalTarget = headPos + _torsoBasis * new Vector3(0f, -torsoDrop, TorsoBackOffset);
        Vector3 target = NominalTarget + ReachOffset;
        TargetPosition = target;

        if (!_arrived && _body.GlobalPosition.DistanceTo(target) < 0.3f)
        {
            _arrived = true;
            _mesh.Visible = true;
        }

        if (BodyMath.ResetIfNonFinite(_body, new Transform3D(_torsoBasis, target), "PhysicsTorso"))
            return;

        if (_body.GlobalPosition.DistanceTo(target) > SnapDistance)
        {
            _body.GlobalTransform = new Transform3D(_torsoBasis, target);
            _body.LinearVelocity = Vector3.Zero;
            _body.AngularVelocity = Vector3.Zero;
            return;
        }

        Vector3 desiredVelocity = (target - _body.GlobalPosition) / dt;
        if (desiredVelocity.Length() > MaxSpeed)
            desiredVelocity = desiredVelocity.Normalized() * MaxSpeed;
        Vector3 dv = desiredVelocity - _body.LinearVelocity;
        Vector3 force = dv / dt * TorsoMass;
        if (force.Length() > MaxForce)
            force = force.Normalized() * MaxForce;
        _body.LinearVelocity += force / TorsoMass * dt;

        Vector3 rotErr = BodyMath.RotationError(_torsoBasis.GetRotationQuaternion(),
            _body.GlobalTransform.Basis.GetRotationQuaternion());
        Vector3 spinDesired = (rotErr * OrientKp).LimitLength(OrientMaxSpin);

        Vector3 wErr = spinDesired - _body.AngularVelocity;
        float maxDeltaW = OrientMaxAlpha * dt;
        if (wErr.Length() > maxDeltaW)
            wErr = wErr.Normalized() * maxDeltaW;
        _body.AngularVelocity += wErr;
    }

    private void UpdateTorsoYaw(Vector3 headPos, float delta)
    {
        float hmdYaw = YawOf(-_yawBasis.Z);
        float targetYaw = hmdYaw;
        if (_handL != null && _handR != null)
        {
            Vector3 toHands = (_handL.GlobalPosition + _handR.GlobalPosition) * 0.5f - headPos;
            Vector3 horizontal = new(toHands.X, 0f, toHands.Z);
            if (horizontal.LengthSquared() > 0.0001f)
            {
                float handsYaw = YawOf(horizontal.Normalized());
                float forwardness = Mathf.Clamp(horizontal.Dot(-_yawBasis.Z) / YawForwardness, 0f, 1f);
                targetYaw = Mathf.LerpAngle(hmdYaw, handsYaw, forwardness * YawHandsWeight);
            }
        }
        if (!_yawInitialized)
        {
            _torsoYaw = hmdYaw;
            _yawInitialized = true;
        }
        _torsoYaw = Mathf.LerpAngle(_torsoYaw, targetYaw, 1f - Mathf.Exp(-delta * YawSmoothRate));
        Vector3 fwd = new(-Mathf.Sin(_torsoYaw), 0f, -Mathf.Cos(_torsoYaw));
        _torsoBasis = Basis.LookingAt(fwd, Vector3.Up);
    }

    private static Basis FlatYawBasis(Basis basis)
    {
        Vector3 fwd = -basis.Z;
        fwd.Y = 0f;
        if (fwd.LengthSquared() < 0.0001f)
            return Basis.Identity;
        return Basis.LookingAt(fwd.Normalized(), Vector3.Up);
    }

    private static float YawOf(Vector3 forward)
    {
        return Mathf.Atan2(-forward.X, -forward.Z);
    }
}