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

namespace ServiceSim
{
    /// <summary>
    /// 人がロボットをよける。スタッフと歩く客に付ける。
    ///
    /// 実際の店で、人はロボットに自分から当たりに行かない。いまは人がロボットを
    /// 気にせず歩き、細い通路でロボットを両側からはさんで押し合う場面が出ていたので、
    /// 人の側がよけるようにする。
    ///
    ///   0. 譲る順番で、道から横へどく（2026-10-23。譲る順番 v6）：
    ///      順番は ロボット＞スタッフ＞客（RobotSpec の1か所）。
    ///      **スタッフはロボットが近づいたら必ず譲り、その場で下がるのではなく
    ///      ロボットの道（サイトの routes の線）から横へ 0.6 m 以上どいた、
    ///      通れる床の点で待つ**。無ければ来た道を戻って通路の外で待つ。
    ///      細い通路の中だけでなく **どこでも** 見る（受け渡し口と客席をつなぐ角
    ///      （x 10.5〜13.5・z 3.5〜5.8）は通路の外なので、下の 1・2 では何も起きず
    ///      向かい合って溜まっていた。前の2回（1311・1536）で、ロボットが止まった
    ///      時間の約3分の1がこの角）。
    ///      客は 4 のとおり、今までどおり（店員優先の版）。
    ///   1. 先を見て待つ・回り道する：細い通路に入る前に、その通路の中にロボットがいたら
    ///      入らない。回り道で行けて遠回りが少しなら回り道、なければ通路の入口の
    ///      ロボットの通り道からはずれた場所で待つ
    ///   2. 向かい合ったら下がる：細い通路の中で向かい合って近づいたら、
    ///      近くの交差点か、壁ぞいの卓のすき間（よけ場所）に下がって、通り過ぎてから進む
    ///   3. 間を空ける：広い通路ですれ違うときは、ロボットの外周から少し空ける
    ///   4. まれな不注意：1回の行動につき決めた割合だけ、上を守らない人を混ぜる
    ///
    /// ロボットのふるまいは変えない（NavMesh も経路も触らない）。
    /// この script は人だけを扱う。
    /// 数字は PeopleSpec の .asset に置く。ただし **譲る順番と、道から横へどく距離・
    /// 待つ上限は RobotSpec の1か所**（ロボットどうしの譲り合いと同じ値を使うため）。
    /// </summary>
    [RequireComponent(typeof(NavMeshAgent))]
    public class PersonRobotYield : MonoBehaviour
    {
        /// <summary>いまのよけ方</summary>
        public enum Mode
        {
            Clear,            // ふつうに歩いている
            WaitAtEntrance,   // 細い通路の入口で待っている
            Detour,           // 回り道している
            BackOff,          // 通路の中で下がってよけている
            StepAside         // ロボットの道から横へどいて待っている（2026-10-23）
        }

        [Header("いまの様子（読むだけ）")]
        [SerializeField] Mode _mode = Mode.Clear;
        [SerializeField] bool _careless;
        [SerializeField] float _modeS;

        PeopleSpec _spec;
        StoreBuilder _store;
        ServiceOps _ops;
        NavMeshAgent _agent;
        PersonBody _body;
        System.Random _rng;

        Vector3 _ownerDest;
        bool _hasOwnerDest;
        Vector3 _goal;              // 待つ・下がる・回り道の行き先
        int _aisle = -1;            // 相手のいる（または入ろうとしている）細い通路
        bool _contactOpen;
        NavMeshPath _path;

        // ---- 店員優先の版（v5）----
        /// <summary>客が「横を詰めて通る」と決めた最中か（すき間を staffFirstSqueezeGapM にする）</summary>
        bool _squeeze;

        /// <summary>
        /// この台だけはよけない（v5 3章）。
        /// 立ち往生したロボットを動かしに行くスタッフは、その台に近づかないと仕事ができないので、
        /// その台について「横に空ける・下がる・入口で待つ・回り道する」をしない。
        /// ほかの台はこれまでどおりよける。入れるのは StaffAgent（仕事を始めるとき）。
        /// </summary>
        public ServingRobotController ignoreRobot;

        // ---- 譲りの記録（yields.csv・2026-10-23）----
        float _yieldStartS = -1f;
        Vector3 _yieldAt;
        ServingRobotController _yieldTo;
        bool _yieldStepped;
        List<Vector3> _sideCandidates;

        /// <summary>よけている最中か（GuestAgent・StaffAgent が「着いた」と間違えないように読む）</summary>
        public bool Intervening { get { return _mode != Mode.Clear; } }

        /// <summary>
        /// 譲る順番と、横へどく距離の決まり（RobotSpec の1か所）。
        /// 相手のロボットの .asset から読む。無ければ null（新しい決まりを使わない）
        /// </summary>
        static RobotSpec RulesOf(ServingRobotController r)
        {
            return r != null ? r.spec : null;
        }

        /// <summary>この人の譲る順番（0=ロボット・1=スタッフ・2=客）</summary>
        int MyRank
        {
            get { return RobotSpec.RankOf(_body != null ? _body.kind : PersonKind.Guest); }
        }

        /// <summary>この人は店員か（店員優先の版で、待ち方を分ける）</summary>
        bool IsStaff
        {
            get
            {
                if (_body == null) _body = GetComponent<PersonBody>();
                return _body != null && _body.kind == PersonKind.Staff;
            }
        }

        /// <summary>店員優先の版で回しているか</summary>
        bool StaffFirst { get { return _spec != null && _spec.staffFirstYield; } }

        /// <summary>
        /// 待つのをやめる秒数。店員優先の版では
        ///   店員 … 諦めない（相手が止まったままのときだけ、下の逃げ道で回り道する）
        ///   客   … staffFirstGuestStopSec（5 秒）
        /// それ以外は、これまでどおり robotYieldMaxSec（20 秒）
        /// </summary>
        float WaitLimitSec
        {
            get
            {
                if (!StaffFirst) return _spec.robotYieldMaxSec;
                return IsStaff ? float.MaxValue : _spec.staffFirstGuestStopSec;
            }
        }

        /// <summary>
        /// 相手のロボットが別の理由で止まったまま staffFirstRobotStuckSec（60 秒）たったか。
        /// 店員優先の版の逃げ道（回り道する）
        /// </summary>
        bool RobotStuckTooLong(ServingRobotController r)
        {
            if (!StaffFirst || !IsStaff || r == null) return false;
            if (_spec.staffFirstRobotStuckSec <= 0f) return false;
            return r.StoppedNowS >= _spec.staffFirstRobotStuckSec;
        }

        /// <summary>よける相手として見るか（ignoreRobot だけは見ない）</summary>
        bool Watches(ServingRobotController r)
        {
            return r != null && r != ignoreRobot;
        }

        /// <summary>いまのよけ方（字幕が読む）</summary>
        public Mode CurrentMode { get { return _mode; } }

        /// <summary>この行動では1〜3を守らない人か（字幕と記録が読む）</summary>
        public bool Careless { get { return _careless; } }

        /// <summary>よけ始めてからの秒数（字幕が読む）</summary>
        public float ModeSeconds { get { return _modeS; } }

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

        /// <summary>置いたあとに呼ぶ。人を出す側（PeopleSpawner・GroupManager）が入れる</summary>
        public void Configure(PeopleSpec spec, StoreBuilder store, ServiceOps ops, int seed)
        {
            _spec = spec;
            _store = store;
            _ops = ops;
            if (_rng == null) _rng = new System.Random(seed);
            if (_agent == null) _agent = GetComponent<NavMeshAgent>();
            if (_body == null) _body = GetComponent<PersonBody>();
            if (_path == null) _path = new NavMeshPath();
            _yieldStartS = -1f;        // 使い回しの人で、前の譲りが残らないように
            _yieldTo = null;
            _yieldStepped = false;
            Release();
            _hasOwnerDest = false;
            _contactOpen = false;
            _squeeze = false;
        }

        void Update()
        {
            if (_spec == null || _agent == null || _store == null) return;
            if (!Application.isPlaying) return;
            if (!_agent.isActiveAndEnabled || !_agent.isOnNavMesh) return;

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

            TrackOwnerDestination();

            // 近づきすぎは、不注意の人もふくめて数える（記録は止めない）
            LogContactIfClose();

            if (!_spec.robotYieldEnabled || _careless)
            {
                if (Intervening) Release();
                return;
            }

            _modeS += dt;
            switch (_mode)
            {
                case Mode.Clear: UpdateClear(); break;
                case Mode.WaitAtEntrance: UpdateWait(dt); break;
                case Mode.Detour: UpdateDetour(); break;
                case Mode.BackOff: UpdateBackOff(dt); break;
                case Mode.StepAside: UpdateStepAside(dt); break;
            }
        }

        // ---------------- 行動の切り替わりを見る（不注意のくじを引き直す）----------------

        /// <summary>
        /// 持ち主（GuestAgent・StaffAgent）が行き先を変えたら「次の行動」とみなし、
        /// 不注意かどうかを引き直す（1回の行動につき robotCarelessChance）。
        /// </summary>
        void TrackOwnerDestination()
        {
            if (Intervening) return;        // よけている最中は自分が行き先を持っている

            Vector3 d = _agent.destination;
            if (_hasOwnerDest && (d - _ownerDest).sqrMagnitude < 0.01f) return;

            _ownerDest = d;
            _hasOwnerDest = true;
            _squeeze = false;      // 次の行動では、もういちどふつうに空ける
            _careless = _rng != null && _rng.NextDouble() < _spec.robotCarelessChance;
        }

        // ---------------- ふつうに歩いているとき ----------------

        void UpdateClear()
        {
            // 0. 譲る順番（2026-10-23）。スタッフはロボットに必ず譲る。
            // 細い通路の中だけでなく **どこでも** 見る
            // （受け渡し口と客席をつなぐ角は通路の外なので、前の決まりでは何も起きず溜まっていた）
            ServingRobotController near;
            if (ShouldStepAsideFor(out near)) { BeginStepAside(near); return; }

            int here = NarrowAisleAt(transform.position, 0f);
            if (here >= 0)
            {
                // 2. 細い通路の中。向かい合って近づいたら下がる。
                // 通路の中では横に押さない（壁に押し付けることになる）
                ServingRobotController r = FacingRobotInAisle(here);
                if (r != null && GapTo(r) < _spec.robotFaceToFaceM) BeginBackOff(here, r);
                return;
            }

            // 3. 広い通路では、ロボットの外周から少し空ける
            KeepClear();

            // 1. これから入る細い通路にロボットがいたら、入らない
            int next = AisleAboutToEnter();
            if (next < 0) return;
            if (RobotInAisle(next) == null) return;

            if (TryDetour(next)) return;
            BeginWait(next);
        }

        /// <summary>
        /// 3. 広い通路ですれ違うとき、ロボットの外周から robotKeepClearM 空ける。
        /// NavMeshAgent.Move で横にずらすだけなので、経路も NavMesh も変えない。
        /// </summary>
        void KeepClear()
        {
            ServingRobotController r = NearestRobot();
            if (r == null) return;

            // 客が「横を詰めて通る」と決めたら、空けるすき間を小さくする（店員優先の版）
            float keep = _squeeze && StaffFirst ? _spec.staffFirstSqueezeGapM : _spec.robotKeepClearM;
            float need = _spec.personRadius + RobotRadius(r) + keep;
            Vector3 d = transform.position - r.transform.position;
            d.y = 0f;
            float dist = d.magnitude;
            if (dist >= need || dist < 0.0001f) return;

            float step = Mathf.Min(_spec.robotNudgeSpeedMps * Time.deltaTime, need - dist);
            _agent.Move(d / dist * step);
        }

        // ---------------- 0. 譲る順番で、道から横へどく（2026-10-23）----------------

        /// <summary>
        /// いま横へどくべきか。相手のロボットを返す。
        ///
        /// 決まり（RobotSpec の1か所）:
        ///   ・譲る順番は ロボット＞スタッフ＞客。順番が後ろなら譲る
        ///   ・いまは **スタッフだけ** この新しいどき方をする。
        ///     客は今までどおり店員優先の版のまま（長くて5秒止まり、回り道か横を詰めて通る）
        ///   ・立ち往生を助けに行くスタッフ（ignoreRobot）は、その台には譲らない（Watches で外す）
        ///
        /// 譲る条件は **次の両方**（v6b。2026-10-23）:
        ///   1. ロボットが**動いていて**、自分がその「これから進む道」
        ///      （サイトの routes の線の、ロボットの今の位置から先 yieldPathAheadM＝2.0 m 分）から
        ///      横に yieldSideClearM＋体の半径（0.6＋0.2 m）以内にいる
        ///   2. ロボットが自分の方へ**近づいている**
        ///
        /// 離れていくロボット・止まっているロボットには譲らない
        /// （止まっている台は、今までどおり「よけて通る物」）。
        ///
        /// v6（cs:138）は「外周から 1.5 m に近づいたら譲る」だったため、ただのすれ違いでも
        /// 譲ってしまい、③の回（2227）でスタッフが 13,173 回・合計 19,535 秒（約5.4時間）
        /// 譲った。そのうち 9 割以上は 0.2 秒未満の「近くにいただけ」で、
        /// 案内・片付けが遅れて入店待ちが 4.3 分 → 11.4 分に伸びていた。
        /// </summary>
        bool ShouldStepAsideFor(out ServingRobotController robot)
        {
            robot = null;
            if (!IsStaff) return false;                      // 客は 4 のとおり今までどおり
            if (MyRank <= RobotSpec.RankRobot) return false; // 順番が前なら譲らない

            IReadOnlyList<ServingRobotController> all = ServingRobotController.All;
            for (int i = 0; i < all.Count; i++)
            {
                ServingRobotController r = all[i];
                if (!Watches(r)) continue;

                RobotSpec rules = RulesOf(r);
                if (rules == null || !rules.yieldOrderEnabled) continue;

                if (!InRobotPathAhead(r, rules, 0f, 0f)) continue;
                if (!ApproachingMe(r, rules)) continue;

                robot = r;
                return true;
            }
            return false;
        }

        /// <summary>
        /// 自分が、そのロボットの「これから進む道」の上にいるか（v6b）。
        /// extraClearM・extraAheadM は、いちど譲り始めたあとに
        /// すぐ解けないようにする余裕（ゆるめる分）。
        /// </summary>
        bool InRobotPathAhead(ServingRobotController r, RobotSpec rules,
                              float extraClearM, float extraAheadM)
        {
            if (r == null || rules == null) return false;

            if (_pathAhead == null) _pathAhead = new List<Vector3>();
            r.PathAhead(rules.yieldPathAheadM + extraAheadM, _pathAhead);

            float clear = rules.yieldSideClearM + _spec.personRadius + extraClearM;
            return YieldSide.LateralDistanceToPath(_pathAhead, transform.position) <= clear;
        }

        /// <summary>
        /// そのロボットが動いていて、自分の方へ近づいているか（v6b）。
        /// 止まっている台・離れていく台には譲らない
        /// </summary>
        bool ApproachingMe(ServingRobotController r, RobotSpec rules)
        {
            if (r == null || rules == null) return false;

            // 動いていない台には譲らない（今までどおり「よけて通る物」）
            float eps = r.spec != null ? r.spec.stoppedSpeedEpsilon : 0.02f;
            if (r.SpeedMps <= eps) return false;

            Vector3 toMe = Flat(transform.position - r.transform.position);
            if (toMe.sqrMagnitude < 0.0001f) return true;    // 重なっている

            Vector3 going = Flat(r.transform.forward);
            if (going.sqrMagnitude < 0.0001f) return false;
            return Vector3.Dot(going.normalized, toMe.normalized) > 0f;
        }

        List<Vector3> _pathAhead;

        /// <summary>
        /// 横へどいて待ち始める。
        /// 行き先は、相手の道（サイトの routes の線）から yieldSideClearM（0.6 m）以上
        /// 横へどいた、通れる床の点。無ければ来た道を戻って通路の外で待つ。
        /// </summary>
        void BeginStepAside(ServingRobotController r)
        {
            RobotSpec rules = RulesOf(r);
            if (rules == null) return;

            _mode = Mode.StepAside;
            _modeS = 0f;
            _aisle = NarrowAisleAt(transform.position, 0f);

            Vector3 goal;
            bool stepped = TrySidePoint(r, rules, out goal);
            if (!stepped) goal = BackAwayPoint(r, rules);

            _goal = goal;
            _yieldStepped = stepped;
            BeginYieldRecord(r);
            if (_ops != null) _ops.AddRobotYieldWait();
            PeopleNav.Go(_agent, _goal);
        }

        /// <summary>
        /// 相手の道から横へ yieldSideClearM 以上どいた、通れる床の点。
        /// 近い順に試し、NavMesh へ寄せたあとも横の距離が足りているものだけを使う
        /// </summary>
        bool TrySidePoint(ServingRobotController r, RobotSpec rules, out Vector3 point)
        {
            point = transform.position;

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

            if (_sideCandidates == null) _sideCandidates = new List<Vector3>();
            float clear = rules.yieldSideClearM + _spec.personRadius;
            YieldSide.Candidates(a, b, transform.position, clear,
                                 rules.yieldSideSearchM, rules.yieldSideStepM, _sideCandidates);

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

        /// <summary>
        /// 横へどく点が無かったとき：来た道を戻って通路の外で待つ。
        /// 細い通路の中なら、その通路のロボットと反対側の出口の外へ。
        /// 通路の外なら、自分の来た向きへ下がって、相手の道から離れた点へ
        /// </summary>
        Vector3 BackAwayPoint(ServingRobotController r, RobotSpec rules)
        {
            Vector3 rp = r.transform.position; rp.y = 0f;

            if (_aisle >= 0) return Snap(AisleExitAwayFrom(_aisle, rp));

            Vector3 a, b;
            bool hasLine = r.CurrentLegSegment(out a, out b);
            float clear = rules.yieldSideClearM + _spec.personRadius;

            // 来た向き（進む向きの逆）へ、刻みながら下がる
            Vector3 ahead = Flat(_agent.desiredVelocity);
            if (ahead.sqrMagnitude < 0.0004f) ahead = Flat(transform.forward);
            if (ahead.sqrMagnitude < 0.0004f) ahead = Flat(transform.position - rp);
            if (ahead.sqrMagnitude < 0.0004f) return transform.position;
            Vector3 back = -ahead.normalized;

            float step = Mathf.Max(0.05f, rules.yieldSideStepM);
            Vector3 best = transform.position;
            NavMeshHit hit;
            for (float d = step; d <= rules.yieldSideSearchM + 0.0001f; d += step)
            {
                Vector3 p = transform.position + back * d;
                if (!NavMesh.SamplePosition(p, out hit, step, PeopleNav.Filter)) continue;
                best = hit.position;
                if (!hasLine) continue;
                if (YieldSide.FarEnough(a, b, hit.position, clear)) return hit.position;
            }
            return best;
        }

        void UpdateStepAside(float dt)
        {
            if (_ops != null) _ops.AddRobotYieldWaitSeconds(dt);

            ServingRobotController r = _yieldTo;
            if (!Watches(r)) { Release(true); return; }

            RobotSpec rules = RulesOf(r);
            if (rules == null) { Release(true); return; }

            // 通り過ぎたか（v6b）。譲り始めた見方と同じで、
            // 「これから進む道の上から外れた」か「もう近づいてこない」で終わる。
            // すぐ解けて入り直すのを防ぐため、robotPassedClearM だけゆるめて見る
            bool stillInPath = InRobotPathAhead(r, rules,
                                                _spec.robotPassedClearM, _spec.robotPassedClearM);
            if (!stillInPath || !ApproachingMe(r, rules)) { Release(true); return; }

            // 相手が別の理由で止まったままなら、店員も回り道する（前からの逃げ道）
            if (RobotStuckTooLong(r))
            {
                if (_ops != null) _ops.AddRobotStuckDetour();
                int blocked = _aisle;
                Release(false);
                if (blocked >= 0) TryDetour(blocked);
                return;
            }

            if (rules.yieldSideWaitMaxSec > 0f && _modeS >= rules.yieldSideWaitMaxSec) Release(false);
        }

        // ---------------- 譲りの記録（yields.csv）----------------

        void BeginYieldRecord(ServingRobotController r)
        {
            _yieldStartS = _ops != null ? _ops.NowS : 0f;
            _yieldAt = transform.position;
            _yieldTo = r;
        }

        /// <summary>譲り終わったので1行残す。passed は相手が通り過ぎて終わったか</summary>
        void EndYieldRecord(bool passed)
        {
            if (_yieldStartS < 0f) { _yieldTo = null; return; }

            if (_ops != null)
                _ops.AddYield(_yieldStartS, _yieldAt, WhoName(), MyRank,
                              _yieldTo != null ? "ロボット" + _yieldTo.robotId + "号機" : "",
                              _modeS, _yieldStepped, passed);

            _yieldStartS = -1f;
            _yieldTo = null;
            _yieldStepped = false;
        }

        // ---------------- 1. 入口で待つ ----------------

        void BeginWait(int aisle)
        {
            _aisle = aisle;
            _mode = Mode.WaitAtEntrance;
            _modeS = 0f;
            _goal = SideStepPoint(aisle);
            _yieldStepped = false;                  // 入口で横にずれるだけ（道から 0.6 m ではない）
            BeginYieldRecord(RobotInAisle(aisle));
            if (_ops != null) _ops.AddRobotYieldWait();
            PeopleNav.Go(_agent, _goal);
        }

        void UpdateWait(float dt)
        {
            if (_ops != null) _ops.AddRobotYieldWaitSeconds(dt);

            ServingRobotController r = RobotInAisle(_aisle);
            if (r == null) { Release(); return; }

            // 店員優先の版の逃げ道：相手が別の理由で止まったままなら、店員も回り道する
            if (RobotStuckTooLong(r))
            {
                if (_ops != null) _ops.AddRobotStuckDetour();
                int blocked = _aisle;
                Release(false);
                if (!TryDetour(blocked)) _squeeze = true;   // 回り道が無ければ、詰めて通る
                return;
            }

            if (_modeS >= WaitLimitSec)
            {
                // 待ちすぎた。客は回り道、無ければ横を詰めて通る（店員優先の版）
                int blocked = _aisle;
                Release(false);
                if (StaffFirst && !IsStaff && !TryDetour(blocked)) _squeeze = true;
            }
        }

        // ---------------- 1. 回り道 ----------------

        /// <summary>
        /// ふさがれた通路のかわりに、別の細い通路を通って行けるか。
        /// 遠回りが robotDetourExtraMaxM 以内で、その通路にロボットがいなければ通る。
        /// </summary>
        bool TryDetour(int blocked)
        {
            if (!_hasOwnerDest) return false;
            if (_spec.robotDetourExtraMaxM <= 0f) return false;

            float direct = PathLength(transform.position, _ownerDest);
            if (direct < 0f) return false;

            IList<StoreBuilder.AisleInfo> aisles = _store.Aisles;
            int best = -1;
            float bestExtra = float.MaxValue;
            Vector3 bestPoint = Vector3.zero;

            for (int i = 0; i < aisles.Count; i++)
            {
                if (i == blocked || !aisles[i].narrow) continue;
                if (RobotInAisle(i) != null) continue;

                Vector3 via = new Vector3(aisles[i].zone.center.x, 0f, aisles[i].zone.center.z);
                float a = PathLength(transform.position, via);
                if (a < 0f) continue;
                float b = PathLength(via, _ownerDest);
                if (b < 0f) continue;

                float extra = a + b - direct;
                if (extra > _spec.robotDetourExtraMaxM) continue;
                if (extra >= bestExtra) continue;
                best = i; bestExtra = extra; bestPoint = via;
            }

            if (best < 0) return false;

            _aisle = best;
            _mode = Mode.Detour;
            _modeS = 0f;
            _goal = bestPoint;
            _yieldStepped = false;                  // 回り道（どくのではなく別の道を通る）
            BeginYieldRecord(RobotInAisle(blocked));
            if (_ops != null) _ops.AddRobotYieldDetour();
            PeopleNav.Go(_agent, _goal);
            return true;
        }

        void UpdateDetour()
        {
            // 回り道の通路にロボットが入ってきたら、やめて考え直す
            if (RobotInAisle(_aisle) != null) { Release(false); return; }
            if (_modeS >= Mathf.Min(WaitLimitSec, _spec.robotYieldMaxSec)) { Release(false); return; }

            Vector3 a = transform.position; a.y = 0f;
            Vector3 b = _goal; b.y = 0f;
            if (Vector3.Distance(a, b) <= _spec.stoppingDistance + 0.1f) Release();
        }

        // ---------------- 2. 通路の中で下がってよける ----------------

        void BeginBackOff(int aisle, ServingRobotController r)
        {
            _aisle = aisle;
            _mode = Mode.BackOff;
            _modeS = 0f;
            _goal = FindRefuge(aisle, r);
            _yieldStepped = false;                  // よけ場所へ下がる（道から 0.6 m ではない）
            BeginYieldRecord(r);
            if (_ops != null) _ops.AddRobotYieldWait();
            PeopleNav.Go(_agent, _goal);
        }

        void UpdateBackOff(float dt)
        {
            if (_ops != null) _ops.AddRobotYieldWaitSeconds(dt);

            ServingRobotController r = NearestRobotInAisle(_aisle);
            if (r == null) { Release(); return; }

            // 通り過ぎたか（よけ場所から離れていったか）
            if (GapTo(r) > _spec.robotFaceToFaceM + _spec.robotPassedClearM) { Release(); return; }

            // 店員優先の版の逃げ道：相手が別の理由で止まったままなら、店員も回り道する
            if (RobotStuckTooLong(r))
            {
                if (_ops != null) _ops.AddRobotStuckDetour();
                int blocked = _aisle;
                Release(false);
                if (!TryDetour(blocked)) _squeeze = true;
                return;
            }

            if (_modeS >= WaitLimitSec)
            {
                int blocked = _aisle;
                Release(false);
                if (StaffFirst && !IsStaff && !TryDetour(blocked)) _squeeze = true;
            }
        }

        /// <summary>
        /// 下がる先。壁ぞいの卓のすき間など、店が持っている「よけ場所」のうち
        /// いちばん近いもの。無ければ、その通路のロボットと反対側の出口。
        /// ロボットの側（いまより近づく方向）には行かない。
        /// </summary>
        Vector3 FindRefuge(int aisle, ServingRobotController r)
        {
            Vector3 me = transform.position; me.y = 0f;
            Vector3 rp = r.transform.position; rp.y = 0f;
            float toRobot = Vector3.Distance(me, rp);

            IList<Vector3> refuges = _store.Refuges;
            Vector3 best = Vector3.zero;
            float bestD = float.MaxValue;
            for (int i = 0; i < refuges.Count; i++)
            {
                Vector3 p = refuges[i]; p.y = 0f;
                float d = Vector3.Distance(me, p);
                if (d > _spec.robotBackOffSearchM) continue;
                if (Vector3.Distance(p, rp) < toRobot) continue;    // ロボットの側へは下がらない
                if (d >= bestD) continue;
                best = refuges[i]; bestD = d;
            }
            if (bestD < float.MaxValue) return Snap(best);

            return Snap(AisleExitAwayFrom(aisle, rp));
        }

        /// <summary>その通路の、ロボットと反対側の出口のすぐ外</summary>
        Vector3 AisleExitAwayFrom(int aisle, Vector3 robotPos)
        {
            StoreBuilder.AisleInfo a = _store.Aisles[aisle];
            Bounds z = a.zone;
            bool northSouth = z.size.z >= z.size.x;
            float out1 = _spec.robotSideStepM;

            if (northSouth)
            {
                bool goNorth = robotPos.z < z.center.z;
                float zz = goNorth ? z.max.z + out1 : z.min.z - out1;
                float sign = transform.position.x >= z.center.x ? 1f : -1f;
                return new Vector3(z.center.x + sign * out1, 0f, zz);
            }

            bool goEast = robotPos.x < z.center.x;
            float xx = goEast ? z.max.x + out1 : z.min.x - out1;
            float signZ = transform.position.z >= z.center.z ? 1f : -1f;
            return new Vector3(xx, 0f, z.center.z + signZ * out1);
        }

        /// <summary>通路の入口で、ロボットの通り道からはずれた場所</summary>
        Vector3 SideStepPoint(int aisle)
        {
            Bounds z = _store.Aisles[aisle].zone;
            bool northSouth = z.size.z >= z.size.x;
            Vector3 p = transform.position;

            if (northSouth)
            {
                float d = p.x - z.center.x;
                float sign = Mathf.Abs(d) < 0.01f ? 1f : Mathf.Sign(d);
                p += new Vector3(sign * _spec.robotSideStepM, 0f, 0f);
            }
            else
            {
                float d = p.z - z.center.z;
                float sign = Mathf.Abs(d) < 0.01f ? 1f : Mathf.Sign(d);
                p += new Vector3(0f, 0f, sign * _spec.robotSideStepM);
            }
            return Snap(p);
        }

        /// <summary>よけるのをやめて、持ち主の行き先へ戻る（相手が通り過ぎたとき）</summary>
        void Release()
        {
            Release(true);
        }

        /// <summary>
        /// よけるのをやめて、持ち主の行き先へ戻る。
        /// passed は相手が通り過ぎて終わったか（false なら待ちすぎてやめた）。
        /// 譲っていたときは yields.csv に1行残す
        /// </summary>
        void Release(bool passed)
        {
            EndYieldRecord(passed);
            _mode = Mode.Clear;
            _modeS = 0f;
            _aisle = -1;
            if (_hasOwnerDest) PeopleNav.Go(_agent, _ownerDest);
        }

        // ---------------- 通路とロボットを見る ----------------

        /// <summary>その点が入っている細い通路。無ければ -1</summary>
        int NarrowAisleAt(Vector3 p, float margin)
        {
            IList<StoreBuilder.AisleInfo> aisles = _store.Aisles;
            for (int i = 0; i < aisles.Count; i++)
            {
                if (!aisles[i].narrow) continue;
                if (ContainsXZ(aisles[i].zone, p, margin)) return i;
            }
            return -1;
        }

        /// <summary>
        /// これから入る細い通路。いまの経路を少し先まで見て、
        /// 「いま入っていない細い通路」に入るならその番号を返す。
        /// </summary>
        int AisleAboutToEnter()
        {
            if (_agent.pathPending) return -1;
            Vector3[] corners = _agent.path != null ? _agent.path.corners : null;
            if (corners == null || corners.Length == 0) return -1;

            float budget = _spec.robotAisleLookAheadM;
            const float Step = 0.25f;
            Vector3 from = transform.position;

            for (int i = 0; i < corners.Length && budget > 0f; i++)
            {
                Vector3 to = corners[i];
                Vector3 d = to - from; d.y = 0f;
                float len = d.magnitude;
                if (len > 0.0001f)
                {
                    Vector3 dir = d / len;
                    for (float t = Step; t <= Mathf.Min(len, budget); t += Step)
                    {
                        int a = NarrowAisleAt(from + dir * t, 0f);
                        if (a >= 0 && !ContainsXZ(_store.Aisles[a].zone, transform.position, 0f)) return a;
                    }
                }
                budget -= len;
                from = to;
            }
            return -1;
        }

        /// <summary>その通路の中にいるロボット（向かってくる・止まっているを問わない）</summary>
        ServingRobotController RobotInAisle(int aisle)
        {
            if (aisle < 0 || aisle >= _store.Aisles.Count) return null;
            Bounds z = _store.Aisles[aisle].zone;

            IReadOnlyList<ServingRobotController> all = ServingRobotController.All;
            for (int i = 0; i < all.Count; i++)
            {
                ServingRobotController r = all[i];
                if (!Watches(r)) continue;
                if (ContainsXZ(z, r.transform.position, _spec.robotAisleMarginM)) return r;
            }
            return null;
        }

        ServingRobotController NearestRobotInAisle(int aisle)
        {
            if (aisle < 0 || aisle >= _store.Aisles.Count) return null;
            Bounds z = _store.Aisles[aisle].zone;

            ServingRobotController best = null;
            float bestD = float.MaxValue;
            IReadOnlyList<ServingRobotController> all = ServingRobotController.All;
            for (int i = 0; i < all.Count; i++)
            {
                ServingRobotController r = all[i];
                if (!Watches(r)) continue;
                if (!ContainsXZ(z, r.transform.position, _spec.robotAisleMarginM)) continue;
                float d = Flat(r.transform.position - transform.position).magnitude;
                if (d >= bestD) continue;
                best = r; bestD = d;
            }
            return best;
        }

        /// <summary>その通路の中で、自分の進む先にいるロボット（向かい合っている）</summary>
        ServingRobotController FacingRobotInAisle(int aisle)
        {
            ServingRobotController r = NearestRobotInAisle(aisle);
            if (r == null) return null;

            Vector3 ahead = Flat(_agent.desiredVelocity);
            if (ahead.sqrMagnitude < 0.0004f) ahead = Flat(transform.forward);
            if (ahead.sqrMagnitude < 0.0004f) return r;      // 向きが分からないときは相手とみなす

            Vector3 toRobot = Flat(r.transform.position - transform.position);
            return Vector3.Dot(ahead.normalized, toRobot.normalized) > 0f ? r : null;
        }

        ServingRobotController NearestRobot()
        {
            ServingRobotController best = null;
            float bestD = float.MaxValue;
            IReadOnlyList<ServingRobotController> all = ServingRobotController.All;
            for (int i = 0; i < all.Count; i++)
            {
                ServingRobotController r = all[i];
                if (!Watches(r)) continue;
                float d = Flat(r.transform.position - transform.position).magnitude;
                if (d >= bestD) continue;
                best = r; bestD = d;
            }
            return best;
        }

        /// <summary>ロボットの外周から自分の外周までの空き（m）。負なら重なっている</summary>
        float GapTo(ServingRobotController r)
        {
            float d = Flat(r.transform.position - transform.position).magnitude;
            return d - RobotRadius(r) - _spec.personRadius;
        }

        float RobotRadius(ServingRobotController r)
        {
            return r != null && r.spec != null ? r.spec.Radius : 0.25f;
        }

        // ---------------- 近づきすぎの記録 ----------------

        /// <summary>
        /// ロボットの外周 robotContactGapM 以内に近づいた回数を残す（接触に近い）。
        /// 1回の近づきで1行。いちど離れるまで同じ近づきを数えない。
        /// </summary>
        void LogContactIfClose()
        {
            ServingRobotController r = NearestRobot();
            if (r == null) { _contactOpen = false; return; }

            float gap = GapTo(r);
            if (!_contactOpen && gap <= _spec.robotContactGapM)
            {
                _contactOpen = true;
                if (_ops != null)
                    _ops.AddRobotContact(transform.position, WhoName(), _careless, r.robotId, gap);
            }
            else if (_contactOpen && gap > _spec.robotContactGapM + _spec.robotKeepClearM)
            {
                _contactOpen = false;
            }
        }

        string WhoName()
        {
            string kind = _body != null ? _body.KindName() : "人";
            return kind + ":" + name;
        }

        // ---------------- 小さな道具 ----------------

        float PathLength(Vector3 from, Vector3 to)
        {
            if (_path == null) _path = new NavMeshPath();

            NavMeshQueryFilter f = PeopleNav.Filter;
            NavMeshHit hit;
            Vector3 a = from, b = to;
            if (NavMesh.SamplePosition(a, out hit, 1.5f, f)) a = hit.position;
            if (NavMesh.SamplePosition(b, out hit, 1.5f, f)) b = hit.position;

            if (!NavMesh.CalculatePath(a, b, f, _path)) return -1f;
            if (_path.status != NavMeshPathStatus.PathComplete) return -1f;

            float len = 0f;
            Vector3[] c = _path.corners;
            for (int i = 1; i < c.Length; i++) len += Vector3.Distance(c[i - 1], c[i]);
            return len;
        }

        static Vector3 Snap(Vector3 p)
        {
            NavMeshHit hit;
            if (NavMesh.SamplePosition(p, out hit, 1.5f, PeopleNav.Filter)) return hit.position;
            return p;
        }

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

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