Rearranged the code to match the algorithm