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

namespace ServiceSim
{
    /// <summary>
    /// 人がロボットをよける。スタッフと歩く客に付ける。
    ///
    /// 実際の店で、人はロボットに自分から当たりに行かない。いまは人がロボットを
    /// 気にせず歩き、細い通路でロボットを両側からはさんで押し合う場面が出ていたので、
    /// 人の側がよけるようにする。
    ///
    ///   1. 先を見て待つ・回り道する：細い通路に入る前に、その通路の中にロボットがいたら
    ///      入らない。回り道で行けて遠回りが少しなら回り道、なければ通路の入口の
    ///      ロボットの通り道からはずれた場所で待つ
    ///   2. 向かい合ったら下がる：細い通路の中で向かい合って近づいたら、
    ///      近くの交差点か、壁ぞいの卓のすき間（よけ場所）に下がって、通り過ぎてから進む
    ///   3. 間を空ける：広い通路ですれ違うときは、ロボットの外周から少し空ける
    ///   4. まれな不注意：1回の行動につき決めた割合だけ、上を守らない人を混ぜる
    ///
    /// ロボットのふるまいは変えない（NavMesh も経路も触らない）。
    /// この script は人だけを扱う。数字はすべて PeopleSpec の .asset に置く。
    /// </summary>
    [RequireComponent(typeof(NavMeshAgent))]
    public class PersonRobotYield : MonoBehaviour
    {
        /// <summary>いまのよけ方</summary>
        public enum Mode
        {
            Clear,            // ふつうに歩いている
            WaitAtEntrance,   // 細い通路の入口で待っている
            Detour,           // 回り道している
            BackOff           // 通路の中で下がってよけている
        }

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

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

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

        /// <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();
            Release();
            _hasOwnerDest = false;
            _contactOpen = 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;
            }
        }

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

        /// <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;
            _careless = _rng != null && _rng.NextDouble() < _spec.robotCarelessChance;
        }

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

        void UpdateClear()
        {
            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 need = _spec.personRadius + RobotRadius(r) + _spec.robotKeepClearM;
            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);
        }

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

        void BeginWait(int aisle)
        {
            _aisle = aisle;
            _mode = Mode.WaitAtEntrance;
            _modeS = 0f;
            _goal = SideStepPoint(aisle);
            if (_ops != null) _ops.AddRobotYieldWait();
            PeopleNav.Go(_agent, _goal);
        }

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

            if (RobotInAisle(_aisle) == null) { Release(); return; }
            if (_modeS >= _spec.robotYieldMaxSec) { Release(); return; }   // 待ちすぎたら進む
        }

        // ---------------- 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;
            if (_ops != null) _ops.AddRobotYieldDetour();
            PeopleNav.Go(_agent, _goal);
            return true;
        }

        void UpdateDetour()
        {
            // 回り道の通路にロボットが入ってきたら、やめて考え直す
            if (RobotInAisle(_aisle) != null) { Release(); return; }
            if (_modeS >= _spec.robotYieldMaxSec) { Release(); 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);
            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 (_modeS >= _spec.robotYieldMaxSec) { Release(); return; }
        }

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

        void Release()
        {
            _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;
        }
    }
}