Almost finished fixing the algo. Small modifications needed.