Working on implementing pathfinding library

Not merged fully yet.