Updates to main algorithm. Still might have bugs