Fixed algorithm; initially wrote the algorithm in mathematical form wrong.