AutoFocusTool/Calib/CalibrationEngine.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading;
using AutoFocusTool.Serial;
using AutoFocusTool.Imaging;
using SerialCamera = AutoFocusTool.Camera.Camera;

namespace AutoFocusTool.Calib
{
    /// <summary>
    /// 自动标定引擎（可复用，GUI/命令行共用）。封装单well标定四步：
    ///   ①粗对焦(中央40% ROI) → ②水平扫描找最居中位置 → ③曝光二分(well内ROI) → ④精对焦(0.95r ROI + 平滑插值)
    /// 通过回调上报进度(文字)与实时帧(byte[])，便于界面实时显示。
    /// </summary>
    public class CalibrationEngine
    {
        readonly int W, H;  // 取自相机实际分辨率，绝不能硬编码

        readonly HouseMotor _motor;
        readonly SerialCamera _cam;

        public Action<string> Log;
        public Action<byte[]> OnFrame;          // 实时帧回调(原始BGR)
        public Action<string, WellCircle, ExposureInfo> OnStep; // 步骤+当前圆/曝光

        /// <summary>居中容差(Y偏移百分比)，落入即合格，避免来回震荡。</summary>
        public double CenterTolPct = 12;
        /// <summary>粗扫范围(脉冲，零点±range)与步数。</summary>
        public int HScanRange = 4000;
        public int HScanSteps = 7;
        /// <summary>细扫步数(在粗扫最佳点附近精调Y)。</summary>
        public int FineScanSteps = 7;
        /// <summary>Z扫描半幅与层数。</summary>
        public int ZHalf = 1500, ZLayers = 9;
        public int ExpLo = 10, ExpHi = 800;
        /// <summary>扫描时电机稳定延时(ms)。小步移动无需1500ms，用短延时大幅提速。</summary>
        public int ScanDelayMs = 350;
        /// <summary>粗对焦层数(先让well清晰，再居中)。</summary>
        public int CoarseFocusLayers = 7;
        /// <summary>居中扫描用的固定曝光(well是暗背景上中灰盘,HScan实测~60可清晰检圆)。</summary>
        public Action<byte[], string> DebugSave;     // 诊断存图(帧,文件名前缀)，可选
        public int CenterScanExposure = 60;

        // ── 行程限位（所有电机移动前钳到该区间）──
        /// <summary>水平电机行程下/上限脉冲（旧工程自检值 70000）。</summary>
        public int HMin = 0, HMax = 70000;
        /// <summary>垂直电机行程下/上限脉冲（旧工程软上限 125000）。</summary>
        public int ZMin = 0, ZMaxPulse = 125000;

        // ── 水平全行程粗扫定位 ──
        /// <summary>水平全行程粗扫起点/终点/步距。命中完整圆即停。</summary>
        public int HCoarseStart = 0;
        public int HCoarseEnd = 70000;
        public int HCoarseStep = 2000;

        // ── Z 全范围粗对焦（固定中心大窗口）──
        /// <summary>Z 粗对焦固定中心（实测焦面集中区间 60000~120000 的中点）。</summary>
        public int ZCoarseCenter = 90000;
        /// <summary>Z 粗对焦半幅 → 区间 60000~120000。</summary>
        public int ZCoarseHalf = 30000;
        /// <summary>Z 粗对焦步距 → 约 31 层。</summary>
        public int ZCoarseStep = 2000;

        // ── Z 精对焦（围绕粗峰）──
        /// <summary>精对焦半幅（覆盖粗扫 ±2000 峰定位误差并留余量）。</summary>
        public int FineZHalf = 6000;
        /// <summary>精对焦步距 → 约 24 层（精度优先）。</summary>
        public int FineZStep = 500;

        public CalibrationEngine(HouseMotor motor, SerialCamera cam)
        {
            _motor = motor; _cam = cam;
            W = cam.Width; H = cam.Height;
        }

        /// <summary>水平脉冲钳到 [HMin,HMax]，越界写 Log。</summary>
        int ClampH(int p)
        {
            int c = Math.Max(HMin, Math.Min(HMax, p));
            if (c != p) Log?.Invoke($"  ⚠ 水平脉冲 {p} 越界，钳到 {c} [{HMin},{HMax}]");
            return c;
        }

        /// <summary>垂直脉冲钳到 [ZMin,ZMaxPulse]，越界写 Log。</summary>
        int ClampZ(int p)
        {
            int c = Math.Max(ZMin, Math.Min(ZMaxPulse, p));
            if (c != p) Log?.Invoke($"  ⚠ 垂直脉冲 {p} 越界，钳到 {c} [{ZMin},{ZMaxPulse}]");
            return c;
        }

        /// <summary>中央40% ROI(精对焦/粗对焦圆未检出时的降级，避免背景边缘带偏)。</summary>
        System.Drawing.Rectangle CenterRoi40()
        {
            int roiW = (int)(W * 0.4), roiH = (int)(H * 0.4);
            return new System.Drawing.Rectangle((W - roiW) / 2, (H - roiH) / 2, roiW, roiH);
        }

        /// <summary>
        /// P0-6: 关键定位移动带重试。下位机偶发不回复(串口噪声/忙)时直接放弃整个well过于脆弱，
        /// 重试最多 maxRetry 次，每次间隔后再试。
        /// </summary>
        bool RetryMove(Func<bool> move, string what, int maxRetry = 3)
        {
            for (int i = 0; i < maxRetry; i++)
            {
                if (move()) return true;
                Log?.Invoke($"  {what} 移动失败，重试 {i + 1}/{maxRetry}");
                Thread.Sleep(400);
            }
            return false;
        }

        byte[] Grab()
        {
            int retry = 0;
            while (retry < 3)
            {
                try
                {
                    _cam.GrabRgb();
                    var b = _cam.GetSourceBuffer();
                    if (b != null && b.Length > 0)
                    {
                        OnFrame?.Invoke(b);
                        return b;
                    }
                }
                catch (Exception ex)
                {
                    Log?.Invoke($"抓帧失败(重试{retry + 1}/3): {ex.Message}");
                }
                retry++;
                Thread.Sleep(50);
            }
            throw new Exception("抓帧失败，已重试3次");
        }

        /// <summary>
        /// 绕 center±range 扫 steps 个水平位置，找 well 检出且 |Y偏移| 最小的位置。
        /// 转动培养皿让 well 在画面上下(Y)移动，故按 Y 偏移评分；优先完整。
        /// </summary>
        (int bestHPos, WellCircle bestCircle) ScanForCenter(int well, int center, int range, int steps, int delayMs = -1)
        {
            int bestHPos = center; double bestScore = double.MaxValue; WellCircle best = null;
            int step = steps > 1 ? 2 * range / (steps - 1) : 1;
            int actualDelay = delayMs > 0 ? delayMs : ScanDelayMs;  // 使用传入延时或默认值
            // 固定中低曝光设一次即可：well 是暗背景上的中灰盘，不能按全图均值自动曝光(会过曝well盘)
            _cam.SetExposure(CenterScanExposure);
            for (int i = 0; i < steps; i++)
            {
                int hp = ClampH(center - range + step * i);
                _motor.HorizontalMoveTo(hp, actualDelay);
                var b = Grab();
                var c = WellDetector.Detect(b, W, H);
                DebugSave?.Invoke(b, $"center_w{well}_hp{hp}");
                Log?.Invoke(c.Found
                    ? $"  水平{hp}: Y偏移={c.OffsetYPct:F1}% X={c.OffsetXPct:F1}% 完整={c.Complete}"
                    : $"  水平{hp}: 未检出圆");
                OnStep?.Invoke($"居中扫描 hp={hp}", c, null);
                if (c.Found)
                {
                    // 按 |Y偏移| 评分，不完整重罚
                    double score = Math.Abs(c.OffsetYPct) + (c.Complete ? 0 : 100);
                    if (score < bestScore) { bestScore = score; bestHPos = hp; best = c; }
                }
            }
            return (bestHPos, best);
        }

        /// <summary>
        /// 水平全行程粗扫定位 + 局部密扫居中。不依赖 EEPROM 水平位置准确：
        /// ① 从 HCoarseStart 到 HCoarseEnd 按 HCoarseStep 扫，命中完整圆即停，记录该位置；
        /// ② 以命中点为中心做局部密扫(ScanForCenter)优化 Y 居中；
        /// ③ 全程未命中完整圆则取扫描中 |Y偏移| 最小且检出的位置；仍无返回 (-1,null)。
        /// </summary>
        (int bestHPos, WellCircle bestCircle) HCoarseLocate(int well)
        {
            _cam.SetExposure(CenterScanExposure);

            int hitHPos = -1; WellCircle hitCircle = null;
            int fallbackHPos = -1; WellCircle fallbackCircle = null;
            double fallbackScore = double.MaxValue;

            // ① 全行程粗扫，命中完整圆即停
            for (int hp = HCoarseStart; hp <= HCoarseEnd; hp += HCoarseStep)
            {
                int p = ClampH(hp);
                _motor.HorizontalMoveTo(p, ScanDelayMs);
                var b = Grab();
                var c = WellDetector.Detect(b, W, H);
                DebugSave?.Invoke(b, $"hcoarse_w{well}_hp{p}");
                Log?.Invoke(c.Found
                    ? $"  粗扫水平{p}: Y偏移={c.OffsetYPct:F1}% 完整={c.Complete}"
                    : $"  粗扫水平{p}: 未检出圆");
                OnStep?.Invoke($"水平粗扫 hp={p}", c, null);
                if (c.Found)
                {
                    double score = Math.Abs(c.OffsetYPct) + (c.Complete ? 0 : 100);
                    if (score < fallbackScore) { fallbackScore = score; fallbackHPos = p; fallbackCircle = c; }
                    if (c.Complete) { hitHPos = p; hitCircle = c; break; }
                }
            }

            // 命中完整圆 → 局部密扫；否则用检出最优的降级点
            int center = hitHPos >= 0 ? hitHPos : fallbackHPos;
            if (center < 0)
            {
                Log?.Invoke($"[well{well}] ✗ 水平全行程未检出任何圆");
                return (-1, null);
            }

            // ② 以命中/降级点为中心局部密扫居中（范围取粗扫步距量级）
            int fineRange = HCoarseStep;
            var fine = ScanForCenter(well, center, fineRange, FineScanSteps, 800);
            if (fine.bestCircle != null) return (fine.bestHPos, fine.bestCircle);

            // ③ 局部密扫没检出 → 回退到粗扫命中/降级结果
            return (center, hitCircle ?? fallbackCircle);
        }

        /// <summary>
        /// 标定单个 well。eepromHPos=该well的EEPROM水平位置(扫描中心)，eepromZ=Z焦准零点。
        /// 返回标定结果。
        /// </summary>
        public WellCalib CalibrateWell(int well, int eepromHPos, int eepromZ)
        {
            var r = new WellCalib { Well = well };

            // 先到 EEPROM 水平位置 + 中低曝光
            if (!RetryMove(() => _motor.HorizontalMoveTo(ClampH(eepromHPos), ScanDelayMs), $"well{well}初始水平"))
            {
                Log?.Invoke($"[well{well}] ✗ 电机移动到初始位置失败(已重试)，跳过该well");
                return new WellCalib { Well = well, Note = "电机移动失败" };
            }
            _cam.SetExposure(CenterScanExposure);

            // ── ① 粗对焦：先让 well 清晰，否则画面模糊根本找不到圆（采纳用户洞察：
            //     对焦与居中耦合，焦不对→画面糊→检不出圆→无法居中。故先粗对焦）──
            Log?.Invoke($"[well{well}] ①粗对焦(让well清晰可检)...");
            int coarseZ = CoarseFocus(well);
            _motor.VerticalMoveTo(ClampZ(coarseZ), ScanDelayMs);
            Log?.Invoke($"[well{well}] → 粗对焦Z={coarseZ}");

            // ── ② 旋转居中（此时画面清晰，圆可稳定检出；只优化Y偏移）──
            // 转动培养皿让well在画面里上下(Y)移动；X的~5%固定偏移是相机/转盘硬件偏移,旋转修不了。
            Log?.Invoke($"[well{well}] ②水平全行程定位+居中...");
            var located = HCoarseLocate(well);
            if (located.bestHPos < 0)
            {
                Log?.Invoke($"[well{well}] ✗ 水平全行程未找到圆，跳过该well");
                return new WellCalib { Well = well, Note = "水平全程未检出圆" };
            }
            int bestHPos = located.bestHPos;
            WellCircle bestCircle = located.bestCircle;
            r.HorizontalPulse = bestHPos;
            r.CircleFound = bestCircle != null;
            r.CenterOffsetPct = bestCircle?.OffsetYPct ?? 0;
            if (!RetryMove(() => _motor.HorizontalMoveTo(ClampH(bestHPos), ScanDelayMs), $"well{well}居中水平"))
            {
                Log?.Invoke($"[well{well}] ✗ 电机移动到居中位置失败(已重试)，跳过该well");
                return new WellCalib { Well = well, Note = "居中后电机移动失败" };
            }
            bool centered = bestCircle != null && Math.Abs(r.CenterOffsetPct) < CenterTolPct && bestCircle.Complete;
            Log?.Invoke($"[well{well}] → 居中水平={bestHPos} 残留Y偏移={r.CenterOffsetPct:F1}% " +
                        $"(X={bestCircle?.OffsetXPct ?? 0:F1}%硬件偏移) {(centered ? "✓合格" : "(尽力而为)")}");

            // ── ③ 曝光二分 (well内ROI) ──
            Log?.Invoke($"[well{well}] ③曝光二分...");
            int exp = ExposureMeter.BinarySearch(ExpLo, ExpHi, e =>
            {
                _cam.SetExposure(e);
                // 修复: 等待足够时间让新曝光生效,并丢弃旧帧
                Thread.Sleep(Math.Max(200, e / 5));  // 至少2×曝光时间(单位100µs)
                Grab(); // 丢弃第一帧(可能是旧曝光)
                var b = Grab(); // 使用第二帧
                var c = WellDetector.Detect(b, W, H);
                var info = ExposureMeter.Measure(b, W, H, c);
                OnStep?.Invoke($"曝光二分 e={e}", c, info);
                return info;
            }, m => Log?.Invoke(m));
            r.Exposure = exp;
            _cam.SetExposure(exp);
            Log?.Invoke($"[well{well}] → 曝光={exp}");

            // ── ④ 精对焦 (well圆内ROI, 围绕粗对焦Z小范围密扫) ──
            Log?.Invoke($"[well{well}] ④精对焦...");
            // 修复P0-5: 丢弃旧帧，确保获取稳定画面
            Grab(); // 丢弃第一帧
            var b0 = Grab(); // 使用第二帧
            var circle = WellDetector.Detect(b0, W, H);

            // P1-1: 统一对焦ROI策略 - 使用0.95r保留边缘特征，避免精对焦峰值过弱
            // P0-4: 圆未检出时降级到中央40% ROI(与粗对焦一致)，绝不用全图(背景/反光严重干扰对焦)
            System.Drawing.Rectangle roi = circle.Found
                ? new System.Drawing.Rectangle(
                    Math.Max(0, (int)(circle.Cx - circle.Radius * 0.95)),
                    Math.Max(0, (int)(circle.Cy - circle.Radius * 0.95)),
                    (int)(circle.Radius * 1.9), (int)(circle.Radius * 1.9))
                : CenterRoi40();
            if (!circle.Found)
                Log?.Invoke($"[well{well}] ⚠ 精对焦未检出well圆，对焦ROI降级为中央40%(非全图)");

            int zstep = FineZStep > 0 ? FineZStep : 500;
            int fineLayers = 2 * FineZHalf / zstep + 1;   // 半幅6000步距500 → 25层
            var curve = new List<(int z, double s)>();
            for (int i = 0; i < fineLayers; i++)
            {
                int z = coarseZ - FineZHalf + zstep * i;
                _motor.VerticalMoveTo(ClampZ(z), ScanDelayMs);
                // P0-5: 每次移动后丢弃旧帧
                Grab(); // 丢弃第一帧
                var b = Grab(); // 使用第二帧
                double sc = Sharpness.Compute(b, W, H, roi);
                curve.Add((z, sc));
                OnStep?.Invoke($"精对焦 {i + 1}/{fineLayers} Z={z} 分={sc:F4}", circle, null);
            }

            // P1-3: 峰值平滑与插值 - 提升对焦精度
            // 3点滑动平均平滑
            var smoothed = new List<double>();
            for (int i = 0; i < curve.Count; i++)
            {
                double sum = curve[i].s;
                int cnt = 1;
                if (i > 0) { sum += curve[i - 1].s; cnt++; }
                if (i < curve.Count - 1) { sum += curve[i + 1].s; cnt++; }
                smoothed.Add(sum / cnt);
            }

            // 找平滑后的峰值
            double max = smoothed.Max();
            int pk = smoothed.IndexOf(max);
            double mean = smoothed.Average();

            // 抛物线插值峰顶（如果峰值不在边界）
            int focusZ = curve[pk].z;
            if (pk > 0 && pk < curve.Count - 1)
            {
                double y0 = smoothed[pk - 1], y1 = smoothed[pk], y2 = smoothed[pk + 1];
                double denominator = y0 - 2 * y1 + y2;
                if (Math.Abs(denominator) > 1e-9)
                {
                    double delta = 0.5 * (y0 - y2) / denominator; // 抛物线顶点偏移
                    focusZ = curve[pk].z + (int)(delta * zstep);
                }
            }

            r.FocusZ = focusZ;
            r.PeakSharp = max;
            r.PeakRatio = mean > 1e-9 ? max / mean : 1;

            // 检查对焦质量：峰过弱可能是空well或对焦失败
            if (r.PeakRatio < 1.2)
            {
                Log?.Invoke($"[well{well}] ⚠ 对焦峰过弱(ratio={r.PeakRatio:F2})，可能空well或对焦失败");
                r.Note += "对焦峰弱;";
            }

            _motor.VerticalMoveTo(ClampZ(r.FocusZ), ScanDelayMs);
            Log?.Invoke($"[well{well}] → 最清晰Z={r.FocusZ} 峰值={max:F4} max/mean={r.PeakRatio:F2} " +
                        $"{(r.PeakRatio < 1.2 ? "(弱峰/可能空well)" : "✓有焦点")}");

            r.Note = $"{(r.CircleFound ? "" : "未检到well圆;")}{(r.PeakRatio < 1.2 ? "对焦峰弱;" : "")}";
            return r;
        }

        /// <summary>
        /// 粗对焦：固定中心 ZCoarseCenter±ZCoarseHalf 按 ZCoarseStep 扫描，中央40% ROI 找焦点。
        /// 实测所有 well 焦面集中在 60000~120000，故用固定大窗口，不依赖 EEPROM 零点。
        /// </summary>
        int CoarseFocus(int well)
        {
            int lo = ZCoarseCenter - ZCoarseHalf;
            int hi = ZCoarseCenter + ZCoarseHalf;
            int bestZ = ZCoarseCenter; double bestS = -1;

            // P0-6: 粗对焦使用中央40%区域ROI，避免背景干扰
            var centerROI = CenterRoi40();

            int layers = 0;
            for (int z = lo; z <= hi; z += ZCoarseStep)
            {
                layers++;
                _motor.VerticalMoveTo(ClampZ(z), ScanDelayMs);
                // P0-5: 丢弃旧帧
                Grab();
                var b = Grab();
                double sc = Sharpness.Compute(b, W, H, centerROI); // 中央ROI(避免背景带偏)
                if (sc > bestS) { bestS = sc; bestZ = z; }
                OnStep?.Invoke($"粗对焦 Z={z} (区间{lo}~{hi})", null, null);
            }
            Log?.Invoke($"[well{well}] 粗对焦扫{layers}层 区间[{lo},{hi}] 步距{ZCoarseStep}");
            return bestZ;
        }
    }
}