Another tip that I find help.
I tend to program top-down: meaning that I put high level functional blocks in my code and then implement them individually.
Like in your case, I would do this:
//define pins here
...
//define major macros here
...
//define major blocks here
//keep them empty for now
void stage1(void) {
}
void stage2(void) {
}
void stage3(void) {
}
...
//code starts here
void setup(void) {
//initialize my module
}
void loop(void) {
stage1();
stage2();
if (executing_stage3()) stage3(); //conditional execute stage 3
}
Once I have the framework setup, I would implement individual blocks, again, top down.
I find that easier for me.