using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Globalization;
using System.IO;
using System.IO.Compression;
using System.Text;
using UnityEngine;

namespace ServiceSim
{
    /// <summary>
    /// ロボットが走れる所の地図（サイトの {名前}_map.json）。
    ///
    /// 2026-10-24 にエイイチと決めたこと（依頼 v9）:
    ///   ロボットは**動ける範囲を地図で持ち、道はその都度自分で引く**。
    ///   地図はサイトが正（版 2fa10547a2b7 で公開）。Unity は塗り直さない・通れる所を足さない。
    ///   NavMesh はロボットの道探しには使わない（人に自分をよけてもらうためだけに残す）。
    ///
    /// マスの意味（JSON の free_class_values そのまま）:
    ///   2 … ふだん走らせる幅の道（normal_width 1.0 m）が通れる
    ///   1 … 通れる幅の道だけ（pass_width 0.8 m）通れる
    ///   0 … 通れない
    /// clear_mm … そのマスから、いちばん近い障害物までの距離（mm）
    ///
    /// 大事な読み方: free_class は「**ロボットの中心を置ける所**」で、
    /// 道の幅のぶんをもう削ってある（体の半径 0.25 m ではなく、道の幅の半分で削ってある）。
    /// だから 1.2 m の通路でも、中心を置ける帯は 0.45 m しかない。
    /// 2 台（中心どうし 0.5 m）は通路の中では並べない。
    ///
    /// 座標: マス (ix, iy) の中心は x=(ix+0.5)*res、y=(iy+0.5)*res。
    ///       JSON の x → Unity の X、y → Unity の Z。番号は iy*nx+ix。
    /// </summary>
    public class RobotMap
    {
        public readonly string generation = "";
        public readonly float res;
        public readonly int nx, ny;
        public readonly float normalWidthM, passWidthM;

        readonly byte[] _class;
        readonly ushort[] _clearMm;

        /// <summary>JSON が持っている counts（読んだものと突き合わせる）</summary>
        readonly int[] _jsonCounts = new int[3];
        readonly int[] _realCounts = new int[3];

        /// <summary>つながりの番号。2 だけでたどれる組と、1 以上でたどれる組</summary>
        int[] _region2, _region1;

        GridPlanner _planner;

        RobotMap(string generation, float res, int nx, int ny,
                 float normalWidthM, float passWidthM,
                 byte[] cls, ushort[] clearMm, int[] jsonCounts)
        {
            this.generation = generation ?? "";
            this.res = res;
            this.nx = nx;
            this.ny = ny;
            this.normalWidthM = normalWidthM;
            this.passWidthM = passWidthM;
            _class = cls;
            _clearMm = clearMm;
            for (int i = 0; i < 3; i++) _jsonCounts[i] = jsonCounts[i];
            for (int i = 0; i < cls.Length; i++)
            {
                int c = cls[i];
                if (c >= 0 && c <= 2) _realCounts[c]++;
            }
        }

        public int CellCount { get { return nx * ny; } }

        /// <summary>この地図を使う道探し（1つを使い回す。配列が大きいので台ごとに作らない）</summary>
        public GridPlanner Planner
        {
            get
            {
                if (_planner == null) _planner = new GridPlanner(this);
                return _planner;
            }
        }

        // ---------------- 読む ----------------

        /// <summary>
        /// 地図の JSON を読む。読めなければ error に理由を入れて null を返す。
        /// free_class・clear_mm は base64 → gzip を解く（JSON の encoding のとおり）。
        /// </summary>
        public static RobotMap Read(string json, out string error)
        {
            error = null;
            JsonValue root = JsonValue.Parse(json, out error);
            if (root == null || !root.IsObject)
            {
                error = "地図の JSON を読めません: " + error;
                return null;
            }

            float res = root["res"].F(0f);
            int nx = root["nx"].I(0);
            int ny = root["ny"].I(0);
            if (res <= 0.0001f || nx <= 0 || ny <= 0)
            {
                error = "地図の res / nx / ny がありません";
                return null;
            }

            int want = nx * ny;
            byte[] cls = Unpack(root["free_class"].S(""), 1, want, "free_class", out error);
            if (cls == null) return null;
            byte[] raw = Unpack(root["clear_mm"].S(""), 2, want, "clear_mm", out error);
            if (raw == null) return null;

            ushort[] clear = new ushort[want];
            for (int i = 0; i < want; i++)
                clear[i] = (ushort)(raw[i * 2] | (raw[i * 2 + 1] << 8));   // 小さい桁が先

            int[] counts = new int[3];
            JsonValue cj = root["counts"];
            for (int c = 0; c <= 2; c++)
                counts[c] = cj[c.ToString(CultureInfo.InvariantCulture)].I(-1);

            return new RobotMap(root["meta"]["generation"].S(""), res, nx, ny,
                                root["normal_width"].F(0f), root["pass_width"].F(0f),
                                cls, clear, counts);
        }

        static byte[] Unpack(string b64, int bytesPerCell, int cells, string what, out string error)
        {
            error = null;
            if (string.IsNullOrEmpty(b64))
            {
                error = what + " がありません";
                return null;
            }
            byte[] packed;
            try { packed = Convert.FromBase64String(b64); }
            catch (Exception e)
            {
                error = what + " の base64 を解けません: " + e.Message;
                return null;
            }

            byte[] data;
            try
            {
                using (MemoryStream src = new MemoryStream(packed))
                using (GZipStream gz = new GZipStream(src, CompressionMode.Decompress))
                using (MemoryStream dst = new MemoryStream(cells * bytesPerCell))
                {
                    byte[] buf = new byte[1 << 16];
                    int n;
                    while ((n = gz.Read(buf, 0, buf.Length)) > 0) dst.Write(buf, 0, n);
                    data = dst.ToArray();
                }
            }
            catch (Exception e)
            {
                error = what + " の gzip を解けません: " + e.Message;
                return null;
            }

            if (data.Length != cells * bytesPerCell)
            {
                error = what + " の長さが合いません: " + data.Length + " バイト / 要るのは " +
                        (cells * bytesPerCell) + " バイト（nx × ny × " + bytesPerCell + "）";
                return null;
            }
            return data;
        }

        // ---------------- マスを引く ----------------

        public int Index(int ix, int iy) { return iy * nx + ix; }

        /// <summary>その点のマスの番号。店の外なら -1</summary>
        public int CellAt(Vector3 p)
        {
            int ix = Mathf.FloorToInt(p.x / res);
            int iy = Mathf.FloorToInt(p.z / res);
            if (ix < 0 || ix >= nx || iy < 0 || iy >= ny) return -1;
            return iy * nx + ix;
        }

        /// <summary>マスの真ん中（Unity の XZ。y は 0）</summary>
        public Vector3 Center(int index)
        {
            int iy = index / nx;
            int ix = index - iy * nx;
            return new Vector3((ix + 0.5f) * res, 0f, (iy + 0.5f) * res);
        }

        /// <summary>そのマスの種類（2 / 1 / 0）。店の外は 0</summary>
        public int ClassOf(int index)
        {
            return index < 0 || index >= _class.Length ? 0 : _class[index];
        }

        /// <summary>その点の種類（店の外は 0）</summary>
        public int ClassAt(Vector3 p) { return ClassOf(CellAt(p)); }

        /// <summary>いちばん近い障害物までの距離（m）。店の外は 0</summary>
        public float ClearM(int index)
        {
            return index < 0 || index >= _clearMm.Length ? 0f : _clearMm[index] * 0.001f;
        }

        /// <summary>読んだマスの数（種類ごと）</summary>
        public int CountOf(int cls) { return cls >= 0 && cls <= 2 ? _realCounts[cls] : 0; }

        /// <summary>JSON の counts と読んだ数が合っているか</summary>
        public bool CountsMatch
        {
            get
            {
                for (int c = 0; c <= 2; c++)
                    if (_jsonCounts[c] != _realCounts[c]) return false;
                return true;
            }
        }

        /// <summary>
        /// 斜めに進めるか（両隣が通れるときだけ。サイトの道の引き方と同じ）。
        /// ix,iy → jx,jy は斜め隣のこと
        /// </summary>
        public bool DiagonalOpen(int ix, int iy, int jx, int jy, int minClass)
        {
            return _class[iy * nx + jx] >= minClass && _class[jy * nx + ix] >= minClass;
        }

        // ---------------- つながり（2 だけで行けるかを一瞬で見る）----------------

        /// <summary>
        /// 「行き先まで 2 のマスだけで行けるか」を、毎回 A* を空振りさせずに見るための組分け。
        /// 道の引き方（8方向・斜めは両隣が通れるときだけ）とまったく同じつなぎ方で塗る。
        /// </summary>
        public void BuildRegions()
        {
            if (_region2 != null) return;
            _region2 = Flood(2);
            _region1 = Flood(1);
        }

        int[] Flood(int minClass)
        {
            int n = nx * ny;
            int[] region = new int[n];
            for (int i = 0; i < n; i++) region[i] = -1;
            int[] stack = new int[n];
            int next = 0;

            for (int s = 0; s < n; s++)
            {
                if (region[s] >= 0 || _class[s] < minClass) continue;
                int id = next++;
                int top = 0;
                stack[top++] = s;
                region[s] = id;
                while (top > 0)
                {
                    int i = stack[--top];
                    int iy = i / nx;
                    int ix = i - iy * nx;
                    for (int d = 0; d < 8; d++)
                    {
                        int jx = ix + GridPlanner.Dx[d];
                        int jy = iy + GridPlanner.Dy[d];
                        if (jx < 0 || jx >= nx || jy < 0 || jy >= ny) continue;
                        int j = jy * nx + jx;
                        if (region[j] >= 0 || _class[j] < minClass) continue;
                        if (GridPlanner.Dx[d] != 0 && GridPlanner.Dy[d] != 0 &&
                            !DiagonalOpen(ix, iy, jx, jy, minClass)) continue;
                        region[j] = id;
                        stack[top++] = j;
                    }
                }
            }
            return region;
        }

        /// <summary>2 つのマスが、その種類だけでつながっているか</summary>
        public bool SameRegion(int a, int b, int minClass)
        {
            BuildRegions();
            int[] r = minClass >= 2 ? _region2 : _region1;
            if (a < 0 || b < 0 || a >= r.Length || b >= r.Length) return false;
            return r[a] >= 0 && r[a] == r[b];
        }

        // ---------------- 通れるマスへ寄せる ----------------

        /// <summary>
        /// その点のいちばん近い「通れるマス」。
        ///
        /// なぜ要るか: ロボットがいま立っている所が、地図では 0（通れない）のことがある。
        /// NavMesh は体の半径 0.25 m で焼いてあるのに、free_class は道の幅（0.4 m／0.5 m）で
        /// 削ってあるので、通路の端・通路の入口で待つ点・横へどいた点は 0 になる。
        /// （v8b の 20261024_1004 の回では、60 秒の止まり 51 回のうち 36 回がそういう所だった）
        /// 地図は塗り直さず、**出発点だけを近くの通れるマスへ寄せる**。寄せた回数は記録に出す。
        ///
        /// 見つからなければ -1。
        /// </summary>
        public int NearestPassable(Vector3 p, int minClass, float searchM)
        {
            int here = CellAt(p);
            if (here >= 0 && _class[here] >= minClass) return here;

            int ix0 = Mathf.FloorToInt(p.x / res);
            int iy0 = Mathf.FloorToInt(p.z / res);
            int maxR = Mathf.Max(1, Mathf.CeilToInt(searchM / res));

            int best = -1;
            float bestD2 = float.MaxValue;
            for (int r = 1; r <= maxR; r++)
            {
                for (int iy = iy0 - r; iy <= iy0 + r; iy++)
                {
                    if (iy < 0 || iy >= ny) continue;
                    for (int ix = ix0 - r; ix <= ix0 + r; ix++)
                    {
                        if (ix < 0 || ix >= nx) continue;
                        // 輪の上だけ見る（中は前の回で見た）
                        if (Mathf.Abs(ix - ix0) != r && Mathf.Abs(iy - iy0) != r) continue;
                        int i = iy * nx + ix;
                        if (_class[i] < minClass) continue;
                        Vector3 c = Center(i);
                        float dx = c.x - p.x, dz = c.z - p.z;
                        float d2 = dx * dx + dz * dz;
                        // 同じ距離なら番号の小さい方（毎回同じ答えにする）
                        if (d2 < bestD2 - 1e-9f) { bestD2 = d2; best = i; }
                    }
                }
                if (best >= 0) return best;      // 輪を1つ見終わって見つかれば、それが最短
            }
            return best;
        }

        // ---------------- 報告 ----------------

        public string Report()
        {
            StringBuilder sb = new StringBuilder();
            sb.AppendLine("  地図 (map.json): 版 " + generation +
                          " / マス " + nx + " × " + ny + " = " + CellCount +
                          " / 1 マス " + F(res) + " m");
            sb.AppendLine("    ふだんの道の幅 " + F(normalWidthM) + " m / 通れる幅 " + F(passWidthM) + " m");
            sb.AppendLine("    マスの数: 2（ふだんの道）" + _realCounts[2] +
                          " / 1（通れる幅だけ）" + _realCounts[1] +
                          " / 0（通れない）" + _realCounts[0] +
                          "  … JSON の counts と " + (CountsMatch ? "一致" : "ちがう") +
                          (CountsMatch ? "" : "（JSON: 2=" + _jsonCounts[2] + " 1=" + _jsonCounts[1] +
                                              " 0=" + _jsonCounts[0] + "）"));
            return sb.ToString().TrimEnd();
        }

        static string F(float v) { return v.ToString("0.###", CultureInfo.InvariantCulture); }
    }

    /// <summary>
    /// 地図の上で道を引く（依頼 v9 の 2 章）。ロボットだけが使う。NavMesh は使わない。
    ///
    /// 決まり:
    ///   ・8 方向。斜めは両隣が通れるときだけ（サイトと同じ）
    ///   ・行き先まで 2 のマスだけで行けるなら 2 だけで引く。行けなければ 1 も使い、
    ///     1 のマスを narrowCellCost 倍（目安 3 倍）重くする
    ///   ・通路の真ん中寄りを走る: 1 歩の重さに
    ///     centerWeight × max(0, preferClearM − clear) / preferClearM を足す
    ///   ・乱数を使わない。同じ条件なら毎回同じ道（同じ重さのときは h → マスの番号の順）
    ///   ・引けた道は曲がり角の点だけに縮める。近道は「もとの道で通っていた所より
    ///     障害物に近づかない所」だけ（端に寄った線に戻さないため）
    ///
    /// 回り込み・よけのために、その 1 回だけ「通れないマス」「重いマス」を差し込める。
    /// 配列が大きいので、1 つを台どうしで使い回す（道探しはその場で終わるので取り合わない）。
    /// </summary>
    public class GridPlanner
    {
        /// <summary>8 方向の順（決まった順。乱数を使わない）</summary>
        public static readonly int[] Dx = { 1, 1, 0, -1, -1, -1, 0, 1 };
        public static readonly int[] Dy = { 0, 1, 1, 1, 0, -1, -1, -1 };

        /// <summary>道探しの結果</summary>
        public enum Status
        {
            Ok,             // 引けた
            StartBlocked,   // 出発点が通れないマス（寄せても見つからない）
            GoalBlocked,    // 行き先が通れないマス
            NoPath,         // つながっていない
            TooMany         // 探しすぎ（maxExpand を超えた）
        }

        /// <summary>道の引き方の数字（RobotSpec から写す）</summary>
        public struct Settings
        {
            public float narrowCellCost;
            public float centerWeight;
            public float preferClearM;
            public int maxExpand;
            public float snapM;

            /// <summary>曲がり角だけに縮めるとき、1 本の近道の長さの上限（m）</summary>
            public float simplifyMaxM;

            public static Settings Default
            {
                get
                {
                    Settings s = new Settings();
                    s.narrowCellCost = 3f;
                    s.centerWeight = 2f;
                    s.preferClearM = 0.6f;
                    s.maxExpand = 200000;
                    s.snapM = 1.0f;
                    s.simplifyMaxM = 6f;
                    return s;
                }
            }
        }

        readonly RobotMap _map;
        readonly int _nx, _ny, _n;
        readonly float _res;

        // ---- 道探しの作業場（毎回作り直さない）----
        readonly float[] _g;
        readonly int[] _from;
        readonly int[] _seen;          // この回で見たか（_runStamp と比べる）
        readonly int[] _closed;
        int _runStamp;

        // ---- 差し込んだ「通れないマス」「重いマス」----
        readonly int[] _blockStamp;
        readonly int[] _costStamp;
        readonly float[] _costAdd;
        int _markStamp;
        bool _hasMarks;

        // ---- 二分の山（f が小さい順。同じなら h → 番号）----
        int[] _heap;
        float[] _heapF, _heapH;
        int _heapCount;

        readonly List<Vector3> _points = new List<Vector3>();
        readonly List<int> _cells = new List<int>();
        readonly Stopwatch _watch = new Stopwatch();

        public GridPlanner(RobotMap map)
        {
            _map = map;
            _nx = map.nx; _ny = map.ny; _n = map.nx * map.ny; _res = map.res;
            _g = new float[_n];
            _from = new int[_n];
            _seen = new int[_n];
            _closed = new int[_n];
            _blockStamp = new int[_n];
            _costStamp = new int[_n];
            _costAdd = new float[_n];
            _heap = new int[1024];
            _heapF = new float[1024];
            _heapH = new float[1024];
            // 印は 1 から始める。0 のままだと、何も印をつけていないマス（0）が
            // 「印がついている」に見えてしまう
            _markStamp = 1;
            map.BuildRegions();
        }

        public RobotMap Map { get { return _map; } }

        // ---- 直前の結果 ----
        public Status LastStatus { get; private set; }

        /// <summary>引けた道の曲がり角の点（最初は出発のマスの真ん中、最後は行き先のマスの真ん中）</summary>
        public IReadOnlyList<Vector3> Points { get { return _points; } }

        /// <summary>道に 1 のマスが入ったか（＝狭い道を使った）</summary>
        public bool UsedNarrow { get; private set; }

        /// <summary>この 1 回にかかった時間（ミリ秒）</summary>
        public float LastPlanMs { get; private set; }

        /// <summary>広げたマスの数（重さを調べるため）</summary>
        public int LastExpanded { get; private set; }

        /// <summary>マスをたどった長さ（縮める前, m）</summary>
        public float LastRawLengthM { get; private set; }

        /// <summary>縮めたあとの長さ（m）</summary>
        public float LastLengthM { get; private set; }

        /// <summary>出発のマスの真ん中と、渡された点のあいだの距離（m）</summary>
        public float LastStartSnapM { get; private set; }

        /// <summary>行き先のマスの真ん中と、渡された点のあいだの距離（m）</summary>
        public float LastGoalSnapM { get; private set; }

        /// <summary>
        /// 出発点が、地図では通れないマス（0）だったか。
        /// ロボットは NavMesh（体の半径 0.25 m）で焼いた面を歩くので、
        /// 通路の端・通路の入口で待つ点・横へどいた点は free_class では 0 になる。
        /// そういうときは近くの通れるマスへ寄せて引く。何回あったかを記録に出す
        /// </summary>
        public bool LastStartWasBlocked { get; private set; }

        /// <summary>行き先が、地図では通れないマス（0）だったか</summary>
        public bool LastGoalWasBlocked { get; private set; }

        public string StatusText
        {
            get
            {
                switch (LastStatus)
                {
                    case Status.Ok: return UsedNarrow ? "引けた（狭い道）" : "引けた";
                    case Status.StartBlocked: return "出発点が通れない";
                    case Status.GoalBlocked: return "行き先が通れない";
                    case Status.NoPath: return "つながっていない";
                    default: return "探しすぎ";
                }
            }
        }

        // ---------------- 差し込み（その 1 回だけ）----------------

        /// <summary>差し込んだものをぜんぶ消す（道を引く前に必ず呼ぶ）</summary>
        public void ClearMarks()
        {
            _markStamp++;
            _hasMarks = false;
        }

        /// <summary>丸い所を「通れないマス」にする（人・ほかのロボットの体）</summary>
        public void BlockCircle(Vector3 center, float radiusM)
        {
            MarkCapsule(center, center, radiusM, true, 0f);
        }

        /// <summary>線のまわりを「通れないマス」にする</summary>
        public void BlockLine(Vector3 a, Vector3 b, float radiusM)
        {
            MarkCapsule(a, b, radiusM, true, 0f);
        }

        /// <summary>線のまわりを重くする（相手の先の道から離れて走らせる）</summary>
        public void CostLine(Vector3 a, Vector3 b, float radiusM, float add)
        {
            MarkCapsule(a, b, radiusM, false, add);
        }

        void MarkCapsule(Vector3 a, Vector3 b, float radiusM, bool block, float add)
        {
            if (radiusM <= 0f) return;
            _hasMarks = true;

            float minX = Mathf.Min(a.x, b.x) - radiusM, maxX = Mathf.Max(a.x, b.x) + radiusM;
            float minZ = Mathf.Min(a.z, b.z) - radiusM, maxZ = Mathf.Max(a.z, b.z) + radiusM;
            int ix0 = Mathf.Max(0, Mathf.FloorToInt(minX / _res));
            int ix1 = Mathf.Min(_nx - 1, Mathf.FloorToInt(maxX / _res));
            int iy0 = Mathf.Max(0, Mathf.FloorToInt(minZ / _res));
            int iy1 = Mathf.Min(_ny - 1, Mathf.FloorToInt(maxZ / _res));
            float r2 = radiusM * radiusM;

            for (int iy = iy0; iy <= iy1; iy++)
            {
                for (int ix = ix0; ix <= ix1; ix++)
                {
                    float cx = (ix + 0.5f) * _res, cz = (iy + 0.5f) * _res;
                    if (SegDist2(cx, cz, a.x, a.z, b.x, b.z) > r2) continue;
                    int i = iy * _nx + ix;
                    if (block)
                    {
                        _blockStamp[i] = _markStamp;
                        continue;
                    }
                    if (_costStamp[i] == _markStamp) _costAdd[i] = Mathf.Max(_costAdd[i], add);
                    else { _costStamp[i] = _markStamp; _costAdd[i] = add; }
                }
            }
        }

        static float SegDist2(float px, float pz, float ax, float az, float bx, float bz)
        {
            float vx = bx - ax, vz = bz - az;
            float l2 = vx * vx + vz * vz;
            float t = 0f;
            if (l2 > 1e-12f)
            {
                t = ((px - ax) * vx + (pz - az) * vz) / l2;
                t = t < 0f ? 0f : (t > 1f ? 1f : t);
            }
            float dx = px - (ax + t * vx), dz = pz - (az + t * vz);
            return dx * dx + dz * dz;
        }

        // ---------------- 道を引く ----------------

        /// <summary>
        /// from から to まで道を引く。引けたら Status.Ok で、Points に曲がり角の点が入る。
        ///
        /// 「2 のマスだけで行けるなら 2 だけ」は、つながりの組（RobotMap.SameRegion）で
        /// 一瞬で見る。A* を空振りさせない（⑦では卓の半分が狭い道なので、空振りが重い）。
        /// </summary>
        public Status Plan(Vector3 from, Vector3 to, Settings st)
        {
            _watch.Reset();
            _watch.Start();

            _points.Clear();
            _cells.Clear();
            UsedNarrow = false;
            LastExpanded = 0;
            LastRawLengthM = 0f;
            LastLengthM = 0f;
            LastStartSnapM = 0f;
            LastGoalSnapM = 0f;
            LastStartWasBlocked = _map.ClassAt(from) < 1;
            LastGoalWasBlocked = _map.ClassAt(to) < 1;

            // 行き先は、まず 1 以上のマスに乗っているかを見る（サイトが通れる所に置いている）
            int goal = _map.NearestPassable(to, 1, st.snapM);
            if (goal < 0)
            {
                LastStatus = Status.GoalBlocked;
                return Done();
            }
            LastGoalSnapM = Flat(_map.Center(goal) - to).magnitude;

            int start = _map.NearestPassable(from, 1, st.snapM);
            if (start < 0)
            {
                LastStatus = Status.StartBlocked;
                return Done();
            }
            LastStartSnapM = Flat(_map.Center(start) - from).magnitude;

            // 2 のマスだけで行けるか（出発点・行き先がどちらも 2 で、同じ組）
            int minClass = 1;
            if (_map.ClassOf(start) >= 2 && _map.ClassOf(goal) >= 2 &&
                _map.SameRegion(start, goal, 2) && !_hasMarks)
            {
                minClass = 2;
            }
            else if (_map.ClassOf(start) >= 2 && _map.ClassOf(goal) >= 2 &&
                     _map.SameRegion(start, goal, 2))
            {
                // 差し込み（回り込み・よけ）があるときは、つながりの組では決められない。
                // まず 2 だけで試し、だめなら 1 も使う
                if (Search(start, goal, 2, st) == Status.Ok)
                {
                    Finish(st, 2);
                    LastStatus = Status.Ok;
                    return Done();
                }
            }

            if (minClass == 1 && !_map.SameRegion(start, goal, 1))
            {
                LastStatus = Status.NoPath;
                return Done();
            }

            Status s = Search(start, goal, minClass, st);
            if (s != Status.Ok)
            {
                LastStatus = s;
                return Done();
            }
            Finish(st, minClass);
            LastStatus = Status.Ok;
            return Done();
        }

        Status Done()
        {
            _watch.Stop();
            LastPlanMs = (float)(_watch.Elapsed.TotalMilliseconds);
            return LastStatus;
        }

        Status Search(int start, int goal, int minClass, Settings st)
        {
            _runStamp++;
            _heapCount = 0;
            _cells.Clear();

            if (Blocked(start, minClass) || Blocked(goal, minClass)) return Status.NoPath;

            int gy = goal / _nx, gx = goal - gy * _nx;
            float prefMm = Mathf.Max(0.001f, st.preferClearM);
            float d2 = _res * 1.41421356f;
            int expanded = 0;

            _g[start] = 0f;
            _from[start] = -1;
            _seen[start] = _runStamp;
            Push(start, Heuristic(start, gx, gy, d2), Heuristic(start, gx, gy, d2));

            while (_heapCount > 0)
            {
                int i = Pop();
                if (_closed[i] == _runStamp) continue;
                _closed[i] = _runStamp;
                expanded++;
                if (i == goal)
                {
                    LastExpanded = expanded;
                    Trace(start, goal);
                    return Status.Ok;
                }
                if (expanded > st.maxExpand)
                {
                    LastExpanded = expanded;
                    return Status.TooMany;
                }

                int iy = i / _nx, ix = i - iy * _nx;
                float gi = _g[i];

                for (int d = 0; d < 8; d++)
                {
                    int jx = ix + Dx[d], jy = iy + Dy[d];
                    if (jx < 0 || jx >= _nx || jy < 0 || jy >= _ny) continue;
                    int j = jy * _nx + jx;
                    if (_closed[j] == _runStamp) continue;
                    if (Blocked(j, minClass)) continue;

                    float step;
                    if (Dx[d] != 0 && Dy[d] != 0)
                    {
                        // 斜めは両隣が通れるときだけ
                        if (Blocked(iy * _nx + jx, minClass) || Blocked(jy * _nx + ix, minClass)) continue;
                        step = d2;
                    }
                    else step = _res;

                    float mult = _map.ClassOf(j) == 1 ? st.narrowCellCost : 1f;
                    float clearM = _map.ClearM(j);
                    if (clearM < st.preferClearM)
                        mult += st.centerWeight * (st.preferClearM - clearM) / prefMm;
                    if (_costStamp[j] == _markStamp) mult += _costAdd[j];

                    float ng = gi + step * mult;
                    if (_seen[j] == _runStamp && ng >= _g[j] - 1e-9f) continue;

                    _g[j] = ng;
                    _from[j] = i;
                    _seen[j] = _runStamp;
                    float h = Heuristic(j, gx, gy, d2);
                    Push(j, ng + h, h);
                }
            }

            LastExpanded = expanded;
            return Status.NoPath;
        }

        /// <summary>そのマスは通れないか（地図の種類＋差し込んだ「通れないマス」）</summary>
        bool Blocked(int i, int minClass)
        {
            if (_map.ClassOf(i) < minClass) return true;
            return _hasMarks && _blockStamp[i] == _markStamp;
        }

        /// <summary>残りの見込み（8 方向のまっすぐな長さ）。1 歩の重さの下限は 1 倍なので、下から見ている</summary>
        float Heuristic(int i, int gx, int gy, float d2)
        {
            int iy = i / _nx, ix = i - iy * _nx;
            int dx = ix > gx ? ix - gx : gx - ix;
            int dy = iy > gy ? iy - gy : gy - iy;
            int lo = dx < dy ? dx : dy;
            return _res * (dx + dy) + (d2 - 2f * _res) * lo;
        }

        void Trace(int start, int goal)
        {
            _cells.Clear();
            int i = goal;
            while (i != start)
            {
                _cells.Add(i);
                i = _from[i];
                if (i < 0) break;
            }
            _cells.Add(start);
            _cells.Reverse();
        }

        void Finish(Settings st, int minClass)
        {
            // 1 のマスが入ったか
            for (int k = 0; k < _cells.Count; k++)
                if (_map.ClassOf(_cells[k]) == 1) { UsedNarrow = true; break; }

            float raw = 0f;
            for (int k = 1; k < _cells.Count; k++)
                raw += Flat(_map.Center(_cells[k]) - _map.Center(_cells[k - 1])).magnitude;
            LastRawLengthM = raw;

            Simplify(st, minClass);

            float len = 0f;
            for (int k = 1; k < _points.Count; k++)
                len += Flat(_points[k] - _points[k - 1]).magnitude;
            LastLengthM = len;
        }

        // ---------------- 曲がり角の点だけにする ----------------

        /// <summary>
        /// マスの並びを曲がり角の点だけに縮める。
        /// 近道してよいのは「もとの道で通っていた所より障害物に近づかない」ときだけ
        /// （＝近道の上のどのマスも、飛ばしたマスのいちばん狭い所より clear が大きい）。
        /// 端に寄った線に戻らないようにするため。
        /// </summary>
        void Simplify(Settings st, int minClass)
        {
            _points.Clear();
            if (_cells.Count == 0) return;
            _points.Add(_map.Center(_cells[0]));
            if (_cells.Count == 1) return;

            int step = Mathf.Max(1, Mathf.RoundToInt(0.1f / _res));   // 0.1 m ごとに試す
            int maxCells = Mathf.Max(step, Mathf.RoundToInt(st.simplifyMaxM / _res));

            int i = 0;
            while (i < _cells.Count - 1)
            {
                int best = i + 1;
                float floorM = Mathf.Min(_map.ClearM(_cells[i]), _map.ClearM(_cells[i + 1]));

                int j = i + step;
                float runFloor = floorM;
                while (j < _cells.Count && j - i <= maxCells)
                {
                    for (int k = Mathf.Max(i + 1, j - step + 1); k <= j; k++)
                        runFloor = Mathf.Min(runFloor, _map.ClearM(_cells[k]));
                    if (!SegmentOk(_cells[i], _cells[j], runFloor, minClass)) break;
                    best = j;
                    j += step;
                }
                // 最後の点までちょうど届くなら、そこで終わる
                int last = _cells.Count - 1;
                if (best < last && last - i <= maxCells)
                {
                    float f = runFloor;
                    for (int k = best + 1; k <= last; k++) f = Mathf.Min(f, _map.ClearM(_cells[k]));
                    if (SegmentOk(_cells[i], _cells[last], f, minClass)) best = last;
                }

                _points.Add(_map.Center(_cells[best]));
                i = best;
            }
        }

        /// <summary>まっすぐ結んだ線が通れて、もとの道より障害物に近づかないか</summary>
        bool SegmentOk(int a, int b, float floorM, int minClass)
        {
            Vector3 pa = _map.Center(a), pb = _map.Center(b);
            float len = Flat(pb - pa).magnitude;
            int n = Mathf.Max(1, Mathf.CeilToInt(len / (_res * 0.5f)));
            for (int k = 0; k <= n; k++)
            {
                float t = (float)k / n;
                float x = pa.x + (pb.x - pa.x) * t;
                float z = pa.z + (pb.z - pa.z) * t;
                int ix = Mathf.FloorToInt(x / _res);
                int iy = Mathf.FloorToInt(z / _res);
                if (ix < 0 || ix >= _nx || iy < 0 || iy >= _ny) return false;
                int i = iy * _nx + ix;
                if (Blocked(i, minClass)) return false;
                if (_map.ClearM(i) < floorM - 1e-6f) return false;
                if (_hasMarks && _costStamp[i] == _markStamp) return false;   // 重い所へは近道しない
            }
            return true;
        }

        // ---------------- 二分の山 ----------------

        void Push(int index, float f, float h)
        {
            if (_heapCount == _heap.Length) Grow();
            int k = _heapCount++;
            _heap[k] = index; _heapF[k] = f; _heapH[k] = h;
            while (k > 0)
            {
                int p = (k - 1) >> 1;
                if (!Less(k, p)) break;
                Swap(k, p);
                k = p;
            }
        }

        int Pop()
        {
            int top = _heap[0];
            _heapCount--;
            if (_heapCount > 0)
            {
                _heap[0] = _heap[_heapCount];
                _heapF[0] = _heapF[_heapCount];
                _heapH[0] = _heapH[_heapCount];
                int k = 0;
                while (true)
                {
                    int l = (k << 1) + 1, r = l + 1, m = k;
                    if (l < _heapCount && Less(l, m)) m = l;
                    if (r < _heapCount && Less(r, m)) m = r;
                    if (m == k) break;
                    Swap(k, m);
                    k = m;
                }
            }
            return top;
        }

        /// <summary>同じ重さのときの並べ方を決めておく（f → h → マスの番号）。乱数を使わない</summary>
        bool Less(int a, int b)
        {
            if (_heapF[a] < _heapF[b] - 1e-9f) return true;
            if (_heapF[a] > _heapF[b] + 1e-9f) return false;
            if (_heapH[a] < _heapH[b] - 1e-9f) return true;
            if (_heapH[a] > _heapH[b] + 1e-9f) return false;
            return _heap[a] < _heap[b];
        }

        void Swap(int a, int b)
        {
            int i = _heap[a]; _heap[a] = _heap[b]; _heap[b] = i;
            float f = _heapF[a]; _heapF[a] = _heapF[b]; _heapF[b] = f;
            float h = _heapH[a]; _heapH[a] = _heapH[b]; _heapH[b] = h;
        }

        void Grow()
        {
            int size = _heap.Length * 2;
            int[] hi = new int[size];
            float[] hf = new float[size];
            float[] hh = new float[size];
            Array.Copy(_heap, hi, _heapCount);
            Array.Copy(_heapF, hf, _heapCount);
            Array.Copy(_heapH, hh, _heapCount);
            _heap = hi; _heapF = hf; _heapH = hh;
        }

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