Al contrario, lavorare con funzioni (purché ogni funzione "lavori" su una sola cosa/aspetto, non mischiare mele e pere, vedi il mio esempio...) serve a strutturare meglio il codice perché sarà così più "modulare" ed identificare "dove" cercare quando qualcosa non funziona come atteso è più facile.
Allora per prima cosa "disegnati" il tuo diagramma di stato come ti ho suggerito, ed inizia a codificare solo DOPO che la MSF ti soddisfa e contiene tutte le condizioni, azioni, e funzionalità che desideri. E per farlo, allora, torna al codice con la gestione degli stati che ti avevo abbozzato.
Ovviamente quando avrai disegnato lo schema degli stati della tua MSF imposta il codice per ora solo con lo switch() ed i commenti (mi raccomando!) per ora senza le funzioni, poi posta schema e codice qui e ti dò qualche "dritta" più specifica su come implementarlo.
Come preferisci, puoi inserirlo qui come "code" se non è troppo lungo (all'inizio non lo sarà), oppure lo alleghi come file *.ino se dovesse diventare più lungo, oppure carichi su GitHub o Google Drive e metti qui solo il link.