using System.Collections.Generic;
using UnityEngine;

namespace ServiceSim
{
    /// <summary>
    /// すれ違い1回ぶん（passes.csv に1行。依頼 v13 の 依頼1）。
    /// 同じ2台の1回は1行（2台で2行にしない）。
    /// </summary>
    public struct PassRow
    {
        /// <summary>「すれ違い中」に入った・終わった店の中の時刻（秒）</summary>
        public float startS, endS;

        /// <summary>すれ違った2台（号機の小さい方を a にする）</summary>
        public int robotA, robotB;

        /// <summary>いちばん近づいたときの、2台の真ん中の位置</summary>
        public float x, z;

        /// <summary>いちばん近づいたときの体どうしの間（m）。中心どうしの距離 − 体の幅</summary>
        public float minGapM;

        /// <summary>そのあいだで、2台のうちいちばん遅かった速さ（m/秒）</summary>
        public float minSpeedMps;

        /// <summary>
        /// 終わり方。
        ///   すれ違えた … 止まらずに通り過ぎた
        ///   幅が足りず今の決まりへ … 寄っても体の幅×2＋間 に足りず、譲る順番の決まりに入った
        ///   向かい合って止まった … そのあいだに2台とも1秒以上止まった
        /// </summary>
        public string outcome;

        public const string OutcomePassed = "すれ違えた";
        public const string OutcomeNarrow = "幅が足りず今の決まりへ";
        public const string OutcomeFaceStop = "向かい合って止まった";
    }

    /// <summary>
    /// ロボットどうしのすれ違いの **形の計算**（依頼 v13 の 依頼1）。
    ///
    /// 実店舗の動画では、空の2台が狭い所で2台とも速さを落とし、止まらずに
    /// 体どうしの間を詰めてすれ違っていた。ここはその判定だけを置く。
    /// 動かす所は ServingRobotController、数える所は PassByWatch。
    ///
    /// 通れる床かどうかは呼ぶ側が確かめる（人とロボットで見る区画が違うため）。
    /// 寄れるかどうかは **NavMesh**（体の半径 0.25 m で焼いてある）で見る。
    /// 地図の free_class は「中心を置ける所」を道の幅の半分で削ったものなので、
    /// 1.2 m の通路でも帯が 0.45 m しかなく、すれ違いの判定には使えない。
    /// </summary>
    public static class RobotPassBy
    {
        /// <summary>「向かい合っている」と見る向きの差（度）。依頼 v13 の 135 度</summary>
        public const float FaceDeg = 135f;

        /// <summary>向かい合って止まったと数える速さ（m/秒。依頼 v13）</summary>
        public const float FaceStopSpeedMps = 0.05f;

        /// <summary>向かい合って止まったと数えるまでの長さ（秒。依頼 v13）</summary>
        public const float FaceStopMinS = 1.0f;

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

        /// <summary>進む向き（止まっていても transform の向きを使う）</summary>
        static Vector3 Heading(ServingRobotController r)
        {
            Vector3 f = Flat(r.transform.forward);
            return f.sqrMagnitude < 0.000001f ? Vector3.forward : f.normalized;
        }

        /// <summary>中心どうしの距離（m）</summary>
        public static float CenterDistance(ServingRobotController a, ServingRobotController b)
        {
            return Vector3.Distance(Flat(a.transform.position), Flat(b.transform.position));
        }

        /// <summary>体どうしの間（m）。重なっていれば 0 より小さくなる</summary>
        public static float BodyGap(ServingRobotController a, ServingRobotController b, RobotSpec spec)
        {
            return CenterDistance(a, b) - spec.diameter;
        }

        /// <summary>進む向きの差が FaceDeg 以上か（＝こちらへ向かってくる）</summary>
        public static bool HeadingsOppose(ServingRobotController a, ServingRobotController b)
        {
            return Vector3.Dot(Heading(a), Heading(b)) <= Mathf.Cos(FaceDeg * Mathf.Deg2Rad);
        }

        /// <summary>相手が自分の前にいるか（進む向きより前）</summary>
        public static bool IsAhead(ServingRobotController me, ServingRobotController other)
        {
            Vector3 d = Flat(other.transform.position) - Flat(me.transform.position);
            if (d.sqrMagnitude < 0.000001f) return true;
            return Vector3.Dot(Heading(me), d.normalized) > 0f;
        }

        /// <summary>
        /// 「すれ違い中」に入る形か（依頼 v13）。
        /// 中心どうしが passByDistanceM 以内・向きの差が 135 度以上・互いに相手が前。
        /// 2台のどちらから見ても同じ答えになる（順番に頼らない）
        /// </summary>
        public static bool Starts(ServingRobotController a, ServingRobotController b, RobotSpec spec)
        {
            if (a == null || b == null || spec == null) return false;
            if (CenterDistance(a, b) > spec.passByDistanceM) return false;
            if (!HeadingsOppose(a, b)) return false;
            return IsAhead(a, b) && IsAhead(b, a);
        }

        /// <summary>
        /// 「すれ違い中」が続いているか（依頼 v13）。
        /// 相手が自分の後ろ側に回るか、2台の距離が passByDistanceM を超えたら終わり。
        /// 向きの差は見ない（曲がっている途中で切れないようにする）
        /// </summary>
        public static bool Continues(ServingRobotController a, ServingRobotController b, RobotSpec spec)
        {
            if (a == null || b == null || spec == null) return false;
            if (CenterDistance(a, b) > spec.passByDistanceM) return false;
            return IsAhead(a, b) && IsAhead(b, a);
        }

        /// <summary>
        /// すれ違いの **走り方** を続けるか。
        ///
        /// 数の数え方（Continues）は「相手が後ろ側に回ったら終わり」だが、
        /// 真横に並んだ瞬間に寄せを戻すと、体が離れないうちに相手側へ戻ってしまう。
        /// そこで走り方だけは、体どうしが robotYieldClearM（0.6 m）空くまで続ける
        /// </summary>
        public static bool ContinuesMoving(ServingRobotController a, ServingRobotController b,
                                          RobotSpec spec)
        {
            if (a == null || b == null || spec == null) return false;
            if (CenterDistance(a, b) > spec.passByDistanceM) return false;
            if (IsAhead(a, b) && IsAhead(b, a)) return true;
            return BodyGap(a, b, spec) < spec.robotYieldClearM;
        }

        /// <summary>
        /// 向かい合って止まっている形か（入でも切でも数える。依頼 v13）。
        /// 距離が passByDistanceM 以内・向きの差 135 度以上・2台とも 0.05 m/秒 以下
        /// </summary>
        public static bool FacingAndStopped(ServingRobotController a, ServingRobotController b,
                                            RobotSpec spec)
        {
            if (a == null || b == null || spec == null) return false;
            if (CenterDistance(a, b) > spec.passByDistanceM) return false;
            if (!HeadingsOppose(a, b)) return false;
            return a.SpeedMps <= FaceStopSpeedMps && b.SpeedMps <= FaceStopSpeedMps;
        }

        /// <summary>進む向きの右（Unity は左手系なので up × 前）</summary>
        public static Vector3 RightOf(Vector3 forward)
        {
            Vector3 f = Flat(forward);
            if (f.sqrMagnitude < 0.000001f) return Vector3.right;
            f.Normalize();
            return new Vector3(f.z, 0f, -f.x);
        }
    }

    /// <summary>
    /// すれ違いの数を数える（依頼 v13 の 依頼1「数の定義」）。
    ///
    /// ★ ここは **見るだけ**。動きには1つも効かない。
    ///   passByEnabled が切の回でも同じように数える（入の回と比べるため）。
    /// 2台組ごとに1つずつ見るので、同じ2台の1回は1回（2台で2回にしない）。
    /// ExperimentRunner が1コマごとに Tick を呼ぶ。
    /// </summary>
    public class PassByWatch
    {
        /// <summary>いま続いている、2台組ぶんの様子</summary>
        class Live
        {
            public int idA, idB;
            public bool passing;          // すれ違い中
            public float startS;
            public float minGapM;
            public float minSpeedMps;
            public Vector3 closestAt;     // いちばん近づいたときの真ん中
            public bool sawNarrow;        // 幅が足りず今の決まりに入った
            public bool sawFaceStop;      // そのあいだに向かい合って止まった

            public float faceStopS;       // 2台とも止まっている秒数
            public bool faceStopCounted;  // 1秒を超えて1回と数えたか
            public string faceStopPlace;  // 1回と数えた時の場所（2026-10-26）
            public bool faceStopOverlap;  // そのあいだ体が重なったか（間が負）
        }

        readonly List<Live> _live = new List<Live>();
        readonly List<PassRow> _rows = new List<PassRow>();

        /// <summary>すれ違いの回数（終わった分だけ数える）</summary>
        public int PassCount { get; private set; }

        /// <summary>減速していた秒数（ロボットごとの「すれ違い中」の合計）</summary>
        public float PassSlowS { get; private set; }

        /// <summary>向かい合って止まった回数と、いちばん長かった秒数（ぜんぶ合わせた数）</summary>
        public int FaceStopCount { get; private set; }
        public float FaceStopMaxS { get; private set; }

        // ---- 2026-10-26（依頼 v13 の直し③）：場所で分けて数える ----
        //
        // 受け渡し口は点が1つしかないので、2台が同じ点へ向かうと必ず向かい合って
        // 両方止まる。これは「通路でふさぎ合っている」のとは別の現象なので、
        // 混ぜずに分けて数える（2026-10-26 の確かめで、切の 28 件のうち
        // 26 件が受け渡し口だった）。
        // 体が重なった（間が負）ものも別に数える（依頼 v8b の A の再発を見張る）。

        /// <summary>受け渡し口のそばで向かい合って止まった回数・最長（荷積みの順番待ち）</summary>
        public int FaceStopAtPassCount { get; private set; }
        public float FaceStopAtPassMaxS { get; private set; }

        /// <summary>待機場所のそばで向かい合って止まった回数・最長</summary>
        public int FaceStopAtStandbyCount { get; private set; }
        public float FaceStopAtStandbyMaxS { get; private set; }

        /// <summary>
        /// 通路でふさぎ合った回数・最長（受け渡し口・待機場所のそばを除いた分）。
        /// **すれ違いの直しが効いたかは、この数で見る**
        /// </summary>
        public int FaceStopInAisleCount { get; private set; }
        public float FaceStopInAisleMaxS { get; private set; }

        /// <summary>そのうち、体が重なっていた（間が負）回数</summary>
        public int FaceStopOverlapCount { get; private set; }

        /// <summary>場所を見分けるための点（ExperimentRunner が回し始めに入れる）</summary>
        public void SetPlaces(Vector3 passPoint, IList<Vector3> standby)
        {
            _passPoint = passPoint;
            _standby.Clear();
            if (standby != null) for (int i = 0; i < standby.Count; i++) _standby.Add(standby[i]);
            _hasPlaces = true;
        }

        Vector3 _passPoint;
        readonly List<Vector3> _standby = new List<Vector3>();
        bool _hasPlaces;

        /// <summary>受け渡し口・待機場所のそばと見る半径（m）</summary>
        const float PlaceNearM = 1.2f;

        /// <summary>その場所の呼び名（受け渡し口／待機場所／通路）</summary>
        string PlaceOf(Vector3 at)
        {
            if (!_hasPlaces) return PlaceAisle;
            Vector3 p = at; p.y = 0f;
            Vector3 q = _passPoint; q.y = 0f;
            if (Vector3.Distance(p, q) <= PlaceNearM) return PlaceAtPass;
            for (int i = 0; i < _standby.Count; i++)
            {
                Vector3 r = _standby[i]; r.y = 0f;
                if (Vector3.Distance(p, r) <= PlaceNearM) return PlaceAtStandby;
            }
            return PlaceAisle;
        }

        public const string PlaceAtPass = "受け渡し口";
        public const string PlaceAtStandby = "待機場所";
        public const string PlaceAisle = "通路";

        public IReadOnlyList<PassRow> Rows { get { return _rows; } }

        public void Reset()
        {
            _live.Clear();
            _rows.Clear();
            PassCount = 0;
            PassSlowS = 0f;
            FaceStopCount = 0;
            FaceStopMaxS = 0f;
            FaceStopAtPassCount = 0; FaceStopAtPassMaxS = 0f;
            FaceStopAtStandbyCount = 0; FaceStopAtStandbyMaxS = 0f;
            FaceStopInAisleCount = 0; FaceStopInAisleMaxS = 0f;
            FaceStopOverlapCount = 0;
        }

        /// <summary>1コマぶん見る。robots の並びのまま見るので、同じ回なら同じ順番になる</summary>
        public void Tick(float dt, float nowS, IList<ServingRobotController> robots, RobotSpec spec)
        {
            if (spec == null || robots == null || dt <= 0f) return;

            // 「すれ違い中」のロボット（減速していた秒数は台ごとに足す）
            _slowNow.Clear();

            for (int i = 0; i < robots.Count; i++)
            {
                ServingRobotController a = robots[i];
                if (a == null) continue;
                for (int k = i + 1; k < robots.Count; k++)
                {
                    ServingRobotController b = robots[k];
                    if (b == null) continue;
                    TickPair(dt, nowS, a, b, spec);
                }
            }

            PassSlowS += dt * _slowNow.Count;
        }

        readonly List<int> _slowNow = new List<int>();

        void TickPair(float dt, float nowS, ServingRobotController a, ServingRobotController b,
                      RobotSpec spec)
        {
            int idA = Mathf.Min(a.robotId, b.robotId);
            int idB = Mathf.Max(a.robotId, b.robotId);
            Live L = Find(idA, idB);

            // ---- 向かい合って止まった（入でも切でも数える）----
            if (RobotPassBy.FacingAndStopped(a, b, spec))
            {
                if (L == null) L = Add(idA, idB);
                L.faceStopS += dt;

                // 2026-10-26（直し③）：体が重なったら覚えておく
                float gapNow = RobotPassBy.BodyGap(a, b, spec);
                if (gapNow < 0f) L.faceStopOverlap = true;

                if (!L.faceStopCounted && L.faceStopS >= RobotPassBy.FaceStopMinS)
                {
                    L.faceStopCounted = true;
                    FaceStopCount++;
                    // 2026-10-26（直し③）：1回と数えたときの場所で振り分ける
                    Vector3 mid = (a.transform.position + b.transform.position) * 0.5f;
                    L.faceStopPlace = PlaceOf(mid);
                    if (L.faceStopPlace == PlaceAtPass) FaceStopAtPassCount++;
                    else if (L.faceStopPlace == PlaceAtStandby) FaceStopAtStandbyCount++;
                    else FaceStopInAisleCount++;
                    if (L.faceStopOverlap) FaceStopOverlapCount++;
                    if (L.passing) L.sawFaceStop = true;
                }
                if (L.faceStopCounted)
                {
                    if (L.faceStopS > FaceStopMaxS) FaceStopMaxS = L.faceStopS;
                    if (L.faceStopPlace == PlaceAtPass)
                    { if (L.faceStopS > FaceStopAtPassMaxS) FaceStopAtPassMaxS = L.faceStopS; }
                    else if (L.faceStopPlace == PlaceAtStandby)
                    { if (L.faceStopS > FaceStopAtStandbyMaxS) FaceStopAtStandbyMaxS = L.faceStopS; }
                    else
                    { if (L.faceStopS > FaceStopInAisleMaxS) FaceStopInAisleMaxS = L.faceStopS; }
                }
            }
            else if (L != null)
            {
                L.faceStopS = 0f;
                L.faceStopCounted = false;
                L.faceStopPlace = null;
                L.faceStopOverlap = false;
            }

            // ---- すれ違い中 ----
            bool passing = L != null && L.passing
                ? RobotPassBy.Continues(a, b, spec)
                : RobotPassBy.Starts(a, b, spec);

            if (passing)
            {
                if (L == null) L = Add(idA, idB);
                if (!L.passing)
                {
                    L.passing = true;
                    L.startS = nowS;
                    L.minGapM = float.MaxValue;
                    L.minSpeedMps = float.MaxValue;
                    L.sawNarrow = false;
                    L.sawFaceStop = L.faceStopCounted;
                }

                float gap = RobotPassBy.BodyGap(a, b, spec);
                if (gap < L.minGapM)
                {
                    L.minGapM = gap;
                    L.closestAt = (a.transform.position + b.transform.position) * 0.5f;
                }
                float slower = Mathf.Min(a.SpeedMps, b.SpeedMps);
                if (slower < L.minSpeedMps) L.minSpeedMps = slower;
                if (a.PassByNarrow || b.PassByNarrow) L.sawNarrow = true;
                if (L.faceStopCounted) L.sawFaceStop = true;

                if (!_slowNow.Contains(a.robotId)) _slowNow.Add(a.robotId);
                if (!_slowNow.Contains(b.robotId)) _slowNow.Add(b.robotId);
                return;
            }

            if (L == null || !L.passing) return;

            // ---- すれ違いが終わった。1行にする ----
            PassRow r = new PassRow();
            r.startS = L.startS;
            r.endS = nowS;
            r.robotA = L.idA;
            r.robotB = L.idB;
            r.x = L.closestAt.x;
            r.z = L.closestAt.z;
            r.minGapM = L.minGapM == float.MaxValue ? 0f : L.minGapM;
            r.minSpeedMps = L.minSpeedMps == float.MaxValue ? 0f : L.minSpeedMps;
            r.outcome = L.sawFaceStop ? PassRow.OutcomeFaceStop
                      : L.sawNarrow ? PassRow.OutcomeNarrow
                                    : PassRow.OutcomePassed;
            _rows.Add(r);
            PassCount++;

            L.passing = false;
        }

        Live Find(int idA, int idB)
        {
            for (int i = 0; i < _live.Count; i++)
                if (_live[i].idA == idA && _live[i].idB == idB) return _live[i];
            return null;
        }

        Live Add(int idA, int idB)
        {
            Live L = new Live();
            L.idA = idA;
            L.idB = idB;
            _live.Add(L);
            return L;
        }
    }
}