Tidied up the algorithm