[Nathan] Added pathfinding capabilities and updated Main function