Feature/code optimization

[jdlph]

The major changes include,

1. enhance the existing FIFO implementation by adding if to_node in SEList check using status array;
2. provide Deque and heap-Dijkstra implementations;
3. enhance the existing deque implementation in cpp by changing return type from double to void, removing unused variables, and moving declarations of variables to where they are defined;
4. update the mechanism of assigning agent_seq_no.