implementation of wavefront algorithm

[DrJones]

I am trying to implement the wavefront algorithm, but my solution keep getting more complex with if statements and so..


Anyone who could have somekind of pseudocode which describes it's mechanism??