Simplify overengineered code

The original code is correct but very hard to read. The new code is fully equivalent but easier to read and debug.

This was found using Cppcheck.