Add null check for finalPathPoint