Make command scheduling order consistent

[Gold856]

Fixes #5467.
end is now called first along with interrupt actions on both languages. Sets are used to defer cancels, ensuring infinite loops aren't possible.

[Starlight220]

Also, the most important thing this PR should be adding is tests. Any possible edge case should be added and tested on both new and old impls; if there are differences they should be exactly what we want.

[Starlight220]

I just realized that SchedulingRecursionTest.cancelFromInitialize doesn't check if end is actually called: it's an edge case that this PR might change, and if behavior has changed then we should be conscious of that.

[KangarooKoala]

Glad to see more consistency! However, I have a few nitpicks (in addition to updating the PR description).

[Gold856]

Getting an error for the CancelDefaultCommandFromEnd test on C++:
Assertion failed: isHandleInSync() && "invalid iterator access!", file C:\Users\gold2\OneDrive\Documents\GitHub\allwpilib\wpiutil\src\main\native\thirdparty\llvm\include\wpi/SmallPtrSet.h, line 285

What's causing it?

[Starlight220]

Probably concurrent modification.

[Gold856]

The CancelDefaultCommandFromEnd test on C++ actually found a problem: when cancelDefaultCommand schedules the other command, it will cancel the default command. The problem is that the cancel is deferred, but the requirements are moved anyways to the other command (which explains the failed assertion), and the other command is scheduled. After that, cancelDefaultCommand finishes its cancel, and the deferred cancel is also run.

Requirements are being transferred before the command actually ends; this seems to cause a problem for C++, and also might be problematic in general. However, the command is guaranteed to be canceled, so this might be fine to leave in. The test also passes in main, so there's behavior that needs to be preserved. How do we want to deal with this?

[KangarooKoala]

Things have gotten kind of thorny, so I tried completely rethinking the whole thing, and here's the proposal I have: Instead of dealing with the queue headaches, use a set to keep track of which commands are currently ending to detect and prevent infinite recursion.

In more detail:

• Add a set field, which will store the currently ending commands
• Before every call to command->End, add the command to the set of currently ending commands
• After we finish ending the command, remove it from the set of currently ending commands
• Inside Cancel(), check if the command is in the set of currently ending commands- If it is, return
• Revert the changes to inRunLoop, toSchedule, and toCancel so that they're only to prevent CME/iterator invalidation inside the run loop

Notes:

• If we don't add the command to the set of currently ending commands when IsFinished() returns true and a command cancels itself in End(), then we'll trigger the interrupt actions and then the finish actions on it. If that's desired behavior, we shouldn't add the command to the set in that case.

[Gold856]

Using a set works, but I'm still getting that invalid iterator access error on C++, and I don't know how to fix it.

[KangarooKoala]

After tracking the problem to scheduler.Cancel(&cancelDefaultCommand) in the test (by commenting out lines to see what caused the problem) and then to m_impl->scheduledCommands(*find) in CommandScheduler::Cancel (by looking for iterator code in Cancel), it seems that this is the problem: We moved the call to End() between when we get the iterator and when we deference it, so if End() does something that invalidates the pointer- such as schedule a new command- we'll get an error.
The fix is to pass command instead of *find, which also has the benefit of letting us use scheduledCommands.contains() instead of scheduledCommands.find() == scheduledCommands.end().

[Gold856]

Thank you!

[Gold856]

/format

[Starlight220]

Besides that nitpick and the cancel() deduplication, should be good to go.

[KangarooKoala]

Looks good to me, except for this one change.

[rzblue]

This will need conflicts resolved from #5461 - let us know if you need help with that.

[Gold856]

Squashed everything down and rebased; should be good to go!

[rzblue]

Looks good to me. We need to keep an eye on this one during beta to ensure no unexpected breakages occur.

[Starlight220]

Hopefully this won't break anything.