[#81] Fix test matrix
[#81] Add working Dijkstra's algorithm implementation, need to optimize