Simplify and comment implementation of floyd warshall. Add tests for floyd warshall algorithm