Fixed multiple mistakes in the algorithm, still some need to be tackled