Improved path debug: yellow - recalculation, Green: Movement needed, Red - Cannot solve