First draft on pathfinding
Added pathfinding using point-graph