Removed deubg statements, implemented sorting but it wont work unless I separate out goal floors and pickup floors