Fixed error in main algorithm!