makcar/app/src/main/java/com/aros/apron/tools/ApronArucoDetect.java

package com.aros.apron.tools;

import android.os.Handler;
import android.os.Looper;

import com.aros.apron.constant.AMSConfig;
import com.aros.apron.entity.ArucoMarker;
import com.aros.apron.entity.Movement;
import com.aros.apron.manager.AlternateLandingManager;

import org.opencv.calib3d.Calib3d;
import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfInt;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
import org.opencv.objdetect.ArucoDetector;
import org.opencv.objdetect.DetectorParameters;
import org.opencv.objdetect.Dictionary;

import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.Executors;
import java.util.concurrent.Future;
import java.util.concurrent.ScheduledExecutorService;
import java.util.concurrent.TimeUnit;

import dji.sdk.keyvalue.key.FlightControllerKey;
import dji.sdk.keyvalue.key.KeyTools;
import dji.v5.common.callback.CommonCallbacks;
import dji.v5.common.error.IDJIError;
import dji.v5.manager.KeyManager;

/**
 * 2帧一处理：0处理-1跳过-2处理-3跳过
 * 先预处理检测，失败再回退原图检测
 */
public class ApronArucoDetect  {

    private static final String TAG = "ApronArucoDetect";

    // ========== 原有参数 ==========
    private static double LENS_OFFSET_X = 0;
    private static double LENS_OFFSET_Y = 0;
    private static final int CENTER_ERR_MAX = 30;
    private static final float SLOW_LAND_SPEED = -0.2f;
    private static final float SLOW_SUPER_SPEED = -0.1f;
    private static final int PIXEL_TRIGGER = 360;
    private static final int TRIGGER_FRAME_THRESHOLD = 2;

    // ========== 原有状态 ==========
    private  boolean isTriggerSuccess;
    private boolean arucoNotFoundTag = false;
    private boolean isStartAruco = false;

    public boolean isStartAruco() {
        return isStartAruco;
    }

    public void setStartAruco(boolean startAruco) {
        isStartAruco = startAruco;
    }

    ScheduledExecutorService executor = Executors.newScheduledThreadPool(1);
    Future<?> lastFuture = null;
    private String TAG_LOG = getClass().getSimpleName();
    Double resultYaw = 0.0;
    private boolean triggerToAlternateLandingPoint;
    long startTime;
    long endTime;
    private int sigleMarkerDetectFailsTimes;
    private boolean dropTimesTag;
    private int dropTimes = 0;
    private boolean isDoublePayload;
    private int trycount = 0;

    public PIDControl pidControlX = null;
    public PIDControl pidControlY = null;
    public int checkThrowingErrorsTimes;
    private int consecutiveTriggerCount = 0;

    // ========== 【新增】2帧一处理计数器 ==========
    private int frameCounter = 0;

    // ========== 原有方法 ==========
    public int getCheckThrowingErrorsTimes() { return checkThrowingErrorsTimes; }
    public void setCheckThrowingErrorsTimes(int v) { checkThrowingErrorsTimes = v; }
    public boolean isTriggerSuccess() { return isTriggerSuccess; }
    public void setTriggerSuccess(boolean v) { isTriggerSuccess = v; }
    public boolean isDoublePayload() { return isDoublePayload; }
    public void setDoublePayload(boolean v) { isDoublePayload = v; }
    public boolean isStartFastStick() { return startFastStick; }
    public void setStartFastStick(boolean v) { startFastStick = v; }
    public boolean isCanLanding() { return canLanding; }
    public void setCanLanding(boolean v) {
        startTime = 0;
        endTime = 0;
        canLanding = v;
    }

    private ApronArucoDetect() {}
    private static class OpenCVHelperHolder {
        private static final ApronArucoDetect INSTANCE = new ApronArucoDetect();
        static { INSTANCE.init(); }
    }
    public static ApronArucoDetect getInstance() { return OpenCVHelperHolder.INSTANCE; }

    public void init() {
        pidControlX = new PIDControl(0.65f, 0.02f, 0.2f, 0.05f, 2.5f, 0.1f);
        pidControlY = new PIDControl(0.65f, 0.02f, 0.2f, 0.05f, 2.5f, 0.1f);
        pidControlX.reset();
        pidControlY.reset();
    }

    public boolean startFastStick;
    public boolean canLanding;

    // ========== 【核心修改】主入口：2帧一处理 + 预处理回退 ==========
    public void detectArucoTags(int height, int width, byte[] data, Dictionary dictionary) {
        isTriggerSuccess = true;
        Movement.getInstance().setVirtualStickEnableReason(2);

        // 原有过滤
        if (isStartAruco || startFastStick) {
            LogUtil.log(TAG_LOG, "过滤:" + isStartAruco + startFastStick);
            if (!isStartAruco && startFastStick) {
                checkThrowingErrorsTimes++;
            }
            return;
        }

        // ========== 【关键】2帧一处理：0处理-1跳过-2处理-3跳过 ==========
        int currentFrame = frameCounter++;
        boolean shouldProcess = (currentFrame % 2 == 0);  // 偶数帧处理，奇数帧跳过

        if (!shouldProcess) {
            // 【关键】跳过的帧：啥也不干，直接return，让飞机自己飘
            LogUtil.log(TAG_LOG, "【跳过帧】" + currentFrame + " 让飞机自稳");
            return;
        }

        LogUtil.log(TAG_LOG, "【处理帧】" + currentFrame + " 执行修正");

        isStartAruco = true;

        if (lastFuture != null && !lastFuture.isDone()) {
            LogUtil.log(TAG_LOG, "break---");
            lastFuture.cancel(true);
        }

        // 动态偏移（原有）
        int ultHeight = Movement.getInstance().getUltrasonicHeight();
        if (ultHeight >= 30) { LENS_OFFSET_X = 0; LENS_OFFSET_Y = 0; }
        else if (ultHeight >= 20) { LENS_OFFSET_X = 0; LENS_OFFSET_Y = 0; }
        else if (ultHeight >= 10) { LENS_OFFSET_X = 20; LENS_OFFSET_Y = 10; }
        else if (ultHeight >= 5) { LENS_OFFSET_X = 30; LENS_OFFSET_Y = 20; }
        else { LENS_OFFSET_X = 55; LENS_OFFSET_Y = 35; }

        // ========== 【核心逻辑】先预处理检测，失败回退原图 ==========
        lastFuture = executor.schedule(new Runnable() {
            @Override
            public void run() {
                try {
                    Mat yuvMat = new Mat(height + height / 2, width, CvType.CV_8UC1);
                    yuvMat.put(0, 0, data);
                    Mat rgbMat = new Mat();
                    Imgproc.cvtColor(yuvMat, rgbMat, Imgproc.COLOR_YUV2BGR_I420);
                    Mat grayImgMat = new Mat();
                    Imgproc.cvtColor(rgbMat, grayImgMat, Imgproc.COLOR_BGR2GRAY);

                    MatOfInt ids = new MatOfInt();
                    List<Mat> corners = new ArrayList<>();
                    Mat corner6 = new Mat();

                    DetectorParameters parameters = new DetectorParameters();
                    ArucoDetector detector = new ArucoDetector(dictionary, parameters);

                    // ========== 【关键修改】第1次：预处理图检测 ==========
                    Mat processedMat = fixedPreprocess(grayImgMat);
                    detector.detectMarkers(processedMat, corners, ids);
                    LogUtil.log(TAG_LOG, "预处理图检测: " + (ids.empty() ? "未检出" : "成功，数量=" + corners.size()));

                    // ========== 【关键修改】第2次：如果失败，回退原图检测 ==========
                    if (ids.empty()) {
                        LogUtil.log(TAG_LOG, "预处理失败，回退原图检测");
                        corners.clear(); // 清空之前的结果
                        detector.detectMarkers(grayImgMat, corners, ids);
                        LogUtil.log(TAG_LOG, "原图检测: " + (ids.empty() ? "仍未检出" : "成功，数量=" + corners.size()));
                    }

                    // 释放预处理图（必须！避免内存泄漏）
                    processedMat.release();

                    // 只保留6号码
                    ids = keepOnly6(ids, corners);

                    boolean marker6Found = false;
                    if (!ids.empty()) {
                        trycount = 0;
                        arucoNotFoundTag = false;
                        int[] idArray = ids.toArray();
                        int ultrasonicHeight = Movement.getInstance().getUltrasonicHeight();

                        if (idArray.length == 1 && idArray[0] == 6) {
                            marker6Found = true;
                            corner6 = corners.get(0);
                            Point[] points = new Point[4];
                            for (int i = 0; i < 4; i++) {
                                double[] p = corner6.get(0, i);
                                points[i] = new Point(p[0], p[1]);
                            }

                            double pixelWidth = calculateDistance(points[0], points[1]);

                            double centerX = (points[0].x + points[1].x + points[2].x + points[3].x) / 4.0 + LENS_OFFSET_X;
                            double errX = Math.abs(centerX - rgbMat.width() / 2.0);
                            double centerY = (points[0].y + points[1].y + points[2].y + points[3].y) / 4.0 + LENS_OFFSET_Y;
                            double errY = Math.abs(centerY - rgbMat.height() / 2.0);

                            LogUtil.log(TAG_LOG, "像素" + (int) pixelWidth +
                                    " errX=" + (int) errX + " errY=" + (int) errY + " (含偏移" + LENS_OFFSET_X + ")");

                            // 速降判断（原有）
                            if (!startFastStick) {
                                if (pixelWidth >= PIXEL_TRIGGER &&
                                        ultrasonicHeight <= 4 &&
                                        errX < CENTER_ERR_MAX &&
                                        errY < CENTER_ERR_MAX) {

                                    consecutiveTriggerCount++;
                                    LogUtil.log(TAG_LOG, "速降条件满足，累计帧数: " + consecutiveTriggerCount);

                                    if (consecutiveTriggerCount >= TRIGGER_FRAME_THRESHOLD) {
                                        startFastStick = true;
                                        consecutiveTriggerCount = 0;

                                        LogUtil.log(TAG_LOG, "【触发速降】连续满足" + TRIGGER_FRAME_THRESHOLD +
                                                "帧，pixel=" + (int) pixelWidth + " errX=" + (int) errX);

                                        handler.postDelayed(new Runnable() {
                                            @Override
                                            public void run() {
                                                handler.post(runnable);
                                                LogUtil.log(TAG_LOG, "延迟结束，开始速降");
                                            }
                                        }, 300);
                                        return;
                                    }
                                } else {
                                    if (consecutiveTriggerCount > 0) {
                                        LogUtil.log(TAG_LOG, "速降条件中断，重置累计帧数（原因：像素=" + (int)pixelWidth +
                                                " 超声=" + ultrasonicHeight + " errX=" + (int)errX + " errY=" + (int)errY + ")");
                                        consecutiveTriggerCount = 0;
                                    }
                                }
                            }
                        }

                        // 原有修正逻辑
                        if (marker6Found) {
                            moveOnArucoDetected(new ArucoMarker(1, corner6, 0.24f), rgbMat.width(), rgbMat.height());
                        }
                        dropTimesTag = true;

                    } else {
                        // 原有丢失处理
                        LogUtil.log(TAG_LOG, "找不到了二维码");
                        if (!arucoNotFoundTag) {
                            startTime = System.currentTimeMillis();
                            arucoNotFoundTag = true;
                        }
                        endTime = System.currentTimeMillis();
                        long lostDuration = endTime - startTime;

                        if (lostDuration > 1000 && lostDuration <= 12000) {
                            if (Movement.getInstance().getUltrasonicHeight() <= 20) {
                                DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 3f);
                                if (dropTimes > Integer.parseInt(AMSConfig.getInstance().getAlternateLandingTimes())) {
                                    LogUtil.log(TAG_LOG, "超过复降限制,去备降点");
                                    AlternateLandingManager.getInstance().startTaskProcess(null);
                                    Movement.getInstance().setAlternate(true);
                                    return;
                                }
                                if (dropTimesTag) {
                                    dropTimesTag = false;
                                    dropTimes++;
                                    LogUtil.log(TAG_LOG, "复降第:" + dropTimes + "次");
                                }
                            } else {
                                LogUtil.log(TAG_LOG, "执行位移");
                                DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f);
                            }
                        } else if (lostDuration > 8000) {
                            LogUtil.log(TAG_LOG, "判定未识别到二维码,飞往备降点");
                            AlternateLandingManager.getInstance().startTaskProcess(null);
                            Movement.getInstance().setAlternate(true);
                        }
                    }

                    // 释放资源
                    grayImgMat.release();
                    rgbMat.release();
                    yuvMat.release();
                    ids.release();

                    if (!corners.isEmpty()) {
                        for (Mat c : corners) {
                            if (c != null) c.release();
                        }
                    }
                    isStartAruco = false;

                } catch (Exception e) {
                    LogUtil.log(TAG_LOG, "Exception" + e);
                    isStartAruco = false;
                }
            }
        }, 0, TimeUnit.MILLISECONDS);
    }
    // ========== 以下全部原有方法，一点不改 ==========
    private static MatOfInt keepOnly6(MatOfInt ids, List<Mat> corners) {
        if (ids.empty()) return ids;
        int[] idArr = ids.toArray();
        List<Integer> keepIdx = new ArrayList<>();
        for (int i = 0; i < idArr.length; i++) if (idArr[i] == 6) keepIdx.add(i);

        List<Mat> tmpCorners = new ArrayList<>(keepIdx.size());
        for (int i : keepIdx) tmpCorners.add(corners.get(i));

        corners.clear();
        ids.release();
        int[] newIds = new int[keepIdx.size()];
        for (int j = 0; j < keepIdx.size(); j++) newIds[j] = 6;
        corners.addAll(tmpCorners);
        return new MatOfInt(newIds);
    }
    /**
     * 简化预处理：双边滤波+CLAHE+高反差保留
     * 参数别改：适配H30T广角画质
     */
    /**
     * 增强版预处理：双边滤波 + CLAHE + Unsharp Mask（反锐化掩膜）
     * 参数别改：amount=1.2 是 FPV 运动模糊的推荐强度
     */
    private Mat fixedPreprocess(Mat src) {
        Mat result = new Mat();
        try {
            // 【新增】判断亮度，区分白天/夜景
            Scalar meanValue = Core.mean(src);
            double meanBrightness = meanValue.val[0];

            if (meanBrightness < 80) {  // 夜景模式（FPV晚上通常<80）
                LogUtil.log(TAG_LOG, "【夜景预处理】亮度=" + (int)meanBrightness);

                // Step 1: 中值滤波（去椒盐噪点，比双边滤波更适合夜景）
                Mat denoised = new Mat();
                Imgproc.medianBlur(src, denoised, 5);  // 5x5，去噪且保边

                // Step 2: 直方图均衡化（全局对比度拉伸，把黑灰白拉开）
                Mat equalized = new Mat();
                Imgproc.equalizeHist(denoised, equalized);

                // Step 3: 轻度CLAHE（局部微调，避免过曝）
                Mat clahe = new Mat();
                Imgproc.createCLAHE(2.0, new Size(8, 8)).apply(equalized, clahe);  // 2.0比白天4.0保守

                // Step 4: 夜景下降低锐化强度（避免放大残余噪点）
                Mat blurred = new Mat();
                Imgproc.GaussianBlur(clahe, blurred, new Size(0, 0), 2.0);
                Mat detail = new Mat();
                Core.subtract(clahe, blurred, detail);

                double amount = 0.6;  // 【关键】夜景降到0.6，白天1.2会放大噪点
                Core.multiply(detail, new Scalar(amount), detail);
                Core.add(clahe, detail, result);

                // 限制范围
                Core.min(result, new Scalar(255), result);
                Core.max(result, new Scalar(0), result);

                // 释放
                denoised.release();
                equalized.release();
                blurred.release();
                detail.release();
                clahe.release();

                LogUtil.log(TAG_LOG, "夜景处理完成：中值滤波+均衡化+弱锐化(amount=" + amount + ")");
                return result;
            }

            // 【原逻辑】白天模式（亮度>=80）
            LogUtil.log(TAG_LOG, "【白天预处理】亮度=" + (int)meanBrightness);

            // Step 1: 双边滤波（保边缘降噪）
            Mat filtered = new Mat();
            Imgproc.bilateralFilter(src, filtered, 9, 75, 75);

            // Step 2: CLAHE 局部对比度增强
            Mat clahe = new Mat();
            Imgproc.createCLAHE(4.0, new Size(8, 8)).apply(filtered, clahe);
            filtered.release();

            // Step 3: Unsharp Mask（反锐化掩膜）
            Mat blurred = new Mat();
            Imgproc.GaussianBlur(clahe, blurred, new Size(0, 0), 2.0);

            Mat detail = new Mat();
            Core.subtract(clahe, blurred, detail);

            double amount = 1.2;  // 白天可以用1.2
            Core.multiply(detail, new Scalar(amount), detail);
            Core.add(clahe, detail, result);

            Core.min(result, new Scalar(255), result);
            Core.max(result, new Scalar(0), result);

            blurred.release();
            detail.release();
            clahe.release();

            LogUtil.log(TAG_LOG, "白天处理完成：双边滤波+CLAHE+UnsharpMask(amount=" + amount + ")");
            return result;

        } catch (Exception e) {
            LogUtil.log(TAG_LOG, "预处理失败: " + e.getMessage());
            src.copyTo(result);
            return result;
        }
    }

    private double calculateDistance(Point p1, Point p2) {
        double dx = p2.x - p1.x;
        double dy = p2.y - p1.y;
        return Math.sqrt(dx * dx + dy * dy);
    }

    public void setDetectedBigMarkers() {
        startFastStick = false;
        isStartAruco = false;
        consecutiveTriggerCount = 0;
        frameCounter = 0;  // 【新增】重置帧计数
    }


    private double calculateYawErrorFromCorners(Point[] pts) {
        double dxTop = pts[1].x - pts[0].x;
        double dyTop = pts[1].y - pts[0].y;
        double angleTop = Math.toDegrees(Math.atan2(dyTop, dxTop));
        double dxBottom = pts[2].x - pts[3].x;
        double dyBottom = pts[2].y - pts[3].y;
        double angleBottom = Math.toDegrees(Math.atan2(dyBottom, dxBottom));
        double yawError = (angleTop + angleBottom) / 2.0;
        while (yawError > 180) yawError -= 360;
        while (yawError < -180) yawError += 360;
        return yawError;
    }

    private void moveOnArucoDetected(ArucoMarker marker, int imageWidth, int imageHeight) {
        Mat corner6 = marker.getConner();
        Point[] pts = new Point[4];
        for (int i = 0; i < 4; i++) {
            double[] p = corner6.get(0, i);
            pts[i] = new Point(p[0], p[1]);
        }

        double cx = (pts[0].x + pts[1].x + pts[2].x + pts[3].x) / 4.0 + LENS_OFFSET_X;
        double cy = (pts[0].y + pts[1].y + pts[2].y + pts[3].y) / 4.0 + LENS_OFFSET_Y;
        Point screenCenter = new Point(imageWidth / 2.0, imageHeight / 2.0);

        double errX = (cx - screenCenter.x);
        double errY = (cy - screenCenter.y);
        int currentHeight = Movement.getInstance().getUltrasonicHeight();

        double scaleFactor;
        if (currentHeight >= 50) {
            scaleFactor = 300.0;   // 50m: 像素3, 大力修正
        } else if (currentHeight >= 30) {
            scaleFactor = 500.0;   // 30m: 像素5
        } else if (currentHeight >= 20) {
            scaleFactor = 600.0;   // 20m: 像素8
        } else if (currentHeight >= 10) {
            scaleFactor = 700.0;   // 10m: 像素16
        }  else {
            scaleFactor = 900.0;  // 1m: 像素159, 温柔修正
        }

        pidControlX.setInputFilterAll((float) (errX / scaleFactor));
        pidControlY.setInputFilterAll((float) (-errY / scaleFactor));

        float rawVx = pidControlX.get_pid();
        float rawVy = pidControlY.get_pid();
        float yawRate = 0f;

        float vx = (float) Math.max(-0.2, Math.min(0.2, rawVx));
        float vy = (float) Math.max(-0.2, Math.min(0.2, rawVy));

        double pixelWidth = Math.sqrt(Math.pow(pts[1].x - pts[0].x, 2) +
                Math.pow(pts[1].y - pts[0].y, 2));

        if (currentHeight >= 10 && currentHeight <= 55) {
            double yawError = calculateYawErrorFromCorners(pts);
            double absError = Math.abs(yawError);

            if (absError > 10.0) {
                float targetRate;
                if (absError > 100.0) targetRate = 50.0f;
                else if (absError > 50.0) targetRate = 30.0f;
                else if (absError > 30.0) targetRate = 20.0f;
                else targetRate = 10.0f;
                yawRate = yawError > 0 ? targetRate : -targetRate;

                String speedLabel = absError > 100 ? "高速50" :
                        absError > 50 ? "中速30" :
                                absError > 30 ? "低速20" : "微调10";
                LogUtil.log(TAG_LOG, "机头矫正：误差=" + (int)yawError + "° 转速=" + yawRate + "°/s [" + speedLabel + "]");
            } else {
                yawRate = 0.0f;
                LogUtil.log(TAG_LOG, "机头已对准：偏航误差=" + (int)yawError + "°");
            }
        } else {
            yawRate = 0.0f;
        }

        float vz;
        if (currentHeight <= 4) {
            vz = 0.0f;
            if (Math.abs(errX) > 120) {
                vx = rawVx > 0 ? 0.135f : -0.135f;
            } else if (Math.abs(errX) > 80) {
                vx = rawVx > 0 ? 0.09f : -0.09f;
            } else if (Math.abs(errX) > 60) {
                vx = rawVx > 0 ? 0.07f : -0.07f;
            } else if (Math.abs(errX) > 30) {
                vx = rawVx > 0 ? 0.05f : -0.05f;
            } else {
                vx = 0f;
            }


            if (Math.abs(errY) > 120) {
                vy = rawVy > 0 ? 0.135f : -0.135f;
            } else if (Math.abs(errY) > 80) {
                vy = rawVy > 0 ? 0.09f : -0.09f;
            } else if (Math.abs(errY) > 60) {
                vy = rawVy > 0 ? 0.07f : -0.07f;
            } else if (Math.abs(errY) > 30) {
                vy = rawVy > 0 ? 0.05f : -0.05f;
            } else {
                vy = 0f;  // 【修正】
            }

        } else if (currentHeight <= 8) {
            vz = SLOW_SUPER_SPEED;
        } else {
            vz = SLOW_LAND_SPEED;
        }

        DroneHelper.getInstance().moveVxVyYawrateHeight(vx, vy, yawRate, vz);

        LogUtil.log(TAG_LOG, "vx" + vx + "vy" + vy + " errX=" + (int) errX + " errY=" + (int) errY +
                " pixelW=" + (int) pixelWidth + " vz=" + vz + " ult=" + currentHeight + " yaw=" + yawRate);
    }

    private int handlerCallbackCount = 0;
    private Handler handler = new Handler(Looper.getMainLooper());
    private Runnable runnable = new Runnable() {
        @Override
        public void run() {
            performOperation();
            if (handlerCallbackCount < 20) {
                handler.postDelayed(this, 50);
            } else {
                performNextStep();
            }
        }
    };

    private void performOperation() {
        LogUtil.log(TAG_LOG, "快速下拉中..." + handlerCallbackCount);
        DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -5);
        handlerCallbackCount++;
    }

    private void performNextStep() {
        canLanding = true;
        handlerCallbackCount = 0;
        dropTimes = 0;
        handler.removeCallbacks(runnable);
    }
}