package com.multictrl.modules.business.service.impl;

import cn.hutool.core.collection.CollUtil;
import cn.hutool.core.util.IdUtil;
import cn.hutool.core.util.StrUtil;
import cn.hutool.json.JSONObject;
import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
import com.multictrl.common.config.MqttConfig;
import com.multictrl.common.constant.BusinessConstant;
import com.multictrl.common.constant.DockMode;
import com.multictrl.common.constant.FlightTaskProgressStatus;
import com.multictrl.common.exception.ErrorCode;
import com.multictrl.common.exception.RenException;
import com.multictrl.common.utils.CacheUtils;
import com.multictrl.common.utils.Utils;
import com.multictrl.modules.business.dao.DockDeviceDao;
import com.multictrl.modules.business.dto.LatLonDTO;
import com.multictrl.modules.business.dto.command.DrcStick;
import com.multictrl.modules.business.dto.command.FlyToPoint;
import com.multictrl.modules.business.dto.command.TakeoffToPoint;
import com.multictrl.modules.business.entity.DockDeviceEntity;
import com.multictrl.modules.business.service.CommandService;
import com.multictrl.modules.business.service.DJIBaseService;
import com.multictrl.modules.business.service.FlightTaskService;
import com.multictrl.modules.business.service.MqttPushService;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

import java.math.BigDecimal;
import java.math.RoundingMode;
import java.util.*;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.locks.ReentrantLock;

/**
 * 指令飞行
 *
 * @author Sdy
 * @since 1.0.0 2026/4/27
 */
@Slf4j
@Service
@RequiredArgsConstructor
public class CommandServiceImpl implements CommandService {
    private final DJIBaseService djiBaseService;
    private final MqttConfig mqttConfig;
    private final DockDeviceDao dockDeviceDao;
    private final MqttPushService mqttPushService;
    private final FlightTaskService flightTaskService;
    private final Map<String, ReentrantLock> cacheLock = new ConcurrentHashMap<>();

    @Override
    public String takeoffToPoint(String dockSn, TakeoffToPoint takeoffToPoint) {
        ReentrantLock lock = cacheLock.computeIfAbsent(dockSn, k -> new ReentrantLock());
        lock.lock();
        try {
            Double dockEllipsoidHeight = djiBaseService.getDockEllipsoidHeight(dockSn);
            if (dockEllipsoidHeight == null) {
                throw new RenException(ErrorCode.DOCK_OSD_NOT_EXIST, "未获取机场椭球高");
            }
            Integer dockModeCode = djiBaseService.getDockModeCode(dockSn);
            if (!Objects.equals(DockMode.IDLE.getCode(), dockModeCode)) {
                throw new RenException(ErrorCode.DOCK_MODE_NOT_IDLE, DockMode.getDescByCode(dockModeCode));
            }
            //模拟飞行参数重置
            if (takeoffToPoint.getSimulateMission() != null) {
                if (takeoffToPoint.getSimulateMission().getIsEnable() == 0) {
                    takeoffToPoint.setSimulateMission(null);
                }
            }
            //指点飞行失控动作补全
            if (takeoffToPoint.getCommanderModeLostAction() == null) {
                //{"0":"继续执行指点飞行任务","1":"退出指点飞行任务，执行普通失控行为"}
                takeoffToPoint.setCommanderModeLostAction(1);
                //{"0":"悬停","1":"着陆(降落)","2":"返航"}
                takeoffToPoint.setRcLostAction(2);
            }
            //一键起飞的飞行过程中能达到的最大速度补全
            if (takeoffToPoint.getMaxSpeed() == null) {
                takeoffToPoint.setMaxSpeed(10);
            }
            //传入的值是椭球高，需要加上机场的椭球高，转换为相对高，保障飞行安全
            float targetHeight = (float) (dockEllipsoidHeight + takeoffToPoint.getHeight());
            BigDecimal bd = new BigDecimal(String.valueOf(targetHeight));
            takeoffToPoint.setTargetHeight(bd.setScale(1, RoundingMode.HALF_UP).floatValue());
            //设置返航高度 起飞点相对高度+100
            if (takeoffToPoint.getRthAltitude() == null) {
                takeoffToPoint.setRthAltitude((int) Math.round(takeoffToPoint.getHeight() + 100d));
                takeoffToPoint.setRthMode(1);
            }
            //设置安全起飞高度
            if (takeoffToPoint.getSecurityTakeoffHeight() == null) {
                takeoffToPoint.setSecurityTakeoffHeight(Float.parseFloat(takeoffToPoint.getHeight() + ""));
            }
            //设置指点飞行高度、模式
            takeoffToPoint.setCommanderFlightHeight(Float.parseFloat(takeoffToPoint.getHeight() + ""));
            takeoffToPoint.setCommanderFlightMode(1);
            takeoffToPoint.setHeight(null);
            //生成任务id并缓存
            String taskId = IdUtil.fastSimpleUUID();
            CacheUtils.set(BusinessConstant.WORKING_TASK_ID + dockSn, taskId);
            takeoffToPoint.setFlightId(taskId);
            //设置默认相机模式，需要定频推送前端
            CacheUtils.set(BusinessConstant.UAV_VIDEO_TYPE, "wide");

            //发送执行
            String result = djiBaseService.executeAndReturnResult(dockSn, "takeoff_to_point", Utils.beanToSnakeJson(takeoffToPoint));
            flightTaskService.addTakeoffTask(taskId, dockSn);
            return result;
        } finally {
            Utils.sleep(2000);
            lock.unlock();
        }
    }

    @Override
    public String takeOffToAlternateLandPoint(String dockSn) {
        //获取备降点
        LatLonDTO alternateLandPoint = djiBaseService.getDockAlternateLandPoint(dockSn);
        if (alternateLandPoint == null) {
            throw new RenException(ErrorCode.DOCK_LAND_POINT_NOT_EXIST);
        }
        TakeoffToPoint takeoffToPoint = new TakeoffToPoint();
        takeoffToPoint.setTargetLatitude(alternateLandPoint.getLatitude());
        takeoffToPoint.setTargetLongitude(alternateLandPoint.getLongitude());
        takeoffToPoint.setHeight(100d);

        return takeoffToPoint(dockSn, takeoffToPoint);
    }

    @Override
    public String flyToPoint(String dockSn, FlyToPoint flyToPoint) {
        Double dockEllipsoidHeight = djiBaseService.getDockEllipsoidHeight(dockSn);
        if (dockEllipsoidHeight == null) {
            throw new RenException(ErrorCode.DOCK_OSD_NOT_EXIST, "未获取机场椭球高");
        }
        //作业中才能指点飞行
        Integer dockModeCode = djiBaseService.getDockModeCode(dockSn);
        if (!Objects.equals(DockMode.WORKING.getCode(), dockModeCode)) {
            throw new RenException(ErrorCode.DOCK_MODE_NOT_WORKING, DockMode.getDescByCode(dockModeCode));
        }
        //不在任务中才能指点飞行
        String status = (String) CacheUtils.get(BusinessConstant.FLIGHT_TASK_PROGRESS_STATUS + dockSn);
        if (FlightTaskProgressStatus.IN_PROGRESS.getCode().equals(status)) {
            throw new RenException(ErrorCode.ROUTE_TASK_IS_RUNNING, ",请先暂停航线");
        }

        List<FlyToPoint.Point> points = flyToPoint.getPoints();
        for (FlyToPoint.Point point : points) {
            point.setHeight(Float.parseFloat(dockEllipsoidHeight + point.getHeight() + ""));
        }
        flyToPoint.setFlyToId(IdUtil.fastUUID());
        return djiBaseService.executeAndReturnResult(dockSn, "fly_to_point", Utils.beanToSnakeJson(flyToPoint));
    }

    @Override
    public String flyToPointUpdate(String dockSn, FlyToPoint flyToPoint) {
        Double dockEllipsoidHeight = djiBaseService.getDockEllipsoidHeight(dockSn);
        if (dockEllipsoidHeight == null) {
            throw new RenException(ErrorCode.DOCK_OSD_NOT_EXIST, "未获取机场椭球高");
        }
        //作业中才能更新目标点
        Integer dockModeCode = djiBaseService.getDockModeCode(dockSn);
        if (!Objects.equals(DockMode.WORKING.getCode(), dockModeCode)) {
            throw new RenException(ErrorCode.DOCK_MODE_NOT_WORKING, DockMode.getDescByCode(dockModeCode));
        }
        List<FlyToPoint.Point> points = flyToPoint.getPoints();
        for (FlyToPoint.Point point : points) {
            point.setHeight(Float.parseFloat(dockEllipsoidHeight + point.getHeight() + ""));
        }
        return djiBaseService.executeAndReturnResult(dockSn, "fly_to_point_update", Utils.beanToSnakeJson(flyToPoint));
    }

    @Override
    public void seizingControl(String dockSn) {
        //飞行控制权抢夺
        String authorityGrab = djiBaseService.executeAndReturnResult(dockSn, "flight_authority_grab");
        log.debug("飞行控制权抢夺:{}", authorityGrab);
        //负载控制权抢夺
        String cameraIndex = (String) CacheUtils.get(BusinessConstant.UAV_CAMERA_INDEX + dockSn);
        if (StrUtil.isNotBlank(cameraIndex)) {
            JSONObject data = new JSONObject();
            data.set("payload_index", cameraIndex);
            String payloadAuthorityGrab = djiBaseService.executeAndReturnResult(dockSn, "payload_authority_grab", data);
            log.debug("负载控制权抢夺:{}", payloadAuthorityGrab);
        } else {
            log.debug("负载控制权抢夺:cameraIndex 获取失败，跳过负载控制权抢夺");
        }
    }

    @Override
    public String drcModeEnter(String dockSn) {
        if (!djiBaseService.isDockOnline(dockSn)) {
            throw new RenException(ErrorCode.DOCK_OSD_NOT_EXIST, "无法操作");
        }
        //作业中才能进入指令飞行
        Integer dockModeCode = djiBaseService.getDockModeCode(dockSn);
        if (!Objects.equals(DockMode.WORKING.getCode(), dockModeCode)) {
            throw new RenException(ErrorCode.DOCK_MODE_NOT_WORKING, DockMode.getDescByCode(dockModeCode));
        }
        //飞行控制权抢夺
       /* String authorityGrab = djiBaseService.executeAndReturnResult(dockSn, "flight_authority_grab");
        log.debug("飞行控制权抢夺:{}", authorityGrab);*/
        //负载控制权抢夺
        /*String cameraIndex = (String) CacheUtils.get(BusinessConstant.UAV_CAMERA_INDEX + dockSn);
        if (StrUtil.isNotBlank(cameraIndex)) {
            JSONObject data = new JSONObject();
            data.set("payload_index", cameraIndex);
            String payloadAuthorityGrab = djiBaseService.executeAndReturnResult(dockSn, "payload_authority_grab", data);
            log.debug("负载控制权抢夺:{}", payloadAuthorityGrab);
        } else {
            log.debug("负载控制权抢夺:cameraIndex 获取失败，跳过负载控制权抢夺");
        }*/
        //进入指令飞行控制模式
        MqttConfig.Client mqttClient = mqttConfig.getClient2();
        JSONObject data = new JSONObject();
        JSONObject mqttBroker = new JSONObject();
        mqttBroker.set("address", mqttClient.getUrl().replaceAll(BusinessConstant.TCP_PROTOCOL, ""));
        mqttBroker.set("client_id", BusinessConstant.DRC + dockSn + System.currentTimeMillis());
        mqttBroker.set("username", mqttClient.getUsername());
        mqttBroker.set("password", mqttClient.getPassword());
        mqttBroker.set("expire_time", 1672744922);
        mqttBroker.set("enable_tls", false);
        data.set("mqtt_broker", mqttBroker);
        data.set("osd_frequency", 1);
        data.set("hsi_frequency", 1);
        String result = djiBaseService.executeAndReturnResult(dockSn, "drc_mode_enter", data);
        //维护心跳
        List<String> list = new ArrayList<>(Optional.ofNullable(CacheUtils.get(BusinessConstant.DRC_HEART_BEAT_SN))
                .filter(obj -> obj instanceof List)
                .map(obj -> (List<String>) obj)
                .orElse(Collections.emptyList()));
        if (!list.contains(dockSn)) {
            list.add(dockSn);
            CacheUtils.set(BusinessConstant.DRC_HEART_BEAT_SN, list);
        }
        return result;
    }

    @Override
    public String drcModeExit(String dockSn) {
        //退出指令模式没有返回
        String result = djiBaseService.executeOnly(dockSn, "drc_mode_exit");
        List<String> list = Optional.ofNullable(CacheUtils.get(BusinessConstant.DRC_HEART_BEAT_SN))
                .filter(obj -> obj instanceof List)
                .map(obj -> (List<String>) obj)
                .orElse(Collections.emptyList());
        if (CollUtil.isNotEmpty(list)) {
            list.remove(dockSn);
        }
        return result;
    }

    /**
     * 机场1固定了上下、左右旋转的速度
     * 机场2、3固定了上下的速度
     */
    @Override
    public void drcStickControl(String dockSn, DrcStick drcStick) {
        /*DockDeviceEntity dockDevice = dockDeviceDao.selectOne(new QueryWrapper<DockDeviceEntity>().eq("sn", dockSn));
        if (dockDevice == null) {
            throw new RenException(ErrorCode.DOCK_NOT_REGISTER);
        }*/
        String topic = "thing/product/" + dockSn + "/drc/down";
        //数值重置 范围 -100~100
        if (drcStick.getStickInput() > 100) {
            drcStick.setStickInput(100);
        } else if (drcStick.getStickInput() < -100) {
            drcStick.setStickInput(-100);
        }
        //适配机场1
//        if ("3-1-0".equals(dockDevice.getDeviceModelKey())) {//大疆机场1
        int speed = 17 * drcStick.getStickInput() / 100;
        JSONObject message = new JSONObject();
        message.set("method", "drone_control");
        JSONObject data = new JSONObject();
        switch (drcStick.getStickDirection()) {
            case "pitch":
                data.set("x", speed);
                break;
            case "roll":
                data.set("y", speed);
                break;
            case "yaw":
                data.set("w", drcStick.getStickInput() > 0 ? 15 : -15);
                break;
            case "throttle":
                data.set("h", drcStick.getStickInput() > 0 ? 5 : -3);
                break;
        }
        message.set("data", data);
        mqttPushService.pushMessageByClient2(topic, message.toString());
        log.debug("drc --> drone_control:topic:{},message:{}", topic, message);
//        } else {
        //数据处理 上升下降的速度变慢,保持中值±330
        int stick;
        int throttleStick;
        if (drcStick.getStickInput() > 0) {
            stick = 1024 + (int) (660.0 * drcStick.getStickInput() / 100);
            throttleStick = 1024 + 330;
        } else {
            stick = 1024 - (int) (660.0 * Math.abs(drcStick.getStickInput()) / 100);
            throttleStick = 1024 - 330;
        }
        JSONObject message2 = new JSONObject();
        message2.set("seq", System.currentTimeMillis());
        message2.set("method", "stick_control");
        JSONObject data2 = new JSONObject();
        if ("throttle".equals(drcStick.getStickDirection())) {
            data2.set(drcStick.getStickDirection(), throttleStick);
        } else {
            data2.set(drcStick.getStickDirection(), stick);
        }
        message2.set("data", data2);
        mqttPushService.pushMessageByClient2(topic, message2.toString());
        log.debug("drc --> stick_control:topic:{},message:{}", topic, message2);
//        }
    }
}
