Minecraft-Console-Client/MinecraftClient.Tests/Pathing/Execution/GroundedTemplateConvergenceTests.cs

using System;
using MinecraftClient.Mapping;
using MinecraftClient.Pathing.Core;
using MinecraftClient.Pathing.Execution;
using MinecraftClient.Pathing.Execution.Templates;
using MinecraftClient.Physics;
using Xunit;

namespace MinecraftClient.Tests.Pathing.Execution;

public sealed class GroundedTemplateConvergenceTests
{
    [Fact]
    public void PlayerPhysics_SprintingGroundTravel_IsFasterThanWalking()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor(min: -2, max: 16);
        var walking = TemplateSimulationRunner.CreateGroundedPhysics(new Location(0.5, 80, 0.5), yaw: 270f);
        var sprinting = TemplateSimulationRunner.CreateGroundedPhysics(new Location(0.5, 80, 0.5), yaw: 270f);

        for (int tick = 0; tick < 8; tick++)
        {
            walking.ApplyInput(new MovementInput { Forward = true });
            walking.Tick(world);

            sprinting.ApplyInput(new MovementInput { Forward = true, Sprint = true });
            sprinting.Tick(world);
        }

        double walkingTravel = walking.Position.X - 0.5;
        double sprintingTravel = sprinting.Position.X - 0.5;

        Assert.True(
            sprintingTravel > walkingTravel + 0.05,
            $"walkingTravel={walkingTravel:F4} sprintingTravel={sprintingTravel:F4} walkVel={walking.DeltaMovement} sprintVel={sprinting.DeltaMovement}");
    }

    [Fact]
    public void WalkTemplate_FinalStop_Completes_WhenFootprintStaysInsideTargetBlock()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor();
        var segment = new PathSegment
        {
            Start = new Location(0.5, 80, 0.5),
            End = new Location(1.5, 80, 0.5),
            MoveType = MoveType.Traverse,
            ExitTransition = PathTransitionType.FinalStop
        };

        var template = new WalkTemplate(segment, null);
        var physics = TemplateSimulationRunner.CreateGroundedPhysics(segment.Start, yaw: 270f);

        TemplateState state = TemplateSimulationRunner.Run(template, physics, world, maxTicks: 160, out Location finalPos);

        Assert.True(state == TemplateState.Complete, $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}");
        Assert.True(TemplateFootingHelper.IsFootprintInsideTargetBlock(finalPos, segment.End));
    }

    [Fact]
    public void WalkTemplate_FinalStop_Completes_WhenCenterStopsInsideTargetBlockNearEdge()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor();
        var segment = new PathSegment
        {
            Start = new Location(2.5, 80, 0.5),
            End = new Location(3.5, 80, 0.5),
            MoveType = MoveType.Traverse,
            ExitTransition = PathTransitionType.FinalStop
        };

        var template = new WalkTemplate(segment, null);
        var physics = new PlayerPhysics
        {
            Position = new Vec3d(3.2897, 80.0, 0.5),
            DeltaMovement = Vec3d.Zero,
            OnGround = true,
            MovementSpeed = 0.1f,
            Yaw = 270f
        };

        var input = new MovementInput();
        TemplateState state = template.Tick(new Location(physics.Position.X, physics.Position.Y, physics.Position.Z), physics, input, world);

        Assert.Equal(TemplateState.Complete, state);
    }

    [Fact]
    public void WalkTemplate_FinalStop_Completes_FromLiveNearGoalState_WithoutFailure()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor(min: 95, max: 115);
        var segment = new PathSegment
        {
            Start = new Location(102.5, 80, 100.5),
            End = new Location(103.5, 80, 100.5),
            MoveType = MoveType.Traverse,
            ExitTransition = PathTransitionType.FinalStop
        };

        var template = new WalkTemplate(segment, null);
        var physics = new PlayerPhysics
        {
            Position = new Vec3d(103.36, 80.0, 100.50),
            DeltaMovement = new Vec3d(0.0346, 0.0, 0.0),
            OnGround = true,
            MovementSpeed = 0.1f,
            Yaw = 270f
        };

        TemplateState state = TemplateSimulationRunner.Run(template, physics, world, maxTicks: 40, out _);

        Assert.Equal(TemplateState.Complete, state);
    }

    [Fact]
    public void WalkTemplate_PrepareJump_CompletesWithoutSettlingOnRunUpBlock()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor();
        var current = new PathSegment
        {
            Start = new Location(0.5, 80, 0.5),
            End = new Location(1.5, 80, 0.5),
            MoveType = MoveType.Traverse,
            ExitTransition = PathTransitionType.PrepareJump,
            PreserveSprint = true
        };
        var next = new PathSegment
        {
            Start = new Location(1.5, 80, 0.5),
            End = new Location(3.5, 80, 0.5),
            MoveType = MoveType.Parkour,
            ExitTransition = PathTransitionType.FinalStop
        };

        var template = new WalkTemplate(current, next);
        var physics = TemplateSimulationRunner.CreateGroundedPhysics(current.Start, yaw: 270f);

        TemplateState state = TemplateSimulationRunner.Run(template, physics, world, maxTicks: 60, out _);

        Assert.Equal(TemplateState.Complete, state);
        Assert.True(physics.DeltaMovement.X > 0.02);
    }

    [Fact]
    public void WalkTemplate_PrepareJump_WithPlannerHints_CompletesOnRunUpBlock()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor();
        var current = new PathSegment
        {
            Start = new Location(0.5, 80, 0.5),
            End = new Location(1.5, 80, 0.5),
            MoveType = MoveType.Traverse,
            ExitTransition = PathTransitionType.PrepareJump,
            ExitHints = new PathTransitionHints(1, 0, 0.10, double.PositiveInfinity, false, true, true, false, 10),
            PreserveSprint = true
        };
        var next = new PathSegment
        {
            Start = new Location(1.5, 80, 0.5),
            End = new Location(3.5, 80, 0.5),
            MoveType = MoveType.Parkour,
            ExitTransition = PathTransitionType.FinalStop
        };

        var template = new WalkTemplate(current, next);
        var physics = TemplateSimulationRunner.CreateGroundedPhysics(current.Start, yaw: 270f);

        TemplateState state = TemplateSimulationRunner.Run(template, physics, world, maxTicks: 120, out Location finalPos);

        Assert.Equal(TemplateState.Complete, state);
        Assert.True(
            TemplateFootingHelper.IsCenterInsideTargetBlock(finalPos, current.End),
            $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}");
    }

    [Fact]
    public void AscendTemplate_DiagonalPrepareJump_WithPlannerHints_CompletesOnRunUpBlock()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor(min: -1, max: 6);
        FlatWorldTestBuilder.ClearBox(world, -1, 79, -1, 6, 84, 6);
        FlatWorldTestBuilder.SetSolid(world, 0, 79, 0);
        FlatWorldTestBuilder.SetSolid(world, 1, 80, 1);
        FlatWorldTestBuilder.SetSolid(world, 3, 80, 3);

        var current = new PathSegment
        {
            Start = new Location(0.5, 80, 0.5),
            End = new Location(1.5, 81, 1.5),
            MoveType = MoveType.Ascend,
            ExitTransition = PathTransitionType.PrepareJump,
            ExitHints = new PathTransitionHints(1, 1, 0.10, double.PositiveInfinity, false, true, true, false, 10),
            PreserveSprint = true
        };
        var next = new PathSegment
        {
            Start = new Location(1.5, 81, 1.5),
            End = new Location(3.5, 81, 3.5),
            MoveType = MoveType.Parkour,
            ExitTransition = PathTransitionType.FinalStop
        };

        var template = new AscendTemplate(current, next);
        var physics = TemplateSimulationRunner.CreateGroundedPhysics(current.Start, yaw: 315f);
        var input = new MovementInput();
        var trace = new List<string>();
        TemplateState state = TemplateState.InProgress;
        Location finalPos = current.Start;
        for (int tick = 0; tick < 140; tick++)
        {
            input.Reset();
            Location pos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
            state = template.Tick(pos, physics, input, world);
            if (tick < 20 || state != TemplateState.InProgress)
            {
                trace.Add(
                    $"tick={tick} state={state} pos={pos} yaw={physics.Yaw:F1} vel={physics.DeltaMovement} " +
                    $"onGround={physics.OnGround} input(F={input.Forward},J={input.Jump},S={input.Sprint})");
            }

            if (state != TemplateState.InProgress)
            {
                finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
                break;
            }

            physics.ApplyInput(input);
            physics.Tick(world);
            finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
        }

        Assert.True(
            state == TemplateState.Complete,
            $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
        Assert.True(
            TemplateFootingHelper.IsCenterInsideTargetBlock(finalPos, current.End),
            $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
    }

    [Fact]
    public void AscendTemplate_AfterTraversePrepareJump_CompletesWithinTwentyTicks()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor(min: -1, max: 6);
        FlatWorldTestBuilder.ClearBox(world, -1, 79, -1, 6, 84, 2);
        FlatWorldTestBuilder.SetSolid(world, 0, 79, 0);
        FlatWorldTestBuilder.SetSolid(world, 1, 79, 0);
        FlatWorldTestBuilder.SetSolid(world, 2, 80, 0);
        FlatWorldTestBuilder.SetSolid(world, 3, 81, 0);

        var traverse = new PathSegment
        {
            Start = new Location(0.5, 80, 0.5),
            End = new Location(1.5, 80, 0.5),
            MoveType = MoveType.Traverse,
            ExitTransition = PathTransitionType.PrepareJump,
            ExitHints = new PathTransitionHints(1, 0, 0.0, double.PositiveInfinity, false, true, true, false, 10),
            PreserveSprint = true
        };
        var ascend = new PathSegment
        {
            Start = new Location(1.5, 80, 0.5),
            End = new Location(2.5, 81, 0.5),
            MoveType = MoveType.Ascend,
            ExitTransition = PathTransitionType.PrepareJump,
            ExitHints = new PathTransitionHints(1, 0, 0.10, double.PositiveInfinity, false, true, true, false, 10),
            PreserveSprint = true
        };
        var next = new PathSegment
        {
            Start = new Location(2.5, 81, 0.5),
            End = new Location(3.5, 82, 0.5),
            MoveType = MoveType.Ascend,
            ExitTransition = PathTransitionType.FinalStop
        };

        var physics = TemplateSimulationRunner.CreateGroundedPhysics(traverse.Start, yaw: 270f);

        TemplateState traverseState = TemplateSimulationRunner.Run(new WalkTemplate(traverse, ascend), physics, world, maxTicks: 80, out _);

        var template = new AscendTemplate(ascend, next);
        var input = new MovementInput();
        TemplateState state = TemplateState.InProgress;
        int elapsedTicks = 0;
        Location finalPos = ascend.Start;
        for (; elapsedTicks < 80; elapsedTicks++)
        {
            input.Reset();
            Location pos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
            state = template.Tick(pos, physics, input, world);
            if (state != TemplateState.InProgress)
            {
                finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
                break;
            }

            physics.ApplyInput(input);
            physics.Tick(world);
        }

        Assert.Equal(TemplateState.Complete, traverseState);
        Assert.Equal(TemplateState.Complete, state);
        Assert.True(elapsedTicks <= 20, $"elapsedTicks={elapsedTicks} finalPos={finalPos} vel={physics.DeltaMovement}");
    }

    [Fact]
    public void AscendTemplate_PrepareJump_CompletesFromOppositeYawWithinTwentyTicks()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor(min: 338, max: 344);
        FlatWorldTestBuilder.ClearBox(world, 340, 80, 338, 344, 84, 342);
        FlatWorldTestBuilder.FillSolid(world, 341, 80, 339, 341, 80, 341);
        FlatWorldTestBuilder.FillSolid(world, 342, 81, 339, 342, 81, 341);

        var segment = new PathSegment
        {
            Start = new Location(340.5, 80, 340.5),
            End = new Location(341.5, 81, 340.5),
            MoveType = MoveType.Ascend,
            ExitTransition = PathTransitionType.PrepareJump,
            ExitHints = new PathTransitionHints(1, 0, 0.10, double.PositiveInfinity, false, true, true, false, 10),
            PreserveSprint = true
        };
        var next = new PathSegment
        {
            Start = new Location(341.5, 81, 340.5),
            End = new Location(342.5, 82, 340.5),
            MoveType = MoveType.Ascend,
            ExitTransition = PathTransitionType.FinalStop
        };

        var template = new AscendTemplate(segment, next);
        var physics = TemplateSimulationRunner.CreateGroundedPhysics(segment.Start, yaw: 90f);
        var input = new MovementInput();
        TemplateState state = TemplateState.InProgress;
        int elapsedTicks = 0;
        Location finalPos = segment.Start;
        var trace = new List<string>();
        for (; elapsedTicks < 80; elapsedTicks++)
        {
            input.Reset();
            Location pos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
            state = template.Tick(pos, physics, input, world);
            if (elapsedTicks < 20 || state != TemplateState.InProgress)
            {
                trace.Add(
                    $"tick={elapsedTicks} state={state} pos={pos} yaw={physics.Yaw:F1} vel={physics.DeltaMovement} " +
                    $"onGround={physics.OnGround} input(F={input.Forward},J={input.Jump},S={input.Sprint})");
            }
            if (state != TemplateState.InProgress)
            {
                finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
                break;
            }

            physics.ApplyInput(input);
            physics.Tick(world);
            finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
        }

        Assert.Equal(TemplateState.Complete, state);
        Assert.True(elapsedTicks <= 20, $"elapsedTicks={elapsedTicks} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
        Assert.True(
            TemplateFootingHelper.IsCenterInsideTargetBlock(finalPos, segment.End),
            $"elapsedTicks={elapsedTicks} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
    }

    [Fact]
    public void AscendTemplate_PrepareJump_CompletesFromOffCenterRunUpState()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor(min: 338, max: 344);
        FlatWorldTestBuilder.ClearBox(world, 340, 80, 338, 344, 84, 342);
        FlatWorldTestBuilder.FillSolid(world, 341, 80, 339, 341, 80, 341);
        FlatWorldTestBuilder.FillSolid(world, 342, 81, 339, 342, 81, 341);

        var segment = new PathSegment
        {
            Start = new Location(340.5, 80, 340.5),
            End = new Location(341.5, 81, 340.5),
            MoveType = MoveType.Ascend,
            ExitTransition = PathTransitionType.PrepareJump,
            ExitHints = new PathTransitionHints(1, 0, 0.10, double.PositiveInfinity, false, true, true, false, 10),
            PreserveSprint = true
        };
        var next = new PathSegment
        {
            Start = new Location(341.5, 81, 340.5),
            End = new Location(342.5, 82, 340.5),
            MoveType = MoveType.Ascend,
            ExitTransition = PathTransitionType.FinalStop
        };

        var template = new AscendTemplate(segment, next);
        var physics = new PlayerPhysics
        {
            Position = new Vec3d(340.25, 80.0, 340.25),
            DeltaMovement = Vec3d.Zero,
            OnGround = true,
            MovementSpeed = 0.1f,
            Yaw = 270f,
            Pitch = 0f
        };
        var input = new MovementInput();
        TemplateState state = TemplateState.InProgress;
        Location finalPos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
        var trace = new List<string>();
        for (int tick = 0; tick < 80; tick++)
        {
            input.Reset();
            Location pos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
            state = template.Tick(pos, physics, input, world);
            if (tick < 20 || state != TemplateState.InProgress)
            {
                trace.Add(
                    $"tick={tick} state={state} pos={pos} yaw={physics.Yaw:F1} vel={physics.DeltaMovement} " +
                    $"onGround={physics.OnGround} input(F={input.Forward},J={input.Jump},S={input.Sprint})");
            }

            if (state != TemplateState.InProgress)
            {
                finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
                break;
            }

            physics.ApplyInput(input);
            physics.Tick(world);
            finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
        }

        Assert.True(
            state == TemplateState.Complete,
            $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
        Assert.True(
            TemplateFootingHelper.IsCenterInsideTargetBlock(finalPos, segment.End),
            $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
    }

    [Fact]
    public void WalkTemplate_DiagonalPrepareJumpIntoAscend_CompletesFromTargetBlockEntry()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor(min: 418, max: 426);
        FlatWorldTestBuilder.ClearBox(world, 418, 79, 418, 426, 84, 424);
        FlatWorldTestBuilder.SetSolid(world, 420, 79, 420);
        FlatWorldTestBuilder.SetSolid(world, 421, 79, 421);
        FlatWorldTestBuilder.SetSolid(world, 422, 79, 422);
        FlatWorldTestBuilder.SetSolid(world, 423, 80, 422);
        FlatWorldTestBuilder.SetSolid(world, 424, 81, 422);

        var current = new PathSegment
        {
            Start = new Location(421.5, 80, 421.5),
            End = new Location(422.5, 80, 422.5),
            MoveType = MoveType.Diagonal,
            ExitTransition = PathTransitionType.PrepareJump,
            ExitHints = new PathTransitionHints(1, 0, 0.0, double.PositiveInfinity, false, true, true, false, 10),
            PreserveSprint = true
        };
        var next = new PathSegment
        {
            Start = new Location(422.5, 80, 422.5),
            End = new Location(423.5, 81, 422.5),
            MoveType = MoveType.Ascend,
            ExitTransition = PathTransitionType.PrepareJump,
            ExitHints = new PathTransitionHints(1, 0, 0.0, double.PositiveInfinity, false, true, true, false, 10),
            PreserveSprint = true
        };

        var template = new WalkTemplate(current, next);
        var physics = new PlayerPhysics
        {
            Position = new Vec3d(422.25, 80.0, 422.25),
            DeltaMovement = Vec3d.Zero,
            OnGround = true,
            MovementSpeed = 0.1f,
            Yaw = 315f,
            Pitch = 0f
        };
        var input = new MovementInput();
        TemplateState state = TemplateState.InProgress;
        Location finalPos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
        var trace = new List<string>();
        for (int tick = 0; tick < 80; tick++)
        {
            input.Reset();
            Location pos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
            state = template.Tick(pos, physics, input, world);
            if (tick < 20 || state != TemplateState.InProgress)
            {
                trace.Add(
                    $"tick={tick} state={state} pos={pos} yaw={physics.Yaw:F1} vel={physics.DeltaMovement} " +
                    $"onGround={physics.OnGround} input(F={input.Forward},B={input.Back},S={input.Sprint})");
            }

            if (state != TemplateState.InProgress)
            {
                finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
                break;
            }

            physics.ApplyInput(input);
            physics.Tick(world);
            finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
        }

        Assert.True(
            state == TemplateState.Complete,
            $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
        Assert.True(
            TemplateFootingHelper.IsCenterInsideTargetBlock(finalPos, current.End),
            $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
    }

    [Fact]
    public void WalkTemplate_TurnIntoParkour_CompletesOnlyWhenTurnEntryIsSlowAndJumpReady()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor(min: 108, max: 128);

        var current = new PathSegment
        {
            Start = new Location(120.5, 80, 110.5),
            End = new Location(121.5, 80, 110.5),
            MoveType = MoveType.Traverse,
            ExitTransition = PathTransitionType.Turn,
            ExitHints = new PathTransitionHints(0, 1, 0.08, 0.16, false, true, true, true, 12)
        };
        var next = new PathSegment
        {
            Start = new Location(121.5, 80, 110.5),
            End = new Location(121.5, 80, 111.5),
            MoveType = MoveType.Traverse,
            ExitTransition = PathTransitionType.PrepareJump,
            ExitHints = new PathTransitionHints(0, 1, 0.12, double.PositiveInfinity, false, true, true, false, 10),
            PreserveSprint = true
        };

        var template = new WalkTemplate(current, next);
        var physics = TemplateSimulationRunner.CreateGroundedPhysics(current.Start, yaw: 270f);

        TemplateState state = TemplateSimulationRunner.Run(template, physics, world, maxTicks: 140, out Location finalPos);
        double horizontalSpeed = Math.Sqrt(physics.DeltaMovement.X * physics.DeltaMovement.X + physics.DeltaMovement.Z * physics.DeltaMovement.Z);

        Assert.Equal(TemplateState.Complete, state);
        Assert.True(
            TemplateFootingHelper.IsCenterInsideSupportStrip(finalPos, current.End, next.End),
            $"finalPos={finalPos} vel={physics.DeltaMovement}");
        Assert.InRange(horizontalSpeed, 0.08, 0.20);
    }

    [Fact]
    public void DescendTemplate_LandingRecovery_CompletesOnLandingBlock()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor();
        FlatWorldTestBuilder.ClearBox(world, 1, 79, 0, 1, 79, 0);
        FlatWorldTestBuilder.SetSolid(world, 1, 78, 0);

        var segment = new PathSegment
        {
            Start = new Location(0.5, 80, 0.5),
            End = new Location(1.5, 79, 0.5),
            MoveType = MoveType.Descend,
            ExitTransition = PathTransitionType.LandingRecovery
        };

        var template = new DescendTemplate(segment, null);
        var physics = TemplateSimulationRunner.CreateGroundedPhysics(segment.Start, yaw: 270f);

        TemplateState state = TemplateSimulationRunner.Run(template, physics, world, maxTicks: 240, out Location finalPos);

        Assert.True(state == TemplateState.Complete, $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}");
        Assert.True(TemplateFootingHelper.IsFootprintInsideTargetBlock(finalPos, segment.End));
    }

    [Fact]
    public void DescendTemplate_LandingRecovery_ChainCarry_CompletesBeforeSettling()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor(min: 360, max: 369);
        FlatWorldTestBuilder.ClearBox(world, 360, 79, 358, 369, 85, 362);
        FlatWorldTestBuilder.FillSolid(world, 362, 79, 360, 363, 79, 360);
        FlatWorldTestBuilder.SetSolid(world, 364, 77, 360);
        FlatWorldTestBuilder.SetSolid(world, 365, 75, 360);
        FlatWorldTestBuilder.SetSolid(world, 366, 73, 360);

        var segment = new PathSegment
        {
            Start = new Location(363.5, 80, 360.5),
            End = new Location(364.5, 78, 360.5),
            MoveType = MoveType.Descend,
            ExitTransition = PathTransitionType.LandingRecovery,
            ExitHints = new PathTransitionHints(1, 0, 0.03, double.PositiveInfinity, false, true, false, true, 12)
        };
        var next = new PathSegment
        {
            Start = new Location(364.5, 78, 360.5),
            End = new Location(365.5, 76, 360.5),
            MoveType = MoveType.Descend,
            ExitTransition = PathTransitionType.LandingRecovery,
            ExitHints = new PathTransitionHints(1, 0, 0.03, double.PositiveInfinity, false, true, false, true, 12)
        };

        var template = new DescendTemplate(segment, next);
        var physics = TemplateSimulationRunner.CreateGroundedPhysics(segment.Start, yaw: 270f);
        var input = new MovementInput();
        var trace = new List<string>();
        TemplateState state = TemplateState.InProgress;
        Location finalPos = segment.Start;

        for (int tick = 0; tick < 160; tick++)
        {
            input.Reset();
            Location pos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
            state = template.Tick(pos, physics, input, world);
            trace.Add(
                $"tick={tick} state={state} pos={pos} vel={physics.DeltaMovement} onGround={physics.OnGround} " +
                $"input(F={input.Forward},B={input.Back},S={input.Sprint})");

            if (state != TemplateState.InProgress)
            {
                finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
                break;
            }

            physics.ApplyInput(input);
            physics.Tick(world);
            finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
        }

        Assert.True(state == TemplateState.Complete, $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
        Assert.True(TemplateFootingHelper.IsFootprintInsideTargetBlock(finalPos, segment.End), $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
        Assert.True(physics.DeltaMovement.X >= 0.03, $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
    }

    [Fact]
    public void DescendTemplate_LandingRecovery_ShortChain_DoesNotSwingYawAfterPassingEndPlane()
    {
        static bool HasReachedSegmentEndPlane(Location pos, PathSegment segment, double tolerance = 0.05)
        {
            double dirX = Math.Sign(segment.End.X - segment.Start.X);
            double dirZ = Math.Sign(segment.End.Z - segment.Start.Z);
            double relX = pos.X - segment.End.X;
            double relZ = pos.Z - segment.End.Z;
            return relX * dirX + relZ * dirZ >= -tolerance;
        }

        static double HeadingPenaltyDegrees(float yaw, PathSegment segment)
        {
            float targetYaw = (float)(-Math.Atan2(segment.HeadingX, segment.HeadingZ) / Math.PI * 180.0);
            if (targetYaw < 0f)
                targetYaw += 360f;

            float delta = targetYaw - yaw;
            while (delta > 180f) delta -= 360f;
            while (delta < -180f) delta += 360f;
            return Math.Abs(delta);
        }

        World world = FlatWorldTestBuilder.CreateStoneFloor(floorY: 0, min: -2, max: 10);
        FlatWorldTestBuilder.ClearBox(world, -2, 1, -2, 10, 90, 2);
        FlatWorldTestBuilder.FillSolid(world, 0, 79, 0, 3, 79, 0);
        FlatWorldTestBuilder.SetSolid(world, 4, 77, 0);
        FlatWorldTestBuilder.SetSolid(world, 5, 75, 0);
        FlatWorldTestBuilder.SetSolid(world, 6, 73, 0);

        var segment = new PathSegment
        {
            Start = new Location(4.5, 78, 0.5),
            End = new Location(5.5, 76, 0.5),
            MoveType = MoveType.Descend,
            ExitTransition = PathTransitionType.LandingRecovery,
            ExitHints = new PathTransitionHints(1, 0, 0.03, double.PositiveInfinity, false, true, false, true, 12)
        };
        var next = new PathSegment
        {
            Start = new Location(5.5, 76, 0.5),
            End = new Location(6.5, 74, 0.5),
            MoveType = MoveType.Descend,
            ExitTransition = PathTransitionType.LandingRecovery,
            ExitHints = new PathTransitionHints(1, 0, 0.03, double.PositiveInfinity, false, true, false, true, 12)
        };

        var template = new DescendTemplate(segment, next);
        var physics = TemplateSimulationRunner.CreateGroundedPhysics(segment.Start, yaw: 270f);
        var input = new MovementInput();
        var trace = new List<string>();
        double maxHeadingPenaltyPastEnd = 0.0;
        TemplateState state = TemplateState.InProgress;
        Location finalPos = segment.Start;

        for (int tick = 0; tick < 120; tick++)
        {
            input.Reset();
            Location pos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
            state = template.Tick(pos, physics, input, world);
            bool pastEnd = HasReachedSegmentEndPlane(pos, segment);
            if (!physics.OnGround && pastEnd)
                maxHeadingPenaltyPastEnd = Math.Max(maxHeadingPenaltyPastEnd, HeadingPenaltyDegrees(physics.Yaw, segment));

            trace.Add(
                $"tick={tick} state={state} pos={pos} yaw={physics.Yaw:F1} vel={physics.DeltaMovement} " +
                $"onGround={physics.OnGround} pastEnd={pastEnd} input(F={input.Forward},B={input.Back},S={input.Sprint})");

            if (state != TemplateState.InProgress)
            {
                finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
                break;
            }

            physics.ApplyInput(input);
            physics.Tick(world);
            finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
        }

        Assert.True(state == TemplateState.Complete, $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
        Assert.True(
            maxHeadingPenaltyPastEnd <= 35.0,
            $"maxHeadingPenaltyPastEnd={maxHeadingPenaltyPastEnd:F1} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
    }

    [Fact]
    public void DescendTemplate_FinalStop_WithWallAndMisalignedYaw_CompletesOnLandingBlock()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor(min: 198, max: 204);
        FlatWorldTestBuilder.ClearBox(world, 198, 79, 198, 204, 84, 202);
        FlatWorldTestBuilder.FillSolid(world, 201, 79, 199, 203, 79, 201);
        FlatWorldTestBuilder.SetSolid(world, 200, 80, 200);
        FlatWorldTestBuilder.SetSolid(world, 200, 80, 199);
        FlatWorldTestBuilder.SetSolid(world, 201, 80, 199);
        FlatWorldTestBuilder.SetSolid(world, 202, 80, 199);
        FlatWorldTestBuilder.SetSolid(world, 201, 81, 199);
        FlatWorldTestBuilder.SetSolid(world, 202, 81, 199);

        var segment = new PathSegment
        {
            Start = new Location(200.5, 81, 200.5),
            End = new Location(201.5, 80, 200.5),
            MoveType = MoveType.Descend,
            ExitTransition = PathTransitionType.FinalStop
        };

        var template = new DescendTemplate(segment, null);
        var physics = TemplateSimulationRunner.CreateGroundedPhysics(segment.Start, yaw: 0f);

        var input = new MovementInput();
        var trace = new List<string>();
        TemplateState state = TemplateState.InProgress;
        Location finalPos = segment.Start;
        for (int tick = 0; tick < 240; tick++)
        {
            input.Reset();
            Location pos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
            state = template.Tick(pos, physics, input, world);
            if (tick < 20 || state != TemplateState.InProgress || !physics.OnGround)
            {
                trace.Add($"tick={tick} state={state} pos={pos} vel={physics.DeltaMovement} onGround={physics.OnGround} input(F={input.Forward},B={input.Back},S={input.Sprint})");
            }

            if (state != TemplateState.InProgress)
            {
                finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
                break;
            }

            physics.ApplyInput(input);
            physics.Tick(world);
            finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
        }

        Assert.True(state == TemplateState.Complete, $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
        Assert.True(TemplateFootingHelper.IsFootprintInsideTargetBlock(finalPos, segment.End));
    }

    [Fact]
    public void DescendTemplate_AppliesAirBrake_WhenPlannerRequiresBrake()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor(min: -2, max: 4);
        FlatWorldTestBuilder.ClearBox(world, -2, 79, -2, 4, 84, 2);
        FlatWorldTestBuilder.SetSolid(world, 1, 79, 0);

        var segment = new PathSegment
        {
            Start = new Location(0.5, 81, 0.5),
            End = new Location(1.5, 80, 0.5),
            MoveType = MoveType.Descend,
            ExitTransition = PathTransitionType.LandingRecovery,
            ExitHints = new PathTransitionHints(1, 0, 0.0, 0.0, true, true, false, true, 12)
        };

        var template = new DescendTemplate(segment, null);
        var physics = new PlayerPhysics
        {
            Position = new Vec3d(1.38, 80.56, 0.5),
            DeltaMovement = new Vec3d(0.42, -0.22, 0.0),
            OnGround = false,
            MovementSpeed = 0.1f,
            Yaw = 270f
        };

        Location pos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
        TransitionBrakingDecision decision = TransitionBrakingPlanner.Plan(segment, null, pos, physics, world);
        var input = new MovementInput();
        template.Tick(pos, physics, input, world);

        Assert.Equal(TransitionBrakingDecision.Brake, decision);
        Assert.True(input.Back, $"decision={decision} input(F={input.Forward},B={input.Back},S={input.Sprint})");
        Assert.False(input.Forward, $"decision={decision} input(F={input.Forward},B={input.Back},S={input.Sprint})");
    }

    /// <summary>
    /// Bug 2.1 regression: an island diagonal Ascend (heading (-X,+Z,+Y)) is
    /// reached after a preceding cardinal Traverse has built up pure +Z ground
    /// momentum.  Without the execution-layer brake the perpendicular momentum
    /// survives takeoff, collides with the +Z shoulder wall of the target
    /// block, and the bot lands outside the target footprint.  The template
    /// must release Forward/Sprint (and engage Back when the perpendicular
    /// dominates) for a few ground ticks so friction can decay the misaligned
    /// component before the jump fires, and the final landing must be inside
    /// the target block.
    /// </summary>
    [Fact]
    public void AscendTemplate_IslandDiagonalFromCardinalMomentum_BrakesPerpBeforeJumpAndLandsInsideTarget()
    {
        // Build a small island layout at y=79 floor:
        //   source block (0,79,0) stands on (0,78,0)
        //   target block (-1,80,1) stands on (-1,79,1); approach is diagonal (-X,+Z)
        //   the +Z shoulder relative to the target (-1,80,2) is solid at head
        //   height so any over-travel along +Z bonks a wall (matches the live
        //   case where perpendicular momentum pushed past the target)
        World world = FlatWorldTestBuilder.CreateStoneFloor(min: -4, max: 4);
        FlatWorldTestBuilder.ClearBox(world, -4, 79, -4, 4, 84, 4);
        FlatWorldTestBuilder.SetSolid(world, 0, 79, 0);
        FlatWorldTestBuilder.SetSolid(world, -1, 80, 1);
        FlatWorldTestBuilder.SetSolid(world, -1, 81, 2);
        FlatWorldTestBuilder.SetSolid(world, -1, 80, 2);

        var ascend = new PathSegment
        {
            Start = new Location(0.5, 80, 0.5),
            End = new Location(-0.5, 81, 1.5),
            MoveType = MoveType.Ascend,
            ExitTransition = PathTransitionType.FinalStop,
            PreserveSprint = true
        };

        var template = new AscendTemplate(ascend, null);

        // Seed pure +Z cardinal momentum at the source block center: this is
        // the perpendicular axis relative to the diagonal (-X,+Z) / sqrt(2)
        // heading; without the brake gate the bot would take off carrying it.
        var physics = new PlayerPhysics
        {
            Position = new Vec3d(0.5, 80, 0.5),
            DeltaMovement = new Vec3d(0.0, 0.0, 0.22),
            OnGround = true,
            Sprinting = true,
            MovementSpeed = 0.1f,
            Yaw = 0f,
            Pitch = 0f
        };

        var input = new MovementInput();
        TemplateState state = TemplateState.InProgress;
        Location finalPos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
        bool sawBrakeTick = false;
        var trace = new List<string>();
        for (int tick = 0; tick < 120; tick++)
        {
            input.Reset();
            Location pos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
            state = template.Tick(pos, physics, input, world);
            if (physics.OnGround && !input.Forward && !input.Jump)
                sawBrakeTick = true;
            if (tick < 24 || state != TemplateState.InProgress)
            {
                trace.Add(
                    $"tick={tick} state={state} pos={pos} yaw={physics.Yaw:F1} vel={physics.DeltaMovement} " +
                    $"onGround={physics.OnGround} input(F={input.Forward},B={input.Back},J={input.Jump},S={input.Sprint})");
            }
            if (state != TemplateState.InProgress)
            {
                finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
                break;
            }

            physics.ApplyInput(input);
            physics.Tick(world);
            finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
        }

        Assert.True(sawBrakeTick, "expected at least one ground tick where Forward was released to decay perpendicular momentum");
        Assert.True(state == TemplateState.Complete, $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
        Assert.True(
            TemplateFootingHelper.IsFootprintInsideTargetBlock(finalPos, ascend.End),
            $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
    }

    /// <summary>
    /// Live regression: B->A route segment 9/41 Ascend (243.5,97,181.5)->
    /// (244.5,98,182.5) exit=Turn, fed by a Descend->PrepareJump handoff.
    /// The Ascend starts mid-air with cardinal +Z momentum and yaw aimed
    /// at the diagonal target. Without a post-landing completion shortcut
    /// the bot lands hot (footprint partially outside target) and the
    /// AscendTemplate top-level yaw smoothing toward targetYaw fights the
    /// GroundedSegmentController exit-heading rotation, oscillating yaw
    /// each tick until the bot drifts off the 1-block landing's edge and
    /// the segment fails. With the shortcut the segment completes the
    /// instant the bot lands with center inside the target column, which
    /// hands off cleanly to the next template (Traverse) that snaps yaw
    /// on its first tick.
    /// </summary>
    [Fact]
    public void AscendTemplate_TurnExit_FromCarriedAirMomentum_CompletesOnTargetColumnLanding()
    {
        World world = FlatWorldTestBuilder.CreateStoneFloor(min: -4, max: 6);
        FlatWorldTestBuilder.ClearBox(world, -4, 80, -4, 6, 86, 6);
        // Source block (0,79,0) is the Descend's landing column; the bot
        // arrives mid-jump above it.
        FlatWorldTestBuilder.SetSolid(world, 0, 79, 0);
        // Target block (1,80,1) (NE diagonal +1y) is the Ascend's landing.
        FlatWorldTestBuilder.SetSolid(world, 1, 80, 1);
        // Next-segment landing column (Traverse east from target).
        FlatWorldTestBuilder.SetSolid(world, 2, 80, 1);
        // Walkable shoulder block beneath the bot's overshooting +Z
        // footprint so the bot doesn't drop into a 2-block hole on
        // landing (matches the live world geometry where a wide platform
        // existed at the target's elevation).
        FlatWorldTestBuilder.SetSolid(world, 1, 80, 2);

        var ascend = new PathSegment
        {
            Start = new Location(0.5, 80, 0.5),
            End = new Location(1.5, 81, 1.5),
            MoveType = MoveType.Ascend,
            ExitTransition = PathTransitionType.Turn,
            ExitHints = new PathTransitionHints(
                DesiredHeadingX: 1,
                DesiredHeadingZ: 0,
                MinExitSpeed: 0.0,
                MaxExitSpeed: 0.05,
                RequireStableFooting: true,
                RequireGrounded: true,
                RequireJumpReady: false,
                AllowAirBrake: true,
                HorizonTicks: 12),
            PreserveSprint = true
        };
        var next = new PathSegment
        {
            Start = new Location(1.5, 81, 1.5),
            End = new Location(2.5, 81, 1.5),
            MoveType = MoveType.Traverse,
            ExitTransition = PathTransitionType.FinalStop
        };

        var template = new AscendTemplate(ascend, next);

        // Seed mid-arc state mirroring the live PathDiag tick-trace at the
        // first observed tick of seg9/41: bot already airborne with rising
        // vy, cardinal +Z momentum from the preceding Descend, and yaw
        // pointing roughly NE (the AscendTemplate snapped yaw on the
        // takeoff tick).
        var physics = new PlayerPhysics
        {
            Position = new Vec3d(0.7, 80.42, 0.92),
            DeltaMovement = new Vec3d(0.0, 0.333, 0.145),
            OnGround = false,
            Sprinting = true,
            MovementSpeed = 0.1f,
            Yaw = 311f,
            Pitch = 0f
        };

        var input = new MovementInput();
        TemplateState state = TemplateState.InProgress;
        Location finalPos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
        var trace = new List<string>();
        int elapsedTicks = 0;
        for (; elapsedTicks < 80; elapsedTicks++)
        {
            input.Reset();
            Location pos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
            state = template.Tick(pos, physics, input, world);
            if (elapsedTicks < 30 || state != TemplateState.InProgress)
            {
                trace.Add(
                    $"tick={elapsedTicks} state={state} pos={pos} yaw={physics.Yaw:F1} vel={physics.DeltaMovement} " +
                    $"onGround={physics.OnGround} input(F={input.Forward},B={input.Back},J={input.Jump},S={input.Sprint})");
            }
            if (state != TemplateState.InProgress)
            {
                finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
                break;
            }

            physics.ApplyInput(input);
            physics.Tick(world);
            finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
        }

        Assert.True(
            state == TemplateState.Complete,
            $"state={state} elapsed={elapsedTicks} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
        Assert.True(
            physics.OnGround,
            $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}");
        Assert.True(
            TemplateFootingHelper.IsCenterInsideTargetBlock(finalPos, ascend.End),
            $"finalPos={finalPos} target={ascend.End}\n{string.Join('\n', trace)}");
        Assert.True(
            elapsedTicks <= 30,
            $"completion took {elapsedTicks} ticks; expected post-landing shortcut to fire promptly\n{string.Join('\n', trace)}");
    }
}