Fixed algorithm, makes sense now, only requires minor edits to be put
into function.