Adding implementation of path search
Committing project since will not have time to finish