Finalized the algorithm, still need to add in the drawing part: