makcar/app/src/main/java/com/aros/apron/tools/MixedVisionLanding.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.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfInt;
import org.opencv.core.Point;
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.Arrays;
import java.util.List;
import java.util.concurrent.Executors;
import java.util.concurrent.Future;
import java.util.concurrent.ScheduledExecutorService;
import java.util.concurrent.TimeUnit;

/**
 * 融合视觉降落识别器（独立版本）
 * 集成云台相机和下视相机的识别功能，实现相机切换逻辑
 * 
 * 降落策略：
 * 1. 先使用云台相机识别
 * 2. 云台相机识别失败，切换到下视相机
 * 3. 下视相机识别失败，切换回云台相机
 * 4. 最多切换2次，超过则触发备降
 */
public class MixedVisionLanding {

    private static final String TAG_LOG = "MixedVisionLanding";

    // 单例模式
    private static class SingletonHolder {
        private static final MixedVisionLanding INSTANCE = new MixedVisionLanding();
        static { INSTANCE.init(); }
    }
    
    public static MixedVisionLanding getInstance() {
        return SingletonHolder.INSTANCE;
    }

    // ========== 降落模式 ==========
    public enum LandingMode {
        GIMBAL_CAMERA,      // 云台相机
        DOWNWARD_CAMERA     // 下视相机
    }

    // ========== 状态变量 ==========
    private LandingMode currentMode = LandingMode.GIMBAL_CAMERA;
    private int switchCount = 0;
    private static final int MAX_SWITCH_COUNT = 2;
    private boolean isLanding = false;
    private boolean isDoublePayload = false;
    private int startArucoType = 0; // 1执行机库二维码识别  2执行备降点二维码识别

    // ========== 云台相机参数 ==========
    private static double GIMBAL_LENS_OFFSET_X = 0;
    private static double GIMBAL_LENS_OFFSET_Y = 0;
    private static final int GIMBAL_CENTER_ERR_MAX = 50;
    private static final int GIMBAL_PIXEL_TRIGGER = 360;
    private static final int GIMBAL_TRIGGER_FRAME_THRESHOLD = 2;

    // ========== 下视相机参数 ==========
    private static double DOWNWARD_LENS_OFFSET_X = 0;
    private static double DOWNWARD_LENS_OFFSET_Y = 0;
    private static final int DOWNWARD_CENTER_ERR_MAX = 30;
    private static final int DOWNWARD_PIXEL_TRIGGER = 360;
    private static final int DOWNWARD_TRIGGER_FRAME_THRESHOLD = 2;

    // ========== 云台相机状态 ==========
    private boolean gimbalIsTriggerSuccess = false;
    private boolean gimbalArucoNotFoundTag = false;
    private boolean gimbalIsStartAruco = false;
    private boolean gimbalStartFastStick = false;
    private boolean gimbalCanLanding = false;
    private int gimbalConsecutiveTriggerCount = 0;
    private int gimbalFrameCounter = 0;
    private int gimbalDropTimes = 0;
    private boolean gimbalDropTimesTag = false;
    private long gimbalStartTime = 0;
    private long gimbalEndTime = 0;
    private boolean gimbalIsYawAligned = false;
    private int gimbalCurrentLandingMode = 0;
    private int gimbalLastLandingCondition = 0;
    private boolean gimbalSetMF = false;

    // ========== 下视相机状态 ==========
    private boolean downwardIsTriggerSuccess = false;
    private boolean downwardArucoNotFoundTag = false;
    private boolean downwardIsStartAruco = false;
    private boolean downwardStartFastStick = false;
    private boolean downwardCanLanding = false;
    private int downwardConsecutiveTriggerCount = 0;
    private int downwardFrameCounter = 0;
    private int downwardDropTimes = 0;
    private boolean downwardDropTimesTag = false;
    private long downwardStartTime = 0;
    private long downwardEndTime = 0;
    private boolean downwardIsYawAligned = false;

    // ========== 执行器 ==========
    private ScheduledExecutorService executor = Executors.newScheduledThreadPool(2);
    private Future<?> lastGimbalFuture = null;
    private Future<?> lastDownwardFuture = null;

    // ========== PID控制器 ==========
    public PIDControl pidControlX = null;
    public PIDControl pidControlY = null;

    // ========== Handler ==========
    private Handler handler = new Handler(Looper.getMainLooper());
    private int handlerCallbackCount = 0;

    // ========== 初始化 ==========
    public void init() {
        LogUtil.log(TAG_LOG, "初始化融合视觉降落识别器");
        pidControlX = new PIDControl(0.7f, 0.02f, 0.2f, 0.05f, 2.5f, 0.1f);
        pidControlY = new PIDControl(0.7f, 0.02f, 0.2f, 0.05f, 2.5f, 0.1f);
        pidControlX.reset();
        pidControlY.reset();
    }

    // ========== 公共方法 ==========
    public void setDoublePayload(boolean doublePayload) {
        isDoublePayload = doublePayload;
    }

    public void startLanding() {
        synchronized (this) {
            LogUtil.log(TAG_LOG, "开始融合视觉降落");
            isLanding = true;
            currentMode = LandingMode.GIMBAL_CAMERA;
            switchCount = 0;
            resetGimbalState();
            resetDownwardState();
        }
    }

    public void stopLanding() {
        synchronized (this) {
            LogUtil.log(TAG_LOG, "停止融合视觉降落");
            isLanding = false;
            startArucoType = 0;
            resetGimbalState();
            resetDownwardState();
        }
    }

    public LandingMode getCurrentMode() {
        return currentMode;
    }

    public int getSwitchCount() {
        return switchCount;
    }

    public boolean isLanding() {
        return isLanding;
    }

    public int getStartArucoType() {
        return startArucoType;
    }

    public void setStartArucoType(int type) {
        startArucoType = type;
    }

    // ========== 云台相机帧处理 ==========
    public void processGimbalFrame(int height, int width, byte[] data, Dictionary dictionary) {
        synchronized (this) {
            if (!isLanding || currentMode != LandingMode.GIMBAL_CAMERA || startArucoType == 0) {
                return;
            }

            gimbalIsTriggerSuccess = true;
            Movement.getInstance().setVirtualStickEnableReason(2);

            if (gimbalIsStartAruco || gimbalStartFastStick) {
                return;
            }

            int currentFrame = gimbalFrameCounter++;
            if (currentFrame % 3 != 0) {
                return;
            }

            if (currentFrame % 30 != 0) {
                DroneHelper.getInstance().setGimbalPitchdown();
            }

            gimbalIsStartAruco = true;
            if (lastGimbalFuture != null && !lastGimbalFuture.isDone()) {
                lastGimbalFuture.cancel(true);
            }

            int ultHeight = Movement.getInstance().getUltrasonicHeight();
            updateGimbalLensOffset(ultHeight);

            final int finalUltHeight = ultHeight;
            lastGimbalFuture = executor.schedule(() -> {
                try {
                    processGimbalFrameInternal(height, width, data, dictionary, finalUltHeight);
                } catch (Exception e) {
                    LogUtil.log(TAG_LOG, "云台相机处理异常: " + e);
                    synchronized (MixedVisionLanding.this) {
                        gimbalIsStartAruco = false;
                    }
                    handleLandingFailure();
                }
            }, 0, TimeUnit.MILLISECONDS);
        }
    }

    // ========== 下视相机帧处理 ==========
    public void processDownwardFrame(int height, int width, byte[] data, Dictionary dictionary) {
        synchronized (this) {
            if (!isLanding || currentMode != LandingMode.DOWNWARD_CAMERA || startArucoType == 0) {
                return;
            }

            downwardIsTriggerSuccess = true;
            Movement.getInstance().setVirtualStickEnableReason(2);

            if (downwardIsStartAruco || downwardStartFastStick) {
                return;
            }

            int currentFrame = downwardFrameCounter++;
            if (currentFrame % 2 != 0) {
                return;
            }

            downwardIsStartAruco = true;
            if (lastDownwardFuture != null && !lastDownwardFuture.isDone()) {
                lastDownwardFuture.cancel(true);
            }

            int ultHeight = Movement.getInstance().getUltrasonicHeight();
            updateDownwardLensOffset(ultHeight);

            final int finalUltHeight = ultHeight;
            lastDownwardFuture = executor.schedule(() -> {
                try {
                    processDownwardFrameInternal(height, width, data, dictionary, finalUltHeight);
                } catch (Exception e) {
                    LogUtil.log(TAG_LOG, "下视相机处理异常: " + e);
                    synchronized (MixedVisionLanding.this) {
                        downwardIsStartAruco = false;
                    }
                    handleLandingFailure();
                }
            }, 0, TimeUnit.MILLISECONDS);
        }
    }

    // ========== 云台相机内部处理 ==========
    private void processGimbalFrameInternal(int height, int width, byte[] data, Dictionary dictionary, int ultHeight) {
        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<>();
        DetectorParameters parameters = new DetectorParameters();
        ArucoDetector detector = new ArucoDetector(dictionary, parameters);

        detector.detectMarkers(grayImgMat, corners, ids);

        if (!ids.empty()) {
            gimbalArucoNotFoundTag = false;
            gimbalDropTimesTag = true;

            List<ArucoMarker> smallMarkers = new ArrayList<>();
            List<ArucoMarker> bigMarkers1_4 = new ArrayList<>();
            List<ArucoMarker> bigMarker5 = new ArrayList<>();
            List<ArucoMarker> bigMarker6 = new ArrayList<>();

            int[] idArray = ids.toArray();
            List<Integer> excludeIds = isDoublePayload ?
                    Arrays.asList(12, 11, 21, 14, 13, 22) :
                    Arrays.asList(12, 11, 21, 20, 19, 25);

            StringBuilder detectInfo = new StringBuilder();
            for (int i = 0; i < idArray.length; i++) {
                int id = idArray[i];
                Mat corner = corners.get(i);
                double pixelW = calculatePixelWidth(corner);

                if (id >= 11 && id <= 25 && !excludeIds.contains(id)) {
                    smallMarkers.add(new ArucoMarker(id, corner, 0.03f));
                    detectInfo.append(String.format("小码%d(%.0fpx) ", id, pixelW));
                } else if (id >= 1 && id <= 4) {
                    bigMarkers1_4.add(new ArucoMarker(id, corner, 0.18f));
                    detectInfo.append(String.format("大码%d(%.0fpx) ", id, pixelW));
                } else if (id == 5) {
                    bigMarker5.add(new ArucoMarker(5, corner, 0.15f));
                    detectInfo.append(String.format("5号(%.0fpx) ", pixelW));
                } else if (id == 6) {
                    bigMarker6.add(new ArucoMarker(6, corner, 0.24f));
                    detectInfo.append(String.format("6号(%.0fpx) ", pixelW));
                }
            }

            if (detectInfo.length() > 0) {
                LogUtil.log(TAG_LOG, "【云台检测到】" + detectInfo.toString());
            }

            // 优先级1：小码模式
            if (!smallMarkers.isEmpty() && ultHeight < 25) {
                gimbalCurrentLandingMode = 1;
                processGimbalSmallMarkers(smallMarkers, rgbMat.width(), rgbMat.height(), ultHeight);
            }
            // 优先级2：1-4号几何中心
            else if (!bigMarkers1_4.isEmpty()) {
                gimbalCurrentLandingMode = 2;
                if (ultHeight > 30 && !gimbalIsYawAligned) {
                    processGimbalBigMarkersYawOnly(bigMarkers1_4, rgbMat.width(), rgbMat.height(), ultHeight);
                } else {
                    processGimbalBigMarkersCenter(bigMarkers1_4, rgbMat.width(), rgbMat.height(), ultHeight);
                }
            }
            // 优先级3：5/6号纯下降
            else if ((!bigMarker5.isEmpty() || !bigMarker6.isEmpty())) {
                gimbalCurrentLandingMode = 3;
                ArucoMarker target = !bigMarker5.isEmpty() ? bigMarker5.get(0) : bigMarker6.get(0);
                processGimbalLandingOnly(target, rgbMat.width(), rgbMat.height(), ultHeight);
            } else {
                handleGimbalLostMarker(ultHeight);
            }
        } else {
            handleGimbalLostMarker(ultHeight);
        }

        grayImgMat.release();
        rgbMat.release();
        yuvMat.release();
        ids.release();
        if (!corners.isEmpty()) {
            for (Mat c : corners) if (c != null) c.release();
        }
        gimbalIsStartAruco = false;
    }

    // ========== 下视相机内部处理 ==========
    private void processDownwardFrameInternal(int height, int width, byte[] data, Dictionary dictionary, int ultHeight) {
        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<>();
        DetectorParameters parameters = new DetectorParameters();
        ArucoDetector detector = new ArucoDetector(dictionary, parameters);

        detector.detectMarkers(grayImgMat, corners, ids);

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

        if (!ids.empty()) {
            downwardArucoNotFoundTag = false;
            downwardDropTimesTag = true;

            int[] idArray = ids.toArray();
            if (idArray.length == 1 && idArray[0] == 6) {
                Mat 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 + DOWNWARD_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 + DOWNWARD_LENS_OFFSET_Y;
                double errY = Math.abs(centerY - rgbMat.height() / 2.0);

                LogUtil.log(TAG_LOG, "【下视检测到】6号 像素" + (int) pixelWidth + " errX=" + (int) errX + " errY=" + (int) errY);

                // 速降判断
                if (!downwardStartFastStick) {
                    if (pixelWidth >= DOWNWARD_PIXEL_TRIGGER &&
                            ultHeight <= 4 &&
                            errX < DOWNWARD_CENTER_ERR_MAX &&
                            errY < DOWNWARD_CENTER_ERR_MAX) {

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

                        if (downwardConsecutiveTriggerCount >= DOWNWARD_TRIGGER_FRAME_THRESHOLD) {
                            downwardStartFastStick = true;
                            downwardConsecutiveTriggerCount = 0;
                            LogUtil.log(TAG_LOG, "【下视触发速降】");
                            handler.postDelayed(() -> handler.post(downwardRunnable), 300);
                            return;
                        }
                    } else {
                        if (downwardConsecutiveTriggerCount > 0) {
                            LogUtil.log(TAG_LOG, "下视速降条件中断，重置累计帧数");
                            downwardConsecutiveTriggerCount = 0;
                        }
                    }
                }

                // 旋转处理
                if (ultHeight > 30 && !downwardIsYawAligned) {
                    double yawError = calculateYawErrorFromCorners(points);
                    double absYaw = Math.abs(yawError);
                    
                    if (absYaw < 10.0) {
                        downwardIsYawAligned = true;
                        LogUtil.log(TAG_LOG, "【下视旋转到位】偏航已对准");
                        DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 0f);
                    } else {
                        float yawRate = calculateYawRate(yawError, ultHeight);
                        LogUtil.log(TAG_LOG, String.format("【下视执行】纯旋转 yawRate=%.1f avgYaw=%.1f", yawRate, yawError));
                        DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, yawRate, 0f);
                    }
                } else {
                    // 正常修正
                    moveOnArucoDetected(new ArucoMarker(1, corner6, 0.24f), rgbMat.width(), rgbMat.height());
                }
            }
        } else {
            handleDownwardLostMarker(ultHeight);
        }

        grayImgMat.release();
        rgbMat.release();
        yuvMat.release();
        ids.release();
        if (!corners.isEmpty()) {
            for (Mat c : corners) if (c != null) c.release();
        }
        downwardIsStartAruco = false;
    }

    // ========== 云台相机小码处理 ==========
    private void processGimbalSmallMarkers(List<ArucoMarker> markers, int imgWidth, int imgHeight, int ultHeight) {
        if (markers == null || markers.size() < 3) {
            LogUtil.log(TAG_LOG, "【云台小码数量不足】检测到" + (markers == null ? 0 : markers.size()) + "个");
            return;
        }

        double sumX = 0, sumY = 0;
        int validCount = 0;
        double totalPixelWidth = 0;
        ArucoMarker target15 = null;
        ArucoMarker target17 = null;

        for (ArucoMarker marker : markers) {
            Mat corner = marker.getConner();
            totalPixelWidth += calculatePixelWidth(corner);

            double cx = 0, cy = 0;
            for (int i = 0; i < 4; i++) {
                double[] p = corner.get(0, i);
                cx += p[0];
                cy += p[1];
            }
            cx /= 4.0;
            cy /= 4.0;
            sumX += cx;
            sumY += cy;
            validCount++;

            int id = marker.getId();
            if (id == 15) target15 = marker;
            if (id == 17) target17 = marker;
        }

        if (validCount == 0) return;

        double avgPixelWidth = totalPixelWidth / validCount;
        double geoCenterX = sumX / validCount;
        double geoCenterY = sumY / validCount;
        double offsetX = geoCenterX - imgWidth / 2.0 - GIMBAL_LENS_OFFSET_X;
        double offsetY = geoCenterY - imgHeight / 2.0 + GIMBAL_LENS_OFFSET_Y;
        double absX = Math.abs(offsetX);
        double absY = Math.abs(offsetY);
        double z = ultHeight / 10.0;

        // 特殊降落判断
        boolean isSpecialLanding = false;
        if (!isDoublePayload && target15 != null && ultHeight <= 4) {
            Mat corner = target15.getConner();
            double cx = 0, cy = 0;
            for (int i = 0; i < 4; i++) {
                double[] p = corner.get(0, i);
                cx += p[0];
                cy += p[1];
            }
            cx /= 4.0; cy /= 4.0;
            double singleErrX = cx - imgWidth / 2.0 - GIMBAL_LENS_OFFSET_X;
            double singleErrY = cy - imgHeight / 2.0 + GIMBAL_LENS_OFFSET_Y;

            if (Math.abs(singleErrX) < 60 && Math.abs(singleErrY) < 60 && ultHeight <= 4) {
                isSpecialLanding = true;
                offsetX = singleErrX; offsetY = singleErrY;
                absX = Math.abs(singleErrX); absY = Math.abs(singleErrY);
            }
        }

        if (isDoublePayload && target17 != null && ultHeight <= 4) {
            Mat corner = target17.getConner();
            double cx = 0, cy = 0;
            for (int i = 0; i < 4; i++) {
                double[] p = corner.get(0, i);
                cx += p[0];
                cy += p[1];
            }
            cx /= 4.0; cy /= 4.0;
            double singleErrX = cx - imgWidth / 2.0 - GIMBAL_LENS_OFFSET_X;
            double singleErrY = cy - imgHeight / 2.0 + GIMBAL_LENS_OFFSET_Y;

            if (Math.abs(singleErrX) < 60 && Math.abs(singleErrY) < 60 && ultHeight <= 4) {
                isSpecialLanding = true;
                offsetX = singleErrX; offsetY = singleErrY;
                absX = Math.abs(singleErrX); absY = Math.abs(singleErrY);
            }
        }

        // 连续帧判断
        int currentCondition = 0;
        if (isSpecialLanding) {
            currentCondition = 1;
        } else if (ultHeight <= 4 && absX < 60 && absY < 60 && validCount >= 8) {
            currentCondition = 2;
        }

        if (currentCondition == 0 || currentCondition != gimbalLastLandingCondition) {
            if (gimbalConsecutiveTriggerCount > 0) {
                gimbalConsecutiveTriggerCount = 0;
                LogUtil.log(TAG_LOG, "【云台计数器清零】条件变化");
            }
            gimbalLastLandingCondition = currentCondition;
        }

        if (currentCondition > 0) {
            gimbalConsecutiveTriggerCount++;
            LogUtil.log(TAG_LOG, "【云台计数器累加】count=" + gimbalConsecutiveTriggerCount);

            if (gimbalConsecutiveTriggerCount >= GIMBAL_TRIGGER_FRAME_THRESHOLD) {
                gimbalStartFastStick = true;
                gimbalConsecutiveTriggerCount = 0;
                gimbalLastLandingCondition = 0;
                LogUtil.log(TAG_LOG, "【云台触发速降】");
                handler.postDelayed(() -> handler.post(gimbalRunnable), 300);
                return;
            }
        }

        // PID控制
        double outX = 0, outY = 0, outZ = 0;
        if (z <= 0.4) {
            pidControlX.setInputFilterAll((float)offsetX/1750);
            pidControlY.setInputFilterAll(-(float)offsetY/1750);
            outX = Math.max(-0.125, Math.min(0.125, pidControlX.get_pid()));
            outY = Math.max(-0.125, Math.min(0.125, pidControlY.get_pid()));
        } else if (z <= 0.7) {
            pidControlX.setInputFilterAll((float)offsetX/1200);
            pidControlY.setInputFilterAll(-(float)offsetY/1200);
            outX = Math.max(-0.135, Math.min(0.135, pidControlX.get_pid()));
            outY = Math.max(-0.135, Math.min(0.135, pidControlY.get_pid()));
            outZ = (absX < 250 && absY < 250) ? -0.1 : 0;
        } else if (z <= 1.0) {
            pidControlX.setInputFilterAll((float)offsetX/1200);
            pidControlY.setInputFilterAll(-(float)offsetY/1200);
            outX = Math.max(-0.145, Math.min(0.145, pidControlX.get_pid()));
            outY = Math.max(-0.175, Math.min(0.175, pidControlY.get_pid()));
            outZ = (absX < 200 && absY < 200) ? -0.2 : 0;
        } else if (z <= 1.5) {
            pidControlX.setInputFilterAll((float)offsetX/1450);
            pidControlY.setInputFilterAll(-(float)offsetY/1450);
            outX = Math.max(-0.185, Math.min(0.185, pidControlX.get_pid()));
            outY = Math.max(-0.185, Math.min(0.185, pidControlY.get_pid()));
            outZ = (absX < 180 && absY < 180) ? -0.4 : 0;
        } else if (z <= 2.0) {
            pidControlX.setInputFilterAll((float)offsetX/1350);
            pidControlY.setInputFilterAll(-(float)offsetY/1350);
            outX = pidControlX.get_pid();
            outY = pidControlY.get_pid();
            outZ = (absX < 200 && absY < 200) ? -0.4 : 0;
        } else if (z <= 3.0) {
            pidControlX.setInputFilterAll((float)offsetX/1250);
            pidControlY.setInputFilterAll(-(float)offsetY/1250);
            outX = pidControlX.get_pid();
            outY = pidControlY.get_pid();
            outZ = (absX < 200 && absY < 200) ? -0.4 : 0;
        } else {
            pidControlX.setInputFilterAll((float)offsetX/850);
            pidControlY.setInputFilterAll(-(float)offsetY/850);
            outX = pidControlX.get_pid();
            outY = pidControlY.get_pid();
            outZ = (absX < 130 && absY < 130) ? -0.4 : 0;
        }

        LogUtil.log(TAG_LOG, String.format("【云台执行】小码 vx=%.3f vy=%.3f vz=%.3f pixel=%.0f ult=%d", outX, outY, outZ, avgPixelWidth, ultHeight));
        DroneHelper.getInstance().moveVxVyYawrateHeight((float)outX, (float)outY, 0, (float)outZ);
    }

    // ========== 云台相机大码旋转处理 ==========
    private void processGimbalBigMarkersYawOnly(List<ArucoMarker> markers, int imgWidth, int imgHeight, int ultHeight) {
        double sumYaw = 0;
        int count = 0;

        for (ArucoMarker marker : markers) {
            Point[] pts = new Point[4];
            Mat corner = marker.getConner();
            for (int i = 0; i < 4; i++) {
                double[] p = corner.get(0, i);
                pts[i] = new Point(p[0], p[1]);
            }
            sumYaw += calculateYawErrorFromCorners(pts);
            count++;
        }

        double avgYaw = sumYaw / count;
        double absYaw = Math.abs(avgYaw);

        if (absYaw < 10.0) {
            gimbalIsYawAligned = true;
            LogUtil.log(TAG_LOG, "【云台旋转到位】偏航已对准");
            return;
        }

        float yawRate = calculateYawRate(avgYaw, ultHeight);
        LogUtil.log(TAG_LOG, String.format("【云台执行】纯旋转 yawRate=%.1f avgYaw=%.1f", yawRate, avgYaw));
        DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, yawRate, 0f);
    }

    // ========== 云台相机大码中心处理 ==========
    private void processGimbalBigMarkersCenter(List<ArucoMarker> markers, int imgWidth, int imgHeight, int ultHeight) {
        double sumX = 0, sumY = 0;
        StringBuilder markerInfo = new StringBuilder();

        for (ArucoMarker marker : markers) {
            Mat corner = marker.getConner();
            double cx = 0, cy = 0;
            for (int i = 0; i < 4; i++) {
                double[] p = corner.get(0, i);
                cx += p[0];
                cy += p[1];
            }
            sumX += cx / 4.0;
            sumY += cy / 4.0;

            double pixelW = calculatePixelWidth(corner);
            markerInfo.append(String.format("[ID%d:%.0fpx]", marker.getId(), pixelW));
        }

        double geoCenterX = sumX / markers.size();
        double geoCenterY = sumY / markers.size();
        double offsetX = geoCenterX - imgWidth / 2.0;
        double offsetY = geoCenterY - imgHeight / 2.0;
        double absX = Math.abs(offsetX);
        double absY = Math.abs(offsetY);
        double z = ultHeight / 10.0;

        double outX = 0, outY = 0, outZ = 0;

        if (z >= 5.0) {
            pidControlX.setInputFilterAll((float)offsetX/300);
            pidControlY.setInputFilterAll(-(float)offsetY/300);
            outX = Math.max(-0.20, Math.min(0.20, pidControlX.get_pid()));
            outY = Math.max(-0.20, Math.min(0.20, pidControlY.get_pid()));
            outZ = (absX < 100 && absY < 100) ? -0.4 : 0;
        } else if (z >= 4) {
            pidControlX.setInputFilterAll((float)offsetX/600);
            pidControlY.setInputFilterAll(-(float)offsetY/600);
            outX = Math.max(-0.20, Math.min(0.20, pidControlX.get_pid()));
            outY = Math.max(-0.20, Math.min(0.20, pidControlY.get_pid()));
            outZ = (absX < 120 && absY < 120) ? -0.4 : 0;
        } else if (z >= 2) {
            pidControlX.setInputFilterAll((float)offsetX/900);
            pidControlY.setInputFilterAll(-(float)offsetY/900);
            outX = Math.max(-0.20, Math.min(0.20, pidControlX.get_pid()));
            outY = Math.max(-0.20, Math.min(0.20, pidControlY.get_pid()));
            outZ = (absX < 140 && absY < 140) ? -0.4 : 0;
        }

        LogUtil.log(TAG_LOG, String.format("【云台执行】大码几何中心 vx=%.3f vy=%.3f vz=%.3f ult=%d %s", outX, outY, outZ, ultHeight, markerInfo.toString()));
        DroneHelper.getInstance().moveVxVyYawrateHeight((float)outX, (float)outY, 0f, (float)outZ);
    }

    // ========== 云台相机纯下降处理 ==========
    private void processGimbalLandingOnly(ArucoMarker marker, int imgWidth, int imgHeight, int ultHeight) {
        Mat corner = marker.getConner();
        double pixelWidth = calculatePixelWidth(corner);

        float vz;
        if (ultHeight >= 50) {
            vz = -0.7f;
        } else if (ultHeight <= 15) {
            vz = 0.0f;
        } else {
            vz = 0.3f;
        }

        LogUtil.log(TAG_LOG, String.format("【云台执行】5/6号纯下降 vz=%.3f pixel=%.0f ult=%d", vz, pixelWidth, ultHeight));
        DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, vz);
    }

    // ========== 云台相机丢失处理 ==========
    private void handleGimbalLostMarker(int ultHeight) {
        if (!gimbalArucoNotFoundTag) {
            gimbalStartTime = System.currentTimeMillis();
            gimbalArucoNotFoundTag = true;
        }
        gimbalEndTime = System.currentTimeMillis();
        long lostDuration = gimbalEndTime - gimbalStartTime;

        if (lostDuration > 1000 && lostDuration <= 12000) {
            if (ultHeight <= 20) {
                LogUtil.log(TAG_LOG, "【云台执行】丢失上升 vz=3.0");
                DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 3f);
                if (gimbalDropTimes > Integer.parseInt(AMSConfig.getInstance().getAlternateLandingTimes())) {
                    LogUtil.log(TAG_LOG, "云台超过复降限制,切换相机");
                    handleLandingFailure();
                    return;
                }
                if (gimbalDropTimesTag) {
                    gimbalDropTimesTag = false;
                    gimbalDropTimes++;
                    LogUtil.log(TAG_LOG, "云台复降第:" + gimbalDropTimes + "次");
                }
            } else {
                LogUtil.log(TAG_LOG, "【云台执行】丢失下降 vz=-0.3");
                DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f);
            }
        } else if (lostDuration > 8000) {
            LogUtil.log(TAG_LOG, "云台判定未识别到二维码,切换相机");
            handleLandingFailure();
        }
    }

    // ========== 下视相机丢失处理 ==========
    private void handleDownwardLostMarker(int ultHeight) {
        LogUtil.log(TAG_LOG, "下视找不到了二维码");
        if (!downwardArucoNotFoundTag) {
            downwardStartTime = System.currentTimeMillis();
            downwardArucoNotFoundTag = true;
        }
        downwardEndTime = System.currentTimeMillis();
        long lostDuration = downwardEndTime - downwardStartTime;

        if (lostDuration > 1000 && lostDuration <= 12000) {
            if (ultHeight <= 20) {
                LogUtil.log(TAG_LOG, "【下视执行】丢失上升 vz=3.0");
                DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 3f);
                if (downwardDropTimes > Integer.parseInt(AMSConfig.getInstance().getAlternateLandingTimes())) {
                    LogUtil.log(TAG_LOG, "下视超过复降限制,切换相机");
                    handleLandingFailure();
                    return;
                }
                if (downwardDropTimesTag) {
                    downwardDropTimesTag = false;
                    downwardDropTimes++;
                    LogUtil.log(TAG_LOG, "下视复降第:" + downwardDropTimes + "次");
                }
            } else {
                LogUtil.log(TAG_LOG, "【下视执行】丢失下降 vz=-0.3");
                DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f);
            }
        } else if (lostDuration > 8000) {
            LogUtil.log(TAG_LOG, "下视判定未识别到二维码,切换相机");
            handleLandingFailure();
        }
    }

    // ========== 下视相机移动控制 ==========
    private void moveOnArucoDetected(ArucoMarker marker, int imgWidth, int imgHeight) {
        Mat corner = marker.getConner();
        Point[] points = new Point[4];
        for (int i = 0; i < 4; i++) {
            double[] p = corner.get(0, i);
            points[i] = new Point(p[0], p[1]);
        }

        double centerX = (points[0].x + points[1].x + points[2].x + points[3].x) / 4.0 + DOWNWARD_LENS_OFFSET_X;
        double centerY = (points[0].y + points[1].y + points[2].y + points[3].y) / 4.0 + DOWNWARD_LENS_OFFSET_Y;
        double errX = centerX - imgWidth / 2.0;
        double errY = centerY - imgHeight / 2.0;
        double absX = Math.abs(errX);
        double absY = Math.abs(errY);

        int ultHeight = Movement.getInstance().getUltrasonicHeight();
        double z = ultHeight / 10.0;

        double outX = 0, outY = 0, outZ = 0;

        if (z <= 0.4) {
            pidControlX.setInputFilterAll((float)errX/1750);
            pidControlY.setInputFilterAll(-(float)errY/1750);
            outX = Math.max(-0.125, Math.min(0.125, pidControlX.get_pid()));
            outY = Math.max(-0.125, Math.min(0.125, pidControlY.get_pid()));
        } else if (z <= 0.7) {
            pidControlX.setInputFilterAll((float)errX/1200);
            pidControlY.setInputFilterAll(-(float)errY/1200);
            outX = Math.max(-0.135, Math.min(0.135, pidControlX.get_pid()));
            outY = Math.max(-0.135, Math.min(0.135, pidControlY.get_pid()));
            outZ = (absX < 250 && absY < 250) ? -0.1 : 0;
        } else if (z <= 1.0) {
            pidControlX.setInputFilterAll((float)errX/1200);
            pidControlY.setInputFilterAll(-(float)errY/1200);
            outX = Math.max(-0.145, Math.min(0.145, pidControlX.get_pid()));
            outY = Math.max(-0.175, Math.min(0.175, pidControlY.get_pid()));
            outZ = (absX < 200 && absY < 200) ? -0.2 : 0;
        } else if (z <= 1.5) {
            pidControlX.setInputFilterAll((float)errX/1450);
            pidControlY.setInputFilterAll(-(float)errY/1450);
            outX = Math.max(-0.185, Math.min(0.185, pidControlX.get_pid()));
            outY = Math.max(-0.185, Math.min(0.185, pidControlY.get_pid()));
            outZ = (absX < 180 && absY < 180) ? -0.4 : 0;
        } else if (z <= 2.0) {
            pidControlX.setInputFilterAll((float)errX/1350);
            pidControlY.setInputFilterAll(-(float)errY/1350);
            outX = pidControlX.get_pid();
            outY = pidControlY.get_pid();
            outZ = (absX < 200 && absY < 200) ? -0.4 : 0;
        } else if (z <= 3.0) {
            pidControlX.setInputFilterAll((float)errX/1250);
            pidControlY.setInputFilterAll(-(float)errY/1250);
            outX = pidControlX.get_pid();
            outY = pidControlY.get_pid();
            outZ = (absX < 200 && absY < 200) ? -0.4 : 0;
        } else {
            pidControlX.setInputFilterAll((float)errX/850);
            pidControlY.setInputFilterAll(-(float)errY/850);
            outX = pidControlX.get_pid();
            outY = pidControlY.get_pid();
            outZ = (absX < 130 && absY < 130) ? -0.4 : 0;
        }

        LogUtil.log(TAG_LOG, String.format("【下视执行】移动 vx=%.3f vy=%.3f vz=%.3f ult=%d errX=%.0f errY=%.0f", outX, outY, outZ, ultHeight, absX, absY));
        DroneHelper.getInstance().moveVxVyYawrateHeight((float)outX, (float)outY, 0f, (float)outZ);
    }

    // ========== 降落失败处理（切换相机） ==========
    private void handleLandingFailure() {
        synchronized (this) {
            if (!isLanding) {
                return;
            }

            switchCount++;
            LogUtil.log(TAG_LOG, "切换相机，当前切换次数: " + switchCount);

            // 切换到另一个相机
            if (currentMode == LandingMode.GIMBAL_CAMERA) {
                currentMode = LandingMode.DOWNWARD_CAMERA;
                LogUtil.log(TAG_LOG, "切换到下视相机");
                resetDownwardState();
            } else {
                currentMode = LandingMode.GIMBAL_CAMERA;
                LogUtil.log(TAG_LOG, "切换到云台相机");
                resetGimbalState();
            }

            // 如果达到最大切换次数，触发备降
            if (switchCount >= MAX_SWITCH_COUNT) {
                LogUtil.log(TAG_LOG, "达到最大切换次数，触发备降");
                stopLanding();
                AlternateLandingManager.getInstance().startTaskProcess(null);
                Movement.getInstance().setAlternate(true);
            }
        }
    }

    // ========== 重置云台相机状态 ==========
    private void resetGimbalState() {
        gimbalIsTriggerSuccess = false;
        gimbalArucoNotFoundTag = false;
        gimbalIsStartAruco = false;
        gimbalStartFastStick = false;
        gimbalCanLanding = false;
        gimbalConsecutiveTriggerCount = 0;
        gimbalFrameCounter = 0;
        gimbalDropTimes = 0;
        gimbalDropTimesTag = false;
        gimbalStartTime = 0;
        gimbalEndTime = 0;
        gimbalIsYawAligned = false;
        gimbalCurrentLandingMode = 0;
        gimbalLastLandingCondition = 0;
        gimbalSetMF = false;
    }

    // ========== 重置下视相机状态 ==========
    private void resetDownwardState() {
        downwardIsTriggerSuccess = false;
        downwardArucoNotFoundTag = false;
        downwardIsStartAruco = false;
        downwardStartFastStick = false;
        downwardCanLanding = false;
        downwardConsecutiveTriggerCount = 0;
        downwardFrameCounter = 0;
        downwardDropTimes = 0;
        downwardDropTimesTag = false;
        downwardStartTime = 0;
        downwardEndTime = 0;
        downwardIsYawAligned = false;
    }

    // ========== 更新云台相机镜头偏移 ==========
    private void updateGimbalLensOffset(int ultHeight) {
        if (isDoublePayload) {
            if (ultHeight <= 5) {
                GIMBAL_LENS_OFFSET_X = -120;
                GIMBAL_LENS_OFFSET_Y = 120;
            } else if (ultHeight < 10) {
                GIMBAL_LENS_OFFSET_X = -80;
                GIMBAL_LENS_OFFSET_Y = 80;
            } else if (ultHeight < 20) {
                GIMBAL_LENS_OFFSET_X = -60;
                GIMBAL_LENS_OFFSET_Y = 60;
            }
        } else {
            if (ultHeight <= 5) {
                GIMBAL_LENS_OFFSET_X = 120;
                GIMBAL_LENS_OFFSET_Y = 100;
            } else if (ultHeight < 10) {
                GIMBAL_LENS_OFFSET_X = 80;
                GIMBAL_LENS_OFFSET_Y = 80;
            } else if (ultHeight < 20) {
                GIMBAL_LENS_OFFSET_X = 60;
                GIMBAL_LENS_OFFSET_Y = 60;
            }
        }
    }

    // ========== 更新下视相机镜头偏移 ==========
    private void updateDownwardLensOffset(int ultHeight) {
        if (ultHeight >= 30) {
            DOWNWARD_LENS_OFFSET_X = 0;
            DOWNWARD_LENS_OFFSET_Y = 0;
        } else if (ultHeight >= 20) {
            DOWNWARD_LENS_OFFSET_X = 0;
            DOWNWARD_LENS_OFFSET_Y = 0;
        } else if (ultHeight >= 10) {
            DOWNWARD_LENS_OFFSET_X = 20;
            DOWNWARD_LENS_OFFSET_Y = 10;
        } else if (ultHeight >= 5) {
            DOWNWARD_LENS_OFFSET_X = 30;
            DOWNWARD_LENS_OFFSET_Y = 20;
        } else {
            DOWNWARD_LENS_OFFSET_X = 55;
            DOWNWARD_LENS_OFFSET_Y = 35;
        }
    }

    // ========== 工具方法 ==========
    private double calculatePixelWidth(Mat corner) {
        try {
            Point[] pts = new Point[4];
            for (int i = 0; i < 4; i++) {
                double[] p = corner.get(0, i);
                pts[i] = new Point(p[0], p[1]);
            }
            double top = Math.sqrt(Math.pow(pts[1].x - pts[0].x, 2) + Math.pow(pts[1].y - pts[0].y, 2));
            double bottom = Math.sqrt(Math.pow(pts[2].x - pts[3].x, 2) + Math.pow(pts[2].y - pts[3].y, 2));
            return (top + bottom) / 2.0;
        } catch (Exception e) {
            return 0.0;
        }
    }

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

    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 float calculateYawRate(double yawError, int ultHeight) {
        if (ultHeight < 10 || ultHeight > 50) return 0f;

        double absError = Math.abs(yawError);
        if (absError < 10.0) return 0f;

        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;

        return yawError > 0 ? targetRate : -targetRate;
    }

    private MatOfInt keepOnly6(MatOfInt ids, List<Mat> corners) {
        if (ids.empty()) return ids;

        int[] idArray = ids.toArray();
        List<Integer> indicesToKeep = new ArrayList<>();

        for (int i = 0; i < idArray.length; i++) {
            if (idArray[i] == 6) {
                indicesToKeep.add(i);
            }
        }

        if (indicesToKeep.isEmpty()) {
            ids.release();
            return new MatOfInt();
        }

        int[] newIds = new int[indicesToKeep.size()];
        List<Mat> newCorners = new ArrayList<>();

        for (int i = 0; i < indicesToKeep.size(); i++) {
            newIds[i] = 6;
            newCorners.add(corners.get(indicesToKeep.get(i)));
        }

        corners.clear();
        corners.addAll(newCorners);

        MatOfInt newIdsMat = new MatOfInt();
        newIdsMat.fromArray(newIds);
        ids.release();
        return newIdsMat;
    }

    // ========== 云台速降Runnable ==========
    private Runnable gimbalRunnable = new Runnable() {
        @Override
        public void run() {
            LogUtil.log(TAG_LOG, String.format("【云台执行】速降 vz=-5 count=%d/20", handlerCallbackCount));
            DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -5);
            handlerCallbackCount++;
            if (handlerCallbackCount < 20) {
                handler.postDelayed(this, 50);
            } else {
                gimbalCanLanding = true;
                handlerCallbackCount = 0;
                gimbalDropTimes = 0;
            }
        }
    };

    // ========== 下视速降Runnable ==========
    private Runnable downwardRunnable = new Runnable() {
        @Override
        public void run() {
            LogUtil.log(TAG_LOG, String.format("【下视执行】速降 vz=-5 count=%d/20", handlerCallbackCount));
            DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -5);
            handlerCallbackCount++;
            if (handlerCallbackCount < 20) {
                handler.postDelayed(this, 50);
            } else {
                downwardCanLanding = true;
                handlerCallbackCount = 0;
                downwardDropTimes = 0;
            }
        }
    };
}