Agree, diagramming helps. Sketch out the high level function, as tasks/events in a loop, or a flowchart, etc. This can be implemented as a massive if-else in the main loop, or jumps between functions (make sure they don't loop forever of course...), or a more formal state machine structure (which in turn can be a big switch statement, or say, precalculated elsewhere using a state transition table).
Maybe you need to do something special-case, interrupting overall flow of the loop, and it's just not feasible to fold it into the loop normally -- that's fine, you can jump into another lengthy function and do your work there. Don't do this lightly: you don't have access to all the clauses and functionality of the main loop, outside of it; more than a few special cases like this, will quickly lead to spaghetti. See #1, diagram it out, try to integrate it better, maybe there's a pattern you're missing that would allow both operating modes to coexist, etc.
May also help to implement simple yet meaningful functions, and run them from a sort of command shell. I've been doing this the last little while, and find it useful. At least, because I don't have a debugger for my favored device family... Example, figuring out a peripheral: well, start with the basics, write a command to PEEK/POKE its control registers. This can be as basic as the MCU's own registers; which if they're memory mapped, simultaneously gives you insight into internal RAM, handy for inspecting the live program -- albeit from the shell's main loop, not at just any point along any particular function. Maybe it's an SPI device, so you boot up the MCU's SPI peripheral, and implement a short data/command R/W function. Maybe the next level up is an init command, or something that runs a sequence of values or commands or whatever. Then commands to use specific internal functions, like uh, for an LCD display you'd certainly want some GRAM address and data commands so you can start drawing pixels. Then maybe basic shapes, images, etc.
So you can start at a very low level, implementing bits and pieces, which don't go anywhere else in your program -- but you can still test them in this way (and yes, preferably with more detailed/formalized tests, as in TDD), meanwhile you can diagram the very highest of levels, and mark out what the overall program state goes through, what the highest level functions will be, etc.
And then finally, you can add in optimizations or customizations or whatever. Maybe it's rather slow, so you need to reconsider some algorithms (accidentally quadratic?), or implement them in ASM. Maybe you've been using it a while, and yeah it's usable, but it's still a bit of a pain and wouldn't it be great if it did X automatically?
Reminds me, I still need to add a filter calculator to my reverb effects box... It's usable, yes, but the filter coefficients are literally just right there on the menu, and good luck tweaking them just by eye! That should be interesting by itself actually, I might do it in floating point (needs several trig functions; library is probably bloated and slow), or I might look into shaving a fixed-point-math-shaped yak...
Oh, anyway, the source to that project is here if you're curious,
https://github.com/T3sl4co1l/Reverbconsole.c and commands.c may be of interest (the command shell, and the commands which plug into it), or main.c and menu.c for the UI loop.
Tim