/*
package cn.iocoder.yudao.module.system.service.robot.job;

import cn.hutool.core.util.ObjectUtil;
import cn.iocoder.yudao.framework.common.util.object.BeanUtils;
import cn.iocoder.yudao.framework.mybatis.core.query.LambdaQueryWrapperX;
import cn.iocoder.yudao.module.mqtt.api.common.CommonApi;
import cn.iocoder.yudao.module.mqtt.api.task.dto.Pose2ds;
import cn.iocoder.yudao.module.mqtt.api.task.dto.RobotAcceptTaskDTO;
import cn.iocoder.yudao.module.mqtt.api.task.dto.RobotAcceptTaskData;
import cn.iocoder.yudao.module.mqtt.api.task.dto.RobotArgDTO;
import cn.iocoder.yudao.module.system.constant.robot.RobotTaskChcheConstant;
import cn.iocoder.yudao.module.system.constant.robot.RobotTopicConstant;
import cn.iocoder.yudao.module.system.controller.admin.robot.task.TaskPPDistribution;
import cn.iocoder.yudao.module.system.controller.admin.robot.task.TaskPathPlanningDTO;
import cn.iocoder.yudao.module.system.dal.dataobject.actionlog.RobotTaskDetailActionLogDO;
import cn.iocoder.yudao.module.system.dal.dataobject.houselocation.WareHouseLocationDO;
import cn.iocoder.yudao.module.system.dal.dataobject.robot.RobotChargeLogDO;
import cn.iocoder.yudao.module.system.dal.dataobject.robot.RobotTaskDetailDO;
import cn.iocoder.yudao.module.system.dal.mysql.actionlog.RobotTaskDetailActionLogMapper;
import cn.iocoder.yudao.module.system.dal.mysql.houselocation.WareHouseLocationMapper;
import cn.iocoder.yudao.module.system.dal.mysql.robot.RobotTaskDetailMapper;
import cn.iocoder.yudao.module.system.enums.robot.CommandTypeEnum;
import cn.iocoder.yudao.module.system.enums.robot.RobotTaskTypeEnum;
import cn.iocoder.yudao.module.system.enums.robot.task.RobotTaksOrderTypeEnum;
import cn.iocoder.yudao.module.system.service.actionlog.RobotTaskDetailActionLogService;
import cn.iocoder.yudao.module.system.service.robot.RobotInformationService;
import cn.iocoder.yudao.module.system.util.redis.RedisUtil;
import com.alibaba.fastjson.JSON;
import lombok.extern.slf4j.Slf4j;
import org.apache.commons.lang3.tuple.Pair;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.beans.factory.annotation.Value;
import org.springframework.scheduling.annotation.Async;
import org.springframework.stereotype.Service;

import javax.annotation.Resource;
import java.math.BigDecimal;
import java.util.*;
import java.util.function.Function;
import java.util.stream.Collectors;

import static cn.iocoder.yudao.module.system.config.SystemJobConfiguration.NOTIFY_THREAD_POOL_TASK_EXECUTOR;
import static com.baomidou.mybatisplus.core.toolkit.IdWorker.getId;

@Service
@Slf4j
public class RobotCommonTaskServiceImpl implements RobotCommonTaskService {

    @Resource
    private RobotTaskDetailMapper taskDetailMapper;

    @Autowired
    private RobotInformationService robotInformationService;

    @Resource
    private RobotTaskDetailActionLogMapper taskDetailActionLogMapper;

    @Value("${zn.init_height:0.0}")
    private double initHeight;

    @Value("${zn.default_scan_height:0.1}")
    private double defaultScanHeight;

    @Value("${zn.take_fork_height:0.1}")
    private double takeForkHeight;

    @Value("${zn.get_pallet_topic_height:0.4}")
    private double getPalletTopicHeight;

    @Value("${zn.parm:5000}")
    private Integer parm;

    @Value("${zn.lift_height:0.1}")
    private double liftHeight;

    @Value("${zn.move_height:0.1}")
    private double moveHeight;

    @Value("${zn.full_electricity:100}")
    private String fullElectricity;

    @Value("${zn.robot_doing_action.doing_take_msg}")
    private String dongTakeMsg;

    @Value("${zn.robot_doing_action.doing_release_msg}")
    private String dongReleaseMsg;

    @Value("${zn.robot_doing_action.complete_task}")
    private String completeTask;

    @Value("${zn.robot_doing_action.get_pallet_topic}")
    private String getPalletTopic;

    @Value("${zn.robot_doing_action.scan}")
    private String scan;

    @Value("${zn.robot_doing_action.parking_space}")
    private String parkingSpace;

    @Value("${zn.robot_doing_action.charge}")
    private String charge;

    @Resource
    private WareHouseLocationMapper houseLocationMapper;

    @Resource
    private CommonApi commonApi;

    @Resource
    private RobotTaskDetailActionLogService taskDetailActionLogService;

    @Resource
    private RedisUtil redisUtil;

    private static String SERIAL_KEY = "SERIAL_KEY";

    */
/**
     * 让机器人自动充电
     *
     * @param robotNo
     *//*

    @Override
    @Async(NOTIFY_THREAD_POOL_TASK_EXECUTOR)
    public void startAutoCharge(String robotNo) {
        //todo 后续沟通下手动模式的需不需要自动充电


    }

    */
/**
     * 客户下发的任务
     *
     *//*

    @Override
    @Async(NOTIFY_THREAD_POOL_TASK_EXECUTOR)
    public void sendTaskToRobot(TaskPPDistribution v) {
        RobotTaskDetailDO robotTaskDetailDO = taskDetailMapper.selectById(v.getId());

        Set<Long> locationIds = new HashSet<>();
        if (ObjectUtil.isNotEmpty(robotTaskDetailDO.getFromLocationId())) {
            locationIds.add(robotTaskDetailDO.getFromLocationId());
        }
        if (ObjectUtil.isNotEmpty(robotTaskDetailDO.getToLocationId())) {
            locationIds.add(robotTaskDetailDO.getToLocationId());
        }

        List<WareHouseLocationDO> wareHouseLocationDOS =
                houseLocationMapper.selectList(new LambdaQueryWrapperX<WareHouseLocationDO>()
                        .in(WareHouseLocationDO::getId, locationIds));
        Map<Long, WareHouseLocationDO> locationMap =
                wareHouseLocationDOS.stream().collect(Collectors.toMap(s -> s.getId(), Function.identity()));

        List<RobotTaskDetailActionLogDO> logs = new ArrayList<>();

        RobotAcceptTaskDTO robotTask = new RobotAcceptTaskDTO();
        robotTask.setOrder_id(v.getId().toString());

        String mac = robotInformationService.getMacByRobotNo(v.getRobotNo());
        robotTask.setTopic(RobotTopicConstant.ROBOT_TASK_TOPIC + mac);
        robotTask.setOrder_type(RobotTaksOrderTypeEnum.TASK.getType());
        //取放
        Pair<List<RobotAcceptTaskData>, List<RobotTaskDetailActionLogDO>> pair = null;
        if (RobotTaskTypeEnum.TAKE_RELEASE.getType().equals(robotTaskDetailDO.getTaskType())) {
            pair = takeReleaseTask(v, robotTaskDetailDO, locationMap);
            //移动
        } else if (RobotTaskTypeEnum.MOVE.getType().equals(robotTaskDetailDO.getTaskType())) {
            pair = moveTask(v, robotTaskDetailDO, locationMap);
            //仅取货
        } else if (RobotTaskTypeEnum.TAKE.getType().equals(robotTaskDetailDO.getTaskType())) {
            pair = takeTask(v, robotTaskDetailDO, locationMap);
            //仅放货
        } else if (RobotTaskTypeEnum.RELEASE.getType().equals(robotTaskDetailDO.getTaskType())) {
            pair = releaseTask(v, robotTaskDetailDO, locationMap);
        }

        if (ObjectUtil.isNotEmpty(pair)) {
            robotTask.setData(pair.getLeft());
            logs.addAll(pair.getRight());
            log.info("发送给车机下发任务 :{}", JSON.toJSONString(robotTask));
            commonApi.commonMethod(robotTask, robotTask.getTopic());
        }

        taskDetailActionLogService.addLogInCache(logs);
    }

    */
/**
     * 仅放货
     *
     * @param v
     * @param robotTaskDetailDO
     * @param locationMap
     * @return
     *//*

    private Pair<List<RobotAcceptTaskData>, List<RobotTaskDetailActionLogDO>> releaseTask(TaskPPDistribution v,
                                                                                          RobotTaskDetailDO robotTaskDetailDO,
                                                                                          Map<Long, WareHouseLocationDO> locationMap) {

        Map<String,Integer> serialMap = new HashMap<>();
        serialMap.put(SERIAL_KEY, 1);

        List<RobotAcceptTaskData> list = new ArrayList<>();
        List<RobotTaskDetailActionLogDO> logList = new ArrayList<>();
        //行走-放
        RobotTaskDetailActionLogDO logNight = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData nightMove = getToMovePose(logNight,  v, robotTaskDetailDO,serialMap);
        //抬高高度
        RobotTaskDetailActionLogDO logTen = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData tenFork = getTakeFork(logTen,  v, robotTaskDetailDO, false, locationMap, takeForkHeight,
                dongReleaseMsg,serialMap);
        //放货点位
        RobotTaskDetailActionLogDO logEleven = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData elevenFork = getMovePose(logEleven,  v, robotTaskDetailDO, locationMap, false,
                dongReleaseMsg,serialMap);
        //抬高高度
        RobotTaskDetailActionLogDO logTwelve = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData twelveFork = getInitFork(logTwelve,  v, robotTaskDetailDO, true, locationMap,serialMap);

        //放货完成使用
        String robotActionReleaseGoodsKey = RobotTaskChcheConstant.ROBOT_ACTION_RELEASE_GOODS + robotTaskDetailDO.getId();
        redisUtil.set(robotActionReleaseGoodsKey, twelveFork.getCommand_id());

        if (ObjectUtil.isNotEmpty(nightMove)) {
            list.add(nightMove);
            logList.add(logNight);
        }
        Collections.addAll(list, tenFork, elevenFork, twelveFork);

        Collections.addAll(logList, logTen, logEleven, logTwelve);

        Pair<List<RobotAcceptTaskData>, List<RobotTaskDetailActionLogDO>> pair = Pair.of(list, logList);
        return pair;
    }

    */
/**
     * 仅取货任务
     *
     * @param v
     * @param robotTaskDetailDO
     * @param locationMap
     * @return
     *//*

    private Pair<List<RobotAcceptTaskData>, List<RobotTaskDetailActionLogDO>> takeTask(TaskPPDistribution v, RobotTaskDetailDO robotTaskDetailDO,
                                                                                       Map<Long, WareHouseLocationDO> locationMap) {

        Map<String,Integer> serialMap = new HashMap<>();
        serialMap.put(SERIAL_KEY, 1);

        List<RobotAcceptTaskData> list = new ArrayList<>();
        List<RobotTaskDetailActionLogDO> logList = new ArrayList<>();
        //移动
        RobotTaskDetailActionLogDO logOne = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData oneMoveAndFork = getOneMoveAndFork(logOne,  v, robotTaskDetailDO,serialMap);
        //抬叉
        RobotTaskDetailActionLogDO logTwo = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData twoFork = getTakeFork(logTwo,  v, robotTaskDetailDO, true, locationMap, getPalletTopicHeight,
                dongTakeMsg,serialMap);
        //等待
        RobotTaskDetailActionLogDO logThree = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData threeWait = getWait(logThree,  v, robotTaskDetailDO,serialMap);
        //获取托盘位置
        RobotTaskDetailActionLogDO logFour = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData fourGetPalletTopic = getPalletTopic(logFour,  v, robotTaskDetailDO,serialMap);
        //抬高高度
        RobotTaskDetailActionLogDO logFive = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData fiveFork = getTakeFork(logFive,  v, robotTaskDetailDO, true, locationMap, null,
                dongTakeMsg,serialMap);
        //移动到取货终点
        RobotTaskDetailActionLogDO logSix = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData sixMoveToPalletPose = moveToPalletPose(logSix,  v, robotTaskDetailDO, dongTakeMsg,serialMap);
        //抬高高度
        RobotTaskDetailActionLogDO logSeven = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData sevenFork = getTakeFork(logSeven,  v, robotTaskDetailDO, true, locationMap, takeForkHeight,
                dongTakeMsg,serialMap);
        //路径最后一个点
        RobotTaskDetailActionLogDO logEight = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData eightFork = getMovePose(logEight,  v, robotTaskDetailDO, locationMap, true,
                dongTakeMsg,serialMap);

        //取货完成使用
        String robotActionTakeGoodsKey = RobotTaskChcheConstant.ROBOT_ACTION_TAKE_GOODS + robotTaskDetailDO.getId();
        redisUtil.set(robotActionTakeGoodsKey, eightFork.getCommand_id());

        if (ObjectUtil.isNotEmpty(oneMoveAndFork)) {
            list.add(oneMoveAndFork);
            logList.add(logOne);
        }
        Collections.addAll(list, twoFork, threeWait, fourGetPalletTopic, fiveFork, sixMoveToPalletPose,
                sevenFork, eightFork);

        Collections.addAll(logList, logTwo, logThree, logFour, logFive, logSix, logSeven, logEight);

        Pair<List<RobotAcceptTaskData>, List<RobotTaskDetailActionLogDO>> pair = Pair.of(list, logList);
        return pair;
    }

    */
/**
     * 移动的任务、充电任务
     *
     * @param v
     * @param robotTaskDetailDO
     * @param locationMap
     * @return
     *//*

    private Pair<List<RobotAcceptTaskData>, List<RobotTaskDetailActionLogDO>> moveTask(TaskPPDistribution v,
                                                                                       RobotTaskDetailDO robotTaskDetailDO,
                                                                                       Map<Long, WareHouseLocationDO> locationMap) {

        Map<String,Integer> serialMap = new HashMap<>();
        serialMap.put(SERIAL_KEY, 1);

        List<RobotAcceptTaskData> list = new ArrayList<>();
        List<RobotTaskDetailActionLogDO> logList = new ArrayList<>();
        //移动
        RobotTaskDetailActionLogDO logOne = new RobotTaskDetailActionLogDO();
        String msg = parkingSpace + robotTaskDetailDO.getToLocationNo();
        RobotAcceptTaskData oneMoveAndFork = onlyMove(logOne,  v, msg,serialMap);

        list.add(oneMoveAndFork);
        logList.add(logOne);

        Pair<List<RobotAcceptTaskData>, List<RobotTaskDetailActionLogDO>> pair = Pair.of(list, logList);
        return pair;
    }

    */
/**
     * 仅放货、移动
     *
     * @param logOne
     * @param v
     *//*

    private RobotAcceptTaskData onlyMove(RobotTaskDetailActionLogDO logOne, TaskPPDistribution v,
                                         String msg,Map<String,Integer> serialMap) {
        logOne.setCommandId(getId());
        logOne.setTaskDetailId(v.getId());
        RobotAcceptTaskData robotAcceptTaskData = new RobotAcceptTaskData();
        int serial = serialMap.get(SERIAL_KEY);
        robotAcceptTaskData.setSerial(String.valueOf(serial));
        serialMap.put(SERIAL_KEY, ++serial);

        robotAcceptTaskData.setCommand_id(String.valueOf(logOne.getCommandId()));

        robotAcceptTaskData.setCommand_type(CommandTypeEnum.MOVE_POSES_AND_FORK.getType());
        List<TaskPathPlanningDTO> takes = v.getReleases();
        List<Pose2ds> pose2ds = BeanUtils.toBean(takes, Pose2ds.class);
        RobotArgDTO arg = RobotArgDTO.builder().pose2ds(pose2ds).target_height(initHeight).build();
        robotAcceptTaskData.setArg(arg);

        logOne.setActionMsg(msg);
        setCommand(logOne, robotAcceptTaskData, v.getRobotNo());
        return robotAcceptTaskData;
    }

    */
/**
     * 取放
     *
     * @param v
     * @param robotTaskDetailDO
     * @return
     *//*

    private Pair<List<RobotAcceptTaskData>, List<RobotTaskDetailActionLogDO>> takeReleaseTask(TaskPPDistribution v, RobotTaskDetailDO robotTaskDetailDO,
                                                                                              Map<Long, WareHouseLocationDO> locationMap) {

        Map<String,Integer> serialMap = new HashMap<>();
        serialMap.put(SERIAL_KEY, 1);

        List<RobotAcceptTaskData> list = new ArrayList<>();
        List<RobotTaskDetailActionLogDO> logList = new ArrayList<>();
        //移动
        RobotTaskDetailActionLogDO logOne = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData oneMoveAndFork = getOneMoveAndFork(logOne,  v, robotTaskDetailDO,serialMap);
        //抬叉
        RobotTaskDetailActionLogDO logTwo = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData twoFork = getTakeFork(logTwo,  v, robotTaskDetailDO, true, locationMap, getPalletTopicHeight,
                dongTakeMsg,serialMap);
        //等待
        RobotTaskDetailActionLogDO logThree = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData threeWait = getWait(logThree,  v, robotTaskDetailDO,serialMap);
        //获取托盘位置
        RobotTaskDetailActionLogDO logFour = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData fourGetPalletTopic = getPalletTopic(logFour,  v, robotTaskDetailDO,serialMap);
        //抬高高度
        RobotTaskDetailActionLogDO logFive = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData fiveFork = getTakeFork(logFive,  v, robotTaskDetailDO, true, locationMap, null,
                dongTakeMsg,serialMap);
        //移动到取货终点
        RobotTaskDetailActionLogDO logSix = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData sixMoveToPalletPose = moveToPalletPose(logSix,  v, robotTaskDetailDO, dongTakeMsg,serialMap);
        //抬高高度
        RobotTaskDetailActionLogDO logSeven = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData sevenFork = getTakeFork(logSeven,  v, robotTaskDetailDO, true, locationMap, takeForkHeight,
                dongTakeMsg,serialMap);
        //路径最后一个点
        RobotTaskDetailActionLogDO logEight = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData eightFork = getMovePose(logEight,  v, robotTaskDetailDO, locationMap, true,
                dongTakeMsg,serialMap);
        //取货完成使用
        String robotActionTakeGoodsKey = RobotTaskChcheConstant.ROBOT_ACTION_TAKE_GOODS + robotTaskDetailDO.getId();
        redisUtil.set(robotActionTakeGoodsKey, eightFork.getCommand_id());
        //行走-放
        RobotTaskDetailActionLogDO logNight = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData nightMove = getToMovePose(logNight,  v, robotTaskDetailDO,serialMap);
        //抬高高度
        RobotTaskDetailActionLogDO logTen = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData tenFork = getTakeFork(logTen,  v, robotTaskDetailDO, false, locationMap, takeForkHeight,
                dongReleaseMsg,serialMap);
        //放货点位
        RobotTaskDetailActionLogDO logEleven = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData elevenFork = getMovePose(logEleven,  v, robotTaskDetailDO, locationMap, false,
                dongReleaseMsg,serialMap);
        //抬初始化高度
        RobotTaskDetailActionLogDO logTwelve = new RobotTaskDetailActionLogDO();
        RobotAcceptTaskData twelveFork = getInitFork(logTwelve,  v, robotTaskDetailDO, true, locationMap,serialMap);
        //放货完成使用
        String robotActionReleaseGoodsKey = RobotTaskChcheConstant.ROBOT_ACTION_RELEASE_GOODS + robotTaskDetailDO.getId();
        redisUtil.set(robotActionReleaseGoodsKey, twelveFork.getCommand_id());

        if (ObjectUtil.isNotEmpty(oneMoveAndFork)) {
            list.add(oneMoveAndFork);
            logList.add(logOne);
        }

        if (ObjectUtil.isNotEmpty(nightMove)) {
            list.add(nightMove);
            logList.add(logNight);
        }

        Collections.addAll(list, twoFork, threeWait, fourGetPalletTopic, fiveFork, sixMoveToPalletPose,
                sevenFork, eightFork, tenFork, elevenFork, twelveFork);

        Collections.addAll(logList, logTwo, logThree, logFour, logFive, logSix, logSeven, logEight, logTen, logEleven, logTwelve);

        Pair<List<RobotAcceptTaskData>, List<RobotTaskDetailActionLogDO>> pair = Pair.of(list, logList);
        return pair;
    }

    private RobotAcceptTaskData getInitFork(RobotTaskDetailActionLogDO logTwelve, TaskPPDistribution v,
                                            RobotTaskDetailDO robotTaskDetailDO, boolean b, Map<Long, WareHouseLocationDO> locationMap,
    Map<String,Integer> serialMap) {
        logTwelve.setCommandId(getId());
        logTwelve.setTaskDetailId(robotTaskDetailDO.getId());
        RobotAcceptTaskData robotAcceptTaskData = new RobotAcceptTaskData();
        int serial = serialMap.get(SERIAL_KEY);
        robotAcceptTaskData.setSerial(String.valueOf(serial));
        serialMap.put(SERIAL_KEY, ++serial);
        robotAcceptTaskData.setCommand_id(String.valueOf(logTwelve.getCommandId()));

        robotAcceptTaskData.setCommand_type(CommandTypeEnum.FORK.getType());
        RobotArgDTO arg = RobotArgDTO.builder().target_height(initHeight).build();
        robotAcceptTaskData.setArg(arg);

        logTwelve.setActionMsg(completeTask);
        setCommand(logTwelve, robotAcceptTaskData, robotTaskDetailDO.getRobotNo());
        return robotAcceptTaskData;

    }

    */
/**
     * 行走--放
     *
     * @param logNight
     * @param v
     * @param robotTaskDetailDO
     * @return
     *//*

    private RobotAcceptTaskData getToMovePose(RobotTaskDetailActionLogDO logNight, TaskPPDistribution v,
                                              RobotTaskDetailDO robotTaskDetailDO,Map<String,Integer> serialMap) {
        List<TaskPathPlanningDTO> releases = v.getReleases();
        if (releases.size() == 1) {
            return null;
        }
        logNight.setCommandId(getId());
        logNight.setTaskDetailId(robotTaskDetailDO.getId());
        RobotAcceptTaskData robotAcceptTaskData = new RobotAcceptTaskData();
        int serial = serialMap.get(SERIAL_KEY);
        robotAcceptTaskData.setSerial(String.valueOf(serial));
        serialMap.put(SERIAL_KEY, ++serial);
        robotAcceptTaskData.setCommand_id(String.valueOf(logNight.getCommandId()));
        robotAcceptTaskData.setCommand_type(CommandTypeEnum.MOVE_POSES_AND_FORK.getType());
        releases.remove(releases.size() - 1);
        List<Pose2ds> pose2ds = BeanUtils.toBean(releases, Pose2ds.class);
        RobotArgDTO arg = RobotArgDTO.builder().pose2ds(pose2ds).target_height(initHeight).build();
        robotAcceptTaskData.setArg(arg);

        logNight.setActionMsg(String.format("机器人正在前往%s%s", robotTaskDetailDO.getToLocationNo(), "放货"));
        setCommand(logNight, robotAcceptTaskData, robotTaskDetailDO.getRobotNo());
        return robotAcceptTaskData;
    }

    */
/**
     * 移动到最后一个点位
     *
     * @param logEight
     * @param v
     * @param robotTaskDetailDO
     * @param locationMap
     * @return
     *//*

    private RobotAcceptTaskData getMovePose(RobotTaskDetailActionLogDO logEight, TaskPPDistribution v,
                                            RobotTaskDetailDO robotTaskDetailDO, Map<Long, WareHouseLocationDO> locationMap,
                                            Boolean isTake, String msg,Map<String,Integer> serialMap) {
        logEight.setCommandId(getId());
        logEight.setTaskDetailId(robotTaskDetailDO.getId());
        RobotAcceptTaskData robotAcceptTaskData = new RobotAcceptTaskData();
        int serial = serialMap.get(SERIAL_KEY);
        robotAcceptTaskData.setSerial(String.valueOf(serial));
        serialMap.put(SERIAL_KEY, ++serial);
        robotAcceptTaskData.setCommand_id(String.valueOf(logEight.getCommandId()));
        robotAcceptTaskData.setCommand_type(CommandTypeEnum.MOVE_POSE.getType());

        TaskPathPlanningDTO taskPathPlanningDTO = null;
        if (isTake) {
            taskPathPlanningDTO = v.getTakes().get(v.getTakes().size() - 1);
        } else {
            taskPathPlanningDTO = v.getReleases().get(v.getReleases().size() - 1);
        }

        Pose2ds pose2d = BeanUtils.toBean(taskPathPlanningDTO, Pose2ds.class);

        RobotArgDTO arg = RobotArgDTO.builder().pose2d(pose2d).build();
        robotAcceptTaskData.setArg(arg);
        logEight.setActionMsg(msg);
        setCommand(logEight, robotAcceptTaskData, robotTaskDetailDO.getRobotNo());
        return robotAcceptTaskData;
    }

    */
/**
     * 移动到取货终点
     *
     * @param logSix
     * @param v
     * @param robotTaskDetailDO
     * @return
     *//*

    private RobotAcceptTaskData moveToPalletPose(RobotTaskDetailActionLogDO logSix, TaskPPDistribution v,
                                                 RobotTaskDetailDO robotTaskDetailDO, String msg,Map<String,Integer> serialMap) {
        logSix.setCommandId(getId());
        logSix.setTaskDetailId(robotTaskDetailDO.getId());
        RobotAcceptTaskData robotAcceptTaskData = new RobotAcceptTaskData();
        int serial = serialMap.get(SERIAL_KEY);
        robotAcceptTaskData.setSerial(String.valueOf(serial));
        serialMap.put(SERIAL_KEY, ++serial);
        robotAcceptTaskData.setCommand_id(String.valueOf(logSix.getCommandId()));
        robotAcceptTaskData.setCommand_type(CommandTypeEnum.MOVE_TO_PALLET_POSE.getType());
        logSix.setActionMsg(msg);
        setCommand(logSix, robotAcceptTaskData, robotTaskDetailDO.getRobotNo());
        return robotAcceptTaskData;
    }

    */
/**
     * 获取托盘位置
     *
     * @param logFour
     * @param v
     * @param robotTaskDetailDO
     * @return
     *//*

    private RobotAcceptTaskData getPalletTopic(RobotTaskDetailActionLogDO logFour, TaskPPDistribution v,
                                               RobotTaskDetailDO robotTaskDetailDO,Map<String,Integer> serialMap) {
        logFour.setCommandId(getId());
        logFour.setTaskDetailId(robotTaskDetailDO.getId());
        RobotAcceptTaskData robotAcceptTaskData = new RobotAcceptTaskData();
        int serial = serialMap.get(SERIAL_KEY);
        robotAcceptTaskData.setSerial(String.valueOf(serial));
        serialMap.put(SERIAL_KEY, ++serial);
        robotAcceptTaskData.setCommand_id(String.valueOf(logFour.getCommandId()));
        robotAcceptTaskData.setCommand_type(CommandTypeEnum.GET_PALLET_TOPIC.getType());
        logFour.setActionMsg(getPalletTopic);
        setCommand(logFour, robotAcceptTaskData, robotTaskDetailDO.getRobotNo());
        return robotAcceptTaskData;
    }

    //等待
    private RobotAcceptTaskData getWait(RobotTaskDetailActionLogDO logThree, TaskPPDistribution v,
                                        RobotTaskDetailDO robotTaskDetailDO,Map<String,Integer> serialMap) {
        logThree.setCommandId(getId());
        logThree.setTaskDetailId(robotTaskDetailDO.getId());
        RobotAcceptTaskData robotAcceptTaskData = new RobotAcceptTaskData();
        int serial = serialMap.get(SERIAL_KEY);
        robotAcceptTaskData.setSerial(String.valueOf(serial));
        serialMap.put(SERIAL_KEY, ++serial);
        robotAcceptTaskData.setCommand_id(String.valueOf(logThree.getCommandId()));
        robotAcceptTaskData.setCommand_type(CommandTypeEnum.WAIT.getType());
        RobotArgDTO arg = RobotArgDTO.builder().time(parm).build();

        robotAcceptTaskData.setArg(arg);
        logThree.setActionMsg(scan);
        setCommand(logThree, robotAcceptTaskData, robotTaskDetailDO.getRobotNo());
        return robotAcceptTaskData;
    }

    */
/**
     * 默认取的抬叉
     *
     * @param logTwo
     * @param v
     * @param robotTaskDetailDO
     * @return
     *//*

    private RobotAcceptTaskData getTakeFork(RobotTaskDetailActionLogDO logTwo, TaskPPDistribution v,
                                            RobotTaskDetailDO robotTaskDetailDO, Boolean isTake,
                                            Map<Long, WareHouseLocationDO> locationMap, Double addHeight,
                                            String msg,Map<String,Integer> serialMap) {
        logTwo.setCommandId(getId());
        logTwo.setTaskDetailId(robotTaskDetailDO.getId());
        RobotAcceptTaskData robotAcceptTaskData = new RobotAcceptTaskData();
        int serial = serialMap.get(SERIAL_KEY);
        robotAcceptTaskData.setSerial(String.valueOf(serial));
        serialMap.put(SERIAL_KEY, ++serial);
        robotAcceptTaskData.setCommand_id(String.valueOf(logTwo.getCommandId()));
        robotAcceptTaskData.setCommand_type(CommandTypeEnum.FORK.getType());

        WareHouseLocationDO wareHouseLocationDO = null;
        if (isTake) {
            wareHouseLocationDO = locationMap.get(robotTaskDetailDO.getFromLocationId());
        } else {
            wareHouseLocationDO = locationMap.get(robotTaskDetailDO.getToLocationId());
        }

        BigDecimal locationTrayHeight = wareHouseLocationDO.getLocationTrayHeight();

        Double targetHeight = ObjectUtil.isNotEmpty(locationTrayHeight) ? Double.valueOf(locationTrayHeight.toString()) : defaultScanHeight;

        if (ObjectUtil.isNotEmpty(addHeight)) {
            targetHeight = targetHeight + addHeight;
        }

        RobotArgDTO arg = RobotArgDTO.builder().target_height(targetHeight).build();

        robotAcceptTaskData.setArg(arg);
        logTwo.setActionMsg(msg);
        setCommand(logTwo, robotAcceptTaskData, robotTaskDetailDO.getRobotNo());
        return robotAcceptTaskData;
    }

    //移动
    public RobotAcceptTaskData getOneMoveAndFork(RobotTaskDetailActionLogDO logOne, TaskPPDistribution v,
                                                 RobotTaskDetailDO robotTaskDetailDO,Map<String,Integer> serialMap) {
        List<TaskPathPlanningDTO> takes = v.getTakes();
        //说明机器人不需要移动
        if (takes.size() == 1) {
            return null;
        }

        logOne.setCommandId(getId());
        logOne.setTaskDetailId(robotTaskDetailDO.getId());
        RobotAcceptTaskData robotAcceptTaskData = new RobotAcceptTaskData();
        int serial = serialMap.get(SERIAL_KEY);
        robotAcceptTaskData.setSerial(String.valueOf(serial));
        serialMap.put(SERIAL_KEY, ++serial);
        robotAcceptTaskData.setCommand_id(String.valueOf(logOne.getCommandId()));
        robotAcceptTaskData.setCommand_type(CommandTypeEnum.MOVE_POSES_AND_FORK.getType());

        if (takes.size() > 0) {
            takes.remove(takes.size() - 1);
        }

        List<Pose2ds> pose2ds = BeanUtils.toBean(takes, Pose2ds.class);
        RobotArgDTO arg = RobotArgDTO.builder().pose2ds(pose2ds).target_height(initHeight).build();
        robotAcceptTaskData.setArg(arg);

        logOne.setActionMsg(String.format("机器人正在前往%s%s", robotTaskDetailDO.getFromLocationNo(), "取货"));
        setCommand(logOne, robotAcceptTaskData, robotTaskDetailDO.getRobotNo());
        return robotAcceptTaskData;
    }

    public void setCommand(RobotTaskDetailActionLogDO logOne, RobotAcceptTaskData robotAcceptTaskData, String robotNo) {
        logOne.setCommandType(robotAcceptTaskData.getCommand_type());
        logOne.setRobotNo(robotNo);
        String str = JSON.toJSONString(robotAcceptTaskData);
        if (str.length() > 210) {
            logOne.setCommandMsg(str.substring(0, 200));
        } else {
            logOne.setCommandMsg(str);
        }

    }

    */
/**
     * 充电任务
     *//*

    @Override
    @Async(NOTIFY_THREAD_POOL_TASK_EXECUTOR)
    public void sendChargeTaskToRobot(TaskPPDistribution v, RobotChargeLogDO chargeLogDO) {
        if (ObjectUtil.isEmpty(v) || ObjectUtil.isEmpty(chargeLogDO)) {
            return;
        }

        List<RobotTaskDetailActionLogDO> logList = new ArrayList<>();

        RobotAcceptTaskDTO robotTask = new RobotAcceptTaskDTO();
        robotTask.setOrder_id(v.getId().toString());

        String mac = robotInformationService.getMacByRobotNo(v.getRobotNo());
        robotTask.setTopic(RobotTopicConstant.ROBOT_TASK_TOPIC + mac);
        robotTask.setOrder_type(RobotTaksOrderTypeEnum.AUTO_CHARGE.getType());

        Map<String,Integer> serialMap = new HashMap<>();
        serialMap.put(SERIAL_KEY, 1);

        List<RobotAcceptTaskData> list = new ArrayList<>();
        //移动
        RobotTaskDetailActionLogDO logOne = new RobotTaskDetailActionLogDO();
        String msg = charge + chargeLogDO.getDeviceNo();
        RobotAcceptTaskData oneMoveAndFork = onlyMove(logOne, v, msg,serialMap);

        list.add(oneMoveAndFork);
        robotTask.setData(list);
        logList.add(logOne);

        log.info("发送给车机下发--充电任务 :{}", JSON.toJSONString(robotTask));
        commonApi.commonMethod(robotTask, robotTask.getTopic());

        taskDetailActionLogService.addLogInCache(logList);
    }


}
*/
