using Godot;

// Placeholder rig VISUALS only: head cube on the HMD, arm boxes drawn from the physical body
// (PhysicsTorso shoulders → PhysicsArms elbow → hand wrist). Arm IK, calibration and the reach
// budget live in PhysicsArms; the torso mesh lives in PhysicsTorso.
public partial class BodyRig : Node3D
{
    private const float HeadBackOffset = 0.06f;
    private const float HeadSize = 0.15f;
    private const float WristOffset = 0.06f;

    private XRCamera3D _camera;
    private XRController3D _left;
    private XRController3D _right;
    private PhysicsArms _arms;

    private MeshInstance3D _headMesh;
    private readonly MeshInstance3D[] _upperArmMeshes = new MeshInstance3D[2];
    private readonly MeshInstance3D[] _forearmMeshes = new MeshInstance3D[2];

    private bool _sized;
    private Basis _yawBasis = Basis.Identity;

    public override void _Ready()
    {
        _camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
        _left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
        _right = GetNode<XRController3D>("../XROrigin3D/RightHand");
        _arms = GetNode<PhysicsArms>("../PhysicsArms");

        _headMesh = MakeMesh(new BoxMesh { Size = new Vector3(HeadSize, HeadSize, HeadSize) }, new Color(0.8f, 0.8f, 0.8f));
        for (int side = 0; side < 2; side++)
        {
            _upperArmMeshes[side] = MakeMesh(new BoxMesh { Size = new Vector3(0.07f, 0.3f, 0.07f) }, new Color(0.3f, 0.3f, 0.33f));
            _forearmMeshes[side] = MakeMesh(new BoxMesh { Size = new Vector3(0.06f, 0.3f, 0.06f) }, new Color(0.3f, 0.3f, 0.33f));
        }
        SetRigVisible(false);
    }

    private MeshInstance3D MakeMesh(Mesh mesh, Color color)
    {
        MeshInstance3D node = new() { Mesh = mesh };
        node.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = color, Roughness = 0.6f });
        AddChild(node);
        return node;
    }

    private void SetRigVisible(bool visible)
    {
        _headMesh.Visible = visible;
        foreach (MeshInstance3D m in _upperArmMeshes)
            m.Visible = visible;
        foreach (MeshInstance3D m in _forearmMeshes)
            m.Visible = visible;
    }

    public override void _Process(double delta)
    {
        UpdateYawBasis();
        if (!_arms.Calibrated || _arms.UpperArm(0) == null)
            return;
        if (!_sized)
        {
            _sized = true;
            for (int side = 0; side < 2; side++)
            {
                ((BoxMesh)_upperArmMeshes[side].Mesh).Size = new Vector3(0.07f, _arms.UpperLen, 0.07f);
                ((BoxMesh)_forearmMeshes[side].Mesh).Size = new Vector3(0.06f, _arms.ForeLen, 0.06f);
            }
            SetRigVisible(true);
        }

        Vector3 headPos = _camera.GlobalPosition - _yawBasis * Vector3.Forward * HeadBackOffset;
        _headMesh.GlobalTransform = new Transform3D(_yawBasis, headPos);

        PlaceArm(0, PhysicsHands.GetBody(_left));
        PlaceArm(1, PhysicsHands.GetBody(_right));
    }

    private void PlaceArm(int side, RigidBody3D hand)
    {
        Vector3 shoulder = _arms.Shoulder(side);
        Vector3 elbow = _arms.Elbow(side);
        Vector3 wrist = hand != null
            ? hand.GlobalPosition + hand.GlobalTransform.Basis * new Vector3(0f, 0f, WristOffset)
            : _arms.Wrist(side);
        if (!shoulder.IsFinite() || !elbow.IsFinite() || !wrist.IsFinite())
            return;
        PlaceSegment(_upperArmMeshes[side], shoulder, elbow);
        PlaceSegment(_forearmMeshes[side], elbow, wrist);
    }

    private void UpdateYawBasis()
    {
        Vector3 fwd = -_camera.GlobalTransform.Basis.Z;
        fwd.Y = 0f;
        if (fwd.LengthSquared() < 0.0001f)
            return;
        _yawBasis = Basis.LookingAt(fwd.Normalized(), Vector3.Up);
    }

    // Stretches the unit-length box between two joints so the drawn arm stays connected even
    // when a physical segment is held back by contact.
    private static void PlaceSegment(MeshInstance3D mesh, Vector3 from, Vector3 to)
    {
        Vector3 segment = to - from;
        float length = segment.Length();
        if (length < 0.001f)
            return;
        Vector3 y = segment / length;
        Vector3 x = Vector3.Up.Cross(y);
        if (x.LengthSquared() < 0.0001f)
            x = Vector3.Right.Cross(y);
        x = x.Normalized();
        Vector3 z = x.Cross(y);
        float scale = length / ((BoxMesh)mesh.Mesh).Size.Y;
        mesh.GlobalTransform = new Transform3D(new Basis(x, y * scale, z), (from + to) * 0.5f);
    }
}