using Godot;

// The derived pose of the player's REAL body (Nimso's "True Body"): from the head + 2 controllers +
// the calibrated BodyProfile, trigonometry — not full IK — places neck, chest (yaw + crouch lean),
// pelvis (lagging spring → sway), a MOVING shoulder girdle and the arms. Pure solver; the physics
// ragdoll (step 2) drives its joints toward this pose. All positions are world space.
public sealed class TrueBodySolver
{
    // Head geometry (head-local, from the eyes): the neck pivot sits behind and below the eyes.
    public static readonly Vector3 EyeToNeckPivot = new(0f, -0.09f, 0.07f);
    public const float NeckBaseAboveShoulders = 0.04f;
    public const float ShouldersBehindEyes = 0.06f;
    public const float TrunkRatio = 0.288f;               // shoulder joint − hip joint height, × stature (Winter 0.818 − 0.530)
    public const float ChestLeanMax = 0.75f;              // rad (~43°) at a deep crouch
    public const float CrouchRange = 0.5f;                // crouch factor 1 when eyes are this fraction of standing eye height lower
    public const float HeadPitchToChest = 0.25f;
    // Crawl: when the head gets so low that the hips would go through the floor, the trunk tips
    // further (pivoting at the hips, hips kicking back) up to nearly horizontal (Nimso: lower = crawl).
    public const float CrawlLeanMax = 1.45f;              // rad (~83°)
    public const float HipMinHeight = 0.15f;              // hip joint above the floor when crawling
    public const float PelvisFrequency = 2.5f;            // Hz, critically damped follow → hip lag/sway
    public const float GirdleReachStart = 0.98f;          // fraction of arm length where the shoulder starts to help
    public const float GirdleSmoothRate = 15f;
    // Girdle travel limits in the chest frame (outward, up, forward), metres. [I] anatomy-scale guesses.
    public const float GirdleOut = 0.03f, GirdleIn = 0.02f, GirdleUp = 0.07f, GirdleDown = 0.02f, GirdleForward = 0.08f, GirdleBack = 0.03f;
    public const float RhythmLift = 0.04f;                // extra shrug with the arm fully overhead (scapulohumeral rhythm)
    public const float YawHandsWeight = 0.6f;
    public const float YawForwardness = 0.3f;
    public const float YawSmoothRate = 12f;
    public const float WristOffset = 0.06f;

    public Vector3 Eye, NeckPivot, NeckBase, UpperChest, Pelvis;
    public Vector3 PelvisTarget;                          // un-lagged pelvis (the ragdoll's ground control adds its own lag)
    public Basis ChestBasis = Basis.Identity;
    public Basis PelvisBasis = Basis.Identity;
    public Basis YawBasis = Basis.Identity;
    public Basis HeadBasis = Basis.Identity;
    public float Lean { get; private set; }
    public readonly Vector3[] Pole = new Vector3[2];      // per arm, see ArmSolver.Pole
    public readonly Vector3[] Shoulder = new Vector3[2];
    public readonly Vector3[] Elbow = new Vector3[2];
    public readonly Vector3[] Wrist = new Vector3[2];
    public readonly Vector3[] WristTarget = new Vector3[2];
    public float Crouch { get; private set; }
    public bool Initialized { get; private set; }

    private readonly ArmSolver[] _arms = { new(), new() };
    private readonly Vector3[] _girdle = new Vector3[2];   // chest-local shoulder offset
    private Vector3 _pelvisVelocity;
    private float _yaw;

    // A discrete move of the whole player (teleport, snap turn): carry the lagging state along
    // instead of letting it swing across the gap.
    public void Shift(Transform3D delta)
    {
        if (!Initialized)
            return;
        Pelvis = delta * Pelvis;
        _pelvisVelocity = delta.Basis * _pelvisVelocity;
        Vector3 f = delta.Basis * new Basis(Vector3.Up, _yaw) * Vector3.Forward;
        _yaw = Mathf.Atan2(-f.X, -f.Z);
    }

    public void Reset()
    {
        Initialized = false;
        _girdle[0] = _girdle[1] = Vector3.Zero;
        _pelvisVelocity = Vector3.Zero;
    }

    // floorY = world height of the tracking floor (XROrigin3D). Controllers are aim poses.
    public void Solve(BodyProfile p, Transform3D head, Transform3D left, Transform3D right, float floorY, float dt)
    {
        Eye = head.Origin;
        Basis headBasis = head.Basis.Orthonormalized();
        HeadBasis = headBasis;
        NeckPivot = Eye + headBasis * EyeToNeckPivot;
        WristTarget[0] = left * new Vector3(0f, 0f, WristOffset);
        WristTarget[1] = right * new Vector3(0f, 0f, WristOffset);

        // Torso yaw: HMD yaw blended toward the hands when they're out in front (decoupled from the head).
        Vector3 headFwd = -headBasis.Z;
        float hmdYaw = Mathf.Atan2(-headFwd.X, -headFwd.Z);
        float targetYaw = hmdYaw;
        Vector3 toHands = (WristTarget[0] + WristTarget[1]) * 0.5f - Eye;
        toHands.Y = 0f;
        Vector3 flatFwd = new Vector3(headFwd.X, 0f, headFwd.Z);
        if (toHands.LengthSquared() > 1e-4f && flatFwd.LengthSquared() > 1e-6f)
        {
            float handsYaw = Mathf.Atan2(-toHands.X, -toHands.Z);
            float forwardness = Mathf.Clamp(toHands.Dot(flatFwd.Normalized()) / YawForwardness, 0f, 1f);
            targetYaw = Mathf.LerpAngle(hmdYaw, handsYaw, forwardness * YawHandsWeight);
        }
        if (!Initialized)
            _yaw = hmdYaw;
        _yaw = Mathf.LerpAngle(_yaw, targetYaw, 1f - Mathf.Exp(-dt * YawSmoothRate));
        Basis yawBasis = new Basis(Vector3.Up, _yaw);
        YawBasis = yawBasis;

        // Crouch lean: measured at the NECK PIVOT (nodding moves the eyes ~10 cm but not the pivot),
        // below its standing height the chest pitches forward; looking down adds a little.
        float pivotAboveFloor = NeckPivot.Y - floorY;
        float standingPivot = p.EyeHeight + EyeToNeckPivot.Y;
        Crouch = Mathf.Clamp((standingPivot - pivotAboveFloor) / (CrouchRange * p.EyeHeight), 0f, 1f);
        float headPitchDown = Mathf.Max(0f, -Mathf.Asin(Mathf.Clamp(headFwd.Y, -1f, 1f)));
        float lean = Crouch * ChestLeanMax + headPitchDown * HeadPitchToChest;
        float trunk = TrunkRatio * p.Stature;
        float neckLen = Mathf.Max(0.05f, p.ShoulderDrop + EyeToNeckPivot.Y - NeckBaseAboveShoulders);
        Vector3 neckDir = Vector3.Up;
        for (int iter = 0; iter < 3; iter++)
        {
            ChestBasis = yawBasis * new Basis(Vector3.Right, -lean);
            // Neck: from the head pivot down along the average of chest-up and head-up, sized so a
            // standing, level head puts the shoulder joints at the measured drop below the eyes.
            neckDir = (ChestBasis.Y + headBasis.Y).Normalized();
            NeckBase = NeckPivot - neckDir * neckLen;
            // Keep the shoulder line behind the eyes by the anthropometric offset (neck pivot already
            // supplies part of it).
            UpperChest = NeckBase - ChestBasis.Y * NeckBaseAboveShoulders
                + ChestBasis * new Vector3(0f, 0f, ShouldersBehindEyes - EyeToNeckPivot.Z);
            float room = (UpperChest.Y - floorY - HipMinHeight) / trunk;
            float crawl = Mathf.Acos(Mathf.Clamp(room, Mathf.Cos(CrawlLeanMax), 1f));
            if (crawl <= lean + 1e-3f)
                break;
            lean = crawl;
        }
        Lean = lean;

        // Pelvis: hangs below the upper chest along the trunk; follows through a critically damped
        // spring so it lags quick head motion (hip sway) instead of being rigidly attached.
        Vector3 pelvisTarget = UpperChest - ChestBasis.Y * (TrunkRatio * p.Stature);
        PelvisTarget = pelvisTarget;
        if (!Initialized)
            Pelvis = pelvisTarget;
        float w = Mathf.Tau * PelvisFrequency;
        Vector3 accel = (pelvisTarget - Pelvis) * (w * w) - _pelvisVelocity * (2f * w);
        _pelvisVelocity += accel * dt;
        Pelvis += _pelvisVelocity * dt;
        PelvisBasis = yawBasis;

        for (int side = 0; side < 2; side++)
            SolveArm(p, side, side == 0 ? left.Basis : right.Basis, dt);
        Initialized = true;
    }

    private void SolveArm(BodyProfile p, int side, Basis handBasis, float dt)
    {
        float sign = side == 0 ? -1f : 1f;
        Vector3 outward = ChestBasis.X * sign;
        Vector3 neutral = UpperChest + outward * p.ShoulderHalfWidth;

        // Girdle: once the hand is beyond ~full reach the shoulder joint shifts toward it, within
        // anatomical travel, plus a small shrug as the arm goes overhead.
        Vector3 toWrist = WristTarget[side] - neutral;
        float excess = toWrist.Length() - p.ArmLength * GirdleReachStart;
        Vector3 desired = Vector3.Zero;
        if (excess > 0f && toWrist.LengthSquared() > 1e-8f)
        {
            Vector3 local = ChestBasis.Transposed() * (toWrist.Normalized() * excess);
            float outLocal = Mathf.Clamp(local.X * sign, -GirdleIn, GirdleOut);
            desired = new Vector3(outLocal * sign,
                Mathf.Clamp(local.Y, -GirdleDown, GirdleUp),
                Mathf.Clamp(local.Z, -GirdleForward, GirdleBack));
        }
        if (toWrist.LengthSquared() > 1e-8f)
        {
            float raise = Mathf.Clamp((toWrist.Normalized().Dot(ChestBasis.Y) + 0.5f) / 1.5f, 0f, 1f);  // 0 at ~30° below horizontal, 1 overhead
            desired.Y = Mathf.Min(desired.Y + raise * RhythmLift, GirdleUp);
        }
        _girdle[side] = Initialized ? _girdle[side].Lerp(desired, 1f - Mathf.Exp(-dt * GirdleSmoothRate)) : desired;
        Shoulder[side] = neutral + ChestBasis * _girdle[side];

        _arms[side].Solve(Shoulder[side], WristTarget[side], ChestBasis, outward, handBasis, p.UpperArm, p.Forearm, dt);
        Elbow[side] = _arms[side].Elbow;
        Pole[side] = _arms[side].Pole;
        Wrist[side] = _arms[side].Wrist;
    }
}

// Runs the TrueBodySolver every physics tick once a profile exists and draws a translucent debug
// skeleton of it (ShowDebug) so the derived pose can be judged in-headset.
public partial class TrueBody : Node3D
{
    public static bool ShowDebug = true;

    private XROrigin3D _origin;
    private XRCamera3D _camera;
    private XRController3D _left;
    private XRController3D _right;
    private StandardMaterial3D _material;
    private readonly System.Collections.Generic.List<MeshInstance3D> _bones = new();
    private readonly System.Collections.Generic.List<MeshInstance3D> _joints = new();

    public TrueBodySolver Solver { get; } = new();
    // Set by PlayerRig, which solves at the right point of its own tick (after moving the origin).
    public bool ExternallySolved;
    public bool HasPose => Solver.Initialized;

    public override void _Ready()
    {
        _origin = GetNode<XROrigin3D>("../XROrigin3D");
        _camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
        _left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
        _right = GetNode<XRController3D>("../XROrigin3D/RightHand");
        _material = new StandardMaterial3D
        {
            AlbedoColor = new Color(0.3f, 0.9f, 1f, 0.35f),
            Transparency = BaseMaterial3D.TransparencyEnum.Alpha,
            ShadingMode = BaseMaterial3D.ShadingModeEnum.Unshaded,
            NoDepthTest = false,
        };
        BodyProfile.Changed += _ => Solver.Reset();
    }

    public override void _PhysicsProcess(double delta)
    {
        BodyProfile profile = BodyProfile.Current;
        if (profile == null || ExternallySolved)
            return;
        Solver.Solve(profile, _camera.GlobalTransform, _left.GlobalTransform, _right.GlobalTransform, _origin.GlobalPosition.Y, (float)delta);
    }

    public override void _Process(double delta)
    {
        bool show = ShowDebug && HasPose;
        if (!show)
        {
            foreach (MeshInstance3D m in _bones)
                m.Visible = false;
            foreach (MeshInstance3D m in _joints)
                m.Visible = false;
            return;
        }
        TrueBodySolver s = Solver;
        int b = 0, j = 0;
        Bone(ref b, s.NeckPivot, s.NeckBase, 0.025f);
        Bone(ref b, s.NeckBase, s.UpperChest, 0.03f);
        Bone(ref b, s.UpperChest, s.Pelvis, 0.05f);
        for (int side = 0; side < 2; side++)
        {
            Bone(ref b, s.UpperChest, s.Shoulder[side], 0.025f);
            Bone(ref b, s.Shoulder[side], s.Elbow[side], 0.03f);
            Bone(ref b, s.Elbow[side], s.Wrist[side], 0.025f);
            Joint(ref j, s.Shoulder[side]);
            Joint(ref j, s.Elbow[side]);
            Joint(ref j, s.Wrist[side]);
        }
        Joint(ref j, s.Pelvis, 0.06f);
        Joint(ref j, s.NeckPivot);
    }

    private void Bone(ref int index, Vector3 from, Vector3 to, float thickness)
    {
        if (index >= _bones.Count)
        {
            MeshInstance3D mesh = new() { Mesh = new BoxMesh { Size = Vector3.One }, MaterialOverride = _material, CastShadow = GeometryInstance3D.ShadowCastingSetting.Off };
            AddChild(mesh);
            _bones.Add(mesh);
        }
        MeshInstance3D bone = _bones[index++];
        Vector3 seg = to - from;
        float len = seg.Length();
        bone.Visible = len > 1e-4f;
        if (!bone.Visible)
            return;
        Vector3 y = seg / len;
        Vector3 x = Mathf.Abs(y.Y) < 0.99f ? Vector3.Up.Cross(y).Normalized() : Vector3.Right.Cross(y).Normalized();
        Vector3 z = x.Cross(y);
        bone.GlobalTransform = new Transform3D(new Basis(x * thickness, y * len, z * thickness), (from + to) * 0.5f);
    }

    private void Joint(ref int index, Vector3 at, float size = 0.04f)
    {
        if (index >= _joints.Count)
        {
            MeshInstance3D mesh = new() { Mesh = new SphereMesh { Radius = 0.5f, Height = 1f }, MaterialOverride = _material, CastShadow = GeometryInstance3D.ShadowCastingSetting.Off };
            AddChild(mesh);
            _joints.Add(mesh);
        }
        MeshInstance3D joint = _joints[index++];
        joint.Visible = true;
        joint.GlobalTransform = new Transform3D(Basis.Identity.Scaled(Vector3.One * size), at);
    }
}