Added initial implementation for shortest path