using Godot;

// Visual procedural legs for the ragdoll player (Nimso: "a very basic procedural leg system", feet
// "don't actually hold the player up" — ground control does). Feet stay planted on the ground and
// step, alternating, when the hips move or turn away from them; the stride leads with the body's
// velocity. Airborne the feet hang under the hips; low (crawling) they trail behind. Knees bend
// forward via a 2-bone solve. Physics-tick state (PlayerRagdoll.Step calls Tick), drawn in
// _Process from the interpolated pelvis. No collision — the lower-body collider is separate.
public partial class ProceduralLegs : Node3D
{
    public const float ThighRatio = 0.245f, ShinRatio = 0.246f, AnkleRatio = 0.039f;   // Winter, × stature
    public const float HipHalfWidth = 0.09f;
    public const float StanceHalfWidth = 0.11f;
    public const float StepTrigger = 0.2f;          // m from the ideal spot
    public const float StepTurnTrigger = 0.6f;      // rad
    public const float StepLead = 0.25f;            // s of velocity the new foothold leads by
    public const float StepTimeSlow = 0.32f, StepTimeFast = 0.2f;
    public const float StepHeight = 0.09f;
    public const float SettleTime = 0.4f;           // standing still this long → the trailing foot squares up

    private struct Foot
    {
        public Vector3 Planted, From, To;
        public float Yaw, FromYaw, ToYaw;
        public float T;           // step progress 0..1 (1 = planted)
        public Vector3 Current;
        public float CurrentYaw;
    }

    private readonly Foot[] _feet = new Foot[2];
    private readonly MeshInstance3D[] _thigh = new MeshInstance3D[2], _shin = new MeshInstance3D[2], _foot = new MeshInstance3D[2];
    private RigidBody3D _pelvis;
    private Vector3 _hipLocal;       // pelvis-body-local hip centre
    private float _thighLen, _shinLen, _ankle, _scale;
    private bool _initialized;
    private int _lastStepped;
    private float _stillTime;

    public void Setup(RigidBody3D pelvis, Vector3 hipLocal, float stature, Color pants, Color shoes)
    {
        _pelvis = pelvis;
        _hipLocal = hipLocal;
        _thighLen = ThighRatio * stature;
        _shinLen = ShinRatio * stature;
        _ankle = AnkleRatio * stature;
        _scale = stature / 1.75f;
        PhysicsInterpolationMode = PhysicsInterpolationModeEnum.Off;  // meshes are placed every frame
        TopLevel = true;
        for (int i = 0; i < 2; i++)
        {
            _thigh[i] = MakeMesh(new Vector3(0.13f * _scale, 1f, 0.14f * _scale), pants);
            _shin[i] = MakeMesh(new Vector3(0.1f * _scale, 1f, 0.11f * _scale), pants);
            _foot[i] = MakeMesh(new Vector3(0.1f * _scale, 0.07f * _scale, 0.25f * _scale), shoes);
        }
        _initialized = false;
    }

    private MeshInstance3D MakeMesh(Vector3 size, Color color)
    {
        MeshInstance3D m = new() { Mesh = new BoxMesh { Size = size } };
        m.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = color, Roughness = 0.7f });
        AddChild(m);
        return m;
    }

    private Vector3 Hip(Transform3D pelvis, int side) =>
        pelvis * (_hipLocal + new Vector3(side == 0 ? -HipHalfWidth * _scale : HipHalfWidth * _scale, 0f, 0f));

    private static float YawOf(Basis b)
    {
        Vector3 f = -b.Z;
        return Mathf.Atan2(-f.X, -f.Z);
    }

    // groundY = where the feet stand (ground control's foot level); crawl = legs collider off.
    public void Tick(float groundY, bool grounded, bool crawl, Basis yawBasis, float dt)
    {
        if (_pelvis == null)
            return;
        Transform3D pelvis = _pelvis.GlobalTransform;
        Vector3 velocity = _pelvis.LinearVelocity;
        velocity.Y = 0f;
        float yaw = YawOf(yawBasis);
        Vector3 back = yawBasis.Z;

        for (int i = 0; i < 2; i++)
        {
            Vector3 hip = Hip(pelvis, i);
            Vector3 side = yawBasis.X * (i == 0 ? -1f : 1f);
            Vector3 ideal = new Vector3(hip.X, groundY, hip.Z) + side * (StanceHalfWidth - HipHalfWidth) * _scale;
            if (crawl)
                ideal += back * (_thighLen + _shinLen) * 0.8f;
            if (!_initialized)
            {
                _feet[i] = new Foot { Planted = ideal, Current = ideal, To = ideal, Yaw = yaw, CurrentYaw = yaw, ToYaw = yaw, T = 1f };
                continue;
            }

            ref Foot f = ref _feet[i];
            if (!grounded)
            {
                // Hanging: feet dangle under the hips, a little forward, and replant on landing.
                Vector3 hang = hip + Vector3.Down * (_thighLen + _shinLen) * 0.92f - back * 0.08f;
                f.Current = f.Current.Lerp(hang, 1f - Mathf.Exp(-dt * 10f));
                f.CurrentYaw = Mathf.LerpAngle(f.CurrentYaw, yaw, 1f - Mathf.Exp(-dt * 10f));
                f.Planted = f.Current;
                f.T = 1f;
                continue;
            }

            if (f.T < 1f)
            {
                float speed = velocity.Length();
                float duration = Mathf.Lerp(StepTimeSlow, StepTimeFast, Mathf.Clamp(speed / 2.5f, 0f, 1f));
                f.T = Mathf.Min(1f, f.T + dt / duration);
                f.To = new Vector3(f.To.X, groundY, f.To.Z);
                float k = Mathf.SmoothStep(0f, 1f, f.T);
                f.Current = f.From.Lerp(f.To, k) + Vector3.Up * (Mathf.Sin(Mathf.Pi * f.T) * (StepHeight + 0.03f * speed));
                f.CurrentYaw = Mathf.LerpAngle(f.FromYaw, f.ToYaw, k);
                if (f.T >= 1f)
                {
                    f.Planted = f.To;
                    f.Yaw = f.ToYaw;
                }
            }
            else
            {
                f.Planted = new Vector3(f.Planted.X, Mathf.MoveToward(f.Planted.Y, groundY, dt * 2f), f.Planted.Z);
                f.Current = f.Planted;
                f.CurrentYaw = f.Yaw;
            }
        }

        if (!_initialized)
        {
            _initialized = true;
            return;
        }
        if (!grounded)
            return;

        // One foot at a time: step the foot that is farthest from where it should be.
        if (_feet[0].T >= 1f && _feet[1].T >= 1f)
        {
            float speed = velocity.Length();
            _stillTime = speed < 0.05f ? _stillTime + dt : 0f;
            int pick = -1;
            float worst = 0f;
            for (int i = 0; i < 2; i++)
            {
                Vector3 hip = Hip(pelvis, i);
                Vector3 side = yawBasis.X * (i == 0 ? -1f : 1f);
                Vector3 ideal = new Vector3(hip.X, groundY, hip.Z) + side * (StanceHalfWidth - HipHalfWidth) * _scale;
                if (crawl)
                    ideal += back * (_thighLen + _shinLen) * 0.8f;
                float off = new Vector2(_feet[i].Planted.X - ideal.X, _feet[i].Planted.Z - ideal.Z).Length();
                float turn = Mathf.Abs(Mathf.AngleDifference(_feet[i].Yaw, yaw));
                bool need = off > StepTrigger * _scale || turn > StepTurnTrigger || (_stillTime > SettleTime && off > 0.06f);
                float score = off + turn * 0.2f + (i == _lastStepped ? -0.01f : 0f);
                if (need && score > worst)
                {
                    worst = score;
                    pick = i;
                }
            }
            if (pick >= 0)
            {
                Vector3 hip = Hip(pelvis, pick);
                Vector3 side = yawBasis.X * (pick == 0 ? -1f : 1f);
                Vector3 to = new Vector3(hip.X, groundY, hip.Z) + side * (StanceHalfWidth - HipHalfWidth) * _scale + velocity * StepLead;
                if (crawl)
                    to += back * (_thighLen + _shinLen) * 0.8f;
                ref Foot f = ref _feet[pick];
                f.From = f.Planted;
                f.FromYaw = f.Yaw;
                f.To = to;
                f.ToYaw = yaw;
                f.T = 0f;
                _lastStepped = pick;
                _stillTime = 0f;
            }
        }
    }

    public override void _Process(double delta)
    {
        if (_pelvis == null || !_initialized || !IsInstanceValid(_pelvis))
            return;
        Transform3D pelvis = _pelvis.GetGlobalTransformInterpolated();
        Vector3 forward = -pelvis.Basis.Z;
        forward.Y = 0f;
        forward = forward.LengthSquared() > 1e-6f ? forward.Normalized() : Vector3.Forward;
        for (int i = 0; i < 2; i++)
        {
            Vector3 hip = Hip(pelvis, i);
            Vector3 ankle = _feet[i].Current + Vector3.Up * _ankle;
            // 2-bone solve, knee bending forward.
            Vector3 d = ankle - hip;
            float reach = _thighLen + _shinLen;
            float len = Mathf.Clamp(d.Length(), 0.05f, reach * 0.999f);
            Vector3 dir = d.LengthSquared() > 1e-8f ? d.Normalized() : Vector3.Down;
            ankle = hip + dir * len;
            float along = (_thighLen * _thighLen - _shinLen * _shinLen + len * len) / (2f * len);
            float radius = Mathf.Sqrt(Mathf.Max(_thighLen * _thighLen - along * along, 0f));
            Vector3 pole = forward - dir * forward.Dot(dir);
            pole = pole.LengthSquared() > 1e-6f ? pole.Normalized() : forward;
            Vector3 knee = hip + dir * along + pole * radius;
            Place(_thigh[i], hip, knee);
            Place(_shin[i], knee, ankle);
            Basis footBasis = new Basis(Vector3.Up, _feet[i].CurrentYaw);
            _foot[i].GlobalTransform = new Transform3D(footBasis, _feet[i].Current + Vector3.Up * (0.035f * _scale) + footBasis * new Vector3(0f, 0f, -0.06f * _scale));
        }
    }

    private static void Place(MeshInstance3D mesh, Vector3 from, Vector3 to)
    {
        Vector3 seg = to - from;
        float len = seg.Length();
        if (len < 1e-4f)
            return;
        Vector3 y = seg / len;
        Vector3 x = Mathf.Abs(y.Y) < 0.99f ? Vector3.Up.Cross(y).Normalized() : Vector3.Right;
        Vector3 z = x.Cross(y);
        // Box mesh is 1 m tall along Y: stretch it to the segment.
        mesh.GlobalTransform = new Transform3D(new Basis(x, y * len, z), (from + to) * 0.5f);
    }
}