Fixed issues
added path cost
checking path cost with respect to M
test with all obstacle or all empty worked fine

issue to fix: loops in path for empty with depth 6