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

import cn.hutool.core.util.ObjectUtil;
import cn.hutool.json.JSONUtil;
import cn.iocoder.yudao.framework.mybatis.core.query.LambdaQueryWrapperX;
import cn.iocoder.yudao.framework.tenant.core.context.TenantContextHolder;
import cn.iocoder.yudao.module.mqtt.api.task.RobotTaskApi;
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.system.api.robot.dto.RobotStatusDataPoseDTO;
import cn.iocoder.yudao.module.system.constant.robot.RobotStatusCodeConstant;
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.config.vo.CommonConfigVO;
import cn.iocoder.yudao.module.system.dal.dataobject.config.CommonConfigDO;
import cn.iocoder.yudao.module.system.dal.dataobject.houselocation.WareHouseLocationDO;
import cn.iocoder.yudao.module.system.dal.dataobject.robot.RobotInformationDO;
import cn.iocoder.yudao.module.system.dal.dataobject.robot.RobotTaskDO;
import cn.iocoder.yudao.module.system.dal.dataobject.robot.RobotTaskDetailDO;
import cn.iocoder.yudao.module.system.dal.mysql.config.CommonConfigMapper;
import cn.iocoder.yudao.module.system.dal.mysql.houselocation.WareHouseLocationMapper;
import cn.iocoder.yudao.module.system.dal.mysql.positionmap.PositionMapItemMapper;
import cn.iocoder.yudao.module.system.dal.mysql.robot.RobotInformationMapper;
import cn.iocoder.yudao.module.system.dal.mysql.robot.RobotTaskDetailMapper;
import cn.iocoder.yudao.module.system.dal.mysql.robot.RobotTaskMapper;
import cn.iocoder.yudao.module.system.enums.common.ZeroOneEnum;
import cn.iocoder.yudao.module.system.enums.config.CommandConfigTypeEnum;
import cn.iocoder.yudao.module.system.enums.robot.*;
import cn.iocoder.yudao.module.system.enums.robot.charge.ChargeModelEnum;
import cn.iocoder.yudao.module.system.enums.robot.information.ChargeTypeEnum;
import cn.iocoder.yudao.module.system.util.redis.RedisUtil;
import com.alibaba.fastjson.JSON;
import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
import lombok.extern.slf4j.Slf4j;
import org.apache.commons.lang3.tuple.ImmutablePair;
import org.apache.commons.lang3.tuple.Pair;
import org.bouncycastle.pqc.crypto.util.PQCOtherInfoGenerator;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.beans.factory.annotation.Value;
import org.springframework.stereotype.Service;
import org.springframework.transaction.annotation.Transactional;

import javax.annotation.Resource;
import java.math.BigDecimal;
import java.time.LocalDateTime;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.Set;
import java.util.stream.Collectors;

@Service
@Slf4j
public class DistributeTasksServiceImpl implements DistributeTasksService {

    @Autowired
    private RobotInformationMapper robotInformationMapper;

    @Autowired
    private RobotTaskMapper robotTaskMapper;

    @Autowired
    private RobotTaskDetailMapper robotTaskDetailMapper;

    @Resource
    private RobotTaskApi robotTaskApi;

    @Resource
    private PositionMapItemMapper positionMapItemMapper;

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

    @Value("${zn.default_scan_height:0.4}")
    private double scanHeight;

    @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;

    @Resource
    private CommonConfigMapper configMapper;


    @Resource
    private WareHouseLocationMapper locationMapper;

    @Resource
    private RedisUtil redisUtil;

    /**
     * 下发搬运任务
     */
    @Override
    @Transactional(rollbackFor = Exception.class)
    public void distributeTasks() {

        TenantContextHolder.setTenantId(1L);
        Pair<List<RobotInformationDO>, List<RobotTaskDetailDO>> robotAndTaskDetails = getRobotAndTaskDetails();
        List<RobotInformationDO> robots = robotAndTaskDetails.getLeft();
        if (ObjectUtil.isEmpty(robots)) {
            return;
        }

        List<RobotTaskDetailDO> taskDetailDOS = robotAndTaskDetails.getRight();
        if (ObjectUtil.isEmpty(taskDetailDOS)) {
            return;
        }

//        List<RobotTaskDetailDO> cycleTaskDetails =  robotTaskMapper.getCycleTaskDetails();
        //任务下发给机器人
        distributeTasks(robots, taskDetailDOS);
    }

    /**
     * 获取待执行的机器人和任务
     */
    @Override
    @Transactional(rollbackFor = Exception.class)
    public Pair<List<RobotInformationDO>, List<RobotTaskDetailDO>> getRobotAndTaskDetails() {
        TenantContextHolder.setTenantId(1L);
        Pair<List<RobotInformationDO>, List<RobotTaskDetailDO>> pair = Pair.of(new ArrayList<>(), new ArrayList<>());

        CommonConfigDO commonConfigDO = configMapper.selectOne(new LambdaQueryWrapper<CommonConfigDO>()
                .eq(CommonConfigDO::getConfigType, CommandConfigTypeEnum.CHARG_CONFIG.getType()));

        Boolean needDoCharge = needDoCharge(commonConfigDO);
        if (needDoCharge) {
            log.info("所有机器人都需要去执行充电任务");
            return pair;
        }

        List<RobotInformationDO> robots = robotInformationMapper.selectList(new LambdaQueryWrapperX<RobotInformationDO>()
                .in(RobotInformationDO::getRobotStatus, RobotStatusEnum.STAND_BY.getType(), RobotStatusEnum.CHARGE.getType())
                .eq(RobotInformationDO::getRobotTaskModel, RobotTaskModelEnum.NORMAL.getType()));

        if (robots.isEmpty()) {
            log.info("暂无空闲的机器人");
            return pair;
        }

        for (RobotInformationDO robot : robots) {
            String taskStatusKey = RobotTaskChcheConstant.ROBOT_TASK_STATUS + robot.getMacAddress();
            String cargoDetectedKey = RobotTaskChcheConstant.ROBOT_CARGO_DETECTED + robot.getMacAddress();
            Object taskStatus = redisUtil.get(taskStatusKey);
            if (ObjectUtil.isEmpty(taskStatus) || RobotStatusCodeConstant.TASK_STATUS_RUNNING.equals(taskStatus.toString())) {
                robot.setRobotStatus(RobotStatusEnum.DOING.getType());
                continue;
            }

            Object cargoDetected = redisUtil.get(cargoDetectedKey);
            // todo 后续需要判断(新建个任务明细表)，如果上一条任务是仅取货，且正常完成， 则能进行仅放货的任务
            if (ObjectUtil.isEmpty(cargoDetected) || RobotStatusCodeConstant.CARGO_DETECTED_TRUE.equals(cargoDetected)) {
                robot.setRobotStatus(RobotStatusEnum.DOING.getType());
                continue;
            }
            //自动充电的车子，电量到达设定的阀值，也能执行任务
            if (ChargeTypeEnum.AUTOMATIC.getType().equals(robot.getChargeType())
                    && RobotStatusEnum.CHARGE.getType().equals(robot.getRobotStatus())) {
                setRobotStatus(commonConfigDO, robot);
            }
        }

        robots = robots.stream()
                .filter(v -> RobotStatusEnum.STAND_BY.getType().equals(v.getRobotStatus()))
                .collect(Collectors.toList());

        if (robots.isEmpty()) {
            log.info("暂无可用的机器人,可能正在充电,可能机器人有任务");
            return pair;
        }

        log.info("完成查找车子");

        List<RobotTaskDO> taskDOList = robotTaskMapper.selectList(new LambdaQueryWrapper<RobotTaskDO>()
                .in(RobotTaskDO::getTaskStatus, RobotTaskStatusEnum.DOING.getType(), RobotTaskStatusEnum.NEW.getType()));

        //拼接
        List<Long> montageTaskIds =taskDOList.stream()
                .filter(v -> MontageTaskEnum.YES.getType().equals(v.getMontageTask()))
                .map(RobotTaskDO::getId)
                .collect(Collectors.toList());

        //不拼接
        List<Long> singleTaskIds =taskDOList.stream()
                .filter(v -> MontageTaskEnum.NO.getType().equals(v.getMontageTask()))
                .map(RobotTaskDO::getId)
                .collect(Collectors.toList());

        log.info("完成查找主任务");

        if (ObjectUtil.isEmpty(montageTaskIds) && ObjectUtil.isEmpty(singleTaskIds)) {
            log.info("暂无需要处理的主任务");
            return pair;
        }

        List<RobotTaskDetailDO> taskDetailDOS = getTaskDetail(montageTaskIds, singleTaskIds);

        if (taskDetailDOS.isEmpty()) {
            log.info("暂无需要处理的明细任务");
            return pair;
        }
        return ImmutablePair.of(robots, taskDetailDOS);
    }

    /**
     * true 需要去充电
     *
     * @param commonConfigDO
     * @return
     */
    public Boolean needDoCharge(CommonConfigDO commonConfigDO) {
        if (ObjectUtil.isEmpty(commonConfigDO) || ObjectUtil.isEmpty(commonConfigDO.getConfigStr())) {
            return false;
        }

        CommonConfigVO chargeConfig = JSONUtil.toBean(commonConfigDO.getConfigStr(), CommonConfigVO.class);
        LocalDateTime now = LocalDateTime.now();

        if (ZeroOneEnum.ZERO.getType().equals(chargeConfig.getEveryDay())
                && now.isAfter(chargeConfig.getOnceStartTime()) && now.isBefore(chargeConfig.getOnceEndTime())) {
            return true;
        } else if (ZeroOneEnum.ONE.getType().equals(chargeConfig.getEveryDay())
                && now.isBefore(chargeConfig.getScheduleChargeEndTime())
                && now.isAfter(chargeConfig.getScheduleChargeStartTime())) {
            return true;
        }

        return false;
    }

    /**
     * 设置机器人的状态
     *
     * @param commonConfigDO
     * @param robot
     */
    private void setRobotStatus(CommonConfigDO commonConfigDO, RobotInformationDO robot) {
        String pose2dKey = RobotTaskChcheConstant.ROBOT_INFORMATION_POSE_BAT_SOC + robot.getMacAddress();
        String chargeModelKey = RobotTaskChcheConstant.ROBOT_CHARGE_MODEL + robot.getRobotNo();
        Object chargeModelCache = redisUtil.get(chargeModelKey);
        Object poseCache = redisUtil.get(pose2dKey);
        RobotStatusDataPoseDTO dataPoseDTO = JSON.parseObject((String) poseCache, RobotStatusDataPoseDTO.class);
        log.info("充电机器人编号:{} ,信息: {}", robot.getRobotNo(), JSON.toJSONString(dataPoseDTO));

        if (ObjectUtil.isEmpty(commonConfigDO) || ObjectUtil.isEmpty(poseCache) || ObjectUtil.isEmpty(dataPoseDTO.getBat_soc())) {
            return;
        }

        CommonConfigVO chargeConfig = JSONUtil.toBean(commonConfigDO.getConfigStr(), CommonConfigVO.class);

        //车子剩余电量
        BigDecimal robotRemainingElectricity = new BigDecimal(dataPoseDTO.getBat_soc());
        //设置离开的电量
        BigDecimal robotEndElectricity = null;

        if (ObjectUtil.isNotEmpty(robot.getAutoCharge()) && ObjectUtil.isNotEmpty(chargeModelCache)
                && !ChargeModelEnum.FULL.getType().equals((Integer) chargeModelCache)) {
            robotEndElectricity = new BigDecimal(String.valueOf(robot.getAutoCharge()));
        } else if (ObjectUtil.isNotEmpty(chargeModelCache) && ChargeModelEnum.FULL.getType().equals((Integer) chargeModelCache)) {
            robotEndElectricity = new BigDecimal(fullElectricity);
        } else if (ObjectUtil.isNotEmpty(chargeModelCache) && ChargeModelEnum.CHANCE.getType().equals((Integer) chargeModelCache)) {
            robotEndElectricity = new BigDecimal(String.valueOf(chargeConfig.getChanceChargeEnd()));
        } else {
            robotEndElectricity = new BigDecimal(String.valueOf(chargeConfig.getEndAutoCharge()));
        }

        if (robotRemainingElectricity.compareTo(robotEndElectricity) >= 0) {
            robot.setRobotStatus(RobotStatusEnum.STAND_BY.getType());
        }
    }

    /**
     * 获取明细
     *
     * @param montageTaskIds 拼接
     * @param singleTaskIds 不拼接
     * @return
     */
    private List<RobotTaskDetailDO> getTaskDetail(List<Long> montageTaskIds, List<Long> singleTaskIds) {
        if (ObjectUtil.isNotEmpty(montageTaskIds) && ObjectUtil.isEmpty(singleTaskIds)) {
            return robotTaskDetailMapper.getMontageUnDoTask(montageTaskIds);
        } else if (ObjectUtil.isEmpty(montageTaskIds) && ObjectUtil.isNotEmpty(singleTaskIds)) {
            return robotTaskDetailMapper.getSingleUnDoTask(singleTaskIds);
        }
        return robotTaskDetailMapper.getUnDoTaskDetail(montageTaskIds, singleTaskIds);
    }

    /**
     * 下发任务给机器人
     *
     * @param robots
     * @param taskDetailDOS
     */
    @Transactional(rollbackFor = Exception.class)
    public void distributeTasks(List<RobotInformationDO> robots, List<RobotTaskDetailDO> taskDetailDOS) {
        List<RobotAcceptTaskDTO> robotTaskDOS = new ArrayList<>();
        //需要更新的任务明细id
        List<RobotTaskDetailDO> updateTaskDetails = new ArrayList<>();
        //需要更新的任务id
        Set<Long> taskIds = new HashSet<>();
        //需要更新的机器人编号
        Set<String> robotNos = new HashSet<>();
        //当前需要移动的库位id
        Set<Long> doingLocationIds = new HashSet<>();
        for (RobotTaskDetailDO taskDetailDO : taskDetailDOS) {
            if (ObjectUtil.isEmpty(robots)) {
                break;
            }
// todo 后续建个移库记录表， 记录 库位A移动到库位B
            RobotTaskTypeEnum robotTaskType = RobotTaskTypeEnum.getRobotTaskType(taskDetailDO.getTaskType());
            switch (robotTaskType) {
                case TAKE_RELEASE:
                    doTakeReleaseDistribute(taskDetailDO, robots, robotTaskDOS, taskIds, updateTaskDetails, robotNos, doingLocationIds);
                    break;
                /*case PARK:
                    doParkDistribute(taskDetailDO, robots,robotTaskDOS);
                    break;
                case CHARGE:
                    doChargeDistribute(taskDetailDO,robots,robotTaskDOS);
                    break;
                case MOVE:
                    doMoveDistribute(taskDetailDO, robots,robotTaskDOS);
                    break;
                case TAKE:
                    doTakeDistribute(taskDetailDO, robots,robotTaskDOS);
                    break;
                case RELEASE:
                    doReleaseDistribute(taskDetailDO, robots,robotTaskDOS);
                    break;
                case SCAN:
                    doScanDistribute(taskDetailDO,robots,robotTaskDOS);
                    break;
                case DETECTING_TRAYS:
                    doDetectingTraysDistribute(taskDetailDO,robots,robotTaskDOS);
                    break;*/
                default:
                    log.error("下发任务的类型不存在");
            }
        }

        if (ObjectUtil.isNotEmpty(robotTaskDOS)) {
            log.info("任务下发给车机 :{}", JSON.toJSONString(robotTaskDOS));
            robotTaskApi.sendTaskToRobot(robotTaskDOS);
        }

        if (ObjectUtil.isNotEmpty(taskIds)) {
            List<RobotTaskDO> taskDOS = robotTaskMapper.selectBatchIds(taskIds);
            for (RobotTaskDO taskDO : taskDOS) {
                if (RobotTaskStatusEnum.NEW.getType().equals(taskDO.getTaskStatus())) {
                    taskDO.setTaskStatus(RobotTaskStatusEnum.DOING.getType());
                    taskDO.setStartTime(LocalDateTime.now());
                }
            }
            robotTaskMapper.updateBatch(taskDOS);
        }
        if (ObjectUtil.isNotEmpty(updateTaskDetails)) {
            robotTaskDetailMapper.updateBatch(updateTaskDetails);
        }
    }

    /**
     * 检测托盘类型
     *
     * @param taskDetailDO
     * @param robots
     * @param robotTaskDOS
     */
    @Transactional(rollbackFor = Exception.class)
    public void doDetectingTraysDistribute(RobotTaskDetailDO taskDetailDO, List<RobotInformationDO> robots,
                                           List<RobotAcceptTaskDTO> robotTaskDOS) {

    }

    /**
     * 扫描码
     *
     * @param taskDetailDO
     * @param robots
     * @param robotTaskDOS
     */
    @Transactional(rollbackFor = Exception.class)
    public void doScanDistribute(RobotTaskDetailDO taskDetailDO, List<RobotInformationDO> robots,
                                 List<RobotAcceptTaskDTO> robotTaskDOS) {

    }

    /**
     * 仅放货
     *
     * @param taskDetailDO
     * @param robots
     * @param robotTaskDOS
     */
    @Transactional(rollbackFor = Exception.class)
    public void doReleaseDistribute(RobotTaskDetailDO taskDetailDO, List<RobotInformationDO> robots,
                                    List<RobotAcceptTaskDTO> robotTaskDOS) {

    }

    /**
     * 取放货
     *
     * @param taskDetailDO
     * @param robots
     * @param robotTaskDOS
     */
    @Transactional(rollbackFor = Exception.class)
    public void doTakeDistribute(RobotTaskDetailDO taskDetailDO, List<RobotInformationDO> robots,
                                 List<RobotAcceptTaskDTO> robotTaskDOS) {

    }

    /**
     * 移动
     *
     * @param taskDetailDO
     * @param robots
     * @param robotTaskDOS
     */
    @Transactional(rollbackFor = Exception.class)
    public void doMoveDistribute(RobotTaskDetailDO taskDetailDO, List<RobotInformationDO> robots,
                                 List<RobotAcceptTaskDTO> robotTaskDOS) {
        String macAddress = "";
        String robotNo = taskDetailDO.getRobotNo();
        if (ObjectUtil.isNotEmpty(taskDetailDO.getRobotNo())) {
            macAddress = robots.stream()
                    .filter(v -> v.getRobotNo().equals(taskDetailDO.getRobotNo()))
                    .map(RobotInformationDO::getMacAddress)
                    .findFirst()
                    .orElse("");
            if (ObjectUtil.isEmpty(macAddress)) {
                return;
            }
        } else {
            macAddress = robots.get(0).getMacAddress();
            robotNo = robots.get(0).getRobotNo();
        }

        RobotAcceptTaskDTO robotTaskDO = new RobotAcceptTaskDTO();
        robotTaskDO.setOrder_id(taskDetailDO.getId().toString());
        robotTaskDO.setOrder_type("出库");//未定
        robotTaskDO.setTopic(RobotTopicConstant.ROBOT_TASK_TOPIC + macAddress);

        //1
        RobotAcceptTaskData taskOne = new RobotAcceptTaskData();
        taskOne.setSerial("1");
        taskOne.setCommand_id(taskDetailDO.getId().toString());
        taskOne.setCommand_type(CommandTypeEnum.MOVE_POSES.getType());

        List<Pose2ds> pose2ds = new ArrayList<>();
        Pose2ds pose2d1 = new Pose2ds();
        pose2d1.setX(-1.305);
        pose2d1.setY(-0.553);
        pose2d1.setYaw(0.091);
        Pose2ds pose2d2 = new Pose2ds();
        pose2d2.setX(-0.407);
        pose2d2.setY(-0.566);
        pose2d2.setYaw(0.117);
        pose2ds.add(pose2d1);
        pose2ds.add(pose2d2);

//        taskOne.setPose2ds(pose2ds);

        List<RobotAcceptTaskData> data = new ArrayList<>();
        data.add(taskOne);

        robotTaskDO.setData(data);
        robotTaskDOS.add(robotTaskDO);

        updateTaskStatus(robotNo, taskDetailDO);
    }

    /**
     * 充电
     *
     * @param taskDetailDO
     * @param robots
     * @param robotTaskDOS
     */
    @Transactional(rollbackFor = Exception.class)
    public void doChargeDistribute(RobotTaskDetailDO taskDetailDO, List<RobotInformationDO> robots,
                                   List<RobotAcceptTaskDTO> robotTaskDOS) {

    }

    /**
     * 停车
     *
     * @param taskDetailDO
     * @param robots
     * @param robotTaskDOS
     */
    @Transactional(rollbackFor = Exception.class)
    public void doParkDistribute(RobotTaskDetailDO taskDetailDO, List<RobotInformationDO> robots,
                                 List<RobotAcceptTaskDTO> robotTaskDOS) {
        String macAddress = "";
        String robotNo = taskDetailDO.getRobotNo();
        if (ObjectUtil.isNotEmpty(taskDetailDO.getRobotNo())) {
            macAddress = robots.stream()
                    .filter(v -> v.getRobotNo().equals(taskDetailDO.getRobotNo()))
                    .map(RobotInformationDO::getMacAddress)
                    .findFirst()
                    .orElse("");
            if (ObjectUtil.isEmpty(macAddress)) {
                return;
            }
        }

        RobotAcceptTaskDTO robotTaskDO = new RobotAcceptTaskDTO();
        robotTaskDO.setOrder_id(taskDetailDO.getId().toString());
        robotTaskDO.setOrder_type("出库");//未定
        robotTaskDO.setTopic(RobotTopicConstant.ROBOT_TASK_TOPIC + macAddress);

        RobotAcceptTaskData taskOne = new RobotAcceptTaskData();
        taskOne.setSerial("1");
        taskOne.setCommand_id(taskDetailDO.getId().toString());
        taskOne.setCommand_type(CommandTypeEnum.STOP.getType());

        List<RobotAcceptTaskData> data = new ArrayList<>();
        data.add(taskOne);

        robotTaskDO.setData(data);
        robotTaskDOS.add(robotTaskDO);

        final String mac = macAddress;
        robots.removeIf(v -> v.getMacAddress().equals(mac));

        updateTaskStatus(robotNo, taskDetailDO);

    }

    /**
     * 取放货
     *
     * @param taskDetailDO
     * @param robots
     * @param robotTaskDOS
     */
    @Transactional(rollbackFor = Exception.class)
    public void doTakeReleaseDistribute(RobotTaskDetailDO taskDetailDO, List<RobotInformationDO> robots,
                                        List<RobotAcceptTaskDTO> robotTaskDOS, Set<Long> taskIdSet,
                                        List<RobotTaskDetailDO> updateTaskDetail, Set<String> robotNos
            , Set<Long> doingLocationIds) {
        WareHouseLocationDO query = WareHouseLocationDO.builder().id(taskDetailDO.getFromLocationId()).build();
        WareHouseLocationDO fromLocation = locationMapper.queryAllByLimit(query);

        WareHouseLocationDO toQuery = WareHouseLocationDO.builder().id(taskDetailDO.getToLocationId()).build();
        WareHouseLocationDO toLocation = locationMapper.queryAllByLimit(toQuery);

        Pair<String, String> pair = getMadAddressRobotNo(taskDetailDO, robots, fromLocation, toLocation);
        String macAddress = pair.getLeft();
        String robotNo = pair.getRight();

        if (ObjectUtil.isEmpty(macAddress) || ObjectUtil.isEmpty(robotNo)) {
            log.info("mac地址为空 :{}, 机器人编号 ：{}", macAddress, robotNo);
            return;
        }

        // 取货/放货 校验有没有其他库位挡住
        Boolean fromCanMove = checkCanMove(fromLocation, doingLocationIds);
        if (!fromCanMove) {
            return;
        }
        Boolean toCanMove = checkCanMove(toLocation, doingLocationIds);
        if (!toCanMove) {
            return;
        }

        RobotAcceptTaskDTO robotTaskDO = new RobotAcceptTaskDTO();
        robotTaskDO.setOrder_id(taskDetailDO.getId().toString());
        robotTaskDO.setOrder_type("出库");//未定
        robotTaskDO.setTopic(RobotTopicConstant.ROBOT_TASK_TOPIC + macAddress);

        //1
        RobotAcceptTaskData taskOne = new RobotAcceptTaskData();
        taskOne.setSerial("1");
        taskOne.setCommand_id(taskDetailDO.getId().toString());
        taskOne.setCommand_type(CommandTypeEnum.MOVE_POSES.getType());

        List<Pose2ds> pose2ds = new ArrayList<>();
        Pose2ds pose2 = positionMapItemMapper.selectByLocationId(taskDetailDO.getFromLocationId());
        pose2ds.add(pose2);
//        taskOne.setPose2ds(pose2ds);

        //2
        RobotAcceptTaskData taskTwo = new RobotAcceptTaskData();
        taskTwo.setSerial("1");
        taskTwo.setCommand_id(taskDetailDO.getId().toString());
        taskTwo.setCommand_type(CommandTypeEnum.FORK.getType());
//        taskTwo.setTarget_height(initHeight);

        //3
        RobotAcceptTaskData taskThree = new RobotAcceptTaskData();
        taskThree.setSerial("2");
        taskThree.setCommand_id(taskDetailDO.getId().toString());
        taskThree.setCommand_type(CommandTypeEnum.FORK.getType());
//        taskThree.setTarget_height(scanHeight);

        //4
        RobotAcceptTaskData taskFour = new RobotAcceptTaskData();
        taskFour.setSerial("3");
        taskFour.setCommand_id(taskDetailDO.getId().toString());
        taskFour.setCommand_type(CommandTypeEnum.WAIT.getType());
//        taskFour.setParm(parm);

        //5
        RobotAcceptTaskData taskFive = new RobotAcceptTaskData();
        taskFive.setSerial("4");
        taskFive.setCommand_id(taskDetailDO.getId().toString());
        taskFive.setCommand_type(CommandTypeEnum.GET_PALLET_TOPIC.getType());

        //6
        RobotAcceptTaskData taskSix = new RobotAcceptTaskData();
        taskSix.setSerial("5");
        taskSix.setCommand_id(taskDetailDO.getId().toString());
        taskSix.setCommand_type(CommandTypeEnum.FORK.getType());


        double fromLocationTotalHeight = fromLocation.getLocationTotalHeight().doubleValue();
//        taskSix.setTarget_height(fromLocationTotalHeight);

        //7
        RobotAcceptTaskData taskSeven = new RobotAcceptTaskData();
        taskSeven.setSerial("6");
        taskSeven.setCommand_id(taskDetailDO.getId().toString());
        taskSeven.setCommand_type(CommandTypeEnum.MOVE_TO_PALLET_POSE.getType());


        //8
        RobotAcceptTaskData taskEigth = new RobotAcceptTaskData();
        taskEigth.setSerial("7");
        taskEigth.setCommand_id(taskDetailDO.getId().toString());
        taskEigth.setCommand_type(CommandTypeEnum.FORK.getType());
        BigDecimal b1 = new BigDecimal(Double.toString(fromLocationTotalHeight));
        BigDecimal b2 = new BigDecimal(Double.toString(liftHeight));
//        taskEigth.setTarget_height(b1.add(b2).doubleValue());


        //8-2
        RobotAcceptTaskData taskEigth2 = new RobotAcceptTaskData();
        taskEigth2.setSerial("8");
        taskEigth2.setCommand_id(taskDetailDO.getId().toString());
        taskEigth2.setCommand_type(CommandTypeEnum.MOVE_POSE.getType());
        Pose2ds poseWait1 = positionMapItemMapper.selectByLocationId(100l);
//        taskEigth2.setPose2d(poseWait1);

        //9
        RobotAcceptTaskData taskNigth = new RobotAcceptTaskData();
        taskNigth.setSerial("9");
        taskNigth.setCommand_id(taskDetailDO.getId().toString());
        taskNigth.setCommand_type(CommandTypeEnum.MOVE_POSES.getType());
        List<Pose2ds> pose2dsNight = new ArrayList<>();

       /* WareHouseLocationDO ware = WareHouseLocationDO.builder()
                .laneId(-2l)
                .build();
        List<WareHouseLocationDO> releaseStockList =
                locationMapper.selectLocations(ware, new ArrayList<>(), new HashSet<>());
        for (WareHouseLocationDO wareHouseLocationDO : releaseStockList) {
            Pose2ds pose = positionMapItemMapper.selectByLocationId(wareHouseLocationDO.getId());
            pose2dsNight.add(pose);
        }*/


//        taskNigth.setPose2ds(pose2dsNight);

        //10
        RobotAcceptTaskData taskTen = new RobotAcceptTaskData();
        taskTen.setSerial("9");
        taskTen.setCommand_id(taskDetailDO.getId().toString());
        taskTen.setCommand_type(CommandTypeEnum.FORK.getType());
//        taskTen.setTarget_height(moveHeight);

        //11
        RobotAcceptTaskData taskEleven = new RobotAcceptTaskData();
        taskEleven.setSerial("10");
        taskEleven.setCommand_id(taskDetailDO.getId().toString());
        taskEleven.setCommand_type(CommandTypeEnum.FORK.getType());

        double toLocationTotalHeight = toLocation.getLocationTotalHeight().doubleValue();
        BigDecimal b3 = new BigDecimal(Double.toString(toLocationTotalHeight));
        BigDecimal b4 = new BigDecimal(Double.toString(liftHeight));
//        taskEleven.setTarget_height(b3.add(b4).doubleValue());

        //12
        RobotAcceptTaskData taskTwelve = new RobotAcceptTaskData();
        taskTwelve.setSerial("11");
        taskTwelve.setCommand_id(taskDetailDO.getId().toString());
        taskTwelve.setCommand_type(CommandTypeEnum.MOVE_POSE.getType());
        Pose2ds poseEnd = positionMapItemMapper.selectByLocationId(taskDetailDO.getToLocationId());
//        taskTwelve.setPose2d(poseEnd);

        //13
        RobotAcceptTaskData taskThirteen = new RobotAcceptTaskData();
        taskThirteen.setSerial("12");
        taskThirteen.setCommand_id(taskDetailDO.getId().toString());
        taskThirteen.setCommand_type(CommandTypeEnum.FORK.getType());
//        taskThirteen.setTarget_height(toLocationTotalHeight);

        //14
        RobotAcceptTaskData taskFourteen = new RobotAcceptTaskData();
        taskFourteen.setSerial("13");
        taskFourteen.setCommand_id(taskDetailDO.getId().toString());
        taskFourteen.setCommand_type(CommandTypeEnum.MOVE_POSE.getType());
        Pose2ds poseWait = positionMapItemMapper.selectByLocationId(99l);
//        taskFourteen.setPose2d(poseWait);
        /*
        Pose2ds poseWait = new Pose2ds();
        poseWait.setX(3.046);
        poseWait.setY(3.219);
        poseWait.setYaw(-3.141);
        taskFourteen.setPose2d(poseWait);*/

        //15
        RobotAcceptTaskData taskFifteen = new RobotAcceptTaskData();
        taskFifteen.setSerial("14");
        taskFifteen.setCommand_id(taskDetailDO.getId().toString());
        taskFifteen.setCommand_type(CommandTypeEnum.FORK.getType());
//        taskFifteen.setTarget_height(initHeight);

        List<RobotAcceptTaskData> data = new ArrayList<>();
        data.add(taskOne);
        data.add(taskTwo);
        data.add(taskThree);
        data.add(taskFour);
        data.add(taskFive);
        data.add(taskSix);
        data.add(taskSeven);
        data.add(taskEigth);
        data.add(taskEigth2);
        data.add(taskNigth);
        data.add(taskTen);
        data.add(taskEleven);
        data.add(taskTwelve);
        data.add(taskThirteen);
        data.add(taskFourteen);
        data.add(taskFifteen);


        robotTaskDO.setData(data);

        robotTaskDOS.add(robotTaskDO);

        final String mac = macAddress;
        robots.removeIf(v -> v.getMacAddress().equals(mac));

        taskDetailDO.setRobotNo(robotNo);
        taskDetailDO.setStartTime(LocalDateTime.now());
        taskDetailDO.setTaskStatus(RobotTaskDetailStatusEnum.DOING.getType());
        updateTaskDetail.add(taskDetailDO);

        robotNos.add(robotNo);
        taskIdSet.add(taskDetailDO.getRobotTaskId());
    }

    /**
     * 校验当前库位能否取放
     *
     * @param fromLocation
     * @return false : 有货，不能移动  ；  true: 没货， 能移动
     */
    private Boolean checkCanMove(WareHouseLocationDO fromLocation, Set<Long> locationIds) {
        Long laneId = fromLocation.getLaneId();
        if (ObjectUtil.isEmpty(laneId)) {
            Set<Long> doingLocationIds = new HashSet<>();
            doingLocationIds.add(fromLocation.getId());
            List<WareHouseLocationDO> locationDOS = locationMapper.selectNeedMoveLocation(doingLocationIds);
            locationDOS = locationDOS.stream()
                    .filter(v -> !locationIds.contains(v.getId()))
                    .collect(Collectors.toList());
            if (ObjectUtil.isNotEmpty(locationDOS)) {
                // todo 后续记录到异常表
                log.info("普通库位--库位上有其他货物, 库位号:{} ,无法进行移动 :{}", fromLocation.getLocationNo(), locationDOS);
                return false;
            }
        } else {
            List<WareHouseLocationDO> locations = new ArrayList<>();
            WareHouseLocationDO location = WareHouseLocationDO.builder().locationNumber(fromLocation.getLocationNumber())
                    .locationUseStatus(LocationUseStatusEnum.YES.getType())
                    .laneId(fromLocation.getLaneId()).build();
            locations.add(location);

            List<WareHouseLocationDO> wareHouseLocationDOS = locationMapper.selectLocationByList(locations);
            wareHouseLocationDOS = wareHouseLocationDOS.stream()
                    .filter(v -> !locationIds.contains(v.getId()))
                    .collect(Collectors.toList());
            if (ObjectUtil.isNotEmpty(wareHouseLocationDOS)) {
                // todo 后续记录到异常表
                log.info("线库-当前库位上有其他货物, 库位号:{}，无法进行移动 :{}", fromLocation.getLocationNo(), wareHouseLocationDOS);
                return false;
            }
        }

        locationIds.add(fromLocation.getId());
        return true;
    }

    public Pair<String, String> getMadAddressRobotNo(RobotTaskDetailDO taskDetailDO, List<RobotInformationDO> robots,
                                                     WareHouseLocationDO fromLocation, WareHouseLocationDO toLocation) {
        String macAddress = "";
        String robotNo = taskDetailDO.getRobotNo();
        if (ObjectUtil.isNotEmpty(robotNo)) {
            macAddress = robots.stream()
                    .filter(v -> v.getRobotNo().equals(taskDetailDO.getRobotNo()))
                    .map(RobotInformationDO::getMacAddress)
                    .findFirst()
                    .orElse("");
        } else {
            RobotInformationDO robotInformationDO = robots.stream()
                    .filter(v ->
                            ObjectUtil.isNotEmpty(v.getMacAddress())
                                    && v.getFloorAreaJson().contains(fromLocation.getMapId())
                                    && v.getFloorAreaJson().contains(toLocation.getMapId()))
                    .findFirst()
                    .orElse(new RobotInformationDO());
            macAddress = robotInformationDO.getMacAddress();
            robotNo = robotInformationDO.getRobotNo();
        }
        Pair<String, String> pair = new ImmutablePair<>(macAddress, robotNo);
        return pair;
    }

    @Transactional(rollbackFor = Exception.class)
    public void updateTaskStatus(String robotNo, RobotTaskDetailDO taskDetailDO) {

        RobotTaskDO robotTaskDO = new RobotTaskDO();
        robotTaskDO.setId(taskDetailDO.getRobotTaskId());
        robotTaskDO.setStartTime(LocalDateTime.now());
        robotTaskDO.setTaskStatus(RobotTaskDetailStatusEnum.DOING.getType());
        robotTaskMapper.updateRobot(robotTaskDO);

        RobotTaskDetailDO detailDO = new RobotTaskDetailDO();
        detailDO.setId(taskDetailDO.getId());
        detailDO.setStartTime(LocalDateTime.now());
        detailDO.setTaskStatus(RobotTaskDetailStatusEnum.DOING.getType());
        robotTaskDetailMapper.updateRobotDetailById(detailDO);

        robotInformationMapper.updateRobotStatus(robotNo, RobotStatusEnum.DOING.getType());
    }


}
