ты с начало удаляешь, а потом вставляешь. Сделай наоборот. это для очереди с индексом <5
а если индекс > 5 то оставляй как есть, т.е удаляй, а потом вставляй.!