HW1 Algorithm

Implemented the HW1 part 1 algorithm