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

import cn.hutool.core.util.IdUtil;
import cn.hutool.core.util.StrUtil;

import cn.hutool.json.JSONObject;
import cn.hutool.json.JSONUtil;
import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
import com.baomidou.mybatisplus.core.metadata.IPage;
import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.multictrl.common.exception.RenException;
import com.multictrl.common.page.PageData;
import com.multictrl.common.utils.ConvertUtils;
import com.multictrl.modules.business.dao.RouteDao;
import com.multictrl.modules.business.dao.RouteWaypointDao;
import com.multictrl.modules.business.dao.WaypointActionDao;
import com.multictrl.modules.business.dto.RouteDTO;
import com.multictrl.modules.business.dto.RouteWaypointDTO;
import com.multictrl.modules.business.dto.WaypointActionDTO;
import com.multictrl.modules.business.entity.RouteEntity;
import com.multictrl.modules.business.entity.RouteWaypointEntity;
import com.multictrl.modules.business.entity.WaypointActionEntity;
import com.multictrl.modules.business.q20.dto.*;
import com.multictrl.modules.business.q20.service.Q20RouteService;
import com.multictrl.modules.business.service.DJIBaseService;
import com.multictrl.modules.business.service.FlightTaskService;

import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;
import org.springframework.transaction.annotation.Transactional;

import java.math.BigInteger;
import java.util.ArrayList;
import java.util.List;
import java.util.Map;
import java.util.regex.Matcher;
import java.util.regex.Pattern;

/**
 * Q20航线任务服务实现
 * 复用 DJIBaseService 的 send/wait 机制，topic 为 thing/device/{sn}/services
 *
 * @author 938693313@qq.com
 * @since 1.0.0 2026/5/20
 */
@Slf4j
@Service
@RequiredArgsConstructor
public class Q20RouteServiceImpl implements Q20RouteService {

    private final DJIBaseService djiBaseService;
    private final FlightTaskService flightTaskService;
    private final ObjectMapper objectMapper;
    private final RouteDao routeDao;
    private final RouteWaypointDao routeWaypointDao;
    private final WaypointActionDao waypointActionDao;

    private static final String SERVICES_TOPIC = "thing/device/%s/services";
    private static final String TEMPLATE_TYPE_Q20 = "q20";
    /** 上传航线的航线ID前缀 */
    private static final String ROUTE_WAYLINE_PREFIX = "q20_route_";
    /** 一键航线的航线ID前缀 */
    private static final String AUTO_WAYLINE_PREFIX = "q20_auto_";
    /** 拆分航线ID的「前缀 + 数字后缀」 */
    private static final Pattern WAYLINE_TAIL = Pattern.compile("^(.*?)(\\d+)$");

    // ────────────────────────────────────────────────
    // 内部通用方法
    // ────────────────────────────────────────────────

    /** 发送指令并等待回复，返回结果描述字符串 */
    private String execCmd(String deviceSn, String method, JSONObject data) {
        String topic = String.format(SERVICES_TOPIC, deviceSn);
        return djiBaseService.sendWaitReplyJudgeResult(topic, djiBaseService.getPayload(method, data));
    }

    /** 发送指令并等待回复（携带 gateway 与 need_reply=1），返回结果描述字符串 */
    private String execCmdWithGateway(String deviceSn, String method, JSONObject data) {
        String topic = String.format(SERVICES_TOPIC, deviceSn);
        JSONObject payload = djiBaseService.getPayload(method, data);
        payload.set("gateway", deviceSn);
        payload.set("need_reply", 1);
        return djiBaseService.sendWaitReplyJudgeResult(topic, payload);
    }

    /** 发送指令并等待回复，返回完整回复JSONObject；超时抛出RenException */
    private JSONObject execCmdGetReply(String deviceSn, String method, JSONObject data) {
        String topic = String.format(SERVICES_TOPIC, deviceSn);
        JSONObject reply = djiBaseService.sendWaitReply(topic, djiBaseService.getPayload(method, data));
        if (reply == null) {
            throw new RenException("设备未响应，请检查设备是否在线");
        }
        return reply;
    }

    /** 发送指令并等待回复（携带 gateway 与 need_reply=1），返回完整回复JSONObject；超时抛出RenException */
    private JSONObject execCmdGetReplyWithGateway(String deviceSn, String method, JSONObject data) {
        String topic = String.format(SERVICES_TOPIC, deviceSn);
        JSONObject payload = djiBaseService.getPayload(method, data);
        payload.set("gateway", deviceSn);
        payload.set("need_reply", 1);
        JSONObject reply = djiBaseService.sendWaitReply(topic, payload);
        if (reply == null) {
            throw new RenException("设备未响应，请检查设备是否在线");
        }
        return reply;
    }

    /** 利用Jackson @JsonProperty注解将DTO序列化为snake_case键的JSONObject */
    private JSONObject dtoToJson(Object dto) {
        try {
            String json = objectMapper.writeValueAsString(dto);
            return JSONUtil.parseObj(json);
        } catch (Exception e) {
            throw new RenException("参数序列化失败: " + e.getMessage());
        }
    }

    // ────────────────────────────────────────────────
    // 接口实现
    // ────────────────────────────────────────────────

    @Override
    public PageData<RouteDTO> pageQ20Routes(Map<String, Object> params) {
        long curPage = parseLong(params.get("page"), 1);
        long limit = parseLong(params.get("limit"), 10);
        String routeName = (String) params.get("routeName");
        String q20RouteId = (String) params.get("q20RouteId");

        QueryWrapper<RouteEntity> wrapper = new QueryWrapper<>();
        wrapper.eq("template_type", TEMPLATE_TYPE_Q20);
        wrapper.like(StrUtil.isNotBlank(routeName), "route_name", routeName);
        wrapper.like(StrUtil.isNotBlank(q20RouteId), "q20_route_id", q20RouteId);
        wrapper.orderByDesc("update_date", "create_date");

        IPage<RouteEntity> result = routeDao.selectPage(new Page<>(curPage, limit), wrapper);

        List<RouteDTO> list = new ArrayList<>();
        for (RouteEntity entity : result.getRecords()) {
            RouteDTO dto = ConvertUtils.sourceToTarget(entity, RouteDTO.class);
            // 关联航点（bus_route_waypoint），按航点顺序升序
            List<RouteWaypointEntity> waypoints = routeWaypointDao.selectList(
                    new QueryWrapper<RouteWaypointEntity>()
                            .eq("route_id", entity.getId())
                            .orderByAsc("waypoint_sort"));
            List<RouteWaypointDTO> waypointDtos = ConvertUtils.sourceToTarget(waypoints, RouteWaypointDTO.class);
            // 关联每个航点的动作（bus_waypoint_action），按动作顺序升序
            for (RouteWaypointDTO wp : waypointDtos) {
                List<WaypointActionEntity> actions = waypointActionDao.selectList(
                        new QueryWrapper<WaypointActionEntity>()
                                .eq("route_id", entity.getId())
                                .eq("waypoint_id", wp.getId())
                                .orderByAsc("action_sort"));
                wp.setWaypointActionList(ConvertUtils.sourceToTarget(actions, WaypointActionDTO.class));
            }
            dto.setRouteWaypointList(waypointDtos);
            list.add(dto);
        }
        return new PageData<>(list, result.getTotal());
    }

    /** 解析分页参数（字符串/数字均兼容），无效时返回默认值 */
    private long parseLong(Object val, long defaultVal) {
        if (val == null) {
            return defaultVal;
        }
        try {
            return Long.parseLong(val.toString());
        } catch (NumberFormatException e) {
            return defaultVal;
        }
    }

    @Override
    public String nextWayline(String type) {
        // 按类型区分前缀：一键航线用 q20_auto_，上传航线用 q20_route_
        String prefix = AUTO_WAYLINE_PREFIX.equals(type) || "auto".equalsIgnoreCase(type)
                ? AUTO_WAYLINE_PREFIX : ROUTE_WAYLINE_PREFIX;
        String defaultId = prefix + "001";
        // 取库中该前缀下最新一条Q20航线的航线ID，按其数字后缀递增
        RouteEntity latest = routeDao.selectOne(new QueryWrapper<RouteEntity>()
                .eq("template_type", TEMPLATE_TYPE_Q20)
                .likeRight("q20_route_id", prefix)
                .orderByDesc("create_date")
                .last("LIMIT 1"));
        String next = latest != null ? incrementWayline(latest.getQ20RouteId()) : defaultId;
        // 兜底：防止递增结果与库中已有ID冲突
        int guard = 0;
        while (waylineExists(next) && guard++ < 1000) {
            next = incrementWayline(next);
        }
        return next;
    }

    /** 航线ID递增：保留前缀与位宽，对数字后缀+1；无数字后缀时追加_001 */
    private String incrementWayline(String base) {
        if (StrUtil.isBlank(base)) {
            return ROUTE_WAYLINE_PREFIX + "001";
        }
        Matcher m = WAYLINE_TAIL.matcher(base.trim());
        if (!m.matches()) {
            return base.trim() + "_001";
        }
        String prefix = m.group(1);
        String num = m.group(2);
        String inc = new BigInteger(num).add(BigInteger.ONE).toString();
        if (inc.length() < num.length()) {
            inc = StrUtil.padPre(inc, num.length(), '0');
        }
        return prefix + inc;
    }

    /** 判断航线ID是否已存在于Q20航线库 */
    private boolean waylineExists(String wayline) {
        if (StrUtil.isBlank(wayline)) {
            return false;
        }
        Long cnt = routeDao.selectCount(new QueryWrapper<RouteEntity>()
                .eq("q20_route_id", wayline)
                .eq("template_type", TEMPLATE_TYPE_Q20));
        return cnt != null && cnt > 0;
    }

    @Override
    @Transactional(rollbackFor = Exception.class)
    public String routeUpload(String deviceSn, Q20RouteUploadDTO dto) {
        validateWaypoints(dto);
        validateMissionConfig(dto);
        checkRouteNotExists(dto.getWayline(), dto.getRouteName());
        JSONObject data = dtoToJson(dto);
        String result = execCmdWithGateway(deviceSn, "route_upload", data);
        saveRouteToLocal(deviceSn, dto);
        return result;
    }

    /** 航点校验：航线至少需要两个有效航点（含经纬度坐标） */
    private void validateWaypoints(Q20RouteUploadDTO dto) {
        List<Q20RoutePlacemarkDTO> placemarks = (dto != null && dto.getFolder() != null)
                ? dto.getFolder().getPlacemark() : null;
        long valid = placemarks == null ? 0 : placemarks.stream()
                .filter(p -> p != null && p.getPoint() != null
                        && StrUtil.isNotBlank(p.getPoint().getCoordinates())
                        && p.getPoint().getCoordinates().split(",").length >= 2)
                .count();
        if (valid < 2) {
            throw new RenException("航线至少需要两个有效航点（含经纬度）");
        }
        // 不使用全局速度(use_global_speed=0)的航点必须填写航点飞行速度(waypoint_speed)
        for (Q20RoutePlacemarkDTO p : placemarks) {
            if (p != null && p.getUseGlobalSpeed() != null && p.getUseGlobalSpeed() == 0
                    && p.getWaypointSpeed() == null) {
                throw new RenException("航点" + p.getIndex() + "未使用全局速度时，必须填写航点飞行速度(waypoint_speed)");
            }
        }
    }

    /**
     * 任务配置条件必填校验：
     * preLand 时 action_value 必填；goHome 时 go_home_type 必填；
     * exit_on_rc_lost 为 executeLostAction 时 execute_rc_lost_action 必填
     */
    private void validateMissionConfig(Q20RouteUploadDTO dto) {
        Q20RouteMissionConfigDTO mc = dto != null ? dto.getMissionConfig() : null;
        if (mc == null) {
            return;
        }
        if ("preLand".equals(mc.getFinishAction()) && StrUtil.isBlank(mc.getActionValue())) {
            throw new RenException("结束动作为精准降落(preLand)时，必须填写精准降落二维码值(action_value)");
        }
        if ("goHome".equals(mc.getFinishAction()) && StrUtil.isBlank(mc.getGoHomeType())) {
            throw new RenException("结束动作为返航(goHome)时，必须填写返航类型(go_home_type)");
        }
        if ("executeLostAction".equals(mc.getExitOnRcLost()) && StrUtil.isBlank(mc.getExecuteRcLostAction())) {
            throw new RenException("失联退出策略为执行失控动作(executeLostAction)时，必须填写失联执行动作(execute_rc_lost_action)");
        }
    }

    /** 上传前校验：航线ID和航线名称均不能与库中已有记录重复 */
    private void checkRouteNotExists(String wayline, String routeName) {
        if (StrUtil.isNotBlank(wayline)) {
            Long cnt = routeDao.selectCount(new QueryWrapper<RouteEntity>()
                    .eq("q20_route_id", wayline)
                    .eq("template_type", TEMPLATE_TYPE_Q20));
            if (cnt != null && cnt > 0) {
                throw new RenException("航线ID「" + wayline + "」已存在，请勿重复上传");
            }
        }
//        if (StrUtil.isNotBlank(routeName)) {
//            Long cnt = routeDao.selectCount(new QueryWrapper<RouteEntity>()
//                    .eq("route_name", routeName)
//                    .eq("template_type", TEMPLATE_TYPE_Q20));
//            if (cnt != null && cnt > 0) {
//                throw new RenException("航线名称「" + routeName + "」已存在，请勿重复上传");
//            }
//        }
    }

    @Override
    public String routeExecute(String deviceSn, Q20RouteExecuteDTO dto) {
        JSONObject data = dtoToJson(dto);
        String result = execCmdWithGateway(deviceSn, "route_execute", data);
        saveFlightTask(deviceSn, dto.getRouteId());
        return result;
    }

    @Override
    @Transactional(rollbackFor = Exception.class)
    public String routeAuto(String deviceSn, Q20RouteAutoDTO dto) {
        validateWaypoints(dto.getRouteInfo());
        validateMissionConfig(dto.getRouteInfo());
        Q20RouteUploadDTO routeInfo = dto.getRouteInfo();
        checkRouteNotExists(routeInfo.getWayline(), routeInfo.getRouteName());
        JSONObject data = new JSONObject();
        data.set("route_info", dtoToJson(dto.getRouteInfo()));
        data.set("execute_info", dtoToJson(dto.getExecuteInfo()));
        data.set("vehicle_config", dtoToJson(dto.getVehicleConfig()));
        String result = execCmdWithGateway(deviceSn, "route_auto", data);
        if (dto.getRouteInfo() != null) {
            saveRouteToLocal(deviceSn, dto.getRouteInfo());
        }
        String waylineId = dto.getRouteInfo() != null ? dto.getRouteInfo().getWayline() : null;
        saveFlightTask(deviceSn, waylineId);
        return result;
    }

    @Override
    public String breakLoopHover(String deviceSn) {
        JSONObject data = new JSONObject();
        data.set("value", 1);
        return execCmdWithGateway(deviceSn, "break_loop_hover", data);
    }

    @Override
    public JSONObject routeInfo(String deviceSn, int mode, String value) {
        JSONObject data = new JSONObject();
        data.set("mode", mode);
        data.set("value", value);
        return execCmdGetReply(deviceSn, "route_info", data);
    }

    @Override
    public JSONObject routeProgress(String deviceSn) {
        JSONObject data = new JSONObject();
        data.set("value", 1);
        return execCmdGetReplyWithGateway(deviceSn, "route_progress", data);
    }

    @Override
    public String routePause(String deviceSn) {
        JSONObject data = new JSONObject();
        data.set("value", 1);
        return execCmdWithGateway(deviceSn, "route_pause", data);
    }

    @Override
    public String routeResume(String deviceSn) {
        JSONObject data = new JSONObject();
        data.set("value", 1);
        return execCmdWithGateway(deviceSn, "route_resume", data);
    }

    @Override
    public String routeFinish(String deviceSn) {
        JSONObject data = new JSONObject();
        data.set("value", 1);
        return execCmdWithGateway(deviceSn, "route_finish", data);
    }

    @Override
    public JSONObject geofenceInfo(String deviceSn) {
        JSONObject data = new JSONObject();
        data.set("value", 1);
        return execCmdGetReply(deviceSn, "geofence_info", data);
    }

    @Override
    public String geofenceUpload(String deviceSn, Q20GeofenceUploadDTO dto) {
        JSONObject data = new JSONObject();
        data.set("geofence", dtoToJson(dto));
        return execCmd(deviceSn, "geofence_upload", data);
    }

    // ────────────────────────────────────────────────
    // 本地航线库存储
    // ────────────────────────────────────────────────

    /**
     * 上传成功后将航线数据持久化到本地航线库
     * 同设备同 wayline ID 已存在时覆盖旧记录（删除旧航点/动作后重新插入）
     */
    private void saveRouteToLocal(String deviceSn, Q20RouteUploadDTO dto) {
        String wayline = dto.getWayline();
        String routeName = StrUtil.isNotBlank(dto.getRouteName())
                ? dto.getRouteName()
                : (StrUtil.isNotBlank(wayline) ? wayline : deviceSn + "_" + System.currentTimeMillis());

        // 同设备相同 wayline 已存在则清除旧记录
        RouteEntity existing = routeDao.selectOne(new QueryWrapper<RouteEntity>()
                .eq("dock_sn", deviceSn)
                .eq(StrUtil.isNotBlank(wayline), "q20_route_id", wayline)
                .eq("template_type", TEMPLATE_TYPE_Q20));
        if (existing != null) {
            Long oldId = existing.getId();
            routeWaypointDao.delete(new QueryWrapper<RouteWaypointEntity>().eq("route_id", oldId));
            waypointActionDao.delete(new QueryWrapper<WaypointActionEntity>().eq("route_id", oldId));
            routeDao.deleteById(oldId);
            log.debug("Q20 routeUpload -> 覆盖旧航线记录 id={}, wayline={}", oldId, wayline);
        }

        // 构建 RouteEntity
        RouteEntity route = new RouteEntity();
        route.setRouteName(routeName);
        route.setDockSn(deviceSn);
        route.setTemplateType(TEMPLATE_TYPE_Q20);
        route.setQ20RouteId(wayline);

        Q20RouteMissionConfigDTO mc = dto.getMissionConfig();
        if (mc != null) {
            route.setFinishAction(mc.getFinishAction());
            route.setExitOnRcLost(mc.getExitOnRcLost());
            route.setExecuteRcLostAction(mc.getExecuteRcLostAction());
            if (mc.getGlobalRthHeight() != null) {
                route.setGlobalRthHeight(mc.getGlobalRthHeight().doubleValue());
            }
        }

        Q20RouteFolderDTO folder = dto.getFolder();
        if (folder != null) {
            route.setHeightModel(folder.getExecuteHeightMode() != null ? folder.getExecuteHeightMode() : "relativeToStartPoint");
            route.setFlightSpeed((double) folder.getAutoFlightSpeed());
            List<Q20RoutePlacemarkDTO> placemarks = folder.getPlacemark();
            route.setWaypointNum(placemarks != null ? placemarks.size() : 0);
        }

        // vehicleConfig 中的 globalRthHeight 优先级低于 missionConfig
        Q20RouteVehicleConfigDTO vc = dto.getVehicleConfig();
        if (vc != null && route.getGlobalRthHeight() == null && vc.getGlobalRthHeight() != null) {
            route.setGlobalRthHeight(vc.getGlobalRthHeight().doubleValue());
        }

        // 前端传入的可选覆盖字段
        if (dto.getFlightHeight() != null)   route.setFlightHeight(dto.getFlightHeight().doubleValue());
        if (dto.getAccurateImport() != null) route.setIsAccurateImport(dto.getAccurateImport());

        // 补全 NOT NULL 字段默认值
        if (route.getFlightHeight() == null)             route.setFlightHeight(100.0);
        if (route.getGlobalRthHeight() == null)          route.setGlobalRthHeight(100.0);
        if (route.getTakeoffSecurityHeight() == null)    route.setTakeoffSecurityHeight(100.0);
        if (route.getGlobalTransitionalSpeed() == null)  route.setGlobalTransitionalSpeed(5.0);
        if (route.getFinishAction() == null)             route.setFinishAction("goHome");
        if (route.getExitOnRcLost() == null)             route.setExitOnRcLost("executeLostAction");
        if (route.getFlyToWaylineMode() == null)         route.setFlyToWaylineMode("safely");
        if (route.getTotalDistance() == null)            route.setTotalDistance(0.0);
        if (route.getExpectFlightTime() == null)         route.setExpectFlightTime("0");
        if (route.getIsAccurateImport() == null)         route.setIsAccurateImport(false);

        routeDao.insert(route);
        Long routeId = route.getId();

        // 保存航点及动作
        if (folder != null && folder.getPlacemark() != null) {
            for (Q20RoutePlacemarkDTO p : folder.getPlacemark()) {
                RouteWaypointEntity wp = buildWaypoint(routeId, p, folder.getAutoFlightSpeed());
                routeWaypointDao.insert(wp);

                if (p.getActionGroup() != null && p.getActionGroup().getAction() != null) {
                    List<Q20RouteActionDTO> actions = p.getActionGroup().getAction();
                    for (int i = 0; i < actions.size(); i++) {
                        Q20RouteActionDTO action = actions.get(i);
                        WaypointActionEntity wa = new WaypointActionEntity();
                        wa.setRouteId(routeId);
                        wa.setWaypointId(wp.getId());
                        wa.setActionType(action.getActionActuatorFunc());
                        wa.setActionSort(i);
                        if (action.getActionActuatorFuncParam() != null) {
                            try {
                                wa.setActionValue(objectMapper.writeValueAsString(action.getActionActuatorFuncParam()));
                            } catch (Exception e) {
                                log.warn("Q20 routeUpload -> 动作参数序列化失败: {}", e.getMessage());
                            }
                        }
                        waypointActionDao.insert(wa);
                    }
                }
            }
        }

        log.info("Q20 routeUpload -> 航线已入库 routeId={}, routeName={}, wayline={}, deviceSn={}",
                routeId, routeName, wayline, deviceSn);
    }

    /**
     * 执行航线后创建飞行任务记录
     * 优先按 waylineId 匹配，找不到则取该设备最近一条 Q20 航线
     */
    private void saveFlightTask(String deviceSn, String waylineId) {
        RouteEntity route = null;
        if (StrUtil.isNotBlank(waylineId)) {
            route = routeDao.selectOne(new QueryWrapper<RouteEntity>()
                    .eq("q20_route_id", waylineId)
                    .eq("template_type", TEMPLATE_TYPE_Q20));
        }
        if (route == null) {
            route = routeDao.selectOne(new QueryWrapper<RouteEntity>()
                    .eq("dock_sn", deviceSn)
                    .eq("template_type", TEMPLATE_TYPE_Q20)
                    .orderByDesc("create_date")
                    .last("LIMIT 1"));
        }
        if (route == null) {
            log.warn("Q20 saveFlightTask -> 未找到航线记录，跳过任务创建 deviceSn={}, waylineId={}", deviceSn, waylineId);
            return;
        }
        String taskId = IdUtil.fastSimpleUUID();
        flightTaskService.addRouteTask(taskId, deviceSn, route.getId());
        log.info("Q20 saveFlightTask -> 任务已创建 taskId={}, routeId={}, deviceSn={}", taskId, route.getId(), deviceSn);
    }

    private RouteWaypointEntity buildWaypoint(Long routeId, Q20RoutePlacemarkDTO p, float globalSpeed) {
        RouteWaypointEntity wp = new RouteWaypointEntity();
        wp.setRouteId(routeId);
        wp.setWaypointSort(p.getIndex());
        wp.setFlightHeight((double) p.getExecuteHeight());

        // 解析坐标 "经度,纬度"
        if (p.getPoint() != null && StrUtil.isNotBlank(p.getPoint().getCoordinates())) {
            String[] parts = p.getPoint().getCoordinates().split(",");
            if (parts.length >= 2) {
                try {
                    wp.setLongitude(Double.parseDouble(parts[0].trim()));
                    wp.setLatitude(Double.parseDouble(parts[1].trim()));
                } catch (NumberFormatException e) {
                    log.warn("Q20 routeUpload -> 坐标解析失败: {}", p.getPoint().getCoordinates());
                }
            }
        }

        boolean useGlobal = p.getUseGlobalSpeed() == null || p.getUseGlobalSpeed() == 1;
        wp.setFollowRouteSpeed(useGlobal);
        wp.setFollowRouteHeight(false);
        wp.setFlightSpeed(useGlobal
                ? (double) globalSpeed
                : (p.getWaypointSpeed() != null ? p.getWaypointSpeed().doubleValue() : (double) globalSpeed));

        // 偏航/转弯模式不再由航点配置，统一使用默认值入库
        wp.setWaypointHeadingMode("followWayline");
        wp.setWaypointHeadingPathMode("followBadArc");
        wp.setWaypointTruningMode("toPointAndStopWithDiscontinuityCurvature");

        return wp;
    }
}
