Added pathfinding and fixed grid memory access