Remove gotos

Code tidy up and remove gotos