using Godot;
using System.Collections.Generic;

// The body-physics sandbox (Nimso-sandbox style), built in code around the spawn/table area:
//   EAST  — mantling ledges (0.3 … 1.5 m), a 1 m platform with stairs, a walkable 18° ramp and a
//           too-steep 40° one, a climbing wall with holds onto a 2.4 m top, a pull-up bar, monkey bars.
//   WEST  — physics toys: a plank bridge on two blocks (walk on it / knock it off), loose planks,
//           a seesaw, a hanging swing board (grab it and hang), a crate pyramid, a heavy box,
//           barrels, a balance beam.
//   SOUTH — a crawl tunnel (0.6 m clearance), a duck-under bar (1.2 m) and a doorway.
// Static pieces are world (layer 1) and grabbable for climbing (PropGrabber grabs any static body
// not in "no_grab"); dynamic pieces are "prop"s (grab/push) and come back with Reset().
public partial class Playground : Node3D
{
    private readonly List<(RigidBody3D Body, Transform3D Start)> _dynamic = new();
    private const uint WorldLayer = 1, WorldMask = 0b11101, PropMask = 0b11101;

    private static readonly Color Concrete = new(0.62f, 0.62f, 0.6f), Wood = new(0.72f, 0.55f, 0.33f),
        DarkWood = new(0.5f, 0.36f, 0.2f), Metal = new(0.45f, 0.47f, 0.52f), Hold = new(0.9f, 0.45f, 0.15f),
        Blue = new(0.3f, 0.45f, 0.75f), Green = new(0.35f, 0.6f, 0.35f), Red = new(0.7f, 0.25f, 0.2f);

    public override void _Ready()
    {
        BuildEast();
        BuildWest();
        BuildSouth();
    }

    public void Reset()
    {
        foreach (var (body, start) in _dynamic)
        {
            if (!IsInstanceValid(body))
                continue;
            body.GlobalTransform = start;
            body.LinearVelocity = Vector3.Zero;
            body.AngularVelocity = Vector3.Zero;
            body.ResetPhysicsInterpolation();
        }
    }

    // ---------------------------------------------------------------- builders

    private StaticBody3D Block(string name, Vector3 size, Vector3 center, Color color, Basis? basis = null)
    {
        StaticBody3D body = new() { Name = name, CollisionLayer = WorldLayer, CollisionMask = WorldMask, Transform = new Transform3D(basis ?? Basis.Identity, center) };
        body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
        body.AddChild(Mesh(new BoxMesh { Size = size }, color));
        AddChild(body);
        return body;
    }

    private StaticBody3D Bar(string name, Vector3 from, Vector3 to, float radius, Color color)
    {
        Vector3 axis = to - from;
        float len = axis.Length();
        Vector3 y = axis / len;
        Vector3 x = Mathf.Abs(y.Y) < 0.99f ? Vector3.Up.Cross(y).Normalized() : Vector3.Right;
        StaticBody3D body = new() { Name = name, CollisionLayer = WorldLayer, CollisionMask = WorldMask, Transform = new Transform3D(new Basis(x, y, x.Cross(y)), (from + to) * 0.5f) };
        body.AddChild(new CollisionShape3D { Shape = new CylinderShape3D { Radius = radius, Height = len } });
        body.AddChild(Mesh(new CylinderMesh { TopRadius = radius, BottomRadius = radius, Height = len, RadialSegments = 12 }, color));
        AddChild(body);
        return body;
    }

    private RigidBody3D Prop(string name, Shape3D shape, Mesh mesh, float mass, Vector3 center, Color color, Basis? basis = null, float friction = 0.8f)
    {
        RigidBody3D body = new()
        {
            Name = name,
            Mass = mass,
            Transform = new Transform3D(basis ?? Basis.Identity, center),
            CollisionMask = PropMask,
            ContactMonitor = true,
            MaxContactsReported = 4,
            PhysicsMaterialOverride = new PhysicsMaterial { Friction = friction, Bounce = 0.05f },
        };
        body.AddToGroup("prop");
        body.AddChild(new CollisionShape3D { Shape = shape });
        body.AddChild(Mesh(mesh, color));
        AddChild(body);
        _dynamic.Add((body, body.GlobalTransform));
        return body;
    }

    private RigidBody3D BoxProp(string name, Vector3 size, float mass, Vector3 center, Color color, Basis? basis = null) =>
        Prop(name, new BoxShape3D { Size = size }, new BoxMesh { Size = size }, mass, center, color, basis);

    private static MeshInstance3D Mesh(Mesh mesh, Color color)
    {
        MeshInstance3D m = new() { Mesh = mesh };
        m.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = color, Roughness = 0.75f });
        return m;
    }

    private void Sign(string text, Vector3 at)
    {
        AddChild(new Label3D
        {
            Text = text,
            FontSize = 96,
            PixelSize = 0.002f,
            OutlineSize = 12,
            Billboard = BaseMaterial3D.BillboardModeEnum.FixedY,
            Position = at,
        });
    }

    // ---------------------------------------------------------------- zones

    private void BuildEast()
    {
        // Mantling ledges: walk up to one, put both hands on it and press yourself up.
        float[] heights = { 0.3f, 0.6f, 0.9f, 1.2f, 1.5f };
        for (int i = 0; i < heights.Length; i++)
        {
            float h = heights[i];
            Block($"Ledge{h:0.0}", new Vector3(1.2f, h, 1.2f), new Vector3(4.5f + i * 1.9f, h * 0.5f, -8f), Concrete);
            Sign($"{h:0.0} m", new Vector3(4.5f + i * 1.9f, h + 0.35f, -7.3f));
        }
        Sign("Mantle: both hands on the ledge, push down", new Vector3(8.3f, 2.4f, -8f));

        // Platform 1 m high with stairs (0.17 m risers) on the south side and ramps on the north.
        Block("Platform", new Vector3(3f, 1f, 3f), new Vector3(8f, 0.5f, -1.5f), Concrete);
        for (int i = 0; i < 5; i++)
        {
            float top = 0.17f * (i + 1);
            Block($"Stair{i}", new Vector3(1.2f, top, 0.3f), new Vector3(8f, top * 0.5f, 0.15f + (4 - i) * 0.3f), Concrete.Lightened(0.1f));
        }
        float walkable = Mathf.DegToRad(18f);
        float rampLen = 1f / Mathf.Sin(walkable);
        // Negative X rotation lifts the slab's +Z end, which touches the platform's north edge.
        Block("RampWalkable", new Vector3(1.2f, 0.1f, rampLen), new Vector3(7f, 0.5f, -3f - Mathf.Cos(walkable) * rampLen * 0.5f),
            Green, new Basis(Vector3.Right, -walkable));
        float steep = Mathf.DegToRad(40f);
        float steepLen = 1f / Mathf.Sin(steep);
        Block("RampSteep", new Vector3(1.2f, 0.1f, steepLen), new Vector3(9f, 0.5f, -3f - Mathf.Cos(steep) * steepLen * 0.5f),
            Red, new Basis(Vector3.Right, -steep));
        Sign("18° ramp", new Vector3(7f, 1.6f, -4.5f));
        Sign("40° (too steep)", new Vector3(9f, 1.6f, -4.5f));

        // Climbing wall: holds on the west face, top at 2.4 m to climb onto.
        Block("ClimbWall", new Vector3(1.5f, 2.4f, 3f), new Vector3(12.25f, 1.2f, -1.5f), Concrete.Darkened(0.15f));
        RandomNumberGenerator rng = new() { Seed = 5 };
        for (float y = 0.4f; y < 2.35f; y += 0.35f)
        {
            for (int k = 0; k < 4; k++)
            {
                float z = -2.7f + k * 0.8f + rng.RandfRange(-0.15f, 0.15f);
                Block("Hold", new Vector3(0.08f, 0.06f, 0.22f), new Vector3(11.46f, y + rng.RandfRange(-0.06f, 0.06f), z), Hold);
            }
        }
        Sign("Climbing wall: grip the holds", new Vector3(11f, 3f, -1.5f));

        // Pull-up bar and monkey bars.
        Bar("PostA", new Vector3(5f, 0f, 2.5f), new Vector3(5f, 2.25f, 2.5f), 0.05f, Metal);
        Bar("PostB", new Vector3(6.4f, 0f, 2.5f), new Vector3(6.4f, 2.25f, 2.5f), 0.05f, Metal);
        Bar("PullUpBar", new Vector3(5f, 2.1f, 2.5f), new Vector3(6.4f, 2.1f, 2.5f), 0.025f, Metal);
        Sign("Pull-up bar", new Vector3(5.7f, 2.6f, 2.5f));
        Bar("MonkeyRailL", new Vector3(5f, 2.25f, 4.5f), new Vector3(10f, 2.25f, 4.5f), 0.04f, Metal);
        Bar("MonkeyRailR", new Vector3(5f, 2.25f, 5.1f), new Vector3(10f, 2.25f, 5.1f), 0.04f, Metal);
        foreach (float x in new[] { 5f, 10f })
            foreach (float z in new[] { 4.5f, 5.1f })
                Bar("MonkeyPost", new Vector3(x, 0f, z), new Vector3(x, 2.25f, z), 0.05f, Metal);
        for (float x = 5.4f; x < 9.8f; x += 0.4f)
            Bar("Rung", new Vector3(x, 2.2f, 4.5f), new Vector3(x, 2.2f, 5.1f), 0.022f, Metal);
        Sign("Monkey bars", new Vector3(7.5f, 2.8f, 4.8f));
    }

    private void BuildWest()
    {
        // Plank bridge: two blocks with a loose 9 kg plank across — walk on it, or knock it off.
        Block("BridgeBlockA", new Vector3(1f, 0.8f, 1f), new Vector3(-4.5f, 0.4f, -5f), Concrete);
        Block("BridgeBlockB", new Vector3(1f, 0.8f, 1f), new Vector3(-8f, 0.4f, -5f), Concrete);
        BoxProp("BridgePlank", new Vector3(3.4f, 0.06f, 0.4f), 9f, new Vector3(-6.25f, 0.83f, -5f), Wood);
        Sign("Plank bridge", new Vector3(-6.25f, 1.6f, -5f));

        // Loose planks to pick up, carry, lean, swing (light enough to wield, long enough to lever).
        for (int i = 0; i < 3; i++)
            BoxProp($"Plank{i}", new Vector3(2f, 0.04f, 0.2f), 4f, new Vector3(-5f, 0.02f + i * 0.045f, -2.6f + i * 0.05f), Wood.Lightened(0.05f * i),
                new Basis(Vector3.Up, 0.1f * i));

        // Seesaw: a plank hinged on a pivot block.
        Block("SeesawPivot", new Vector3(0.3f, 0.45f, 0.5f), new Vector3(-8.5f, 0.225f, -1.5f), Metal);
        RigidBody3D seesaw = BoxProp("Seesaw", new Vector3(3.2f, 0.06f, 0.45f), 12f, new Vector3(-8.5f, 0.48f, -1.5f), DarkWood);
        HingeJoint3D pivot = new()
        {
            Transform = new Transform3D(new Basis(Vector3.Right, Vector3.Up, Vector3.Back), new Vector3(-8.5f, 0.48f, -1.5f)),
            NodeA = GetNode<Node>("SeesawPivot").GetPath(),   // absolute: joint paths resolve relative to the joint
            NodeB = seesaw.GetPath(),
        };
        AddChild(pivot);
        Sign("Seesaw", new Vector3(-8.5f, 1.3f, -1.5f));

        // Swing board: a 10 kg board hinged at its top edge from a 2.4 m beam — grab it and hang.
        Bar("SwingPostA", new Vector3(-5f, 0f, 1.5f), new Vector3(-5f, 2.45f, 1.5f), 0.05f, Metal);
        Bar("SwingPostB", new Vector3(-7f, 0f, 1.5f), new Vector3(-7f, 2.45f, 1.5f), 0.05f, Metal);
        StaticBody3D beam = Bar("SwingBeam", new Vector3(-5f, 2.4f, 1.5f), new Vector3(-7f, 2.4f, 1.5f), 0.05f, Metal);
        RigidBody3D board = BoxProp("SwingBoard", new Vector3(1.2f, 1.2f, 0.05f), 10f, new Vector3(-6f, 1.75f, 1.5f), Wood);
        HingeJoint3D swing = new()
        {
            Transform = new Transform3D(new Basis(Vector3.Back, Vector3.Up, Vector3.Left), new Vector3(-6f, 2.35f, 1.5f)),
            NodeA = beam.GetPath(),
            NodeB = board.GetPath(),
        };
        AddChild(swing);
        Sign("Hanging board", new Vector3(-6f, 2.9f, 1.5f));

        // Crate pyramid (4 kg each), a heavy 25 kg box, barrels.
        Vector3 basePos = new(-9.5f, 0f, 2f);
        int n = 0;
        for (int row = 0; row < 3; row++)
            for (int col = 0; col < 3 - row; col++)
                BoxProp($"Crate{n++}", Vector3.One * 0.4f, 4f, basePos + new Vector3((col + row * 0.5f) * 0.42f, 0.2f + row * 0.401f, 0f), Wood.Darkened(0.1f));
        BoxProp("HeavyBox", Vector3.One * 0.7f, 25f, new Vector3(-9f, 0.35f, 4f), Blue);
        for (int i = 0; i < 2; i++)
            Prop($"Barrel{i}", new CylinderShape3D { Radius = 0.28f, Height = 0.85f }, new CylinderMesh { TopRadius = 0.28f, BottomRadius = 0.28f, Height = 0.85f }, 12f,
                new Vector3(-7.5f + i * 0.7f, 0.425f, 4.5f), Red);

        // Balance beam.
        Block("BalanceBeam", new Vector3(0.12f, 0.3f, 4f), new Vector3(-11f, 0.15f, -2f), DarkWood);
        Sign("Balance beam", new Vector3(-11f, 1.1f, -2f));
    }

    private void BuildSouth()
    {
        // Crawl tunnel: 0.6 m clearance, 3 m long.
        Block("TunnelRoof", new Vector3(1.4f, 0.1f, 3f), new Vector3(-2.5f, 0.65f, 7f), Concrete);
        Block("TunnelWallL", new Vector3(0.1f, 0.6f, 3f), new Vector3(-3.15f, 0.3f, 7f), Concrete);
        Block("TunnelWallR", new Vector3(0.1f, 0.6f, 3f), new Vector3(-1.85f, 0.3f, 7f), Concrete);
        Sign("Crawl tunnel (0.6 m)", new Vector3(-2.5f, 1.2f, 5.3f));

        // Duck-under bar at 1.2 m and a doorway (1.9 × 0.8 m).
        Bar("DuckPostA", new Vector3(1f, 0f, 7f), new Vector3(1f, 1.3f, 7f), 0.05f, Metal);
        Bar("DuckPostB", new Vector3(2.6f, 0f, 7f), new Vector3(2.6f, 1.3f, 7f), 0.05f, Metal);
        Bar("DuckBar", new Vector3(1f, 1.25f, 7f), new Vector3(2.6f, 1.25f, 7f), 0.03f, Metal);
        Sign("Duck under (1.2 m)", new Vector3(1.8f, 1.8f, 7f));
        Block("DoorL", new Vector3(0.2f, 2.1f, 0.3f), new Vector3(4.1f, 1.05f, 7f), Wood);
        Block("DoorR", new Vector3(0.2f, 2.1f, 0.3f), new Vector3(5.1f, 1.05f, 7f), Wood);
        Block("DoorTop", new Vector3(1.2f, 0.2f, 0.3f), new Vector3(4.6f, 2.0f, 7f), Wood);
        Sign("Doorway", new Vector3(4.6f, 2.5f, 7f));
    }
}