aivfo-tl-3.0/ivf_tl_operate_2.0/control/IvfTl.AutoFocus/Calib/CalibrationEngine.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading;
using IvfTl.AutoFocus.Imaging;
using IvfTl.Hardware;

namespace IvfTl.AutoFocus.Calib
{
    /// <summary>
    /// 自动标定引擎（可复用，调试页/采集流共用）。封装单well标定四步：
    ///   ①粗对焦(中央40% ROI) → ②水平扫描找最居中位置 → ③曝光二分(well内ROI) → ④精对焦(0.95r ROI + 平滑插值)
    /// 通过回调上报进度(文字)与实时帧(byte[])，便于界面实时显示。
    ///
    /// 【移植说明 M2-01】移植自 autofocustool/Calib/CalibrationEngine.cs。
    /// 算法逻辑/参数/四步流程逐字保留(不重构)；仅把硬件依赖面从 autofocustool 自带的
    /// HouseMotor/SerialCamera 改为 HAL 接口 ISerialChannel/ICamera（13 §3.2/3.3/§④，方法 1:1 映射）：
    ///   _motor.VerticalMoveTo(z,d)   → _serial.VerticalMoveToWait(z,d)
    ///   _motor.HorizontalMoveTo(h,d) → _serial.HorizontalMoveToWait(h,d)
    ///   _cam.GrabRgb()/GetSourceBuffer() → _cam.GrabRgb()/GetFrameBuffer()（保留"丢残留帧"双 Grab 语义）
    ///   _cam.SetExposure/Width/Height → 同名（ICamera 已定义）
    /// 调用方在 Acquire(HardwareUser.AutoFocus) 拿到 lease 后用 new CalibrationEngine(lease.Serial, lease.Camera) 构造。
    /// </summary>
    public class CalibrationEngine
    {
        readonly int W, H;  // 取自相机实际分辨率，绝不能硬编码

        readonly ISerialChannel _serial;
        readonly ICamera _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>水平电机行程下/上限脉冲（实测16个well EEPROM位置达205800，留余量到220000）。</summary>
        public int HMin = 0, HMax = 220000;
        /// <summary>垂直电机行程下/上限脉冲（旧工程软上限 125000）。</summary>
        public int ZMin = 0, ZMaxPulse = 125000;

        // ── 水平居中微调（以各well的EEPROM位置为中心做小范围Y居中）──
        // 实测well间距~9000、EEPROM位置已准(移到位即|偏移|<5%)，故半幅须远小于间距一半(4500)，
        // 只在小范围微调Y居中，绝不大范围扫描(否则会扫到相邻well)。
        /// <summary>水平微调半幅（脉冲，EEPROM位置±range）。</summary>
        public int HFineRange = 2000;
        /// <summary>水平微调步数（±2000内9步→步距500）。</summary>
        public int HFineSteps = 9;

        // ── 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;
        /// <summary>粗对焦开扫前的额外停稳等待(ms)。复位后Z大行程冲到起点需充分停稳，
        /// 否则前几层抓到运动拖影帧形成伪峰。默认2000。</summary>
        public int CoarseSettleMs = 2000;

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

        public CalibrationEngine(ISerialChannel serial, ICamera cam)
        {
            _serial = serial; _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.GetFrameBuffer();
                    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);
                _serial.HorizontalMoveToWait(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>
        /// 标定单个 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(() => _serial.HorizontalMoveToWait(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);
            _serial.VerticalMoveToWait(ClampZ(coarseZ), ScanDelayMs);
            Log?.Invoke($"[well{well}] → 粗对焦Z={coarseZ}");

            // ── ② 旋转居中（此时画面清晰，圆可稳定检出；只优化Y偏移）──
            // 转动培养皿让well在画面里上下(Y)移动；X的~5%固定偏移是相机/转盘硬件偏移,旋转修不了。
            // 实测EEPROM位置已准(移到位即|偏移|<5%)，故以eepromHPos为中心做小范围微调(半幅<well间距一半)，
            // 绝不大范围扫描(否则会扫到相邻well)。
            Log?.Invoke($"[well{well}] ②以EEPROM位置({eepromHPos})为中心微调Y居中...");
            var located = ScanForCenter(well, ClampH(eepromHPos), HFineRange, HFineSteps, 800);
            int bestHPos = located.bestCircle != null ? located.bestHPos : eepromHPos;
            WellCircle bestCircle = located.bestCircle;
            r.HorizontalPulse = bestHPos;
            r.CircleFound = bestCircle != null;
            r.CenterOffsetPct = bestCircle?.OffsetYPct ?? 0;
            if (!RetryMove(() => _serial.HorizontalMoveToWait(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;
                _serial.VerticalMoveToWait(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 += "对焦峰弱;";
            }

            _serial.VerticalMoveToWait(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 零点。
        ///
        /// 【已知偶发问题 2026-06-16，待复现】偶发某个 well 粗对焦选中 ~74000 的伪峰（真实焦面应在 ~86000-92000），
        /// 导致精对焦被锁在 74000±FineZHalf 的小窗口、最终对焦模糊，该 well 输出弱峰(max/mean≈1.0)。
        /// 现象特征：
        ///   · 同一批标定里只有 1 个 well 坏，坏值固定在 ~74000，但坏的 well 编号在不同批次间变化
        ///     （曾出现 well1 坏、well8 坏，非固定孔位、非“第一个标定的 well”）。
        ///   · 人眼在对焦过程能看到清晰画面，但最终选出的不是它。
        /// 已排除的假设（真机 WellSpacing zcurve/zcurvefar 多次实测均无法复现）：
        ///   · 串口大行程移动未到位（残留位+短延时复现 → 峰仍正确 92000）
        ///   · 相机/光源冷启动预热（冷启动立即扫 → 峰仍正确 92000）
        ///   · 清晰度算法（已修 ÷mean，其余 15 well 全部正确）
        ///   · “第一个 well”时序（数据证明 well1 常常正常，坏的在序列中间）
        /// 当前最可疑但未坐实：偶发时序导致某层电机未真正到位/画面在运动中被采样，在 74000 形成伪高频峰；
        /// 或该次该孔在 74000 处恰有杂质/反光的真实但“错误”的清晰特征。
        /// 复现率低，本轮多次测试未触发，留待下次现场捕获（建议届时开 DebugSave 存粗对焦每层图比对）。
        /// </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();

            // 【偶发伪峰修复 2026-06-16】先移到扫描起点并额外等待，确保大行程移动停稳后再开扫。
            // 根因：手动复位会把 Z 打到远端(2000)，正式标定时 Z 要大行程冲到 lo(60000)；
            // 若直接逐层扫描，前几层在电机尚未停稳时抓到“运动拖影帧”，高频被放大成伪峰
            // (实测 z=74000 拍到 5.30 伪峰，骗过粗对焦；精对焦回到同一 Z 仅 3.21 无峰)。
            // 故开扫前先到位 + 额外停稳等待。(后续可改“轮询回读位置确认到位”更精准，暂用延时。)
            _serial.VerticalMoveToWait(ClampZ(lo), ScanDelayMs);
            Thread.Sleep(CoarseSettleMs);
            Grab(); // 丢弃到位前的残留帧

            int layers = 0;
            for (int z = lo; z <= hi; z += ZCoarseStep)
            {
                layers++;
                _serial.VerticalMoveToWait(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; }
                // 诊断：粗对焦逐层分数落盘(排查偶发伪峰,如74000打败真焦面90000)。格式同精对焦。
                Log?.Invoke($"  粗对焦 {layers} Z={z} 分={sc:F4}");
                OnStep?.Invoke($"粗对焦 Z={z} (区间{lo}~{hi})", null, null);
            }
            Log?.Invoke($"[well{well}] 粗对焦扫{layers}层 区间[{lo},{hi}] 步距{ZCoarseStep}");
            // P1: 峰落在扫描区间边界 → 真实焦面很可能在窗口之外，警示（focusZ会偏，需调整ZCoarseCenter/Half）。
            if (bestZ <= lo || bestZ >= hi)
                Log?.Invoke($"[well{well}] ⚠ 粗对焦峰落在区间边界(z={bestZ}, 区间[{lo},{hi}])，" +
                            $"真实焦面可能在窗口外，建议调整 ZCoarseCenter/ZCoarseHalf");
            return bestZ;
        }
    }
}