Buggy code- But works partially...
before removing collision paths
Converging but with issues
Checking for Goal getting added... Have to check goal checking function as well
Working with Multiple Goal
Fixed indents for whole file