Algorithm updating. Still some minor bugs