using System.Collections.Generic;
using UnityEngine;

namespace ServiceSim
{
    /// <summary>
    /// 譲る側が「相手の道から横へどく」ための形の計算（2026-10-23 エイイチと決めた）。
    ///
    /// 前は、ロボットも人も相手が動くのを待つ作りだった。そのため
    /// 受け渡し口と客席をつなぐ角で向かい合って止まり、溜まっていた
    /// （前の2回（1311・1536）で、ロボットが止まった時間の約3分の1がこの角）。
    /// そこで譲る順番（RobotSpec の RankRobot / RankStaff / RankGuest）を決め、
    /// 譲る側は **その場で下がるのではなく、相手の道から横へどく**。
    ///
    /// ここは **形の計算だけ**。通れる床かどうかは呼ぶ側が確かめる
    /// （人とロボットで見る NavMesh の区画が違うため）。
    /// 距離の値は RobotSpec の1か所に置く。
    /// </summary>
    public static class YieldSide
    {
        /// <summary>
        /// 線 a→b に p をおろした足。線の外側に出たら端で止める
        /// </summary>
        public static Vector3 ProjectOnSegment(Vector3 a, Vector3 b, Vector3 p)
        {
            a.y = 0f; b.y = 0f; p.y = 0f;
            Vector3 ab = b - a;
            float len2 = ab.sqrMagnitude;
            if (len2 < 0.000001f) return a;
            float t = Mathf.Clamp01(Vector3.Dot(p - a, ab) / len2);
            return a + ab * t;
        }

        /// <summary>線 a→b から p までの横の距離（m）</summary>
        public static float LateralDistance(Vector3 a, Vector3 b, Vector3 p)
        {
            Vector3 foot = ProjectOnSegment(a, b, p);
            p.y = 0f;
            return Vector3.Distance(foot, p);
        }

        /// <summary>
        /// 折れ線（points）から p までの横の距離（m）。いちばん近い区間で測る。
        /// 点が 2 つ未満なら float.MaxValue（道が分からない＝上に乗っていないとみなす）。
        /// 「これから進む道の上にいるか」を見るのに使う（v6b）
        /// </summary>
        public static float LateralDistanceToPath(List<Vector3> points, Vector3 p)
        {
            if (points == null || points.Count < 2) return float.MaxValue;

            float best = float.MaxValue;
            for (int i = 1; i < points.Count; i++)
            {
                float d = LateralDistance(points[i - 1], points[i], p);
                if (d < best) best = d;
            }
            return best;
        }

        /// <summary>
        /// 相手の道 a→b から横へ clearM 以上どいた点の候補を、近い順に作る。
        ///
        /// ・足（from をおろした点）から、線に直角の向きへ clearM から searchM まで stepM きざみ
        /// ・いま自分がいる側を先に試す（よけいに道を横切らないため）
        /// ・呼ぶ側が、先頭から順に「通れる床か」を確かめて、最初に通ったものを使う
        ///
        /// 線の長さが 0 のときは何も入れない（呼ぶ側が別の手に回る）。
        /// </summary>
        public static void Candidates(Vector3 a, Vector3 b, Vector3 from,
                                      float clearM, float searchM, float stepM,
                                      List<Vector3> into)
        {
            if (into == null) return;
            into.Clear();

            a.y = 0f; b.y = 0f; from.y = 0f;
            Vector3 ab = b - a;
            ab.y = 0f;
            if (ab.sqrMagnitude < 0.000001f) return;

            Vector3 dir = ab.normalized;
            Vector3 left = new Vector3(-dir.z, 0f, dir.x);    // 線に直角（左）
            Vector3 foot = ProjectOnSegment(a, b, from);

            // いまいる側を先に試す
            float sideSign = Vector3.Dot(from - foot, left) >= 0f ? 1f : -1f;

            float step = Mathf.Max(0.05f, stepM);
            float start = Mathf.Max(0.01f, clearM);
            float end = Mathf.Max(start, searchM);

            for (float d = start; d <= end + 0.0001f; d += step)
            {
                into.Add(foot + left * (sideSign * d));
                into.Add(foot - left * (sideSign * d));
            }
        }

        /// <summary>
        /// その点が、相手の道 a→b から clearM 以上横に離れているか。
        /// 候補を NavMesh へ寄せたあと、寄せたぶんで近づいてしまっていないかを確かめる
        /// </summary>
        public static bool FarEnough(Vector3 a, Vector3 b, Vector3 p, float clearM)
        {
            return LateralDistance(a, b, p) >= clearM - 0.0001f;
        }
    }
}