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; }

        /// <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     // 入口で待った
        }

        /// <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>細い通路の入口で待っている最中か（字幕が読むだけ）</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走りで寄る卓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; } }

        /// <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 _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;

        /// <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();
            if (stops != null)
            {
                for (int i = 0; i < stops.Count; i++)
                {
                    _stops.Add(stops[i]);
                    _tableIds.Add(stops[i].tableId);
                }
            }
            _stopIndex = 0;
            StopsCompleted = 0;

            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;
            _blockedByRobotS = 0f;
            _repathCooldown = 0f;
            _inStop = false;
            _currentStopS = 0f;
            _lastEscapeAtNoPathS = 0f;
            _escapeToEntrance = false;

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

            _phase = Phase.ToTable;
            // v5b: サイトの道順を受け取る（店を建て直すこともあるので、1 走りごとに引き直す）
            StoreBuilder store = FindObjectOfType<StoreBuilder>();
            _routes = store != null ? store.Routes : null;
            // ドリンクバーの並ぶ点（戻りの線を離すために使う。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)
        {
            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) 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;
                PlaceAt(_passPoint, 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）。着いたら、もう一度もとの行き先を目指す ----
            UpdateEscape();

            // ---- 台どうしでふさぎ合ったら、決まった順で片方が譲る（v5）----
            UpdateRobotYield(dt, reading);

            // ---- 細い通路の譲り合い ----
            bool waitingAtEntrance = !_robotYield && 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 (!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;

                    // 止まって10秒ごとに経路を引き直す
                    if (_stoppedTimer - _lastReplanAtStopped >= spec.replanAfterStoppedSec)
                    {
                        _lastReplanAtStopped = _stoppedTimer;
                        _replannedInStop = true;
                        _open.replans++;
                        SetDestination(_destination);
                    }

                    // 途中までの経路に縛られているとき（B7）。
                    // 「道が引けていなかった秒数」がしきい値を超えたら、
                    // 引き直しの間隔（repathRetrySec）を待たずに強く引き直す。
                    // それでも途中までしか引けなければ、いちど通路の入口を目指して出る。
                    if (!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,
                        _replannedInStop ? StopResolve.Replanned : StopResolve.SelfResume);
                }
                _stoppedTimer = 0f;
                _lastReplanAtStopped = 0f;
            }

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

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

        /// <summary>
        /// 2台がふさぎ合ったとき、決まった順で片方が譲る。
        ///
        /// 実際の配膳ロボットは台どうしで連絡を取り合い、片方が道を譲る。
        /// ここでは **番号の小さい台が先**。番号の大きい台が、相手の道からどいて待ち、
        /// 相手が通り過ぎたら進む。
        ///
        /// 下がる先は、いま来た道（道順の1つ前の点）。必ず通れた所なので確実に戻れる。
        /// 無ければ、その場から後ろへ下がる。
        /// 待ちすぎたら（robotYieldMaxSec）あきらめて進む。
        /// </summary>
        void UpdateRobotYield(float dt, FrontSensor.Reading reading)
        {
            if (_robotYield)
            {
                _robotYieldS += dt;

                bool passed = _robotYieldTo == null || Gap(_robotYieldTo) >= spec.robotYieldClearM;
                bool tooLong = spec.robotYieldMaxSec > 0f && _robotYieldS >= spec.robotYieldMaxSec;
                if (passed || tooLong)
                {
                    _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;
            SetDestination(YieldBackPoint(other), true);
        }

        /// <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;
            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;
        }

        void BeginStopRecord(StopCause cause, string detail, bool pathBlocked)
        {
            _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）。行き先は通路の入口

            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;   // この卓には届いた
            }
            else if (_phase == Phase.ToPass)
            {
                Finish();
            }
        }

        void Finish()
        {
            _phase = Phase.Done;
            _speed = 0f;
            _legPoints = null;
            // まだ呼んだままのスタッフがいたら、呼び出しを下げる（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,
                    _replannedInStop ? StopResolve.Replanned : StopResolve.SelfResume);
            }
            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)
        {
            _legIsNarrow = narrow;
            if (narrow) LegWasNarrow = true;
            _legPoints = null;
            _legIndex = 0;

            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>その卓へ行く道順（受け渡し口から / 直前の卓から）</summary>
        List<Vector3> RouteTo(int fromTableId, int toTableId, out bool narrow)
        {
            narrow = false;
            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;
            return pts;
        }

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

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

        void SetDestination(Vector3 point, bool force)
        {
            // 引き直しの間隔を空ける。いまの経路は消さない
            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++;
            }
        }

        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);
        }
    }
}