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.sdk.keyvalue.value.common.EmptyMsg;
import dji.v5.common.callback.CommonCallbacks;
import dji.v5.common.error.IDJIError;
import dji.v5.manager.KeyManager;

/**
 * 2帧一处理：0处理-1跳过-2处理-3跳过
 * 跳过的帧直接return，啥也不干，让飞机自己飘
 */
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;
    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 = new PIDControl(0.8f, 0.002f, 0.09f, 0.05f, 2.0f, 0.05f);
//        pidControlY = new PIDControl(0.8f, 0.002f, 0.09f, 0.05f, 2.0f, 0.05f);
        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);

                    // ========== 【极致容错】双重检测策略 ==========
                    // 策略1：增强图检测（适应低光/反光）
                    Mat enhancedMat = createEnhancedImage(grayImgMat);
                    MatOfInt ids = new MatOfInt();
                    List<Mat> corners = new ArrayList<>();

                    DetectorParameters params = createUltraTolerantParams();
                    ArucoDetector detector = new ArucoDetector(dictionary, params);
                    detector.detectMarkers(enhancedMat, corners, ids);

                    // 策略2：如果失败，用原图兜底（适应过曝）
                    if (ids.empty()) {
                        corners.clear();
                        detector.detectMarkers(grayImgMat, corners, ids);
                    }

                    // 策略3：如果还失败，降采样检测（适应高分辨率模糊）
                    if (ids.empty()) {
                        corners.clear();
                        Mat downscaled = new Mat();
                        Imgproc.resize(grayImgMat, downscaled, new Size(grayImgMat.width() * 0.8, grayImgMat.height() * 0.8));
                        detector.detectMarkers(downscaled, corners, ids);
                        // 坐标映射回原图比例
                        if (!ids.empty()) {
                            for (Mat corner : corners) {
                                Core.multiply(corner, new Scalar(1.25, 1.25), corner);
                            }
                        }
                        downscaled.release();
                    }

                    ids = keepOnly6(ids, corners);

                    boolean marker6Found = false;
                    Mat corner6 = new Mat();

                    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);

                                        // 清理资源
                                        enhancedMat.release();
                                        grayImgMat.release();
                                        rgbMat.release();
                                        yuvMat.release();
                                        ids.release();
                                        if (!corners.isEmpty()) {
                                            for (Mat c : corners) if (c != null) c.release();
                                        }
                                        isStartAruco = false;
                                        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);

                                    // 清理资源
                                    enhancedMat.release();
                                    grayImgMat.release();
                                    rgbMat.release();
                                    yuvMat.release();
                                    ids.release();
                                    if (!corners.isEmpty()) {
                                        for (Mat c : corners) if (c != null) c.release();
                                    }
                                    isStartAruco = false;
                                    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);
                        }
                    }
                    // 原有资源释放
                    enhancedMat.release();
                    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);
    }

    // ========== 【极致容错】图像增强：全高度段通用 ==========
    private Mat createEnhancedImage(Mat src) {
        Mat result = new Mat();
        try {
            // 1. 多尺度CLAHE（适应不同亮度）
            Mat claheMat = new Mat();
            Imgproc.createCLAHE(2.0, new Size(8, 8)).apply(src, claheMat);

            // 2. 中值滤波去噪（比双边快，保边足够）
            Mat filtered = new Mat();
            Imgproc.medianBlur(claheMat, filtered, 5);
            claheMat.release();

            // 3. 自适应阈值（块大小动态，适应全高度）
            Mat binary = new Mat();
            Imgproc.adaptiveThreshold(filtered, binary, 255,
                    Imgproc.ADAPTIVE_THRESH_GAUSSIAN_C,
                    Imgproc.THRESH_BINARY, 41, 3);  // 块41更大，常数3更敏感
            filtered.release();

            // 4. 形态学操作（连接断裂边框）
            Mat morph = new Mat();
            Mat kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5));
            Imgproc.morphologyEx(binary, morph, Imgproc.MORPH_CLOSE, kernel);
            kernel.release();
            binary.release();

            // 5. 轻度锐化（突出边缘，不过度）
            Mat sharpened = new Mat();
            Mat blurred = new Mat();
            Imgproc.GaussianBlur(morph, blurred, new Size(0, 0), 3.0);
            Core.addWeighted(morph, 1.3, blurred, -0.3, 0, sharpened);
            morph.release();
            blurred.release();

            return sharpened;

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



    private DetectorParameters createUltraTolerantParams() {
        DetectorParameters params = new DetectorParameters();

        // 全高度段：4dm=375px, 9dm=190px, 50dm=19px
        params.set_minMarkerPerimeterRate(0.003f);  // 降到0.003，19像素也能检

        // 畸变/反光/模糊宽容
        params.set_polygonalApproxAccuracyRate(0.12f);  // 更松

        params.set_cornerRefinementMethod(1);  // SUBPIX
        params.set_cornerRefinementWinSize(3);  // 降到3更快
        params.set_cornerRefinementMaxIterations(30);
        params.set_cornerRefinementMinAccuracy(0.12f);  // 放宽收敛

        // 阈值范围更大，适应全光照
        params.set_adaptiveThreshWinSizeMin(3);
        params.set_adaptiveThreshWinSizeMax(63);  // 更大
        params.set_adaptiveThreshWinSizeStep(10);
        params.set_adaptiveThreshConstant(3);  // 配合预处理

        params.set_minCornerDistanceRate(0.02f);
        params.set_minMarkerLengthRatioOriginalImg(0.03f);  // 更宽容
        params.set_minDistanceToBorder(1);  // 边缘也检

        params.set_perspectiveRemovePixelPerCell(2);  // 降到2，小像素精细
        params.set_perspectiveRemoveIgnoredMarginPerCell(0.2f);

        params.set_maxErroneousBitsInBorderRate(0.6f);  // 60%容错

        params.set_detectInvertedMarker(false);

        return params;
    }

    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, -4);
        handlerCallbackCount++;
    }

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