Bugfix/wrong queue indexing

See #11 (closed).

@kblantos, keep track of this MR, once Tristan merges it into develop you should pick it up and use it in your testbenches.