Add test class for Queue

- Fix minor indexes bugs