Cleaned the code and reimplementing with a Queue