using System;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.AI;

namespace ServiceSim
{
    /// <summary>
    /// 配膳ロボット1台。受け渡し口 → 卓 → 受け渡し口 の1往復を走り、
    /// 止まる・動き出す・経路を引き直す・動けなくなるを自分で決める。
    ///
    /// NavMeshAgent は経路を探すのと、人に自分をよけてもらうためだけに付ける。
    /// 自動でよける機能（obstacleAvoidanceType）は切り、位置はこの script が動かす。
    ///
    /// この script はロボットだけを扱う。人も店も動かさない。
    /// 細い通路の区画と入口は、店の側で決めた値を StartTrip で受け取るだけ。
    /// </summary>
    [RequireComponent(typeof(NavMeshAgent))]
    public class ServingRobotController : MonoBehaviour
    {
        public enum Phase
        {
            Idle,        // 往復していない
            ToTable,     // 卓へ向かう（細い通路の入口で待つこともある）
            Dwell,       // 卓で25秒止まる（「止まった」に数えない）
            ToPass,      // 受け渡し口へ戻る
            StuckWait,   // 動けなくなって、スタッフが来て動かしてくれるのを待つ（v5 3章）
            Done         // 往復が終わった
        }

        [Header("設定")]
        public RobotSpec spec;
        public FrontSensor sensor;

        [Tooltip("1台めは1、2台めは2")]
        public int robotId = 1;

        [Tooltip("立ち往生したときにスタッフを呼ぶ先（ServiceOps が入れる。v5 3章）")]
        public ServiceOps ops;

        // ---- 戻りの線（v5。受け渡し口の前の点を通らずに待機場所へ抜ける）----
        Vector3 _standbyPoint;
        bool _hasStandby;
        readonly List<Vector3> _drinkQueues = new List<Vector3>();
        int _returnLineFallbacks;
        bool _returnLineUsed;

        /// <summary>この台の待機場所（ServiceOps が入れる）</summary>
        public void SetStandbyPoint(Vector3 p)
        {
            _standbyPoint = new Vector3(p.x, 0f, p.z);
            _hasStandby = true;
        }

        /// <summary>戻りの線が使えず、今までどおり受け渡し口の前へ戻った回数（報告用）</summary>
        public int ReturnLineFallbacks { get { return _returnLineFallbacks; } }

        /// <summary>直前の戻りで、戻りの線を使えたか（報告用）</summary>
        public bool ReturnLineUsed { get { return _returnLineUsed; } }

        [Header("いまの様子（読むだけ）")]
        [SerializeField] Phase _phase = Phase.Idle;
        [SerializeField] bool _enteredAisle;
        [SerializeField] bool _yieldHold;

        // ---- 1往復の測りもの ----
        public int TableId { get; private set; }

        /// <summary>
        /// この走りを始めた、店の中の時刻（秒）。始めた側が渡す。渡されなければ -1。
        /// 「いつ始まったか」をロボット自身が持つ。外から覚えておくと、
        /// 走り終わってすぐ次の仕事が入ったときに取りこぼす。
        /// </summary>
        public float RunStartS { get; private set; }

        public float DistanceM { get; private set; }
        public float TotalS { get; private set; }
        public float StoppedS { get; private set; }
        public int StopCount { get; private set; }
        public float MaxDecelMps2 { get; private set; }
        public float MinClearanceM { get; private set; }
        public bool Stuck { get; private set; }
        public bool MetRobot { get; private set; }
        public bool Aborted { get; private set; }

        /// <summary>
        /// この往復で動けなくなってスタッフを呼んだ回数（v5 3章）。
        /// 動かしてもらったあと、また動けなくなれば2回めを呼ぶので、Stuck（印）とは別に数える。
        /// </summary>
        public int StuckCount { get; private set; }

        /// <summary>スタッフが来るのを待った秒数の合計（StoppedS にも入っている）</summary>
        public float StuckWaitS { get; private set; }

        /// <summary>1往復が打ち切り（maxTripSec 超え）で終わったか</summary>
        public bool TimedOut { get; private set; }

        /// <summary>細い通路の入口で、ほかの台を待った秒数（秒）。stop_durations_s には入れない</summary>
        public float WaitAisleS { get; private set; }

        // ---- 依頼 v9：地図の上で自分で道を引く ----

        /// <summary>この往復で道を引き直した回数（回り込み・よけを含む。trips.csv の replans）</summary>
        public int Replans { get; private set; }

        /// <summary>3 章の回り込みで道を変えた回数（trips.csv の detours）</summary>
        public int Detours { get; private set; }

        /// <summary>
        /// 4 章でよけた回数（trips.csv の robot_yields）。
        /// よける場所へ動いた回数 ＋ 相手の道をよけた道に引き直した回数
        /// </summary>
        public int RobotYields { get; private set; }

        /// <summary>この往復で、1 回の道探しにかかった最大の時間（ミリ秒。trips.csv の plan_ms_max）</summary>
        public float PlanMsMax { get; private set; }

        /// <summary>
        /// この往復で、サイトの道順をたどったときの長さ（routes の length の和, m。
        /// trips.csv の site_route_m）。走りには使わない
        /// </summary>
        public float SiteRouteM { get; private set; }

        /// <summary>
        /// 往復が実際に終わった店の中の時刻（秒。卓での 25 秒を含む。trips.csv の trip_end_s）。
        /// 終わっていなければ -1
        /// </summary>
        public float TripEndS { get; private set; }

        /// <summary>
        /// この往復で、出発点が地図では通れないマス（0）だったために近くへ寄せた回数。
        /// 通路の入口で待つ点・横へどいた点は free_class では 0 になるので、ここに出る
        /// </summary>
        public int MapSnapStarts { get; private set; }

        /// <summary>この往復で、道が引けなかった回数</summary>
        public int PlanFails { get; private set; }

        /// <summary>いま走っているのが MapPlanner か（地図が無ければ SiteRoutes に落とす）</summary>
        public bool UseMapPlanner
        {
            get { return spec != null && spec.UseMapPlanner && _planner != null; }
        }

        /// <summary>run_info.json に出す、実際に使った道の決め方</summary>
        public PathMode ActivePathMode
        {
            get { return UseMapPlanner ? PathMode.MapPlanner : PathMode.SiteRoutes; }
        }

        /// <summary>
        /// 譲る順番（依頼 v9 の 4 章・エイイチの決定）。**小さいほど先に通る**。
        ///   0 … 料理を載せているロボット（卓へ向かっている。卓から次の卓へ向かう間も含む）
        ///   1 … 空で戻るロボット（受け渡し口・待機場所へ向かっている、待機場所へ動いている）
        /// 同じ状態なら 1 号機が先（robotId の小さい方）。
        /// </summary>
        public int YieldRank
        {
            get
            {
                Phase p = _phase == Phase.StuckWait ? _phaseBeforeStuck : _phase;
                return p == Phase.ToTable || p == Phase.Dwell ? 0 : 1;
            }
        }

        /// <summary>自分が相手より後に通る側か（＝自分がよける）</summary>
        public bool YieldsTo(ServingRobotController other)
        {
            if (other == null) return false;
            if (YieldRank != other.YieldRank) return YieldRank > other.YieldRank;
            return robotId > other.robotId;
        }

        /// <summary>いま回り込んだ道を走っているか（モニターが読む）</summary>
        public bool Detouring { get { return _onDetour; } }

        /// <summary>いま相手をよけて待っているか（モニターが読む）</summary>
        public bool YieldingToRobot { get { return _mapYield; } }

        /// <summary>直前の1コマで進んだ距離（m）。コマの粗さを確かめるのに使う</summary>
        public float LastStepM { get; private set; }

        /// <summary>止まった原因</summary>
        public enum StopCause
        {
            Guest,           // 歩く客とすれ違った
            Staff,           // スタッフとすれ違った
            PersonStanding,  // 人が立ち止まっていた
            Chair,           // 椅子
            OtherRobot,      // もう1台のロボット（Unity だけ）
            NoPath,          // 行き先への道が引けない（通路の幅が足りない）
            AisleQueue,      // 細い通路の順番待ち（Unity だけ）
            Unknown          // 分からない
        }

        /// <summary>どう動き出したか</summary>
        public enum StopResolve
        {
            SelfResume,      // 自分で動き出した
            Replanned,       // 経路を引き直した
            Stuck,           // 動けなくなった
            AisleWait,       // 入口で待った
            Detoured,        // 回り込んで動き出した（依頼 v9 の 3 章）
            YieldedToRobot   // よける場所へ動いた（依頼 v9 の 4 章）
        }

        /// <summary>止まり1回ぶんの記録</summary>
        public struct StopEvent
        {
            public float startTimeS;        // 店の中の時刻（秒）
            public Vector3 position;        // 止まった位置
            public float durationS;         // 止まっていた秒数
            public StopCause cause;
            public string causeDetail;      // 何に当たったか
            public StopResolve resolve;
            public float clearanceLeftM;    // 左の空き（ロボットの外周から）
            public float clearanceRightM;   // 右の空き
            public bool pathBlocked;        // 行き先への道が引けていなかったか

            // ---- 止まっている間、何で動けなかったかの内訳（秒）。B7 を調べるため ----
            /// <summary>前のセンサーが「ふさがっている」と言っていた秒数</summary>
            public float blockedS;
            /// <summary>行き先への道が引けていなかった秒数</summary>
            public float noPathS;
            /// <summary>細い通路の入口で順番を待っていた秒数</summary>
            public float yieldS;
            /// <summary>ふさがりが解けたあと、動き出すまで待っていた秒数</summary>
            public float clearWaitS;

            // ---- 道を引いたときの中身（調べるためだけ。動きは変えない）----
            /// <summary>道を引いた出発点（焼いた面へ寄せたあと）</summary>
            public Vector3 solveFrom;
            /// <summary>道を引いた行き先（同じく寄せたあと）</summary>
            public Vector3 solveTo;
            /// <summary>CalculatePath の結果（PathComplete / PathPartial / PathInvalid / なし）</summary>
            public string solveStatus;
            /// <summary>引けた角の数</summary>
            public int solveCorners;
            /// <summary>
            /// ロボットのいる位置から、いちばん近い焼いた面までの距離（m）。
            /// 面の上に立っていれば 0 に近い。探す半径で見つからなければ -1
            /// </summary>
            public float offMeshM;
            /// <summary>止まっている間に経路を引き直した回数</summary>
            public int replans;
        }

        readonly List<StopEvent> _stopEvents = new List<StopEvent>();

        /// <summary>この往復で止まった1回ずつの記録</summary>
        public IReadOnlyList<StopEvent> StopEvents { get { return _stopEvents; } }

        readonly List<float> _stopDurations = new List<float>();

        /// <summary>自分で動き出した止まりの長さ（秒）。動けなくなった分と、入口で待った分は入れない</summary>
        public IReadOnlyList<float> StopDurationsS { get { return _stopDurations; } }

        public Phase CurrentPhase { get { return _phase; } }
        public bool TripFinished { get { return _phase == Phase.Done; } }
        public bool HasEnteredAisle { get { return _enteredAisle; } }

        /// <summary>いまの速さ（m/秒。字幕が読むだけ）</summary>
        public float SpeedMps { get { return _speed; } }

        /// <summary>
        /// いま続けて動けていない秒数（人のよけ方と、キッチンのモニターが読む）。
        /// 動けなくなって人を待っているあいだは、60 秒＋待った分として続けて数える
        /// </summary>
        public float StoppedNowS
        {
            get
            {
                if (_phase != Phase.StuckWait) return _stoppedTimer;
                float base_ = spec != null ? spec.stuckAfterStoppedSec : 60f;
                return base_ + _stuckWaitTimer;
            }
        }

        /// <summary>いま止まっている理由の名前（キッチンのモニターが読む。記録と同じ言葉）</summary>
        public string StopCauseNow
        {
            get
            {
                if (_phase == Phase.StuckWait) return "立ち往生";
                if (_yieldHold) return "通路の順番待ち";
                if (_robotYield || _mapYield) return "もう1台のロボット";
                if (!_recordingStop) return "";
                return TripLoggerCauseName(_open.cause);
            }
        }

        static string TripLoggerCauseName(StopCause c)
        {
            switch (c)
            {
                case StopCause.Guest: return "お客さんとすれ違った";
                case StopCause.Staff: return "スタッフとすれ違った";
                case StopCause.PersonStanding: return "人が立ち止まっていた";
                case StopCause.Chair: return "椅子・荷物・ベビーカー";
                case StopCause.NoPath: return "通路の幅が足りない";
                case StopCause.OtherRobot: return "もう1台のロボット";
                case StopCause.AisleQueue: return "通路の順番待ち";
                default: return "分からない";
            }
        }

        /// <summary>細い通路の入口で待っている最中か（字幕が読むだけ）</summary>
        public bool YieldHolding { get { return _yieldHold; } }

        /// <summary>卓での受け渡しの残り秒数（字幕が読むだけ）</summary>
        public float DwellRemainS
        {
            get { return spec != null ? Mathf.Max(0f, spec.tableDwellSec - _dwellTimer) : 0f; }
        }

        /// <summary>
        /// スタッフを待っている秒数（字幕が読むだけ。v5 3章）。
        /// 受け渡し口へ移すのをやめたので「残り」ではなく、呼んでから待っている長さを出す
        /// </summary>
        public float StuckWaitRemainS
        {
            get { return _stuckWaitTimer; }
        }
        public Bounds AisleZone { get { return _aisleZone; } }

        /// <summary>
        /// いまたどっている区間の線（1つ前の点 → いま目指す点）。
        /// 譲る側が「この道から横へどく」ために読む（2026-10-23）。
        ///
        /// サイトの routes の点をたどっているときは、その2点をそのまま返す。
        /// 地図で自分の道を引いているとき（MapPlanner・依頼 v9）は、引いた道の
        /// いまの区間を返す（人のよけ方は変えないので、ここは新しい道を返すようにして残す）。
        /// 点が無い・長さが 0 のときは、いまの位置と向きから2 m 先までの線にする。
        /// </summary>
        public bool CurrentLegSegment(out Vector3 a, out Vector3 b)
        {
            int n = WayCount, i = WayIndex;
            if (n > 0 && i >= 0 && i < n)
            {
                b = WayPoint(i);
                a = i > 0 ? WayPoint(i - 1) : transform.position;
                a.y = 0f;
                if ((b - a).sqrMagnitude > 0.0001f) return true;
            }

            a = transform.position; a.y = 0f;
            Vector3 f = transform.forward; f.y = 0f;
            if (f.sqrMagnitude < 0.0001f) { b = a; return false; }
            b = a + f.normalized * 2f;
            return true;
        }

        /// <summary>
        /// これから進む道（v6b）。**いまの位置から先**を aheadM 分だけ、
        /// 残りの点をつないで返す（into の先頭はいまの位置）。
        ///
        /// SiteRoutes ではサイトの routes の残りの点、
        /// MapPlanner（依頼 v9）では地図で引いた道の残りの曲がり角。
        /// 譲る側（人・ほかのロボット）が「自分がこの道の上にいるか」を見るために読む。
        /// 点が無いときは、いまの向きへ aheadM の線1本にする。
        /// </summary>
        public void PathAhead(float aheadM, List<Vector3> into)
        {
            if (into == null) return;
            into.Clear();
            if (aheadM <= 0f) return;

            Vector3 here = transform.position; here.y = 0f;
            into.Add(here);

            float left = aheadM;
            Vector3 from = here;

            int n = WayCount;
            for (int i = WayIndex; i >= 0 && i < n && left > 0f; i++)
            {
                Vector3 to = WayPoint(i);
                Vector3 d = to - from;
                float len = d.magnitude;
                if (len < 0.0001f) continue;

                if (len >= left)
                {
                    into.Add(from + d / len * left);
                    return;
                }
                into.Add(to);
                left -= len;
                from = to;
            }

            if (left > 0f && into.Count == 1)
            {
                // 道順の点が無い。いまの向きへ伸ばす
                Vector3 f = transform.forward; f.y = 0f;
                if (f.sqrMagnitude > 0.0001f) into.Add(here + f.normalized * aheadM);
            }
        }


        /// <summary>1走りで寄る卓1つぶん</summary>
        public struct RunStop
        {
            public int tableId;
            public Vector3 point;          // ロボットが止まる位置（通路の上）
            public Vector3 aisleEntrance;  // その通路に入る前に待つ位置
            public Bounds aisleZone;       // 譲り合いの区画
        }

        readonly List<RunStop> _stops = new List<RunStop>();
        readonly List<int> _tableIds = new List<int>();
        int _stopIndex;

        /// <summary>この走りで寄る卓の番号</summary>
        public IReadOnlyList<int> TableIds { get { return _tableIds; } }

        /// <summary>この走りで何卓に届けたか</summary>
        public int StopsCompleted { get; private set; }

        /// <summary>この走りで寄る卓の数</summary>
        public int RunStopCount { get { return _stops.Count; } }

        readonly List<float> _stopArrivedS = new List<float>();

        /// <summary>
        /// 止まる所ごとに着いた時刻（店の中, 秒）。着いていない所は -1（依頼 v8 の 2）。
        /// trips.csv の stop_arrived_s に出す
        /// </summary>
        public IReadOnlyList<float> StopArrivedS { get { return _stopArrivedS; } }

        /// <summary>
        /// 止まる所に着いた（引数：ロボット・止まる所の番号）。依頼 v8 の 1。
        /// ServiceOps が受けて、その所の皿を「届いた」にする。
        /// 受け渡しの 25 秒（tableDwellSec）の **前** に出る
        /// </summary>
        public event Action<ServingRobotController, int> StopArrived;

        /// <summary>1往復が終わったときに呼ばれる</summary>
        public event Action<ServingRobotController> TripCompleted;

        // ---- いまシーンにいるロボット（譲り合いの判定に使う）----
        static readonly List<ServingRobotController> _all = new List<ServingRobotController>();
        public static IReadOnlyList<ServingRobotController> All { get { return _all; } }

        // ---- 内部 ----
        NavMeshAgent _agent;
        NavMeshPath _path;      // Awake で作る。フィールドの初期化で new すると
                                // 「InitializeNavMeshPath is not allowed to be called
                                //  from a MonoBehaviour constructor」になるため
        Vector3[] _corners = new Vector3[0];
        int _cornerIndex;
        Vector3 _destination;
        Vector3 _destinationOnMesh;
        bool _hasDestinationOnMesh;
        Vector3 _tablePoint;
        Vector3 _passPoint;
        Vector3 _aisleEntrance;
        Bounds _aisleZone;
        bool _hasAisleZone;

        float _speed;
        float _dwellTimer;
        float _clearTimer;
        float _stoppedTimer;        // いま続けて止まっている秒数
        float _lastReplanAtStopped;
        float _stuckWaitTimer;
        Phase _phaseBeforeStuck = Phase.ToTable;   // 動けなくなる直前に向かっていた先（v5 3章）

        // ---- 入口での順番待ち（v5）----
        float _yieldHoldS;          // 入口で待っている秒数
        bool _yieldGaveUp;          // 待ちすぎたので、この区間はもう譲らない
        Vector3 _aisleWaitPoint;    // 入口の手前で待つ点

        // ---- 台どうしの譲り合い（v5）----
        bool _robotYield;                    // いま譲って下がっている最中
        float _robotYieldS;                  // 譲り始めてからの秒数
        float _blockedByRobotS;              // もう1台にふさがれている秒数
        ServingRobotController _robotYieldTo;// 道をゆずる相手
        float _robotYieldStartS;             // 譲り始めた時刻（yields.csv）
        Vector3 _robotYieldAt;               // 譲り始めた位置（yields.csv）
        bool _robotYieldStepped;             // 相手の道から横へどけたか（false なら来た道へ戻った）
        float _repathCooldown;
        bool _inStop;
        float _currentStopS;
        bool _inWait;
        float _currentWaitS;
        Vector3 _pathEndPoint;
        bool _hasPathEnd;
        bool _wasBlocked;
        bool _hasBrakeCause;
        StopCause _brakeCause;
        string _brakeDetail = "";
        string _aisleBlockerName = "";
        bool _replannedInStop;
        bool _recordingStop;
        StopEvent _open;
        bool _pathValid;

        // ---- 途中までの経路から出る（B7）----
        /// <summary>この止まりの中で、最後に強く引き直したときの no_path_s</summary>
        float _lastEscapeAtNoPathS;
        /// <summary>いま通路の入口へ出ようとしているか</summary>
        bool _escapeToEntrance;

        // ---- 依頼 v9：地図の上で自分で道を引く（内部）----
        RobotMap _map;
        GridPlanner _planner;

        /// <summary>前がふさがって止まっている秒数（3 章の 3 秒を数える）</summary>
        float _blockedFrontS;

        /// <summary>いま回り込んだ道を走っているか</summary>
        bool _onDetour;

        /// <summary>向かい合って止まっている秒数（4 章の 10 秒を数える）</summary>
        float _headOnS;

        /// <summary>いまよける場所へ動いて待っているか（4 章）</summary>
        bool _mapYield;
        float _mapYieldS;
        ServingRobotController _mapYieldTo;

        /// <summary>いま道を引き直してよけている相手（1 回の出会いを 1 回だけ数えるため）</summary>
        ServingRobotController _avoidTarget;
        float _mapYieldStartS;
        Vector3 _mapYieldAt;
        bool _mapYieldStepped;

        /// <summary>この止まりの終わり方を上書きする（回り込み・よけ）</summary>
        bool _hasResolveOverride;
        StopResolve _resolveOverride;

        /// <summary>先の道を入れる作業場（毎コマ作り直さない）</summary>
        readonly List<Vector3> _aheadMine = new List<Vector3>();
        readonly List<Vector3> _aheadOther = new List<Vector3>();

        static readonly Vector3[] NoCorners = new Vector3[0];

        // ---- いまたどっている点の並び（サイトの道順と、地図で引いた道をまとめて見る）----

        /// <summary>
        /// いまたどっている点の数。
        /// SiteRoutes では道順の点（_legPoints）、MapPlanner では引いた道の曲がり角（_corners）。
        /// PathAhead・CurrentLegSegment は、どちらでも「これから進む道」を返せるように、ここを見る
        /// </summary>
        int WayCount { get { return _legPoints != null ? _legPoints.Count : _corners.Length; } }

        /// <summary>いま目指している点の番号</summary>
        int WayIndex { get { return _legPoints != null ? _legIndex : _cornerIndex; } }

        /// <summary>i 番目の点（y は 0）</summary>
        Vector3 WayPoint(int i)
        {
            Vector3 p = _legPoints != null ? _legPoints[i] : _corners[i];
            p.y = 0f;
            return p;
        }

        /// <summary>
        /// ロボットが歩く NavMesh の種類（agentTypeID）。既定の 0。
        /// NavMesh が人用とロボット用の2枚になったので、引くときは必ず種類を指定する
        /// （指定しないと「どの種類か決められない」と警告が出て、思わぬ面を拾う）。
        /// </summary>
        public const int RobotAgentTypeId = 0;

        static NavMeshQueryFilter RobotFilter
        {
            get
            {
                NavMeshQueryFilter f = new NavMeshQueryFilter();
                f.agentTypeID = RobotAgentTypeId;
                f.areaMask = NavMesh.AllAreas;
                return f;
            }
        }

        /// <summary>
        /// stops.csv の cause_detail に書く印。
        /// この印がある止まりは「途中までの経路に縛られて、引き直して出た」もの。
        /// CSV を割らないように、区切りはカンマを使わない。
        /// </summary>
        public const string PartialPathMark = "途中までの経路";

        void Awake()
        {
            _path = new NavMeshPath();
            _agent = GetComponent<NavMeshAgent>();
            ApplySpecToAgent();
        }

        void OnEnable()
        {
            if (!_all.Contains(this)) _all.Add(this);
        }

        void OnDisable()
        {
            _all.Remove(this);
        }

        void ApplySpecToAgent()
        {
            if (_agent == null || spec == null) return;
            _agent.radius = spec.Radius;
            _agent.height = spec.height;
            _agent.speed = spec.maxSpeed;
            _agent.acceleration = spec.acceleration;
            _agent.angularSpeed = spec.turnSpeedDegPerSec;
            // 自動でよける機能は使わない（実機のよけ方と違うため）
            // ロボットはロボット用の面（半径 navAgentRadius で焼いたもの）を歩く
            if (_agent.agentTypeID != RobotAgentTypeId) _agent.agentTypeID = RobotAgentTypeId;
            _agent.obstacleAvoidanceType = ObstacleAvoidanceType.NoObstacleAvoidance;
            _agent.avoidancePriority = spec.avoidancePriority;
            // 位置と向きはこの script が動かす。agent は経路を探すのと、人に見てもらうためだけ
            _agent.updatePosition = false;
            _agent.updateRotation = false;
            _agent.autoRepath = false;
            // isStopped は NavMesh に乗っている agent にしか使えない
            if (_agent.enabled && _agent.isOnNavMesh) _agent.isStopped = true;
        }

        /// <summary>受け渡し口に置き直して、次の往復を始める準備をする</summary>
        /// <summary>
        /// 仕事がないときの待機場所へ、ゆっくり下がる（B2）。
        /// 往復ではないので、距離・止まり・動けなくなったの記録には数えない。
        /// 止まる決まり（前のセンサー・譲り合い）もここでは働かせない。
        /// すでにその場所の近くにいれば何もしない。
        /// </summary>
        public void GoStandby(Vector3 spot)
        {
            if (_phase != Phase.Idle && _phase != Phase.Done) return;
            if (_agent == null || !Application.isPlaying) return;
            if (!_agent.enabled || !_agent.isOnNavMesh) return;

            Vector3 here = transform.position; here.y = 0f;
            Vector3 to = spot; to.y = 0f;
            if (Vector3.Distance(here, to) <= spec.arriveRadius) { _agent.isStopped = true; return; }

            _agent.isStopped = false;
            _agent.speed = spec.maxSpeed;
            _agent.SetDestination(spot);
        }

        public void PlaceAt(Vector3 position, Quaternion rotation)
        {
            transform.SetPositionAndRotation(position, rotation);
            _speed = 0f;

            if (_agent == null) return;
            if (!Application.isPlaying) return;      // 編集モードでは NavMeshAgent を触らない

            if (!_agent.enabled) _agent.enabled = true;
            _agent.Warp(position);
            if (_agent.isOnNavMesh)
            {
                _agent.isStopped = true;
                _agent.nextPosition = position;
            }
        }

        /// <summary>1往復を始める（細い通路の譲り合いなし。場面の再現などに使う）</summary>
        public void StartTrip(int tableId, Vector3 tablePoint, Vector3 passPoint)
        {
            StartTrip(tableId, tablePoint, passPoint, passPoint, new Bounds());
        }

        /// <summary>1往復を始める（1卓だけ）</summary>
        public void StartTrip(int tableId, Vector3 tablePoint, Vector3 passPoint,
                              Vector3 aisleEntrance, Bounds aisleZone)
        {
            RunStop s = new RunStop();
            s.tableId = tableId;
            s.point = tablePoint;
            s.aisleEntrance = aisleEntrance;
            s.aisleZone = aisleZone;
            List<RunStop> one = new List<RunStop>();
            one.Add(s);
            StartRun(one, passPoint);
        }

        /// <summary>
        /// 1走りを始める。受け渡し口を出て、渡された卓を順に回り、受け渡し口へ戻る。
        /// 止まる決まり（前のセンサー・譲り合い・動けなくなったときの扱い）は1卓のときと同じ。
        /// </summary>
        public void StartRun(IList<RunStop> stops, Vector3 passPoint)
        {
            StartRun(stops, passPoint, -1f);
        }

        /// <summary>始めた時刻（店の中, 秒）も渡して1走りを始める。記録に使う</summary>
        public void StartRun(IList<RunStop> stops, Vector3 passPoint, float startS)
        {
            RunStartS = startS;
            _stops.Clear();
            _tableIds.Clear();
            _stopArrivedS.Clear();
            if (stops != null)
            {
                for (int i = 0; i < stops.Count; i++)
                {
                    _stops.Add(stops[i]);
                    _tableIds.Add(stops[i].tableId);
                    _stopArrivedS.Add(-1f);     // 着いたら時刻を入れる（依頼 v8 の 2）
                }
            }
            _stopIndex = 0;
            StopsCompleted = 0;
            ResetTo = "";               // 打ち切りで置き直したときだけ入れる（依頼 v8b の A）

            if (_stops.Count == 0) { _phase = Phase.Idle; return; }
            BeginRun(passPoint);
        }

        void BeginRun(Vector3 passPoint)
        {
            RunStop first = _stops[0];
            int tableId = first.tableId;
            Vector3 tablePoint = first.point;
            Vector3 aisleEntrance = first.aisleEntrance;
            Bounds aisleZone = first.aisleZone;

            if (_path == null) _path = new NavMeshPath();
            ApplySpecToAgent();
            if (Application.isPlaying && _agent != null)
            {
                if (!_agent.enabled) _agent.enabled = true;
                if (!_agent.isOnNavMesh) _agent.Warp(transform.position);
            }

            TableId = tableId;
            _tablePoint = tablePoint;
            _passPoint = passPoint;
            _aisleEntrance = aisleEntrance;
            _aisleZone = aisleZone;
            _hasAisleZone = aisleZone.size.x > 0.01f && aisleZone.size.z > 0.01f;

            DistanceM = 0f;
            TotalS = 0f;
            StoppedS = 0f;
            StopCount = 0;
            MaxDecelMps2 = 0f;
            MinClearanceM = float.PositiveInfinity;
            Stuck = false;
            StuckCount = 0;
            StuckWaitS = 0f;
            LegWasNarrow = false;
            MetRobot = false;
            Aborted = false;
            TimedOut = false;
            WaitAisleS = 0f;
            _stopDurations.Clear();
            _stopEvents.Clear();

            _speed = 0f;
            _dwellTimer = 0f;
            _clearTimer = 0f;
            _hasDestinationOnMesh = false;
            _wasBlocked = false;
            _hasBrakeCause = false;
            _stoppedTimer = 0f;
            _lastReplanAtStopped = 0f;
            _stuckWaitTimer = 0f;
            _phaseBeforeStuck = Phase.ToTable;
            _yieldHoldS = 0f;
            _yieldGaveUp = false;
            _robotYield = false;
            _robotYieldS = 0f;
            _robotYieldTo = null;
            _robotYieldStepped = false;
            _robotYieldStartS = 0f;
            _blockedByRobotS = 0f;
            _repathCooldown = 0f;
            _inStop = false;
            _currentStopS = 0f;
            _lastEscapeAtNoPathS = 0f;
            _escapeToEntrance = false;

            // ---- 依頼 v9 ----
            Replans = 0;
            Detours = 0;
            RobotYields = 0;
            PlanMsMax = 0f;
            SiteRouteM = 0f;
            TripEndS = -1f;
            MapSnapStarts = 0;
            PlanFails = 0;
            _blockedFrontS = 0f;
            _onDetour = false;
            _headOnS = 0f;
            _mapYield = false;
            _mapYieldS = 0f;
            _mapYieldTo = null;
            _avoidTarget = null;
            _hasResolveOverride = false;

            // 譲り合いを使わない／区画が無いときは、はじめから「入った」扱いにする
            _enteredAisle = !(spec != null && spec.aisleYielding) || !_hasAisleZone;
            _yieldHold = false;

            _phase = Phase.ToTable;
            // v5b: サイトの道順を受け取る（店を建て直すこともあるので、1 走りごとに引き直す）
            StoreBuilder store = FindObjectOfType<StoreBuilder>();
            _routes = store != null ? store.Routes : null;

            // 依頼 v9: ロボットが走れる所の地図を受け取る。道探しはこの上でやる（NavMesh は使わない）。
            // 地図が無いのに MapPlanner だと走れないので、はっきり言ってから SiteRoutes に落とす
            _map = store != null ? store.RobotMap : null;
            _planner = _map != null ? _map.Planner : null;
            if (spec != null && spec.pathMode == PathMode.MapPlanner && _planner == null)
                Debug.LogError("[ServiceSim] ロボット" + robotId + "号機: pathMode が MapPlanner ですが、" +
                               "店に地図（map.json）がありません。この回は SiteRoutes で走ります" +
                               "（JsonStoreLayout の mapJson を確かめてください）");
            // ドリンクバーの並ぶ点（戻りの線を離すために使う。v5）
            _drinkQueues.Clear();
            if (store != null)
            {
                IList<Vector3> qs = store.PointsOf(PlaceKind.DrinkBarQueue);
                for (int i = 0; i < qs.Count; i++) _drinkQueues.Add(new Vector3(qs[i].x, 0f, qs[i].z));
            }
            _snapFallbacks = 0;
            bool narrow;
            BeginLeg(RouteTo(0, tableId, out narrow), _tablePoint, narrow);
        }

        /// <summary>次の卓へ向かう。もう無ければ受け渡し口へ戻る</summary>
        void NextStopOrReturn()
        {
            if (_stopIndex + 1 < _stops.Count)
            {
                int prevTableId = _stops[_stopIndex].tableId;
                _stopIndex++;
                RunStop s = _stops[_stopIndex];
                TableId = s.tableId;
                _tablePoint = s.point;
                _aisleEntrance = s.aisleEntrance;
                _aisleZone = s.aisleZone;
                _hasAisleZone = s.aisleZone.size.x > 0.01f && s.aisleZone.size.z > 0.01f;
                _enteredAisle = !(spec != null && spec.aisleYielding) || !_hasAisleZone;
                _yieldHold = false;
                _escapeToEntrance = false;
                _lastEscapeAtNoPathS = 0f;
                _phase = Phase.ToTable;
                bool narrowNext;
                BeginLeg(RouteTo(prevTableId, s.tableId, out narrowNext), _tablePoint, narrowNext);
                return;
            }
            _phase = Phase.ToPass;
            _escapeToEntrance = false;
            _lastEscapeAtNoPathS = 0f;
            bool narrowBack;
            List<Vector3> back = RouteToPass(_stops[_stopIndex].tableId, out narrowBack);

            // 受け渡し口の前の点を通らず、その少し下を通って右の待機場所へ抜ける（v5）。
            // 線が通れる床に乗らないときは、今までどおり受け渡し口の前へ戻る
            List<Vector3> line = ReturnLine(back);
            _returnLineUsed = line != null;
            if (line != null) back = line;

            BeginLeg(back, _returnLineUsed ? _standbyPoint : _passPoint, narrowBack);
        }

        // ---------------- 戻りの線（v5）----------------

        /// <summary>
        /// 戻る台の線。受け渡し口の前の点のかわりに、その少し下（passReturnOffsetM）の点へ入り、
        /// 右へ抜けて自分の待機場所で終わる。
        ///
        /// 1006 の回で、受け渡し口の前（x 14.0〜15.8 / z 4.8〜6.6）の「もう1台のロボット」が
        /// 209 回あり、そのうち 2 号機の 182 回では、相手の 91% が
        /// 「戻ってきた台」か「待機場所にいる台」だった。出る台と戻る台がここで交差していたため。
        ///
        /// サイトの道順は変えない（最後の点だけ差し替える）。
        /// 線が使えないときは null を返し、今までどおり受け渡し口の前へ戻る。
        /// </summary>
        List<Vector3> ReturnLine(List<Vector3> routeToPass)
        {
            // 依頼 v9：戻りの線は SiteRoutes のときだけ働く。
            // MapPlanner では地図の上で受け渡し口まで自分で引く（後から補う線は使わない）
            if (UseMapPlanner) return null;
            if (spec == null || !spec.useReturnLine || !_hasStandby) return null;
            if (routeToPass == null || routeToPass.Count < 2) return null;

            Vector3 pass = _passPoint; pass.y = 0f;
            float z = pass.z - spec.passReturnOffsetM;
            Vector3 p1 = new Vector3(pass.x, 0f, z);
            Vector3 p2 = new Vector3(_standbyPoint.x, 0f, z);
            Vector3 p3 = _standbyPoint;

            List<Vector3> pts = new List<Vector3>(routeToPass);
            pts[pts.Count - 1] = p1;        // 受け渡し口の前の点を、その少し下へ差し替える
            pts.Add(p2);
            pts.Add(p3);

            // 道順から分かれたあとの区間だけを確かめる（分かれる前はサイトの道順そのまま）
            int from = Mathf.Max(0, pts.Count - 4);
            if (!ReturnLineWalkable(pts, from) ||
                !ReturnLineClear(p1, p2, p3))
            {
                _returnLineFallbacks++;
                return null;
            }
            return pts;
        }

        /// <summary>戻りの線が、ロボット用の焼いた面に乗っているか（刻みで調べる）</summary>
        bool ReturnLineWalkable(List<Vector3> pts, int from)
        {
            float step = Mathf.Max(0.01f, spec.passReturnCheckStepM);
            NavMeshHit hit;

            for (int i = from; i < pts.Count - 1; i++)
            {
                Vector3 a = pts[i], b = pts[i + 1];
                float len = Vector3.Distance(a, b);
                int n = Mathf.Max(1, Mathf.CeilToInt(len / step));
                for (int k = 0; k <= n; k++)
                {
                    Vector3 p = Vector3.Lerp(a, b, (float)k / n);
                    if (!NavMesh.SamplePosition(p, out hit, spec.Radius, RobotFilter)) return false;
                }
            }
            return true;
        }

        /// <summary>
        /// 戻りの線が、出ていく台の線（受け渡し口の前の点を通る線）と
        /// ドリンクバーの並ぶ点から、決めた距離だけ離れているか。
        /// 離れを見るのは、道順から分かれたあとの3点と、そのあいだだけ
        /// （分かれる前はサイトの道順そのままなので、出る線と重なっていてよい）。
        /// </summary>
        bool ReturnLineClear(Vector3 p1, Vector3 p2, Vector3 p3)
        {
            float step = Mathf.Max(0.01f, spec.passReturnCheckStepM);
            Vector3 pass = _passPoint; pass.y = 0f;
            Vector3[] line = { p1, p2, p3 };

            for (int i = 0; i < line.Length - 1; i++)
            {
                float len = Vector3.Distance(line[i], line[i + 1]);
                int n = Mathf.Max(1, Mathf.CeilToInt(len / step));
                for (int k = 0; k <= n; k++)
                {
                    Vector3 p = Vector3.Lerp(line[i], line[i + 1], (float)k / n);
                    if (Vector3.Distance(p, pass) < spec.passReturnSeparationM) return false;
                    for (int q = 0; q < _drinkQueues.Count; q++)
                        if (Vector3.Distance(p, _drinkQueues[q]) < spec.passReturnKeepAwayM) return false;
                }
            }
            return true;
        }

        /// <summary>往復を打ち切る</summary>
        public void AbortTrip()
        {
            _legPoints = null;
            if (_phase == Phase.Idle || _phase == Phase.Done) return;
            Aborted = true;
            Finish();
        }

        void Update()
        {
            if (spec == null) return;
            if (_phase == Phase.Idle || _phase == Phase.Done)
            {
                if (_pathLine != null) _pathLine.Hide();   // 走っていないときは線を消す（6 章）
                return;
            }

            float dt = Time.deltaTime;
            if (dt <= 0f) return;

            LastStepM = 0f;

            FrontSensor.Reading reading = sensor != null ? sensor.Sense() : new FrontSensor.Reading();
            if (reading.sawRobot) MetRobot = true;

            // 卓での25秒。測りの時間には入れない
            if (_phase == Phase.Dwell)
            {
                _dwellTimer += dt;
                if (_dwellTimer >= spec.tableDwellSec) NextStopOrReturn();
                SyncAgent();
                return;
            }

            TotalS += dt;
            UpdateClearance();

            // 1往復の打ち切り（この往復は timeout として1行に残す）
            if (TotalS > spec.maxTripSec)
            {
                TimedOut = true;
                // ほかの台と重ならない所へ置き直す（依頼 v8b の A）。
                // 前は必ず受け渡し口の点に置いていたので、2台が打ち切られると
                // 同じ点で重なり、お互いを「もう1台のロボット」として止まり続けていた
                Vector3 to = PickResetPoint();
                PlaceAt(to, transform.rotation);
                Finish();
                return;
            }

            // 動けなくなって、スタッフが来るのを待っているあいだ（v5 3章）。
            // 受け渡し口へ移す（warp）のはやめた。スタッフが歩いて来て rescue_s 動かすと
            // Rescued() が呼ばれ、止まっていた所から走りを続ける。
            // 待っているあいだは「止まっていた」に数える。
            // いつまでも来ないときは、上の maxTripSec の打ち切りが働く（timeout=1）。
            if (_phase == Phase.StuckWait)
            {
                StoppedS += dt;
                StuckWaitS += dt;
                _stuckWaitTimer += dt;
                SyncAgent();
                return;
            }

            // ---- 通路の入口へ出ている途中（B7）。着いたら、もう一度もとの行き先を目指す ----
            // MapPlanner では escape を使わない（依頼 v9 の 3 章）
            if (!UseMapPlanner) UpdateEscape();

            // ---- 台どうしでふさぎ合ったら、決まった順で片方が譲る ----
            // MapPlanner では「料理を載せている方が先」（依頼 v9 の 4 章）、
            // SiteRoutes ではこれまでの「番号の小さい台が先」（v5〜v8b）
            if (UseMapPlanner) UpdateRobotYieldMap(dt, reading);
            else UpdateRobotYield(dt, reading);

            // ---- 細い通路の譲り合い ----
            bool waitingAtEntrance = !_robotYield && !_mapYield && UpdateAisleYielding();

            // ---- 走る・止まる ----
            bool blocked = reading.blocked;

            // 原因は、ブレーキを踏み始めた瞬間（前が塞がったと分かった瞬間）のものを覚えておく。
            // 速さが0になるまで待つと、そのあいだに相手が通り過ぎて「分からない」になってしまう
            if (blocked && !_wasBlocked)
            {
                _brakeCause = CauseOf(reading);
                _brakeDetail = string.IsNullOrEmpty(reading.nearestName) ? "" : reading.nearestName;
                _hasBrakeCause = true;
            }
            _wasBlocked = blocked;

            if (blocked) _clearTimer = 0f;
            else _clearTimer += dt;

            // 行き先への道が引けていないときも止まる（進めないのに走っている扱いにしない）
            bool hasPath = _corners.Length > 0 && _cornerIndex < _corners.Length;
            bool wantMove = !blocked && _clearTimer >= spec.resumeAfterClearSec && !waitingAtEntrance && hasPath;

            float target = wantMove ? spec.maxSpeed : 0f;
            float prevSpeed = _speed;
            if (target > _speed) _speed = Mathf.Min(target, _speed + spec.acceleration * dt);
            else _speed = Mathf.Max(target, _speed - spec.deceleration * dt);

            float decel = (prevSpeed - _speed) / dt;
            if (decel > MaxDecelMps2) MaxDecelMps2 = decel;

            if (_speed <= spec.stoppedSpeedEpsilon)
            {
                StoppedS += dt;

                if (waitingAtEntrance)
                {
                    // 譲り合いで入口に待っている分。止まった回数・長さには数えない
                    WaitAisleS += dt;
                    _stoppedTimer = 0f;
                    _lastReplanAtStopped = 0f;
                    if (!_inWait)
                    {
                        _inWait = true;
                        _currentWaitS = 0f;
                        BeginStopRecord(StopCause.AisleQueue,
                            _aisleBlockerName.Length > 0 ? "先に入っている台：" + _aisleBlockerName : "順番待ち", false);
                    }
                    _currentWaitS += dt;
                    _open.yieldS += dt;
                }
                else
                {
                    if (_inWait) { _inWait = false; EndStopRecord(_currentWaitS, StopResolve.AisleWait); }

                    _stoppedTimer += dt;
                    if (!_inStop)
                    {
                        _inStop = true;
                        _currentStopS = 0f;
                        _replannedInStop = false;
                        _lastEscapeAtNoPathS = 0f;   // no_path_s は止まりごとに数え直す（B7）

                        string detail;
                        StopCause cause;
                        // 道をゆずって下がっているあいだは、相手の台が原因（v5）。
                        // 下がり終えると道が引けていない状態になるが、それは「幅が足りない」ではない
                        if (_robotYield)
                        {
                            cause = StopCause.OtherRobot;
                            detail = _robotYieldTo != null
                                ? "道をゆずって下がった：" + _robotYieldTo.name : "道をゆずって下がった";
                        }
                        else if (_mapYield)
                        {
                            // よける場所へ動いて相手を待っているあいだ（依頼 v9 の 4 章）
                            cause = StopCause.OtherRobot;
                            detail = _mapYieldTo != null
                                ? "よける場所で待った：" + _mapYieldTo.name : "よける場所で待った";
                        }
                        else if (!hasPath) cause = CauseOfNoPath(out detail);
                        else if (_hasBrakeCause)
                        {
                            cause = _brakeCause;      // ブレーキを踏み始めたときに見たもの
                            detail = _brakeDetail;
                        }
                        else
                        {
                            cause = CauseOf(reading);
                            detail = string.IsNullOrEmpty(reading.nearestName) ? "" : reading.nearestName;
                        }
                        BeginStopRecord(cause, detail, !hasPath);
                        _hasBrakeCause = false;
                    }
                    _currentStopS += dt;

                    // 何で動けないかを秒数で分けて残す（B7 を調べるため）。
                    // 止まる決まりは変えない。数えるだけ
                    if (blocked) _open.blockedS += dt;
                    if (!hasPath) _open.noPathS += dt;
                    if (!blocked && hasPath && _clearTimer < spec.resumeAfterClearSec)
                        _open.clearWaitS += dt;

                    if (UseMapPlanner)
                    {
                        // 依頼 v9 の 3 章：止まって 3 秒たっても前が空かなければ回り込む。
                        // 「10 秒ごとに引き直す」と escape は使わない
                        UpdateDetour(dt, blocked);
                    }
                    else
                    {
                        // 止まって10秒ごとに経路を引き直す
                        if (_stoppedTimer - _lastReplanAtStopped >= spec.replanAfterStoppedSec)
                        {
                            _lastReplanAtStopped = _stoppedTimer;
                            _replannedInStop = true;
                            _open.replans++;
                            SetDestination(_destination);
                        }
                    }

                    // 途中までの経路に縛られているとき（B7）。
                    // 「道が引けていなかった秒数」がしきい値を超えたら、
                    // 引き直しの間隔（repathRetrySec）を待たずに強く引き直す。
                    // それでも途中までしか引けなければ、いちど通路の入口を目指して出る。
                    if (!UseMapPlanner && !hasPath && !_escapeToEntrance &&
                        _open.noPathS - _lastEscapeAtNoPathS >= spec.noPathForceReplanSec)
                    {
                        EscapePartialPath();
                    }

                    // 60秒動けなかったら「動けなくなった」
                    if (_stoppedTimer >= spec.stuckAfterStoppedSec)
                    {
                        EnterStuck();
                        SyncAgent();
                        return;
                    }
                }
            }
            else
            {
                if (_inWait) { _inWait = false; EndStopRecord(_currentWaitS, StopResolve.AisleWait); }
                if (_inStop)
                {
                    _inStop = false;
                    StopCount++;
                    _stopDurations.Add(_currentStopS);
                    EndStopRecord(_currentStopS, ResolveOfStop());
                }
                _stoppedTimer = 0f;
                _lastReplanAtStopped = 0f;
                _blockedFrontS = 0f;
            }

            FaceAndMove(dt);
            SyncAgent();
            UpdatePathLine();
            CheckArrival();
        }

        // ---------------- 見せる（依頼 v9 の 6 章）----------------

        RobotPathLine _pathLine;

        /// <summary>
        /// いま引いている道を床の上に細い線で出す。
        /// 見た目だけで、動きには効かない。RobotSpec.showPathLine で切れる
        /// </summary>
        void UpdatePathLine()
        {
            if (spec == null || !spec.showPathLine)
            {
                if (_pathLine != null) _pathLine.Hide();
                return;
            }
            if (_pathLine == null)
            {
                _pathLine = RobotPathLine.Attach(transform, spec.pathLineWidthM);
                if (_pathLine == null) return;
            }

            int n = WayCount, i = WayIndex;
            if (n == 0 || i >= n) { _pathLine.Hide(); return; }

            _lineBuf.Clear();
            for (int k = i; k < n; k++) _lineBuf.Add(WayPoint(k));
            // 料理を載せているとき（卓へ向かっている）と空のときで色を変える
            _pathLine.Show(_lineBuf, 0, transform.position,
                           YieldRank == 0, spec.pathLineHeightM);
        }

        readonly List<Vector3> _lineBuf = new List<Vector3>();

        // ---------------- 台どうしの譲り合い（v5）----------------

        /// <summary>
        /// 2台がふさぎ合ったとき、決まった順で片方が譲る。
        ///
        /// 実際の配膳ロボットは台どうしで連絡を取り合い、片方が道を譲る。
        /// ここでは **番号の小さい台が先**。番号の大きい台が譲る。
        ///
        /// 2026-10-23：譲る側は **後ろへ下がるのではなく、相手の道から横へ
        /// yieldSideClearM（0.6 m）以上どいた点で待ち、相手が通り過ぎたら出る**。
        /// 前は来た道へ下がっていたので、相手も同じ道を来ると向かい合ったまま溜まっていた。
        /// どく点が無ければ、手前の分かれ道（道順の1つ前の点）の外で待つ。
        /// 待ちすぎたら（yieldSideWaitMaxSec）あきらめて進む。
        /// </summary>
        void UpdateRobotYield(float dt, FrontSensor.Reading reading)
        {
            if (_robotYield)
            {
                _robotYieldS += dt;

                bool passed = _robotYieldTo == null || Gap(_robotYieldTo) >= spec.robotYieldClearM;
                float limit = spec.yieldOrderEnabled && spec.yieldSideWaitMaxSec > 0f
                    ? spec.yieldSideWaitMaxSec : spec.robotYieldMaxSec;
                bool tooLong = limit > 0f && _robotYieldS >= limit;
                if (passed || tooLong)
                {
                    if (ops != null)
                        ops.AddYield(_robotYieldStartS, _robotYieldAt,
                                     "ロボット" + robotId + "号機", RobotSpec.RankRobot,
                                     _robotYieldTo != null ? "ロボット" + _robotYieldTo.robotId + "号機" : "",
                                     _robotYieldS, _robotYieldStepped, passed);

                    _robotYield = false;
                    _robotYieldTo = null;
                    _robotYieldS = 0f;
                    _blockedByRobotS = 0f;
                    ResumeLeg(_phase == Phase.ToPass ? _passPoint : _tablePoint);
                }
                return;
            }

            // ふさいでいるのがもう1台か
            ServingRobotController other = reading.blocked && reading.nearestIsRobot
                ? NearestOtherRobot() : null;
            if (other == null) { _blockedByRobotS = 0f; return; }

            _blockedByRobotS += dt;
            if (_blockedByRobotS < spec.robotYieldAfterSec) return;

            // 番号の小さい台が先。自分が小さければ譲らない（相手がどく）
            if (robotId <= other.robotId) return;

            _robotYield = true;
            _robotYieldS = 0f;
            _blockedByRobotS = 0f;
            _robotYieldTo = other;
            _robotYieldStartS = ops != null ? ops.NowS : 0f;
            _robotYieldAt = transform.position;

            Vector3 goal;
            _robotYieldStepped = TryYieldSidePoint(other, out goal);
            if (!_robotYieldStepped) goal = YieldBackPoint(other);
            SetDestination(goal, true);
        }

        /// <summary>
        /// 相手の道から横へ yieldSideClearM 以上どいた点を探す（2026-10-23）。
        /// 相手がいまたどっている線に直角の向きへ、近い順に試す。
        /// ロボットが通れる面に乗り、寄せたあとも横の距離が足りているものだけを使う。
        /// </summary>
        bool TryYieldSidePoint(ServingRobotController other, out Vector3 point)
        {
            point = transform.position;
            // 依頼 v9：横へどく点（yieldOrder）は SiteRoutes のときだけ働く。
            // MapPlanner では 4 章のよける場所（地図のマスから選ぶ）を使う
            if (UseMapPlanner) return false;
            if (spec == null || !spec.yieldOrderEnabled || other == null) return false;

            Vector3 a, b;
            if (!other.CurrentLegSegment(out a, out b)) return false;

            if (_sideCandidates == null) _sideCandidates = new List<Vector3>();
            YieldSide.Candidates(a, b, transform.position,
                                 spec.yieldSideClearM + spec.Radius,
                                 spec.yieldSideSearchM, spec.yieldSideStepM,
                                 _sideCandidates);

            NavMeshHit hit;
            for (int i = 0; i < _sideCandidates.Count; i++)
            {
                if (!NavMesh.SamplePosition(_sideCandidates[i], out hit, spec.yieldSideStepM, RobotFilter))
                    continue;
                if (!YieldSide.FarEnough(a, b, hit.position, spec.yieldSideClearM + spec.Radius))
                    continue;
                point = hit.position;
                return true;
            }
            return false;
        }

        List<Vector3> _sideCandidates;

        // ---------------- 打ち切りで置き直す先（依頼 v8b の A）----------------

        /// <summary>
        /// 打ち切りで置き直した先（trips.csv の reset_to）。
        /// pass / standby1 / standby2 …。打ち切りでない往復は空
        /// </summary>
        public string ResetTo { get; private set; }

        /// <summary>
        /// 置く先を選ぶ。ほかの台が近く（直径＋余裕）にいない最初の点。
        ///   受け渡し口の点 → 待機場所を順に
        /// どこも空いていなければ、ほかの台からいちばん遠い待機場所
        /// （待機場所が無ければ受け渡し口の点）。
        /// </summary>
        Vector3 PickResetPoint()
        {
            float near = spec != null ? spec.diameter + ResetClearMarginM : 0.8f;

            IList<Vector3> standby = ops != null && ops.store != null
                ? ops.store.RobotStandbyPoints : null;

            // 1. 受け渡し口の点
            if (NearestRobotDistance(_passPoint) > near) { ResetTo = "pass"; return _passPoint; }

            // 2. 待機場所を順に
            if (standby != null)
            {
                for (int i = 0; i < standby.Count; i++)
                {
                    if (NearestRobotDistance(standby[i]) <= near) continue;
                    ResetTo = "standby" + (i + 1);
                    return standby[i];
                }

                // 3. どこも空いていない。ほかの台からいちばん遠い待機場所
                int best = -1;
                float bestD = -1f;
                for (int i = 0; i < standby.Count; i++)
                {
                    float d = NearestRobotDistance(standby[i]);
                    if (d <= bestD) continue;
                    best = i; bestD = d;
                }
                if (best >= 0)
                {
                    ResetTo = "standby" + (best + 1);
                    return standby[best];
                }
            }

            ResetTo = "pass";
            return _passPoint;
        }

        /// <summary>置く先として見るときの、直径に足す余裕（m）</summary>
        const float ResetClearMarginM = 0.3f;

        /// <summary>その点から、いちばん近いほかの台までの距離（m）。ほかに台が無ければ大きい値</summary>
        float NearestRobotDistance(Vector3 p)
        {
            p.y = 0f;
            float best = float.MaxValue;
            for (int i = 0; i < _all.Count; i++)
            {
                ServingRobotController o = _all[i];
                if (o == null || o == this) continue;
                Vector3 q = o.transform.position; q.y = 0f;
                float d = Vector3.Distance(p, q);
                if (d < best) best = d;
            }
            return best;
        }

        /// <summary>相手の外周と自分の外周のあいだの空き（m）</summary>
        float Gap(ServingRobotController other)
        {
            Vector3 d = other.transform.position - transform.position;
            d.y = 0f;
            return d.magnitude - spec.Radius * 2f;
        }

        /// <summary>いちばん近いもう1台（譲る相手を決めるのに使う）</summary>
        ServingRobotController NearestOtherRobot()
        {
            ServingRobotController best = null;
            float bestD = float.MaxValue;
            for (int i = 0; i < _all.Count; i++)
            {
                ServingRobotController o = _all[i];
                if (o == null || o == this) continue;
                Vector3 d = o.transform.position - transform.position;
                d.y = 0f;
                float m = d.magnitude;
                if (m >= bestD) continue;
                best = o; bestD = m;
            }
            return best;
        }

        /// <summary>
        /// 横へどく点が無かったときの逃げ道。
        /// 手前の分かれ道（道順の1つ前の点）まで戻って、そこで待つ。
        /// それも無ければ、相手と反対の向きへ robotYieldBackM 下がる。
        /// </summary>
        Vector3 YieldBackPoint(ServingRobotController other)
        {
            NavMeshHit hit;

            if (_legPoints != null && _legIndex > 0 && _legIndex - 1 < _legPoints.Count)
            {
                Vector3 back = _legPoints[_legIndex - 1];
                if (NavMesh.SamplePosition(back, out hit, spec.Radius * 2f, RobotFilter))
                    return hit.position;
            }

            Vector3 away = transform.position - other.transform.position;
            away.y = 0f;
            if (away.sqrMagnitude < 0.0001f) away = -transform.forward;
            away.Normalize();
            Vector3 p = transform.position + away * spec.robotYieldBackM;
            if (NavMesh.SamplePosition(p, out hit, spec.Radius * 2f, RobotFilter)) return hit.position;
            return transform.position;
        }

        /// <summary>
        /// 細い通路に入る前の譲り合い。
        /// 行き先の通路にもう1台がいたら、太い通路側の入口で待つ。
        /// 戻り（ToPass）では譲らない（出ないと詰まるため）。
        /// </summary>
        bool UpdateAisleYielding()
        {
            if (_phase != Phase.ToTable) return false;
            if (!spec.aisleYielding || !_hasAisleZone || _enteredAisle) return false;

            if (ContainsXZ(_aisleZone, transform.position))
            {
                _enteredAisle = true;
                if (_yieldHold)
                {
                    _yieldHold = false;
                    ResumeLeg(_tablePoint);
                }
                return false;
            }

            // 待ちすぎたら譲らずに進む（人の robotYieldMaxSec と同じ考え方。v5）。
            // この区間のあいだは、もう譲らない
            if (_yieldHold)
            {
                _yieldHoldS += Time.deltaTime;
                if (spec.aisleWaitMaxSec > 0f && _yieldHoldS >= spec.aisleWaitMaxSec)
                {
                    _yieldGaveUp = true;
                    _yieldHold = false;
                    _yieldHoldS = 0f;
                    ResumeLeg(_tablePoint);
                    return false;
                }
            }

            if (!_yieldGaveUp && MustWaitForAisle())
            {
                if (!_yieldHold)
                {
                    _yieldHold = true;
                    _yieldHoldS = 0f;
                    // 入口の点そのものに立つと、中の台が出られなくなる。
                    // 通路の外へ下がり、出てくる道をふさがない所で待つ
                    _aisleWaitPoint = AisleWaitPoint();
                    SetDestination(_aisleWaitPoint, true);
                }
            }
            else if (_yieldHold)
            {
                _yieldHold = false;
                _yieldHoldS = 0f;
                ResumeLeg(_tablePoint);
            }

            if (!_yieldHold) return false;

            Vector3 a = transform.position; a.y = 0f;
            Vector3 b = _aisleWaitPoint; b.y = 0f;
            return Vector3.Distance(a, b) <= spec.arriveRadius;
        }

        /// <summary>
        /// 通路の入口で待つ点（v5）。入口の点から通路の外へ下がり、横へずれる。
        /// 中の台が出てくる道をふさがないため。焼いた面に乗らなければ、下がるだけ・入口そのものの順に落とす。
        /// </summary>
        Vector3 AisleWaitPoint()
        {
            Vector3 entrance = _aisleEntrance; entrance.y = 0f;
            if (!_hasAisleZone) return entrance;

            Vector3 center = _aisleZone.center; center.y = 0f;
            Vector3 outward = entrance - center;
            outward.y = 0f;
            if (outward.sqrMagnitude < 0.0001f) return entrance;
            outward.Normalize();
            Vector3 side = new Vector3(-outward.z, 0f, outward.x);

            Vector3 back = entrance + outward * spec.aisleWaitSetbackM;
            Vector3[] tries =
            {
                back + side * spec.aisleWaitSideM,
                back - side * spec.aisleWaitSideM,
                back
            };

            NavMeshHit hit;
            for (int i = 0; i < tries.Length; i++)
                if (NavMesh.SamplePosition(tries[i], out hit, spec.Radius, RobotFilter)) return hit.position;

            return entrance;
        }

        /// <summary>
        /// 途中までの経路に縛られた止まりから出る（B7）。
        ///   1. 引き直しの間隔（repathRetrySec）を待たずに強く引き直す
        ///   2. それでも最後までつながらなければ、いちど通路の入口を目指して出る
        /// どちらも「経路の引き直し」なので、この止まりは resolved が「経路を引き直した」になる。
        /// cause_detail には「途中までの経路」の印を足す。
        /// </summary>
        void EscapePartialPath()
        {
            _lastEscapeAtNoPathS = _open.noPathS;
            _replannedInStop = true;
            _open.replans++;
            MarkPartialPath();

            // 1. 間隔を無視して引き直す
            SetDestination(_destination, true);
            if (_pathValid && _cornerIndex < _corners.Length) return;

            // 2. それでも途中まで。通路の入口へ出る
            if (!spec.escapeToAisleEntranceOnPartialPath) return;

            Vector3 here = transform.position; here.y = 0f;
            Vector3 exit = _aisleEntrance; exit.y = 0f;
            if (Vector3.Distance(here, exit) <= spec.arriveRadius) return;

            _escapeToEntrance = true;
            SetDestination(_aisleEntrance, true);
        }

        /// <summary>通路の入口へ出ている途中。着いたら、もとの行き先をもう一度目指す</summary>
        void UpdateEscape()
        {
            if (!_escapeToEntrance) return;

            Vector3 here = transform.position; here.y = 0f;
            Vector3 exit = _aisleEntrance; exit.y = 0f;
            if (Vector3.Distance(here, exit) > spec.arriveRadius) return;

            _escapeToEntrance = false;
            ResumeLeg(_phase == Phase.ToPass ? _passPoint : _tablePoint);
        }

        /// <summary>この止まりに「途中までの経路」の印をつける（1回だけ）</summary>
        void MarkPartialPath()
        {
            if (!_recordingStop) return;
            string d = _open.causeDetail;
            if (!string.IsNullOrEmpty(d) && d.Contains(PartialPathMark)) return;
            _open.causeDetail = string.IsNullOrEmpty(d) ? PartialPathMark : d + "／" + PartialPathMark;
        }

        bool MustWaitForAisle()
        {
            for (int i = 0; i < _all.Count; i++)
            {
                ServingRobotController o = _all[i];
                if (o == null || o == this) continue;

                // その通路の中にいる台がいれば入らない
                if (ContainsXZ(_aisleZone, o.transform.position)) { _aisleBlockerName = o.name; return true; }

                // 同じ通路を目指していて、まだ入っていない台。番号の小さい方が先に入る
                if (o._phase == Phase.ToTable && !o._enteredAisle && o._hasAisleZone &&
                    SameZone(o._aisleZone, _aisleZone) && o.robotId < robotId)
                {
                    _aisleBlockerName = o.name;
                    return true;
                }
            }
            _aisleBlockerName = "";
            return false;
        }

        static bool ContainsXZ(Bounds b, Vector3 p)
        {
            return p.x >= b.min.x && p.x <= b.max.x && p.z >= b.min.z && p.z <= b.max.z;
        }

        static bool SameZone(Bounds a, Bounds b)
        {
            Vector3 d = a.center - b.center;
            d.y = 0f;
            return d.sqrMagnitude < 0.0025f;   // 5 cm 以内なら同じ通路
        }

        /// <summary>
        /// 動けなくなった。スタッフを呼んで、来るまで待つ（v5 3章）。
        /// Stuck は「この往復で1回でも動けなくなったか」の印なので、1往復に1回だけ立てる。
        /// 呼ぶ方は何度でも起こりうる（動かしてもらった先でまた動けなくなる）ので、
        /// 呼び出しは Stuck の印とは別に毎回出す。
        /// </summary>
        void EnterStuck()
        {
            if (_phase == Phase.StuckWait) return;   // すでに待っている
            Stuck = true;
            _mapYield = false;
            _mapYieldTo = null;
            _headOnS = 0f;
            if (_inStop) EndStopRecord(_currentStopS, StopResolve.Stuck);
            // 動けなくなった分は「自分で動き出した止まり」には数えない
            _inStop = false;
            _currentStopS = 0f;
            _stuckWaitTimer = 0f;
            _escapeToEntrance = false;
            _lastEscapeAtNoPathS = 0f;
            StuckCount++;
            _phaseBeforeStuck = _phase;
            _phase = Phase.StuckWait;
            if (ops != null) ops.PostRescue(this);
        }

        /// <summary>
        /// スタッフが来て rescue_s 動かし終わった（StaffAgent が呼ぶ。v5 3章）。
        /// 止まっていた所から、同じ道順の続きを走り直す。道順そのものは変えない。
        /// </summary>
        public void Rescued()
        {
            if (_phase != Phase.StuckWait) return;
            if (ops != null) ops.RescueFinished(_stuckWaitTimer);
            _stuckWaitTimer = 0f;
            _stoppedTimer = 0f;
            _lastReplanAtStopped = 0f;
            _clearTimer = 0f;
            bool toPass = _phaseBeforeStuck == Phase.ToPass;
            _phase = toPass ? Phase.ToPass : Phase.ToTable;
            ResumeLeg(toPass ? _passPoint : _tablePoint);
        }

        /// <summary>止まった原因を、前のセンサーが見たものから決める</summary>
        StopCause CauseOf(FrontSensor.Reading r)
        {
            if (r.nearestIsRobot) return StopCause.OtherRobot;
            if (r.nearestIsChair) return StopCause.Chair;
            if (r.nearestIsPerson)
            {
                if (r.nearestPersonStanding) return StopCause.PersonStanding;
                return r.nearestPersonKind == PersonKind.Staff ? StopCause.Staff : StopCause.Guest;
            }
            if (r.sawRobot) return StopCause.OtherRobot;
            if (r.sawChair) return StopCause.Chair;
            if (r.sawPerson) return StopCause.Guest;
            return StopCause.Unknown;
        }

        /// <summary>
        /// 行き先への道が引けないときの原因。
        /// ふさいでいるもの（引かれた椅子・もう1台のロボット）が見つかれば、その原因にする。
        /// 何も見つからないときだけ「通路の幅が足りない」にする。
        /// </summary>
        StopCause CauseOfNoPath(out string detail)
        {
            StopCause cause;
            string name;

            // 行き止まりの点 → 行き先 → 自分のまわり、の順に探す
            if (_hasPathEnd && FindBlocker(_pathEndPoint, out cause, out name))
            {
                detail = "経路なし：" + name;
                return cause;
            }
            if (FindBlocker(_destination, out cause, out name))
            {
                detail = "経路なし：" + name;
                return cause;
            }
            if (FindBlocker(transform.position, out cause, out name))
            {
                detail = "経路なし：" + name;
                return cause;
            }

            detail = "経路なし：ふさいでいるものが見つからない";
            return StopCause.NoPath;
        }

        const int BlockMax = 32;
        readonly Collider[] _blockHits = new Collider[BlockMax];

        /// <summary>
        /// その点のまわりで、道をふさぎうるもの（引かれた椅子・ほかのロボット）のうち
        /// いちばん近いものを探す。見つかれば true。
        /// </summary>
        bool FindBlocker(Vector3 around, out StopCause cause, out string name)
        {
            cause = StopCause.NoPath;
            name = "";

            Vector3 center = new Vector3(around.x, spec.clearanceProbeHeight, around.z);
            int n = Physics.OverlapSphereNonAlloc(center, spec.pathBlockSearchRadius, _blockHits,
                                                  ~0, QueryTriggerInteraction.Ignore);
            if (n > BlockMax) n = BlockMax;

            float best = float.MaxValue;
            for (int i = 0; i < n; i++)
            {
                Collider col = _blockHits[i];
                if (col == null) continue;

                Chair chair = col.GetComponentInParent<Chair>();
                if (chair != null)
                {
                    // 引かれていない椅子は NavMesh を削らないので、道をふさがない
                    if (!chair.IsPulled) continue;
                    float d = Vector3.Distance(center, chair.transform.position);
                    if (d < best)
                    {
                        best = d;
                        cause = StopCause.Chair;
                        name = chair.transform.parent != null
                            ? chair.transform.parent.name + "/" + chair.name : chair.name;
                    }
                    continue;
                }

                ServingRobotController other = col.GetComponentInParent<ServingRobotController>();
                if (other != null && other != this)
                {
                    float d = Vector3.Distance(center, other.transform.position);
                    if (d < best)
                    {
                        best = d;
                        cause = StopCause.OtherRobot;
                        name = other.name;
                    }
                }
            }
            return best < float.MaxValue;
        }

        // ---- 道を引いたときの中身（調べるためだけ。動きは変えない）----
        Vector3 _lastSolveFrom, _lastSolveTo;
        string _lastSolveStatus = "";
        int _lastSolveCorners;
        float _lastOffMeshM = -1f;

        /// <summary>
        /// いま開いている止まりの記録に、最後に道を引いたときの中身を写す。
        /// 止まっているあいだは 10 秒ごとに引き直すので、最後の1回ぶんが残る。
        /// 記録するだけで、止まる・動くの決まりには使わない。
        /// </summary>
        void RecordSolveIntoOpenStop()
        {
            if (!_recordingStop) return;
            _open.solveFrom = _lastSolveFrom;
            _open.solveTo = _lastSolveTo;
            _open.solveStatus = _lastSolveStatus;
            _open.solveCorners = _lastSolveCorners;
            _open.offMeshM = _lastOffMeshM;
        }

        /// <summary>
        /// この止まりの終わり方。
        /// 依頼 v9 の 3 章（回り込んで動き出した）・4 章（よける場所へ動いた）が働いたら、
        /// 「経路を引き直した」よりそちらを出す
        /// </summary>
        StopResolve ResolveOfStop()
        {
            if (_hasResolveOverride) return _resolveOverride;
            return _replannedInStop ? StopResolve.Replanned : StopResolve.SelfResume;
        }

        void BeginStopRecord(StopCause cause, string detail, bool pathBlocked)
        {
            _hasResolveOverride = false;
            _open = new StopEvent();
            _open.startTimeS = Time.time;
            _open.position = transform.position;
            _open.cause = cause;
            _open.causeDetail = detail == null ? "" : detail;
            _open.pathBlocked = pathBlocked;
            float left, right;
            MeasureSideClearance(out left, out right);
            _open.clearanceLeftM = left;
            _open.clearanceRightM = right;
            _recordingStop = true;
            RecordSolveIntoOpenStop();   // 止まり始めの時点の、直前に引いた中身
        }

        void EndStopRecord(float durationS, StopResolve resolve)
        {
            if (!_recordingStop) return;
            _recordingStop = false;
            _open.durationS = durationS;
            _open.resolve = resolve;
            _stopEvents.Add(_open);
        }

        /// <summary>
        /// 止まった位置での、左右の空き（ロボットの外周から壁・席・椅子まで）。
        /// 人とロボットは数えない（動くもののため）。
        /// </summary>
        void MeasureSideClearance(out float left, out float right)
        {
            left = ProbeSide(-transform.right);
            right = ProbeSide(transform.right);
        }

        const int ProbeMax = 16;
        readonly RaycastHit[] _probe = new RaycastHit[ProbeMax];

        float ProbeSide(Vector3 dir)
        {
            float range = spec.clearanceProbeRange;
            Vector3 origin = transform.position + Vector3.up * spec.clearanceProbeHeight;
            int n = Physics.RaycastNonAlloc(origin, dir, _probe, range, ~0, QueryTriggerInteraction.Ignore);
            if (n > ProbeMax) n = ProbeMax;

            float best = range;
            for (int i = 0; i < n; i++)
            {
                Collider col = _probe[i].collider;
                if (col == null) continue;
                if (col.GetComponentInParent<PersonBody>() != null) continue;
                if (col.GetComponentInParent<ServingRobotController>() != null) continue;
                if (_probe[i].distance < best) best = _probe[i].distance;
            }
            return Mathf.Max(0f, best - spec.Radius);
        }

        void CheckArrival()
        {
            if (_yieldHold) return;         // いまの行き先は通路の入口なので、着いても卓ではない
            if (_escapeToEntrance) return;  // 途中までの経路から出ている途中（B7）。行き先は通路の入口
            if (_mapYield) return;          // いまの行き先はよける場所（依頼 v9 の 4 章）

            Vector3 here = transform.position; here.y = 0f;
            Vector3 goal = _hasDestinationOnMesh ? _destinationOnMesh : _destination; goal.y = 0f;

            // v5b: 道順の途中の点は、少し手前でも「着いた」として次の点へ進む。
            // 最後の点（卓の止まる位置・受け渡し口）は今までどおり arriveRadius で見る
            bool midWaypoint = _legPoints != null && _legIndex + 1 < _legPoints.Count;
            float reach = midWaypoint
                ? Mathf.Max(spec.arriveRadius, spec.waypointReachDistance)
                : spec.arriveRadius;
            bool near = Vector3.Distance(here, goal) <= reach;

            // 最後までつながった経路の角を使い切ったら、着いたとみなす
            bool consumed = _pathValid && _corners.Length > 0 && _cornerIndex >= _corners.Length;
            if (!near && !consumed) return;

            if (AdvanceWaypoint()) return;     // まだ区間の途中

            if (_phase == Phase.ToTable)
            {
                _phase = Phase.Dwell;
                _dwellTimer = 0f;
                _speed = 0f;
                StopsCompleted = _stopIndex + 1;   // この卓には届いた

                // 卓に着いた時点を残して知らせる（依頼 v8 の 1）。
                // サイトの試算と同じで、ここで皿は「届いた」。
                // 受け渡しの 25 秒（tableDwellSec）はこのあと
                int at = _stopIndex;
                if (at >= 0 && at < _stopArrivedS.Count)
                    _stopArrivedS[at] = ops != null ? ops.NowS : 0f;
                if (StopArrived != null) StopArrived(this, at);
            }
            else if (_phase == Phase.ToPass)
            {
                Finish();
            }
        }

        void Finish()
        {
            _phase = Phase.Done;
            _speed = 0f;
            _legPoints = null;
            _mapYield = false;
            _mapYieldTo = null;
            // 往復が実際に終わった店の中の時刻（卓での25秒を含む。依頼 v9 の 5 章）
            TripEndS = ops != null ? ops.NowS : -1f;
            // まだ呼んだままのスタッフがいたら、呼び出しを下げる（v5 3章）
            if (ops != null) ops.CancelRescue(this);
            if (_inWait) { _inWait = false; EndStopRecord(_currentWaitS, StopResolve.AisleWait); }
            if (_inStop)
            {
                _inStop = false;
                StopCount++;
                _stopDurations.Add(_currentStopS);
                EndStopRecord(_currentStopS, ResolveOfStop());
            }
            if (float.IsInfinity(MinClearanceM)) MinClearanceM = -1f;   // 人がいなかった
            if (TripCompleted != null) TripCompleted(this);
        }

        // ---- サイトの道順（v5b）----
        RouteBook _routes;
        List<Vector3> _legPoints;         // いまの区間の曲がり角の点（最後が行き先そのもの）
        int _legIndex;
        bool _legIsNarrow;                // この区間が 0.8 m の道か（記録用）
        int _snapFallbacks;               // 止まる位置を 0.1 m で面に寄せられず、広げた回数

        /// <summary>いまの区間が 0.8 m の道か（記録用。v5b）</summary>
        public bool UsedNarrowAisle { get { return _legIsNarrow; } }

        /// <summary>
        /// この往復のどこかで 0.8 m の道を使ったか（trips.csv の via_narrow。v5 3章）。
        /// 1往復で何卓も回るので、区間ごとの印（UsedNarrowAisle）とは別に、
        /// 1回でも使ったら立てたままにする。
        /// </summary>
        public bool LegWasNarrow { get; private set; }

        /// <summary>道順の点を 0.1 m で面に寄せられなかった回数（報告用。v5b）</summary>
        public int RouteSnapFallbacks { get { return _snapFallbacks; } }

        /// <summary>
        /// 区間（受け渡し口→卓 / 卓→卓 / 卓→受け渡し口）を始める（v5b）。
        /// サイトの道順があればその点を順にたどり、無ければ行き先まで直接向かう。
        /// いまいる所に近い先頭の点は飛ばす（受け渡し口で始めるときなど）。
        /// </summary>
        void BeginLeg(List<Vector3> points, Vector3 fallbackTarget, bool narrow)
        {
            // サイトの道順をたどったときの長さを足す（走りには使わない。trips.csv の site_route_m）
            SiteRouteM += _lastRouteLenM;
            _lastRouteLenM = 0f;

            _legIsNarrow = narrow;
            if (narrow) LegWasNarrow = true;
            _legPoints = null;
            _legIndex = 0;
            _onDetour = false;
            _blockedFrontS = 0f;

            // 依頼 v9: 地図の上で、行き先まで自分で道を引く。サイトの routes の点はたどらない。
            // 「狭い道を使ったか」は、引いた道に 1 のマスが入ったかで決まる（narrow は使わない）
            if (UseMapPlanner)
            {
                _legIsNarrow = false;
                PlanOnMap(fallbackTarget, MarkMode.None, true, false);
                return;
            }

            if (spec != null && spec.followSiteRoutes && points != null && points.Count >= 2)
            {
                _legPoints = points;
                Vector3 here = transform.position; here.y = 0f;
                // 先頭の点は、いまいる所と同じことが多い（受け渡し口）。近ければ飛ばす
                while (_legIndex < _legPoints.Count - 1)
                {
                    Vector3 p = _legPoints[_legIndex]; p.y = 0f;
                    if (Vector3.Distance(here, p) > spec.waypointReachDistance) break;
                    _legIndex++;
                }
                SetDestination(_legPoints[_legIndex], true);
                return;
            }
            SetDestination(fallbackTarget, true);
        }

        /// <summary>いまの区間の、いま目指している点をもう一度行き先にする（v5b）</summary>
        void ResumeLeg(Vector3 fallbackTarget)
        {
            if (_legPoints != null && _legIndex < _legPoints.Count)
            {
                SetDestination(_legPoints[_legIndex], true);
                return;
            }
            SetDestination(fallbackTarget, true);
        }

        /// <summary>
        /// 道順の途中の点に着いたら、次の点へ進む（v5b）。
        /// 戻り値 true なら、まだ区間の途中（卓にも受け渡し口にも着いていない）。
        /// </summary>
        bool AdvanceWaypoint()
        {
            if (_legPoints == null || _legIndex + 1 >= _legPoints.Count) return false;
            _legIndex++;
            SetDestination(_legPoints[_legIndex], true);
            return true;
        }

        /// <summary>
        /// 直前に引いたサイトの道順の長さ（m）。BeginLeg が SiteRouteM に足す。
        /// MapPlanner でも「サイトの道順をたどったらこの長さ」として記録に残す（依頼 v9 の 5 章）
        /// </summary>
        float _lastRouteLenM;

        /// <summary>その卓へ行く道順（受け渡し口から / 直前の卓から）</summary>
        List<Vector3> RouteTo(int fromTableId, int toTableId, out bool narrow)
        {
            narrow = false;
            _lastRouteLenM = 0f;
            if (_routes == null) return null;
            RouteBook.Route used;
            List<Vector3> pts;
            if (fromTableId <= 0)
            {
                pts = _routes.PointsFromPass(toTableId, out used);
            }
            else
            {
                bool reversed;
                pts = _routes.PointsBetween(fromTableId, toTableId, out used, out reversed);
            }
            if (used != null) { narrow = used.IsNarrow; _lastRouteLenM = used.length; }
            return pts;
        }

        /// <summary>卓 → 受け渡し口の道順</summary>
        List<Vector3> RouteToPass(int fromTableId, out bool narrow)
        {
            narrow = false;
            _lastRouteLenM = 0f;
            if (_routes == null) return null;
            RouteBook.Route used;
            List<Vector3> pts = _routes.PointsToPass(fromTableId, out used);
            if (used != null) { narrow = used.IsNarrow; _lastRouteLenM = used.length; }
            return pts;
        }

        void SetDestination(Vector3 point) { SetDestination(point, false); }

        void SetDestination(Vector3 point, bool force)
        {
            // 依頼 v9: 地図の上で引く（NavMesh はロボットの道探しには使わない）
            if (UseMapPlanner)
            {
                PlanOnMap(point, MarkMode.None, force);
                return;
            }

            // 引き直しの間隔を空ける。いまの経路は消さない
            if (_repathCooldown > 0f && !force)
            {
                _destination = point;
                return;
            }
            _repathCooldown = spec.repathRetrySec;
            if (_path == null) _path = new NavMeshPath();

            _destination = point;
            _pathValid = false;
            _corners = new Vector3[0];
            _cornerIndex = 0;

            // v5b: 道順の最後の点（止まる位置そのもの）だけは寄せない（0.1 m 以下）。
            // 途中の点・通路の入口・道順を使わないときは、今までどおり広めに寄せる。
            bool lastRoutePoint =
                _legPoints != null && _legIndex == _legPoints.Count - 1 &&
                Vector3.Distance(point, _legPoints[_legIndex]) < 0.001f;
            float snap = lastRoutePoint
                ? Mathf.Min(spec.routePointSnapM, spec.Radius * 4f)
                : spec.Radius * 4f;
            SolvePath(point, RobotFilter, snap);
        }

        /// <summary>
        /// 行き先まで道を引き、_path・_corners に入れる。
        /// snap は、点を焼いた面へ寄せるときに探す半径（m）。
        /// 止まる位置がその半径で面に乗らなければ、広げて引き直し、回数を数える（v5b）。
        /// </summary>
        void SolvePath(Vector3 point, NavMeshQueryFilter filter, float snap)
        {
            if (_path == null) _path = new NavMeshPath();

            NavMeshHit hit;
            Vector3 from = transform.position;
            // いまの位置が焼いた面からどれだけ離れているか（調べるためだけ。動きは変えない）
            _lastOffMeshM = NavMesh.SamplePosition(transform.position, out hit, spec.Radius * 4f, filter)
                ? Vector3.Distance(transform.position, hit.position) : -1f;
            if (_lastOffMeshM >= 0f) from = hit.position;

            Vector3 to = point;
            if (NavMesh.SamplePosition(to, out hit, snap, filter)) to = hit.position;
            else if (snap < spec.Radius * 4f)
            {
                _snapFallbacks++;
                if (NavMesh.SamplePosition(to, out hit, spec.Radius * 4f, filter)) to = hit.position;
            }

            _lastSolveFrom = from;
            _lastSolveTo = to;

            if (!NavMesh.CalculatePath(from, to, filter, _path) || _path.corners.Length == 0)
            {
                _lastSolveStatus = "引けない";
                _lastSolveCorners = _path != null ? _path.corners.Length : 0;
                RecordSolveIntoOpenStop();
                return;
            }

            bool complete = _path.status == NavMeshPathStatus.PathComplete;
            _lastSolveStatus = _path.status.ToString();
            _lastSolveCorners = _path.corners.Length;
            RecordSolveIntoOpenStop();

            _destinationOnMesh = to;
            _hasDestinationOnMesh = true;
            _corners = _path.corners;
            _cornerIndex = 0;
            _pathValid = complete;
            if (!complete && _corners.Length > 0)
            {
                _pathEndPoint = _corners[_corners.Length - 1];
                _hasPathEnd = true;
            }
            else _hasPathEnd = false;

            // 足もとの角は飛ばす
            Vector3 here = transform.position; here.y = 0f;
            while (_cornerIndex < _corners.Length)
            {
                Vector3 c = _corners[_cornerIndex]; c.y = 0f;
                if (Vector3.Distance(here, c) > spec.cornerReachDistance) break;
                _cornerIndex++;
            }
        }

        // ================= 依頼 v9：地図の上で自分で道を引く =================

        /// <summary>道を引くときに差し込むもの</summary>
        enum MarkMode
        {
            None,     // そのまま引く
            Detour,   // 近くの人・ロボットを通れないマスとして置く（3 章）
            Avoid     // 優先が高い台の先の道を重くする（4 章）
        }

        /// <summary>
        /// 地図の上で、行き先まで道を引く（依頼 v9 の 2 章）。
        /// 引き直しは 1 台につき replanMinIntervalSec（0.5 秒）に 1 回まで（force で無視できる）。
        /// </summary>
        bool PlanOnMap(Vector3 point, MarkMode marks, bool force)
        {
            return PlanOnMap(point, marks, force, true);
        }

        /// <summary>
        /// isReplan が false のときは「引き直し」に数えない
        /// （区間を始めるときの1回目は引き直しではない）
        /// </summary>
        bool PlanOnMap(Vector3 point, MarkMode marks, bool force, bool isReplan)
        {
            if (_repathCooldown > 0f && !force)
            {
                _destination = point;
                return false;
            }
            _repathCooldown = Mathf.Max(0.01f, spec.replanMinIntervalSec);

            _destination = point;
            _pathValid = false;
            _hasPathEnd = false;
            _corners = NoCorners;
            _cornerIndex = 0;

            _planner.ClearMarks();
            if (marks == MarkMode.Detour) AddDetourMarks();
            else
            {
                // ふつうに引き直したら、もう回り込みの道ではない（モニターの出し分けに使う）
                _onDetour = false;
                if (marks == MarkMode.Avoid) AddAvoidMarks();
            }

            GridPlanner.Status s = _planner.Plan(transform.position, point, spec.PlannerSettings());
            return AfterMapPlan(s, isReplan);
        }

        /// <summary>引いた結果を受け取る。引けたら true</summary>
        bool AfterMapPlan(GridPlanner.Status status, bool isReplan)
        {
            if (isReplan)
            {
                Replans++;
                if (_recordingStop) _open.replans++;
            }
            if (_planner.LastPlanMs > PlanMsMax) PlanMsMax = _planner.LastPlanMs;
            if (_planner.LastStartWasBlocked) MapSnapStarts++;

            // stops.csv に出す中身（調べるためだけ。動きは変えない）
            _lastSolveFrom = transform.position;
            _lastSolveTo = _destination;
            _lastSolveStatus = _planner.StatusText;
            _lastSolveCorners = _planner.Points.Count;
            _lastOffMeshM = _planner.LastStartSnapM;
            RecordSolveIntoOpenStop();

            if (status != GridPlanner.Status.Ok || _planner.Points.Count == 0)
            {
                PlanFails++;
                return false;
            }

            _corners = new Vector3[_planner.Points.Count];
            for (int i = 0; i < _corners.Length; i++) _corners[i] = _planner.Points[i];
            _cornerIndex = 0;
            _pathValid = true;
            _hasDestinationOnMesh = true;
            _destinationOnMesh = _corners[_corners.Length - 1];

            if (_planner.UsedNarrow) { _legIsNarrow = true; LegWasNarrow = true; }

            // 足もとの角は飛ばす（最後の1つは残す。着いたと見るのに使う）
            Vector3 here = transform.position; here.y = 0f;
            while (_cornerIndex < _corners.Length - 1)
            {
                Vector3 c = _corners[_cornerIndex]; c.y = 0f;
                if (Vector3.Distance(here, c) > spec.cornerReachDistance) break;
                _cornerIndex++;
            }
            return true;
        }

        /// <summary>
        /// 回り込むために、近くの人・ほかのロボットをその 1 回だけ通れないマスとして置く（3 章）。
        /// 人は 人の半径＋ロボットの半径＋detourPersonMarginM、
        /// ロボットは 2 台の半径の和＋detourPersonMarginM。
        /// </summary>
        void AddDetourMarks()
        {
            Vector3 me = transform.position; me.y = 0f;
            float near = spec.detourNearM;
            float margin = spec.detourPersonMarginM;

            IReadOnlyList<PersonBody> people = PersonBody.All;
            for (int i = 0; i < people.Count; i++)
            {
                PersonBody p = people[i];
                if (p == null || p.kind == PersonKind.Seated) continue;
                Vector3 q = p.transform.position; q.y = 0f;
                if (Vector3.Distance(me, q) > near) continue;
                _planner.BlockCircle(q, p.radius + spec.Radius + margin);
            }

            for (int i = 0; i < _all.Count; i++)
            {
                ServingRobotController o = _all[i];
                if (o == null || o == this) continue;
                Vector3 q = o.transform.position; q.y = 0f;
                if (Vector3.Distance(me, q) > near) continue;
                _planner.BlockCircle(q, spec.Radius * 2f + margin);
            }
        }

        /// <summary>
        /// 優先が高い台の先の道を重くして、相手の道とかぶらない道を選ばせる（4 章）。
        /// 相手の体は通れないマスとして置く。
        /// </summary>
        void AddAvoidMarks()
        {
            ServingRobotController other = _mapYieldTo;
            if (other == null) return;

            other.PathAhead(spec.robotPathAheadM, _aheadOther);
            float costR = spec.Radius * 2f + spec.robotPathCostMarginM;
            for (int i = 1; i < _aheadOther.Count; i++)
                _planner.CostLine(_aheadOther[i - 1], _aheadOther[i], costR, spec.robotPathCostAdd);

            Vector3 q = other.transform.position; q.y = 0f;
            _planner.BlockCircle(q, spec.Radius * 2f);
        }

        /// <summary>いまの道の、ここから行き先までの残りの長さ（m）</summary>
        float RemainingPathM()
        {
            if (_corners.Length == 0 || _cornerIndex >= _corners.Length)
                return Vector3.Distance(Flat(transform.position), Flat(_destination));

            float sum = 0f;
            Vector3 from = Flat(transform.position);
            for (int i = _cornerIndex; i < _corners.Length; i++)
            {
                Vector3 to = Flat(_corners[i]);
                sum += Vector3.Distance(from, to);
                from = to;
            }
            return sum;
        }

        static Vector3 Flat(Vector3 v) { v.y = 0f; return v; }

        /// <summary>
        /// 前がふさがって止まっているとき（3 章）。
        /// detourAfterSec（3 秒）たっても空かなければ、回り込む道を探す。
        /// 引けなければ待ち、3 秒ごとに探し直す。
        /// 止まる位置そのものに人が立っているときは、回り込まずに待つ。
        /// </summary>
        void UpdateDetour(float dt, bool blocked)
        {
            if (!blocked)
            {
                _blockedFrontS = 0f;
                return;
            }
            _blockedFrontS += dt;
            if (_blockedFrontS < spec.detourAfterSec) return;
            _blockedFrontS = 0f;                       // 次の 3 秒を数え直す
            if (PersonOnGoal()) return;
            TryDetour();
        }

        /// <summary>行き先（止まる位置）そのものに人が立っているか</summary>
        bool PersonOnGoal()
        {
            Vector3 goal = Flat(_destination);
            IReadOnlyList<PersonBody> people = PersonBody.All;
            for (int i = 0; i < people.Count; i++)
            {
                PersonBody p = people[i];
                if (p == null || p.kind == PersonKind.Seated) continue;
                Vector3 q = p.transform.position; q.y = 0f;
                if (Vector3.Distance(goal, q) <= p.radius + spec.Radius) return true;
            }
            return false;
        }

        /// <summary>
        /// 回り込む道を引いてみる。
        /// 延びた距離が大きすぎる道（もとの残りの道の detourMaxFactor 倍＋detourExtraAddM より長い）は
        /// 取らずに待つ。引けなければ、もとの道に戻す。
        /// </summary>
        bool TryDetour()
        {
            float remain = RemainingPathM();
            float limit = remain * spec.detourMaxFactor + spec.detourExtraAddM;

            Vector3[] prevCorners = _corners;
            int prevIndex = _cornerIndex;
            bool prevValid = _pathValid;
            Vector3 prevOnMesh = _destinationOnMesh;
            bool prevHasOnMesh = _hasDestinationOnMesh;

            if (PlanOnMap(_destination, MarkMode.Detour, true) && _planner.LastLengthM <= limit)
            {
                Detours++;
                _onDetour = true;
                SetResolveOverride(StopResolve.Detoured);
                return true;
            }

            // 取らない。もとの道に戻す
            _corners = prevCorners;
            _cornerIndex = prevIndex;
            _pathValid = prevValid;
            _destinationOnMesh = prevOnMesh;
            _hasDestinationOnMesh = prevHasOnMesh;
            return false;
        }

        /// <summary>この止まりの終わり方を上書きする（回り込み・よけ）</summary>
        void SetResolveOverride(StopResolve r)
        {
            _hasResolveOverride = true;
            _resolveOverride = r;
            _replannedInStop = true;
        }

        // ---------------- ロボットどうし（4 章）----------------

        /// <summary>
        /// 優先の順で、片方が道をゆずる（依頼 v9 の 4 章・エイイチの決定）。
        ///   1. 料理を載せているロボットが先
        ///   2. 空で戻るロボットは後
        ///   3. 同じ状態なら 1 号機が先
        ///
        /// 優先が低い側は、相手が robotReplanNearM（3 m）以内に来て先の道がかぶったら、
        /// 相手の道を重くして引き直す。かぶらない道が引けないときは、
        /// 近く（robotYieldSpotSearchM）でよける場所を探してそこで待つ（最大 robotYieldMaxSec）。
        /// 向かい合って headOnStuckSec（10 秒）止まったら、引き直しを待たずによける場所へ動く。
        ///
        /// 優先が高い側はここで何もしない（相手を気にせず道を引く）。
        /// どちらも止まり続けないよう、前のセンサーの止まりからは 3 章の回り込みで抜ける。
        /// </summary>
        void UpdateRobotYieldMap(float dt, FrontSensor.Reading reading)
        {
            if (_mapYield)
            {
                _mapYieldS += dt;
                bool passed = _mapYieldTo == null || Gap(_mapYieldTo) >= spec.robotYieldClearM;
                bool tooLong = spec.robotYieldMaxSec > 0f && _mapYieldS >= spec.robotYieldMaxSec;
                if (!passed && !tooLong) return;

                if (ops != null)
                    ops.AddYield(_mapYieldStartS, _mapYieldAt,
                                 "ロボット" + robotId + "号機", RobotSpec.RankRobot,
                                 _mapYieldTo != null ? "ロボット" + _mapYieldTo.robotId + "号機" : "",
                                 _mapYieldS, _mapYieldStepped, passed);

                _mapYield = false;
                _mapYieldTo = null;
                _mapYieldS = 0f;
                _headOnS = 0f;
                ResumeLeg(GoalOfPhase());
                return;
            }

            ServingRobotController other = HigherPriorityNear();
            if (other == null)
            {
                _headOnS = 0f;
                _avoidTarget = null;       // この出会いは終わった
                return;
            }

            // 向かい合って止まっているか
            bool stopped = _speed <= spec.stoppedSpeedEpsilon;
            bool headOn = stopped && IsHeadOn(other);
            if (headOn) _headOnS += dt; else _headOnS = 0f;

            // ここから先は道を引く。引き直しは 1 台につき replanMinIntervalSec（0.5 秒）に 1 回まで。
            // 毎コマ引くと、1 回 10〜80 ms の道探しが回を押しつぶす
            if (_repathCooldown > 0f) return;

            // 2台が向かい合って headOnStuckSec（10 秒）止まったら、よける場所へ動く
            if (spec.headOnStuckSec > 0f && _headOnS >= spec.headOnStuckSec)
            {
                StartMapYield(other);
                return;
            }

            if (!PathsOverlap(other)) return;

            // 相手の道とかぶらない道があれば、そちらを選ぶ
            _mapYieldTo = other;
            if (TryAvoidPlan(other)) return;
            _mapYieldTo = null;

            // かぶらない道が引けない。止まっているならよける場所へ動く
            if (stopped) StartMapYield(other);
        }

        Vector3 GoalOfPhase()
        {
            return _phase == Phase.ToPass ? _passPoint : _tablePoint;
        }

        /// <summary>
        /// 近く（robotReplanNearM）にいて、自分より先に通る台。いなければ null。
        /// いちばん近い 1 台だけを見る
        /// </summary>
        ServingRobotController HigherPriorityNear()
        {
            ServingRobotController best = null;
            float bestD = spec.robotReplanNearM;
            Vector3 me = Flat(transform.position);
            for (int i = 0; i < _all.Count; i++)
            {
                ServingRobotController o = _all[i];
                if (o == null || o == this) continue;
                if (!YieldsTo(o)) continue;
                float d = Vector3.Distance(me, Flat(o.transform.position));
                if (d > bestD) continue;
                best = o; bestD = d;
            }
            return best;
        }

        /// <summary>2 台が向かい合っているか（向きの内積が headOnDotMax 以下）</summary>
        bool IsHeadOn(ServingRobotController other)
        {
            Vector3 a = transform.forward; a.y = 0f;
            Vector3 b = other.transform.forward; b.y = 0f;
            if (a.sqrMagnitude < 0.0001f || b.sqrMagnitude < 0.0001f) return false;
            return Vector3.Dot(a.normalized, b.normalized) <= spec.headOnDotMax;
        }

        /// <summary>自分の先の道と、相手の先の道がかぶっているか</summary>
        bool PathsOverlap(ServingRobotController other)
        {
            PathAhead(spec.robotPathAheadM, _aheadMine);
            other.PathAhead(spec.robotPathAheadM, _aheadOther);
            float limit = spec.Radius * 2f + spec.robotPathCostMarginM;
            return PolylineGap(_aheadMine, _aheadOther) < limit;
        }

        /// <summary>
        /// 相手の道を重くして引き直す。かぶらない道になったら使う。
        /// かぶったままなら、もとの道に戻して false
        /// </summary>
        bool TryAvoidPlan(ServingRobotController other)
        {
            Vector3[] prevCorners = _corners;
            int prevIndex = _cornerIndex;
            bool prevValid = _pathValid;
            Vector3 prevOnMesh = _destinationOnMesh;
            bool prevHasOnMesh = _hasDestinationOnMesh;

            if (PlanOnMap(_destination, MarkMode.Avoid, true) && !PathsOverlap(other))
            {
                // 同じ相手との 1 回の出会いは 1 回だけ数える
                // （近くにいるあいだ 0.5 秒ごとに引き直すので、毎回数えると膨らむ）
                if (_avoidTarget != other) { RobotYields++; _avoidTarget = other; }
                SetResolveOverride(StopResolve.YieldedToRobot);
                return true;
            }

            _corners = prevCorners;
            _cornerIndex = prevIndex;
            _pathValid = prevValid;
            _destinationOnMesh = prevOnMesh;
            _hasDestinationOnMesh = prevHasOnMesh;
            return false;
        }

        /// <summary>
        /// よける場所へ動いて、相手が通り過ぎるまで待つ。
        /// 場所が無い・そこへの道が引けないときは何もしない（待って、次のコマでまた見る）
        /// </summary>
        void StartMapYield(ServingRobotController other)
        {
            FindYieldSpots(other, _yieldSpots, 3);
            if (_yieldSpots.Count == 0) return;

            // 近い順に試す。道が引けた所へ動く
            Vector3[] prevCorners = _corners;
            int prevIndex = _cornerIndex;
            bool prevValid = _pathValid;
            Vector3 prevOnMesh = _destinationOnMesh;
            bool prevHasOnMesh = _hasDestinationOnMesh;
            Vector3 prevGoal = _destination;

            for (int i = 0; i < _yieldSpots.Count; i++)
            {
                if (!PlanOnMap(_yieldSpots[i], MarkMode.None, true)) continue;

                _mapYield = true;
                _mapYieldS = 0f;
                _mapYieldTo = other;
                _mapYieldStepped = true;
                _mapYieldStartS = ops != null ? ops.NowS : 0f;
                _mapYieldAt = transform.position;
                _headOnS = 0f;
                RobotYields++;
                SetResolveOverride(StopResolve.YieldedToRobot);
                return;
            }

            // どこへも引けない。もとの道に戻す
            _corners = prevCorners;
            _cornerIndex = prevIndex;
            _pathValid = prevValid;
            _destinationOnMesh = prevOnMesh;
            _hasDestinationOnMesh = prevHasOnMesh;
            _destination = prevGoal;
        }

        readonly List<Vector3> _yieldSpots = new List<Vector3>();

        /// <summary>
        /// よける場所を探す（4 章）。
        /// 近く（robotYieldSpotSearchM）で、相手の先の道から
        /// 2 台の半径の和＋robotYieldSeparationM 以上離れ、自分が入れるマス（free_class 1 以上）。
        /// 近い順に、互いに YieldSpotApartM 以上離れたものを max 個まで返す
        /// （となりのマスばかり並べても、どれも同じ所なので意味がない）。
        /// 同じ距離ならマスの番号の小さい方（乱数を使わない）。
        /// </summary>
        void FindYieldSpots(ServingRobotController other, List<Vector3> into, int max)
        {
            into.Clear();
            if (_map == null || other == null) return;

            other.PathAhead(spec.robotPathAheadM, _aheadOther);
            if (_aheadOther.Count < 2) return;

            float need = spec.Radius * 2f + spec.robotYieldSeparationM;
            float search = spec.robotYieldSpotSearchM;
            Vector3 me = Flat(transform.position);

            // 自分のまわりを輪の形に、内側から外側へ見ていく（並べ替えをしないで近い順にする）
            float res = _map.res;
            int cx = Mathf.FloorToInt(me.x / res);
            int cy = Mathf.FloorToInt(me.z / res);
            int maxR = Mathf.Max(1, Mathf.CeilToInt(search / res));
            float search2 = search * search;
            float apart2 = YieldSpotApartM * YieldSpotApartM;

            for (int r = 0; r <= maxR && into.Count < max; r++)
            {
                for (int iy = cy - r; iy <= cy + r && into.Count < max; iy++)
                {
                    if (iy < 0 || iy >= _map.ny) continue;
                    for (int ix = cx - r; ix <= cx + r; ix++)
                    {
                        if (ix < 0 || ix >= _map.nx) continue;
                        // 輪の上だけ見る（中は前の回で見た）
                        if (r > 0 && Mathf.Abs(ix - cx) != r && Mathf.Abs(iy - cy) != r) continue;

                        int i = _map.Index(ix, iy);
                        if (_map.ClassOf(i) < 1) continue;
                        Vector3 c = _map.Center(i);
                        float dx = c.x - me.x, dz = c.z - me.z;
                        if (dx * dx + dz * dz > search2) continue;
                        if (PointToPolyline(c, _aheadOther) < need) continue;

                        bool tooClose = false;
                        for (int k = 0; k < into.Count; k++)
                            if ((into[k] - c).sqrMagnitude < apart2) { tooClose = true; break; }
                        if (tooClose) continue;

                        into.Add(c);
                        if (into.Count >= max) break;
                    }
                }
            }
        }

        /// <summary>よける場所の候補どうしを、これだけ離してから並べる（m）</summary>
        const float YieldSpotApartM = 0.5f;

        // ---------------- 線と線の離れ ----------------

        /// <summary>2 本の折れ線のいちばん近いところの距離（m）</summary>
        static float PolylineGap(List<Vector3> a, List<Vector3> b)
        {
            if (a == null || b == null || a.Count < 2 || b.Count < 2) return float.MaxValue;
            float best = float.MaxValue;
            for (int i = 1; i < a.Count; i++)
            {
                for (int k = 1; k < b.Count; k++)
                {
                    float d = SegmentGap(a[i - 1], a[i], b[k - 1], b[k]);
                    if (d < best) best = d;
                }
            }
            return best;
        }

        /// <summary>点から折れ線までの距離（m）</summary>
        static float PointToPolyline(Vector3 p, List<Vector3> line)
        {
            float best = float.MaxValue;
            for (int i = 1; i < line.Count; i++)
            {
                float d = PointToSegment(p, line[i - 1], line[i]);
                if (d < best) best = d;
            }
            return best;
        }

        static float PointToSegment(Vector3 p, Vector3 a, Vector3 b)
        {
            Vector3 v = b - a; v.y = 0f;
            Vector3 w = p - a; w.y = 0f;
            float l2 = v.sqrMagnitude;
            float t = l2 > 1e-12f ? Mathf.Clamp01(Vector3.Dot(w, v) / l2) : 0f;
            Vector3 q = a + v * t;
            q.y = 0f;
            Vector3 pf = p; pf.y = 0f;
            return Vector3.Distance(pf, q);
        }

        /// <summary>
        /// 線分と線分のいちばん近いところの距離（m）。
        /// 交わっているときは 0（端点だけ見ると、交わっていても離れていることになってしまう）
        /// </summary>
        static float SegmentGap(Vector3 a1, Vector3 a2, Vector3 b1, Vector3 b2)
        {
            if (SegmentsCross(a1, a2, b1, b2)) return 0f;
            float d = PointToSegment(a1, b1, b2);
            d = Mathf.Min(d, PointToSegment(a2, b1, b2));
            d = Mathf.Min(d, PointToSegment(b1, a1, a2));
            d = Mathf.Min(d, PointToSegment(b2, a1, a2));
            return d;
        }

        static bool SegmentsCross(Vector3 a1, Vector3 a2, Vector3 b1, Vector3 b2)
        {
            float d1 = Cross(b1, b2, a1), d2 = Cross(b1, b2, a2);
            float d3 = Cross(a1, a2, b1), d4 = Cross(a1, a2, b2);
            return ((d1 > 0f && d2 < 0f) || (d1 < 0f && d2 > 0f)) &&
                   ((d3 > 0f && d4 < 0f) || (d3 < 0f && d4 > 0f));
        }

        static float Cross(Vector3 a, Vector3 b, Vector3 p)
        {
            return (b.x - a.x) * (p.z - a.z) - (b.z - a.z) * (p.x - a.x);
        }

        void FaceAndMove(float dt)
        {
            if (_repathCooldown > 0f) _repathCooldown -= dt;

            if (_cornerIndex >= _corners.Length)
            {
                if (_repathCooldown <= 0f) SetDestination(_destination);
                return;
            }

            Vector3 here = transform.position;
            Vector3 corner = _corners[_cornerIndex];
            Vector3 to = corner - here; to.y = 0f;
            float d = to.magnitude;

            while (d <= spec.cornerReachDistance && _cornerIndex < _corners.Length - 1)
            {
                _cornerIndex++;
                corner = _corners[_cornerIndex];
                to = corner - here; to.y = 0f;
                d = to.magnitude;
            }

            if (d > 0.0001f)
            {
                Vector3 dir = to / d;
                // 向きを変える
                Quaternion want = Quaternion.LookRotation(dir, Vector3.up);
                transform.rotation = Quaternion.RotateTowards(transform.rotation, want, spec.turnSpeedDegPerSec * dt);

                float step = Mathf.Min(_speed * dt, d);
                if (step > 0f)
                {
                    transform.position = here + dir * step;
                    DistanceM += step;
                    LastStepM = step;
                }
            }

            if (d <= spec.cornerReachDistance && _cornerIndex >= _corners.Length - 1) _cornerIndex = _corners.Length;
        }

        void SyncAgent()
        {
            if (_agent == null || !_agent.enabled) return;
            if (!_agent.isOnNavMesh)
            {
                _agent.Warp(transform.position);
                return;
            }
            _agent.nextPosition = transform.position;
        }

        void UpdateClearance()
        {
            float myR = spec.Radius;
            Vector3 me = transform.position; me.y = 0f;
            IReadOnlyList<PersonBody> all = PersonBody.All;
            for (int i = 0; i < all.Count; i++)
            {
                PersonBody p = all[i];
                if (p == null || !p.walksInAisle) continue;
                Vector3 q = p.transform.position; q.y = 0f;
                float gap = Vector3.Distance(me, q) - myR - p.radius;
                if (gap < MinClearanceM) MinClearanceM = gap;
            }
        }

        /// <summary>経路が最後までつながっているか（引き直しに失敗しているときは false）</summary>
        public bool PathComplete { get { return _pathValid; } }

        void OnDrawGizmosSelected()
        {
            if (!_hasAisleZone) return;
            Gizmos.color = _yieldHold ? new Color(1f, 0.8f, 0.1f, 0.6f) : new Color(0.2f, 0.9f, 0.5f, 0.35f);
            Gizmos.DrawWireCube(new Vector3(_aisleZone.center.x, 0.05f, _aisleZone.center.z),
                                new Vector3(_aisleZone.size.x, 0.1f, _aisleZone.size.z));
            Gizmos.DrawWireSphere(_aisleEntrance + Vector3.up * 0.05f, 0.25f);
        }
    }
}