Changed maps to vectors and queue now holds pointers