Fixed queue bug and wrote tests