added pathfinding code
Signed-off-by: timothy <timothy@timothy-linux.(none)>