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

package com.aros.apron.tools;


import android.os.Environment;
import android.os.Handler;
import android.os.Looper;
import android.util.Log;

import androidx.annotation.NonNull;

import com.aros.apron.constant.AMSConfig;
import com.aros.apron.entity.ArucoMarker;
import com.aros.apron.entity.ArucoMarkerDimensions;
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.imgcodecs.Imgcodecs;
import org.opencv.imgproc.Imgproc;
import org.opencv.objdetect.ArucoDetector;
import org.opencv.objdetect.DetectorParameters;
import org.opencv.objdetect.Dictionary;

import java.io.File;
import java.text.SimpleDateFormat;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Date;
import java.util.List;
import java.util.Locale;
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;

public class ApronArucoDetect {

    //是否触发识别(如果丢失图传，此值为false)
    private boolean isTriggerSuccess;

    //没识别到二维码
    private boolean arucoNotFoundTag=false;

    private boolean isStartAruco = false;
    //    public ExecutorService mThreadPool = Executors.newSingleThreadExecutor();
    ScheduledExecutorService executor = Executors.newScheduledThreadPool(1);
    Future<?> lastFuture = null;

    private String TAG = 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;

    // 【关键修改1】M400下视镜头右后方补偿量（像素，正值向右补偿）
    // 如果还往左偏就调大，往右偏就调小，建议范围 30-80
    private static  double LENS_OFFSET_X = 0;
    private static  double LENS_OFFSET_Y = 0;
    // 【关键修改2】放宽居中判定阈值（原40，补偿后改为60更合理）
    private static final int CENTER_ERR_MAX = 25;    // 居中误差（px）

    public PIDControl pidControlX = null;
    public PIDControl pidControlY = null;

    //为了解决降落后不停浆问题，增加记录，开启速降时startFastStick之后的过滤次数
    public int checkThrowingErrorsTimes;

    public int getCheckThrowingErrorsTimes() {
        return checkThrowingErrorsTimes;
    }

    public void setCheckThrowingErrorsTimes(int checkThrowingErrorsTimes) {
        this.checkThrowingErrorsTimes = checkThrowingErrorsTimes;
    }

    public boolean isTriggerSuccess() {
        return isTriggerSuccess;
    }

    public void setTriggerSuccess(boolean triggerSuccess) {
        isTriggerSuccess = triggerSuccess;
    }

    public boolean isDoublePayload() {
        return isDoublePayload;
    }
    private Handler riseHandler = new Handler(Looper.getMainLooper());
    public void setDoublePayload(boolean doublePayload) {
        isDoublePayload = doublePayload;
    }


    private ApronArucoDetect() {
        // 构造函数里也可以调用，但静态代码块更可靠
    }

    private static class OpenCVHelperHolder {
        private static final ApronArucoDetect INSTANCE = new ApronArucoDetect();

        static {
            INSTANCE.init();
        }
    }
    private int trycount=0;

    public static ApronArucoDetect getInstance() {
        return OpenCVHelperHolder.INSTANCE;
    }

    private static final float SLOW_LAND_SPEED = -0.3f;  // 远场慢降
    private static final float SLOW_SUPER_SPEED = -0.1f;  //  超近慢降

    private static final int PIXEL_TRIGGER = 360;   // 近场像素阈值

    public void init() {
        pidControlX = new PIDControl(0.6f, 0.08f, 0.18f, 0.05f, 2.5f, 0.04f);
        pidControlY = new PIDControl(0.6f, 0.08f, 0.18f, 0.05f, 2.5f, 0.04f);
        pidControlX.reset();
        pidControlY.reset();
    }


    public void detectArucoTags(int height, int width, byte[] data, Dictionary dictionary) {

        //这里说明图传正常
        isTriggerSuccess = true;
        //0未启用 虚拟摇杆启用原因 1异常拉高返航触发 2视觉降落触发 3手动触发
        Movement.getInstance().setVirtualStickEnableReason(2);
        //保证一帧进入同时 保证只有一次速降
        if (isStartAruco || startFastStick) {
             LogUtil.log(TAG, "过滤:" + isStartAruco + startFastStick);
            if (!isStartAruco && startFastStick) {
                checkThrowingErrorsTimes++;
            }
            return;
        }

        isStartAruco = true;

        /* 如果上一帧任务还没跑完，直接 cancel */
        if (lastFuture != null && !lastFuture.isDone()) {
            LogUtil.log(TAG, "break---");
            lastFuture.cancel(true);
        }
        // 根据超声波高度分段固定偏移（避免 pixelWidth 跳变导致抖动）
        int ultHeight = Movement.getInstance().getUltrasonicHeight(); // dm
        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 = 50;  // 建议从 60 开始试，80 可能太大
            LENS_OFFSET_Y = 25;
        }



        //LogUtil.log(TAG, "执行了");
        lastFuture = executor.schedule(new Runnable() {
            @Override
            public void run() {
                try {
                    /* ---------- 1. YUV → BGR → Gray ---------- */
                    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);

                    /* 【OpenCV 4.9+】温和预处理（应对飞行模糊，提升识别率） */
                    Mat processedMat = fixedPreprocess(grayImgMat);

                    /* 2. 检测二维码（OpenCV 4.12+） */
                    MatOfInt ids = new MatOfInt();
                    List<Mat> corners = new ArrayList<>();
                    Mat corner6 = new Mat();

                    // 【OpenCV 4.12+】创建 ArucoDetector
                    DetectorParameters parameters = new DetectorParameters();
                    ArucoDetector detector = new ArucoDetector(dictionary, parameters);
                    // 先用预处理图像检测
                    detector.detectMarkers(processedMat, corners, ids);

                    // 如果预处理检测不到，回退到原图
                    if (ids.empty()) {
                        corners.clear();
                        detector.detectMarkers(grayImgMat, corners, ids);
                    }

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

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

                        // 如果只有一个而且是6
                        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]);

                            // 【关键修改3】降落判断加上镜头偏移补偿（向右补偿）
                            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, "像素" + (int) pixelWidth +
                                    " errX=" + (int) errX + " errY=" + (int) errY + " (含偏移" + LENS_OFFSET_X + ")");

                            /* 近场 + 对准 + 像素够 → 延迟0.5s后速降 */
                            if (!startFastStick &&
                                    pixelWidth >= PIXEL_TRIGGER &&
//        flyingHeight <= 1 &&
                                    ultrasonicHeight <= 4 &&
                                    errX < CENTER_ERR_MAX &&
                                    errY < CENTER_ERR_MAX) {

                                startFastStick = true;  // 先标记，防止重复触发
                                LogUtil.log(TAG, "6号居中+近场满足条件，0.5s后触发速降 pixel=" + (int) pixelWidth +
                                        " errX=" + (int) errX + " errY=" + (int) errY);

                                // 延迟500ms执行速降
                                handler.postDelayed(new Runnable() {
                                    @Override
                                    public void run() {
                                        handler.post(runnable);  // 启动速降循环
                                        LogUtil.log(TAG, "0.5s延迟结束，开始速降");
                                    }
                                }, 500);

                                return;
                            }
                        }
                        if(marker6Found){
                            //执行位移修正
                            moveOnArucoDetected(new ArucoMarker(1, corner6, 0.24f), rgbMat.width(), rgbMat.height());
                        }
                        dropTimesTag = true;
                    } else {
                        LogUtil.log(TAG, "找不到了二维码");
                        if (!arucoNotFoundTag) {
                            startTime = System.currentTimeMillis();
                            arucoNotFoundTag = true;
                        }
                        endTime = System.currentTimeMillis();
                        long lostDuration = endTime - startTime;
                        //1s到8s内
                        if (lostDuration > 1200 && lostDuration <= 8000) {
                            if (Movement.getInstance().getUltrasonicHeight()<=30) {
                                DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 0.8f);
                                if (dropTimes > Integer.parseInt(AMSConfig.getInstance().getAlternateLandingTimes())) {
                                    LogUtil.log(TAG, "超过复降限制,去备降点");
                                    AlternateLandingManager.getInstance().startTaskProcess(null);
                                    return;
                                }
                                if (dropTimesTag) {
                                    dropTimesTag = false;
                                    dropTimes++;
                                    LogUtil.log(TAG, "复降第:" + dropTimes + "次");
                                }
                            } else {

                                LogUtil.log(TAG, "执行位移");
                                DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f);
                            }
                            // 超过8s
                        } else if (lostDuration > 8000) {
                            LogUtil.log(TAG, "判定未识别到二维码,飞往备降点");
                            AlternateLandingManager.getInstance().startTaskProcess(null);
                        }
                    }
                    // 释放资源（安全版）
                    processedMat.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, "Exception" + e);  // 第一个日志
                    isStartAruco = false;
                }
            }
        }, 0, TimeUnit.MILLISECONDS);
    }


    /**
     * 只保留 id==6 的 marker
     * 返回新的 MatOfInt（旧对象已 release），corners 列表同步
     */
    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);

        // 1. 先把要保留的角点缓存到临时列表
        List<Mat> tmpCorners = new ArrayList<>(keepIdx.size());
        for (int i : keepIdx) tmpCorners.add(corners.get(i));

        // 2. 再清空原列表
        corners.clear();

        // 3. 重建 ids 并回填 corners
        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 + 锐化（不做二值化）
     */
    private Mat fixedPreprocess(Mat src) {
        Mat result = new Mat();
        try {
            // 1. 双边滤波（去噪 + 保边）
            Mat filtered = new Mat();
            Imgproc.bilateralFilter(src, filtered, 9, 75, 75);

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

            // 3. 锐化（Unsharp Masking）
            Mat blurred = new Mat();
            Imgproc.GaussianBlur(clahe, blurred, new Size(0, 0), 2.0);
            Core.addWeighted(clahe, 1.5, blurred, -0.5, 0, result);

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

            return result;
        } catch (Exception e) {

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


    /**
     * 61     * 计算两个点之间的欧几里得距离
     * 62     * @param p1 第一个点
     * 63     * @param p2 第二个点
     * 64     * @return 两点之间的距离
     * 65
     */
    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;     // 允许下一帧再进检测
    }

    /**
     * 只处理一个 ArucoMarker（6 号码）
     */
    private void moveOnArucoDetected(ArucoMarker marker,   // ← 不再是 List
                                     int imageWidth,
                                     int imageHeight) {

        /* 1. 取角点 */
        Mat corner6 = marker.getConner();   // ← 直接拿，不用 get(0)
        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]);
        }

        /* 2. 中心 + 误差（带镜头偏移补偿） */
        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);

        /* 3. PID 微调水平 */
        pidControlX.setInputFilterAll((float) (errX / 700.0));
        pidControlY.setInputFilterAll((float) (-errY / 700.0));


        float rawVx = pidControlX.get_pid();
        float rawVy = pidControlY.get_pid();


//        if (Math.abs(rawVx) < 0.005f) rawVx = 0.0f;
//        if (Math.abs(rawVy) < 0.005f) rawVy = 0.0f;


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

        /* 4. 远场慢降：像素 < 1500 且 > 2 m */
        double pixelWidth = Math.sqrt(Math.pow(pts[1].x - pts[0].x, 2) +
                Math.pow(pts[1].y - pts[0].y, 2));
      //  double flyingHeight = Movement.getInstance().getElevation();

        int currentHeight = Movement.getInstance().getUltrasonicHeight();

        float vz;
        if (currentHeight <= 4) {
            vz = 0.0f;  // 近场悬停，等速降
        } else if (currentHeight <= 6) {
            vz = SLOW_SUPER_SPEED; // -0.1f，过渡层
        } else {
            vz = SLOW_LAND_SPEED;  // -0.3f，远场慢降
        }
//        if (flyingHeight <= 0.2) {
//            vz = 0.0f;
//            LogUtil.log(TAG, "相对高度" + flyingHeight + "强制悬停");
//        }

        /* 5. 输出 */
        DroneHelper.getInstance().moveVxVyYawrateHeight(vx, vy, 0f, vz);

        /* 6. 日志（加上偏移提示） */
        LogUtil.log(TAG, "vx" + vx + "vy" + vy + "像素居中 errX=" + (int) errX + "(含偏移" + LENS_OFFSET_X + ") errY=" + (int) errY +
                " pixelW=" + (int) pixelWidth + " vz=" + vz + "ultheight" + Movement.getInstance().getUltrasonicHeight());
    }


    public boolean startFastStick;

    public boolean isStartFastStick() {
        return startFastStick;
    }

    public void setStartFastStick(boolean startFastStick) {
        this.startFastStick = startFastStick;
    }

    public boolean canLanding;

    public boolean isCanLanding() {
        return canLanding;
    }

    public void setCanLanding(boolean canLanding) {
        //测试重置未识别和识别时间,避免刚触发识别就飞向备降点
        startTime = 0;
        endTime = 0;
        this.canLanding = canLanding;

    }


    private int handlerCallbackCount = 0; // 记录回调次数
    private Handler handler = new Handler(Looper.getMainLooper());
    private Runnable runnable = new Runnable() {
        @Override
        public void run() {
            performOperation();
            if (handlerCallbackCount < 25) {
                handler.postDelayed(this, 50); // 每 50 毫秒执行一次，1 秒内执行 20 次
            } else {
                performNextStep();
            }
        }
    };

    private void performOperation() {
        LogUtil.log(TAG, "快速下拉中..." + handlerCallbackCount);
        DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -4);
        handlerCallbackCount++; // 增加计数器
    }

    private void performNextStep() {
        canLanding = true;
        handlerCallbackCount = 0;
        dropTimes = 0;//手动测试避免多次累加后直接飞往备降点
//        KeyManager.getInstance().performAction(KeyTools.createKey(FlightControllerKey.KeyStartAutoLanding), new CommonCallbacks.CompletionCallbackWithParam<EmptyMsg>() {
//            @Override
//            public void onSuccess(EmptyMsg emptyMsg) {
//                LogUtil.log(TAG, "下拉完成：触发下一步自动降落");
//            }
//
//            @Override
//            public void onFailure(@NonNull IDJIError idjiError) {
//                LogUtil.log(TAG, "下拉完成：触发下一步自动降落失败");
//            }
//        });
        handler.removeCallbacks(runnable); // 防止重复执行
    }


}