Added heatgraph and pathfinder. Fixed some issues with routing