Finished code, seems to work