Almost got the algorithm fully functional