Finished intial revision of algorithm