Finish the algorithm
need organized the code and set up a test to debug!