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

import cn.hutool.core.util.IdUtil;
import cn.hutool.core.util.ObjectUtil;
import cn.iocoder.yudao.framework.common.util.date.DateUtils;
import cn.iocoder.yudao.framework.common.util.object.BeanUtils;
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.common.CommonApi;
import cn.iocoder.yudao.module.mqtt.api.path.task.TaskLimitationAreaDTO;
import cn.iocoder.yudao.module.mqtt.api.path.task.TaskRobotNoLimittationAreaDTO;
import cn.iocoder.yudao.module.mqtt.api.path.task.TaskToPathPlanningDTO;
import cn.iocoder.yudao.module.system.config.mqtt.util.MqttUtils;
import cn.iocoder.yudao.module.system.config.poperties.ZnConfigConstant;
import cn.iocoder.yudao.module.system.constant.path.PathPlanningChcheConstant;
import cn.iocoder.yudao.module.system.constant.path.PathPlanningTopicConstant;
import cn.iocoder.yudao.module.system.constant.robot.RobotStatusCodeConstant;
import cn.iocoder.yudao.module.system.constant.robot.RobotTaskChcheConstant;
import cn.iocoder.yudao.module.system.controller.admin.robot.vo.RobotTaskDetailAddVO;
import cn.iocoder.yudao.module.system.dal.dataobject.houselocation.WareHouseLocationDO;
import cn.iocoder.yudao.module.system.dal.dataobject.houselocation.WareHouseTakePointDO;
import cn.iocoder.yudao.module.system.dal.dataobject.positionmap.PositionMapDO;
import cn.iocoder.yudao.module.system.dal.dataobject.positionmap.PositionMapItemDO;
import cn.iocoder.yudao.module.system.dal.dataobject.robot.RobotChargeLogDO;
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.houselocation.WareHouseLocationMapper;
import cn.iocoder.yudao.module.system.dal.mysql.positionmap.PositionMapItemMapper;
import cn.iocoder.yudao.module.system.dal.mysql.positionmap.PositionMapMapper;
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.item.PositionMapItemEnum;
import cn.iocoder.yudao.module.system.enums.item.UseStatusEnum;
import cn.iocoder.yudao.module.system.enums.path.PathTaskTypeEnum;
import cn.iocoder.yudao.module.system.enums.path.PathTaskTypeToRobotEnum;
import cn.iocoder.yudao.module.system.enums.redis.RobotCacheLockEnum;
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.service.houselocation.WareHouseTakePointService;
import cn.iocoder.yudao.module.system.service.robot.RobotInformationService;
import cn.iocoder.yudao.module.system.service.robot.RobotTaskService;
import cn.iocoder.yudao.module.system.service.robot.job.DistributeTasksService;
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.ImmutablePair;
import org.apache.commons.lang3.tuple.Pair;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.beans.factory.annotation.Value;
import org.springframework.context.annotation.Lazy;
import org.springframework.stereotype.Service;
import org.springframework.transaction.annotation.Transactional;
import org.springframework.validation.annotation.Validated;

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

import static org.codehaus.groovy.runtime.DefaultGroovyMethods.collect;

@Service
@Validated
@Slf4j
public class RobotPathPlanningServiceImpl implements RobotPathPlanningService {

    @Autowired
    private RobotTaskDetailMapper robotTaskDetailMapper;

    @Resource
    private PositionMapItemMapper positionMapItemMapper;

    @Resource
    private WareHouseLocationMapper locationMapper;

    @Resource
    private RedisUtil redisUtil;

    @Autowired
    private DistributeTasksService distributeTasksService;

    @Resource
    private PositionMapMapper positionMapMapper;

    @Autowired
    private ZnConfigConstant znConfigConstant;

    @Autowired
    private RobotTaskMapper robotTaskMapper;

    /*@Resource
    private CommonApi commonApi;*/

    @Autowired
    private MqttUtils mqttUtils;

    @Resource
    @Lazy
    private RobotTaskService taskService;

    @Resource
    @Lazy
    private RobotInformationService informationService;

    @Resource
    private WareHouseTakePointService houseTakePointService;

    /**
     * 下发任务给PP
     */
    @Override
    @Transactional(rollbackFor = Exception.class)
    public void sendTaskToPP() {
        TenantContextHolder.setTenantId(1L);

        Pair<List<RobotInformationDO>, List<RobotTaskDetailDO>> robotAndTaskDetails =
                distributeTasksService.getRobotAndTaskDetails();

        List<RobotInformationDO> robots = robotAndTaskDetails.getLeft();
        if (ObjectUtil.isEmpty(robots)) {
            return;
        }

        List<RobotTaskDetailDO> taskDetailDOS = robotAndTaskDetails.getRight();
        if (ObjectUtil.isEmpty(taskDetailDOS)) {
            log.info("--不存在需要执行的任务--派车辆去等待点");
            moveRobotToWait(robots);
            return;
        }

        //停车不锁车
        taskDetailDOS = doPark(taskDetailDOS);
        if (ObjectUtil.isEmpty(taskDetailDOS)) {
            log.info("--不存在需要执行的任务---");
            return;
        }

        log.info("-------可以分配任务的车辆编号------ ,:{}", JSON.toJSONString(robots.stream().map(RobotInformationDO::getRobotNo).collect(Collectors.toList())));
        log.info("-------需要执行的任务id------ ,:{}", JSON.toJSONString(taskDetailDOS.stream().map(RobotTaskDetailDO::getId).collect(Collectors.toList())));
        distributeTasksToPP(robots, taskDetailDOS);

    }

    /**
     * 处理停车不锁车的任务
     *
     * @param taskDetailDOS
     * @return
     */
    private List<RobotTaskDetailDO> doPark(List<RobotTaskDetailDO> taskDetailDOS) {
        List<RobotTaskDetailDO> parkList = new ArrayList<>();
        List<RobotTaskDetailDO> detailList = new ArrayList<>();
        for (RobotTaskDetailDO v : taskDetailDOS) {
            if (RobotTaskTypeEnum.PARK.getType().equals(v.getTaskType())
                    && ZeroOneEnum.ZERO.getType().equals(v.getNeedLock())) {
                parkList.add(v);
            } else if (!RobotTaskTypeEnum.CHARGE.getType().equals(v.getTaskType())) {
                detailList.add(v);
            }
        }

        if (ObjectUtil.isNotEmpty(parkList)) {
            List<Long> detailIds = parkList.stream().map(RobotTaskDetailDO::getId).collect(Collectors.toList());
            List<RobotTaskDetailDO> taskDetails = robotTaskDetailMapper.selectBatchIds(detailIds);
            List<Long> taskIds = taskDetails.stream().map(RobotTaskDetailDO::getRobotTaskId).collect(Collectors.toList());
            robotTaskDetailMapper.updateDoneByIds(detailIds);
            robotTaskMapper.updateDoneByIds(taskIds);
        }

        return detailList;
    }

    /**
     * 派车辆去等待点
     *
     * @param robots
     */
    @Override
    public void moveRobotToWait(List<RobotInformationDO> robots) {

        List<RobotTaskDetailDO> detailDOS = robotTaskDetailMapper.selectAutoCreateCycleTask();

        if (ObjectUtil.isNotEmpty(detailDOS)) {
            log.info("---存在需要自动创建循环的任务，不移动到等待点--");
            return;
        }

        List<PositionMapItemDO> positionMapItems = positionMapItemMapper.selectList(new LambdaQueryWrapperX<PositionMapItemDO>()
                .eq(PositionMapItemDO::getType, PositionMapItemEnum.STOP.getType()));

        if (ObjectUtil.isEmpty(positionMapItems)) {
            log.info("------没有设置停车点，无法派发空闲的车辆去停车-----");
            return;
        }

        Map<Long, Long> sortMap = positionMapItems.stream().collect(Collectors.toMap(PositionMapItemDO::getId, PositionMapItemDO::getSortNum));

        List<String> inStopPointRobotNos = new ArrayList<>();
        for (PositionMapItemDO positionMapItem : positionMapItems) {
            if (UseStatusEnum.USEING.getType().equals(positionMapItem.getUseStatus()) && ObjectUtil.isNotEmpty(positionMapItem.getRobotNo())) {
                inStopPointRobotNos.add(positionMapItem.getRobotNo());
            }
        }

        List<String> robotNos = new ArrayList<>();
        Map<String, Set<Long>> robotFloorAreaJsonMap = new HashMap<>();
        for (RobotInformationDO robot : robots) {
            if (!RobotStatusEnum.STAND_BY.getType().equals(robot.getRobotStatus())) {
                continue;
            }

            if (ObjectUtil.isEmpty(robot.getFloorAreaJson())) {
                log.info("车辆没有能行走的区域 :{}", robot.getRobotNo());
                continue;
            }

            if (ObjectUtil.isNotEmpty(inStopPointRobotNos) && inStopPointRobotNos.contains(robot.getRobotNo())) {
                log.info("车辆已经在停车点了 :{}", robot.getRobotNo());
                continue;
            }

            robotNos.add(robot.getRobotNo());
            robotFloorAreaJsonMap.put(robot.getRobotNo(), robot.getFloorAreaJson());
        }

        if (ObjectUtil.isEmpty(robotNos)) {
            log.info("------没有空闲的机器人可以去等待点-----");
            return;
        }

        List<PositionMapItemDO> existMapItems = positionMapItems.stream()
                .filter(v -> ObjectUtil.isNotEmpty(v.getRobotNo())
                        && robotNos.contains(v.getRobotNo())
                        && robotFloorAreaJsonMap.get(v.getRobotNo()).contains(v.getPositionMapId()))
                .collect(Collectors.toList());

        //车辆直接分配的停车点集合
        List<PositionMapItemDO> robotMapItems = new ArrayList<>();
        if (ObjectUtil.isNotEmpty(existMapItems)) {
            for (PositionMapItemDO positionMapItem : existMapItems) {
                if (ObjectUtil.isEmpty(robotNos)) {
                    break;
                }
                robotNos.removeIf(v -> v.equals(positionMapItem.getRobotNo()));
                positionMapItem.setUseStatus(UseStatusEnum.PRE_OCCUPANCY.getType());
                robotMapItems.add(positionMapItem);
                positionMapItems.removeIf(v -> v.getId().equals(positionMapItem.getId()));
            }
        }

        if (ObjectUtil.isNotEmpty(robotNos)) {
            List<PositionMapItemDO> emptyMapItems = positionMapItems.stream()
                    .filter(v -> UseStatusEnum.FREE.getType().equals(v.getUseStatus()))
                    .collect(Collectors.toList());
            if (ObjectUtil.isNotEmpty(emptyMapItems)) {
                for (PositionMapItemDO positionMapItem : emptyMapItems) {
                    if (ObjectUtil.isEmpty(robotNos)) {
                        break;
                    }
                    String robotNo = robotNos.get(0);
                    if (!robotFloorAreaJsonMap.get(robotNo).contains(positionMapItem.getPositionMapId())) {
                        log.info("车辆不能在此停车点 :{}, 车辆编号 :{}", positionMapItem.getSortNum(), robotNo);
                        continue;
                    }
                    positionMapItem.setRobotNo(robotNo);
                    robotNos.removeIf(v -> v.equals(positionMapItem.getRobotNo()));
                    positionMapItem.setUseStatus(UseStatusEnum.PRE_OCCUPANCY.getType());
                    robotMapItems.add(positionMapItem);
                }
            }
        }

        if (ObjectUtil.isEmpty(robotMapItems)) {
            log.info("车辆找不到合适的停车点,或者车辆都在停车点了,或者停车点不够用");
            return;
        }

        //机器人不能行走的区域
        List<TaskRobotNoLimittationAreaDTO> robotNoLimitationArea = getRobotNoLimitationArea(robots);
        Map<String, TaskRobotNoLimittationAreaDTO> robotNoLimittationAreaDTOMap =
                robotNoLimitationArea.stream().collect(Collectors.toMap(v -> v.getRobotNo(), Function.identity()));

        List<TaskToPathPlanningDTO> pathPlanningList = new ArrayList<>();
        List<RobotTaskDetailAddVO> taskDetails = new ArrayList<>();
        List<RobotTaskDO> tasks = new ArrayList<>();
        for (PositionMapItemDO v : robotMapItems) {

            taskService.chargeDone(v.getRobotNo());

            RobotTaskDO task = new RobotTaskDO();
            String incrementByKey = redisUtil.getIncrementByKey(RobotCacheLockEnum.TASK_NO.getKey());
            task.setTaskNo(znConfigConstant.getMoveNo() + DateUtils.getYearMonthDay() + incrementByKey);
            task.setId(IdUtil.getSnowflakeNextId());
            task.setCycleNumber(0L);
            task.setRemainingCycleNumber(0L);
            task.setStartTime(LocalDateTime.now());
            tasks.add(task);

            RobotTaskDetailAddVO detailAddVO = new RobotTaskDetailAddVO();
            detailAddVO.setId(IdUtil.getSnowflakeNextId());
            detailAddVO.setRobotTaskId(task.getId());
            detailAddVO.setTaskType(RobotTaskTypeEnum.MOVE_TO_POINT.getType());
            detailAddVO.setReleaseType(ReleaseTakeEnum.TO_LOCATION.getType());
            detailAddVO.setReleaseId(v.getId());
            detailAddVO.setToLocationNo(sortMap.get(v.getId()) + "");
            detailAddVO.setRobotNo(v.getRobotNo());
            taskDetails.add(detailAddVO);

            TaskToPathPlanningDTO pathPlanning = TaskToPathPlanningDTO.builder()
                    .orderId(detailAddVO.getId() + "")
                    .orderType(PathTaskTypeEnum.MOVE_TO_WAIT.getType())
                    .priority(1l)
                    .createTime(LocalDateTime.now())
                    .taskType(PathTaskTypeToRobotEnum.MOVE.getType())
                    .waitIds(Collections.singletonList(v.getId() + ""))
                    .build();

            TaskRobotNoLimittationAreaDTO taskRobotNoLimittationAreaDTO = robotNoLimittationAreaDTOMap.get(v.getRobotNo());
            List<TaskRobotNoLimittationAreaDTO> robotNoLimitions = Collections.singletonList(taskRobotNoLimittationAreaDTO);
            pathPlanning.setRobotNoLimitationAreaDTOS(robotNoLimitions);

            String key = PathPlanningChcheConstant.PATH_PLANNING_TASK + pathPlanning.getOrderId();
            redisUtil.set(key, JSON.toJSONString(pathPlanning), znConfigConstant.getTask().getTaskCacheTime());
            pathPlanningList.add(pathPlanning);
        }

        robotTaskMapper.insertBatch(tasks);

        List<RobotTaskDetailDO> bean = BeanUtils.toBean(taskDetails, RobotTaskDetailDO.class);
        robotTaskDetailMapper.insertBatch(bean);

        positionMapItemMapper.updateById(robotMapItems);

        log.info("派车去停车点--任务下发给PP :{}", JSON.toJSONString(pathPlanningList));
//        commonApi.commonMethod(pathPlanningList, PathPlanningTopicConstant.TASK_ASSIGNMENT_REQUEST);
        mqttUtils.pub(PathPlanningTopicConstant.TASK_ASSIGNMENT_REQUEST, JSON.toJSONString(pathPlanningList));

    }


    @Override
    public void sendChargeTaskToPP(List<RobotChargeLogDO> logs, List<RobotTaskDetailDO> taskDetailDOS,
                                   List<RobotInformationDO> robots) {

        //机器人不能行走的区域
        List<TaskRobotNoLimittationAreaDTO> robotNoLimitationArea = getRobotNoLimitationArea(robots);

        Map<String, TaskRobotNoLimittationAreaDTO> robotNoLimittationAreaDTOMap =
                robotNoLimitationArea.stream().collect(Collectors.toMap(v -> v.getRobotNo(), Function.identity()));

        Map<String, RobotInformationDO> robotMap =
                robots.stream().collect(Collectors.toMap(v -> v.getRobotNo(), Function.identity()));

        Map<Long, RobotTaskDetailDO> taskDetailMap =
                taskDetailDOS.stream().collect(Collectors.toMap(v -> v.getId(), Function.identity()));

        List<TaskToPathPlanningDTO> pathPlanningList = new ArrayList<>();

        for (RobotChargeLogDO v : logs) {

            Long priority = znConfigConstant.getRobotCharge().getPriorityConfig();
            LocalDateTime createTime = LocalDateTime.now();
            if (ObjectUtil.isEmpty(ChargeModelEnum.TASK.getType().equals(v.getChargeModel()))
                    && ObjectUtil.isNotEmpty(v.getTaskDetailId())
                    && ObjectUtil.isNotEmpty(taskDetailMap.get(v.getTaskDetailId()))) {
                createTime = taskDetailMap.get(v.getTaskDetailId()).getCreateTime();
                priority = taskDetailMap.get(v.getTaskDetailId()).getPriority();
            }

            String orderType = ObjectUtil.isEmpty(v.getTaskDetailId()) ? PathTaskTypeEnum.AUTO_CHARGE.getType() : PathTaskTypeEnum.CHARGE.getType();
            TaskToPathPlanningDTO pathPlanning = TaskToPathPlanningDTO.builder()
                    .orderId(String.valueOf(v.getId()))
                    .orderType(orderType)
                    .priority(priority)
                    .createTime(createTime)
                    .build();

            TaskRobotNoLimittationAreaDTO taskRobotNoLimittationAreaDTO = robotNoLimittationAreaDTOMap.get(v.getRobotNo());
            List<TaskRobotNoLimittationAreaDTO> robotNoLimitions = Arrays.asList(taskRobotNoLimittationAreaDTO);
            pathPlanning.setRobotNoLimitationAreaDTOS(robotNoLimitions);

            RobotInformationDO robotInformationDO = robotMap.get(taskRobotNoLimittationAreaDTO.getRobotNo());
            String taskType = ChargeTypeEnum.AUTOMATIC.getType().equals(robotInformationDO.getChargeType()) ?
                    PathTaskTypeToRobotEnum.AUTO_CHARGE.getType() : PathTaskTypeToRobotEnum.MANUAL_CHARGE.getType();
            pathPlanning.setTaskType(taskType);

            pathPlanning.setReleaseGroupId("POINT_" + v.getPositionMapItemId());
            pathPlanning.setReleaseLocationNumber(znConfigConstant.getRobotCharge().getReleaseLocationNumberConfig());
            pathPlanning.setWaitIds(Collections.singletonList(v.getPositionMapItemId() + ""));

            String key = PathPlanningChcheConstant.PATH_PLANNING_TASK + pathPlanning.getOrderId();
            redisUtil.set(key, JSON.toJSONString(pathPlanning), znConfigConstant.getTask().getTaskCacheTime());

            pathPlanningList.add(pathPlanning);
        }

        log.info("充电任务下发给PP :{}", JSON.toJSONString(pathPlanningList));
//        commonApi.commonMethod(pathPlanningList, PathPlanningTopicConstant.TASK_ASSIGNMENT_REQUEST);
        mqttUtils.pub(PathPlanningTopicConstant.TASK_ASSIGNMENT_REQUEST, JSON.toJSONString(pathPlanningList));
    }

    @Transactional(rollbackFor = Exception.class)
    public void distributeTasksToPP(List<RobotInformationDO> robots, List<RobotTaskDetailDO> taskDetailDOS) {
        //目前一个点位/线库， 只能派一辆车去做
        taskDetailDOS = getSingleTask(taskDetailDOS);

        if (ObjectUtil.isEmpty(taskDetailDOS)) {
            log.info("相同的点位/线库，目前有处理中的任务, 无法派单给PP");
            return;
        }

        List<TaskToPathPlanningDTO> pathPlanningList = new ArrayList<>();

        Set<Long> locationIds = new HashSet<>();

        Set<Long> fromLocationIds = new HashSet<>();
        Set<Long> toLocationIds = new HashSet<>();

        for (RobotTaskDetailDO taskDetailDO : taskDetailDOS) {
            if (ReleaseTakeEnum.isLocation(taskDetailDO.getTakeType()) && ObjectUtil.isNotEmpty(taskDetailDO.getFromLocationId())) {
                fromLocationIds.add(taskDetailDO.getFromLocationId());
            }

            if (ReleaseTakeEnum.isLocation(taskDetailDO.getTakeType()) && ObjectUtil.isNotEmpty(taskDetailDO.getToLocationId())) {
                toLocationIds.add(taskDetailDO.getToLocationId());
            }
        }

        locationIds.addAll(fromLocationIds);
        locationIds.addAll(toLocationIds);

        Map<Long, WareHouseLocationDO> locationDOMap = new HashMap<>();
        if (ObjectUtil.isNotEmpty(locationIds)) {
            List<WareHouseLocationDO> locations = locationMapper.selectList(new LambdaQueryWrapperX<WareHouseLocationDO>()
                    .in(WareHouseLocationDO::getId, locationIds));

            locationDOMap =
                    locations.stream().collect(Collectors.toMap(v -> v.getId(), Function.identity()));
        }

        //能执行此任务的机器人 和 不能走的区域
        Pair<List<TaskRobotNoLimittationAreaDTO>, Map<String, TaskRobotNoLimittationAreaDTO>> pair = getRobotLimitationArea(robots);
        Map<String, TaskRobotNoLimittationAreaDTO> robotDoReleaseMap = pair.getRight();
        List<TaskRobotNoLimittationAreaDTO> robotNoLimitationArea = pair.getLeft();

        //前一个任务是仅取货
//        List<String> robotDoTake = getRobotDoTake(robots);

        for (RobotInformationDO robot : robots) {
            taskService.chargeDone(robot.getRobotNo());
        }

        List<PositionMapItemDO> positionMapItems = positionMapItemMapper.selectList(new LambdaQueryWrapperX<PositionMapItemDO>()
                .eq(PositionMapItemDO::getType, PositionMapItemEnum.STOP.getType())
                .eq(PositionMapItemDO::getUseStatus, UseStatusEnum.FREE.getType()));
        List<String> waitIds = null;
        if (ObjectUtil.isNotEmpty(positionMapItems)) {
            waitIds = positionMapItems
                    .stream()
                    .map(u -> u.getId() + "")
                    .collect(Collectors.toList());
        }
        Integer i = 0;

        log.info("开始组装需要下发给PP的任务");
        for (RobotTaskDetailDO taskDetailDO : taskDetailDOS) {

            String taskType = PathTaskTypeToRobotEnum.taskDetailGetType(taskDetailDO.getTaskType());
            TaskToPathPlanningDTO pathPlanning = TaskToPathPlanningDTO.builder()
                    .orderId(String.valueOf(taskDetailDO.getId()))
                    .orderType(PathTaskTypeEnum.getTaskType(taskDetailDO.getTaskType()))
                    .priority(taskDetailDO.getPriority())
                    .taskType(taskType)
                    .createTime(taskDetailDO.getCreateTime())
                    .build();

            if (RobotTaskTypeEnum.PARK.getType().equals(taskDetailDO.getTaskType())
                    && (ObjectUtil.isEmpty(waitIds) || waitIds.size() < i)) {
                log.info("停车任务,没有多余的空闲点位 ：{}", taskDetailDO.getId());
                continue;
            } else if (RobotTaskTypeEnum.PARK.getType().equals(taskDetailDO.getTaskType())) {
                pathPlanning.setWaitIds(waitIds);
                i++;
            } else if (RobotTaskTypeEnum.MOVE_TO_POINT.getType().equals(taskDetailDO.getTaskType())) {
                moveToPoint(pathPlanning, taskDetailDO);
            }

            if (!cargoDetected(taskDetailDO)) {
                continue;
            }

            List<TaskRobotNoLimittationAreaDTO> robotNoLimitions = null;
            if (ObjectUtil.isEmpty(taskDetailDO.getRobotNo())) {
                robotNoLimitions = robotNoLimitationArea;
            } else {
                log.info("区域 :{}", JSON.toJSONString(robotDoReleaseMap));
                TaskRobotNoLimittationAreaDTO taskRobotNoLimittationAreaDTO = robotDoReleaseMap.get(taskDetailDO.getRobotNo());
                if (ObjectUtil.isEmpty(taskRobotNoLimittationAreaDTO)) {
                    log.info("车辆没有能行走的区域 :{}", taskDetailDO.getRobotNo());
                    continue;
                }
                robotNoLimitions = Collections.singletonList(taskRobotNoLimittationAreaDTO);
            }

            if (ObjectUtil.isEmpty(robotNoLimitions)) {
                log.info("此任务明细,已经指定了机器人编号, 没有对应的机器人能执行此任务 :{}, 机器人编号:{}", taskDetailDO.getId(), taskDetailDO.getRobotNo());
                continue;
            }
            pathPlanning.setRobotNoLimitationAreaDTOS(robotNoLimitions);

            WareHouseLocationDO fromLocation = locationDOMap.get(taskDetailDO.getFromLocationId());
            WareHouseLocationDO toLocation = locationDOMap.get(taskDetailDO.getToLocationId());

            if (ObjectUtil.isNotEmpty(taskDetailDO.getFromLocationId())) {
                pathPlanning.setTakeLocationNumber(Math.abs(znConfigConstant.getLocationNumberReduce() - taskDetailDO.getFromLocationNumber()));
                pathPlanning.setTakePointId(String.valueOf(fromLocation.getMapItemId()));

                if (isLocation(taskDetailDO.getTakeType())) {
                    pathPlanningSetTakeHeight(pathPlanning, fromLocation);
                    pathPlanningSetTakeForksHeight(pathPlanning, fromLocation);
                    pathPlanning.setTakeLevel(fromLocation.getLocationStorey());
                } else if (ReleaseTakeEnum.TO_POINT.getType().equals(taskDetailDO.getTakeType())) {
                    pathPlanningSetPointTakeHeight(pathPlanning, taskDetailDO);
                }

                pathPlanning.setTakeOffsetHeight(znConfigConstant.getRobotConfig().getOffsetHeight());
                if (ObjectUtil.isNotEmpty(taskDetailDO.getFromLaneId())) {
                    pathPlanning.setTakeGroupId("LINE_" + taskDetailDO.getFromLaneId());
                } else {
                    pathPlanning.setTakeGroupId("POINT_" + taskDetailDO.getFromMapItemId());
                }
            }

            if (ObjectUtil.isNotEmpty(taskDetailDO.getToLocationId())) {
                pathPlanning.setReleaseLocationNumber(taskDetailDO.getToLocationNumber());
                pathPlanning.setReleasePointId(String.valueOf(toLocation.getMapItemId()));

                if (isLocation(taskDetailDO.getTakeType())) {
                    pathPlanningSetReleaseHeight(pathPlanning, toLocation);
                    pathPlanning.setReleaseLevel(toLocation.getLocationStorey());
                } else if (ReleaseTakeEnum.TO_POINT.getType().equals(taskDetailDO.getTakeType())) {
                    pathPlanningSetPointReleaseHeight(pathPlanning, taskDetailDO);
                }

                pathPlanning.setReleaseOffsetHeight(znConfigConstant.getRobotConfig().getOffsetHeight());
                if (ObjectUtil.isNotEmpty(taskDetailDO.getToLaneId())) {
                    pathPlanning.setReleaseGroupId("LINE_" + taskDetailDO.getToLaneId());
                } else {
                    pathPlanning.setReleaseGroupId("POINT_" + taskDetailDO.getToMapItemId());
                }
            }

            String key = PathPlanningChcheConstant.PATH_PLANNING_TASK + pathPlanning.getOrderId();
            redisUtil.set(key, JSON.toJSONString(pathPlanning), znConfigConstant.getTask().getTaskCacheTime());

            pathPlanningList.add(pathPlanning);
        }

        if (ObjectUtil.isNotEmpty(pathPlanningList)) {
            log.info("111111作业任务下发给PP :{}", JSON.toJSONString(pathPlanningList));
//            commonApi.commonMethod(pathPlanningList, PathPlanningTopicConstant.TASK_ASSIGNMENT_REQUEST);
            mqttUtils.pub(PathPlanningTopicConstant.TASK_ASSIGNMENT_REQUEST, JSON.toJSONString(pathPlanningList));
        }
    }

    public Boolean cargoDetected(RobotTaskDetailDO taskDetailDO) {

        if (ObjectUtil.isEmpty(taskDetailDO.getRobotNo())) {
            return true;
        }

        String mac = informationService.getMacByRobotNo(taskDetailDO.getRobotNo());
        String cargoDetectedKey = RobotTaskChcheConstant.ROBOT_CARGO_DETECTED + mac;
        Object cargoDetected = redisUtil.get(cargoDetectedKey);
        log.info("车辆编号 :{}, 传感器是否按下 :{}", taskDetailDO.getRobotNo(), cargoDetected);
        /*if (ObjectUtil.isNotEmpty(cargoDetected) && (RobotStatusCodeConstant.CARGO_DETECTED.equals(Boolean.parseBoolean(String.valueOf(cargoDetected)))
                && !RobotTaskTypeEnum.RELEASE.getType().equals(taskDetailDO.getTaskType()))) {
            log.info("车机上报传感器为空,  或者 被按下且任务非仅放货任务 :{} ,任务id ：{}", taskDetailDO.getRobotNo(), taskDetailDO.getId());
            return false;
        } else if (ObjectUtil.isNotEmpty(cargoDetected) && !RobotStatusCodeConstant.CARGO_DETECTED.equals(Boolean.parseBoolean(String.valueOf(cargoDetected)))
                && RobotTaskTypeEnum.RELEASE.getType().equals(taskDetailDO.getTaskType())) {
            log.info("仅放货任务,传感器未被按下，所以不执行任务 :{}, 任务id ：{}", taskDetailDO.getRobotNo(), taskDetailDO.getId());
            return false;
        }*/
        return true;
    }

    private void pathPlanningSetPointReleaseHeight(TaskToPathPlanningDTO pathPlanning, RobotTaskDetailDO taskDetailDO) {

        WareHouseTakePointDO houseTakePoint = houseTakePointService.getHouseTakePoint(taskDetailDO.getToLocationId());
        pathPlanning.setReleaseLevel(houseTakePoint.getLocationStorey());

        if (ObjectUtil.isNotEmpty(houseTakePoint.getTrayHeight())) {
            pathPlanning.setReleaseHeight(houseTakePoint.getTrayHeight().doubleValue());
        } else if (ZeroOneEnum.ONE.getType().equals(houseTakePoint.getLocationStorey())) {
            pathPlanning.setReleaseHeight(0.0);
        } else {
            WareHouseTakePointDO nextHouseTakePoint =
                    houseTakePointService.getHouseTakePointByItemIdAndStorey(houseTakePoint.getPositionMapItemId(),
                            houseTakePoint.getLocationStorey() - 1);
            if (ObjectUtil.isNotEmpty(nextHouseTakePoint) && ObjectUtil.isNotEmpty(nextHouseTakePoint.getTotalHeight())) {
                pathPlanning.setReleaseHeight(nextHouseTakePoint.getTotalHeight().doubleValue());
                return;
            }

            Integer locationStorey = houseTakePoint.getLocationStorey() - 1;
            double height = locationStorey * znConfigConstant.getRobotConfig().getDefaultTrayHeight();
            log.info("放货设置默认高度 :{}", height);
            pathPlanning.setReleaseHeight(height);
        }
    }

    private void pathPlanningSetPointTakeHeight(TaskToPathPlanningDTO pathPlanning, RobotTaskDetailDO taskDetailDO) {

        WareHouseTakePointDO houseTakePoint = houseTakePointService.getHouseTakePoint(taskDetailDO.getFromLocationId());
        pathPlanning.setTakeLevel(houseTakePoint.getLocationStorey());
        if (ObjectUtil.isNotEmpty(houseTakePoint.getTotalHeight())) {
            pathPlanning.setTakeHeight(Double.valueOf(houseTakePoint.getTotalHeight() + ""));
        } else {
            Integer locationStorey = houseTakePoint.getLocationStorey();
            double height = locationStorey * znConfigConstant.getRobotConfig().getDefaultTrayHeight();
            log.info("取货设置默认取货高度 :{}", height);
            pathPlanning.setTakeHeight(height);
        }

        if (ObjectUtil.isNotEmpty(houseTakePoint.getTrayHeight())) {
            pathPlanning.setDetectHeight(houseTakePoint.getTrayHeight().doubleValue());
        } else if (ZeroOneEnum.ONE.getType().equals(houseTakePoint.getLocationStorey())) {
            pathPlanning.setDetectHeight(znConfigConstant.getDetectHeight());
        } else {
            Integer locationStorey = houseTakePoint.getLocationStorey() - 1;
            double height = locationStorey * znConfigConstant.getRobotConfig().getDefaultTrayHeight();
            BigDecimal d1 = new BigDecimal(znConfigConstant.getDeductionHeight());
            BigDecimal d2 = new BigDecimal(height + "");
            double detectHeight = d2.subtract(d1).doubleValue();
            log.info("设置取货默认抬叉高度 :{}", detectHeight);
            pathPlanning.setDetectHeight(detectHeight);
        }
    }

    public Boolean isLocation(Integer takeType) {
        if (ReleaseTakeEnum.TO_LOCATION.getType().equals(takeType)
                || ReleaseTakeEnum.TO_LANE.getType().equals(takeType)
                || ReleaseTakeEnum.TO_AREA.getType().equals(takeType)) {
            return true;
        }
        return false;
    }

    private void pathPlanningSetTakeForksHeight(TaskToPathPlanningDTO pathPlanning, WareHouseLocationDO fromLocation) {
        if (ZeroOneEnum.ONE.getType().equals(fromLocation.getLocationStorey())) {
            pathPlanning.setDetectHeight(znConfigConstant.getDetectHeight());
            return;
        }

        WareHouseLocationDO nextLocation = locationMapper.selectOne(new LambdaQueryWrapperX<WareHouseLocationDO>()
                .eq(WareHouseLocationDO::getMapItemId, fromLocation.getMapItemId())
                .eq(WareHouseLocationDO::getLocationStorey, fromLocation.getLocationStorey() - 1)
                .last("limit 1"));
        if (ObjectUtil.isNotEmpty(nextLocation) && ObjectUtil.isNotEmpty(nextLocation.getLocationTotalHeight())) {

            BigDecimal d1 = new BigDecimal(znConfigConstant.getDeductionHeight());
            BigDecimal d2 = nextLocation.getLocationTotalHeight();
            double detectHeight = d2.subtract(d1).doubleValue();
            pathPlanning.setDetectHeight(detectHeight);
            return;
        }

        Integer locationStorey = fromLocation.getLocationStorey() - 1;
        double height = locationStorey * znConfigConstant.getRobotConfig().getDefaultTrayHeight();
        BigDecimal d1 = new BigDecimal(znConfigConstant.getDeductionHeight());
        BigDecimal d2 = new BigDecimal(height + "");
        double detectHeight = d2.subtract(d1).doubleValue();
        log.info("设置取货默认抬叉高度 :{}", detectHeight);
        pathPlanning.setDetectHeight(detectHeight);
    }


    /**
     * 设置放货高度
     *
     * @param pathPlanning
     * @param toLocation
     */
    private void pathPlanningSetReleaseHeight(TaskToPathPlanningDTO pathPlanning, WareHouseLocationDO toLocation) {
        if (ZeroOneEnum.ONE.getType().equals(toLocation.getLocationStorey())) {
            pathPlanning.setReleaseHeight(0.0);
            return;
        }

        WareHouseLocationDO nextLocation = locationMapper.selectOne(new LambdaQueryWrapperX<WareHouseLocationDO>()
                .eq(WareHouseLocationDO::getMapItemId, toLocation.getMapItemId())
                .eq(WareHouseLocationDO::getLocationStorey, toLocation.getLocationStorey() - 1)
                .last("limit 1"));
        if (ObjectUtil.isNotEmpty(nextLocation) && ObjectUtil.isNotEmpty(nextLocation.getLocationTotalHeight())) {
            pathPlanning.setReleaseHeight(Double.valueOf(nextLocation.getLocationTotalHeight() + ""));
            return;
        }

        Integer locationStorey = toLocation.getLocationStorey() - 1;
        double height = locationStorey * znConfigConstant.getRobotConfig().getDefaultTrayHeight();
        log.info("放货设置默认高度 :{}", height);
        pathPlanning.setReleaseHeight(height);
    }

    /**
     * 设置取货高度
     *
     * @param pathPlanning
     * @param fromLocation
     */
    private void pathPlanningSetTakeHeight(TaskToPathPlanningDTO pathPlanning, WareHouseLocationDO fromLocation) {
        if (ObjectUtil.isNotEmpty(fromLocation.getLocationTotalHeight())) {
            pathPlanning.setTakeHeight(Double.valueOf(fromLocation.getLocationTotalHeight() + ""));
            return;
        }
        Integer locationStorey = fromLocation.getLocationStorey();
        double height = locationStorey * znConfigConstant.getRobotConfig().getDefaultTrayHeight();
        log.info("取货设置默认取货高度 :{}", height);
        pathPlanning.setTakeHeight(height);
    }

    /**
     * 移动到路径点
     *
     * @param pathPlanning
     * @param taskDetailDO
     */
    private void moveToPoint(TaskToPathPlanningDTO pathPlanning, RobotTaskDetailDO taskDetailDO) {
        pathPlanning.setReleaseLocationNumber(1L);
        pathPlanning.setReleasePointId(String.valueOf(taskDetailDO.getReleaseId()));
        pathPlanning.setReleaseGroupId("POINT_" + taskDetailDO.getReleaseId());
    }

    /**
     * 获取能执行此任务的机器人 和 不能走的区域
     *
     * @param robots
     * @return
     */
    private Pair<List<TaskRobotNoLimittationAreaDTO>, Map<String, TaskRobotNoLimittationAreaDTO>> getRobotLimitationArea(List<RobotInformationDO> robots) {
        List<PositionMapDO> positionMapDOS = positionMapMapper.selectList(new LambdaQueryWrapperX<PositionMapDO>());

        List<TaskRobotNoLimittationAreaDTO> robotNoLimitationAreaDTOS = new ArrayList<>();
        Map<String, TaskRobotNoLimittationAreaDTO> map = new HashMap<>();
        for (RobotInformationDO robot : robots) {
            TaskRobotNoLimittationAreaDTO robotNoLimitationAreaDTO = new TaskRobotNoLimittationAreaDTO();
            List<TaskLimitationAreaDTO> limitationAreaList = new ArrayList<>();
            robotNoLimitationAreaDTO.setRobotNo(robot.getRobotNo());
            for (PositionMapDO v : positionMapDOS) {
                if (!robot.getFloorAreaJson().contains(v.getId())) {
                    TaskLimitationAreaDTO taskLimitationAreaDTO = new TaskLimitationAreaDTO(v.getFloor(), v.getArea());
                    limitationAreaList.add(taskLimitationAreaDTO);
                }
            }

            if (positionMapDOS.size() == limitationAreaList.size()) {
                log.info("车辆不能行走所有的区域所以不下发任务 :{}", robot.getRobotNo());
                continue;
            }
            robotNoLimitationAreaDTO.setLimitationAreaList(limitationAreaList);

            robotNoLimitationAreaDTOS.add(robotNoLimitationAreaDTO);
            map.put(robot.getRobotNo(), robotNoLimitationAreaDTO);

        }
        return ImmutablePair.of(robotNoLimitationAreaDTOS, map);
    }

    /**
     * 前一个任务是仅取货
     *
     * @param robots
     */
    private List<String> getRobotDoTake(List<RobotInformationDO> robots) {
        return robots.stream()
                .map(v -> {
                    if (v.getRobotStatus().equals(RobotStatusEnum.LAST_TASK_IS_TAKE.getType())) {
                        return v.getRobotNo();
                    }
                    return null;
                })
                .collect(Collectors.toList());
    }

    /**
     * 设置机器人编号及不能行走的区域
     *
     * @param robots
     */
    @Override
    public List<TaskRobotNoLimittationAreaDTO> getRobotNoLimitationArea(List<RobotInformationDO> robots) {
        List<PositionMapDO> positionMapDOS = positionMapMapper.selectList(new LambdaQueryWrapperX<PositionMapDO>());

        List<TaskRobotNoLimittationAreaDTO> robotNoLimitationAreaDTOS = new ArrayList<>();
        for (RobotInformationDO robot : robots) {

            TaskRobotNoLimittationAreaDTO robotNoLimitationAreaDTO = new TaskRobotNoLimittationAreaDTO();
            List<TaskLimitationAreaDTO> limitationAreaList = new ArrayList<>();
            robotNoLimitationAreaDTO.setRobotNo(robot.getRobotNo());

            for (PositionMapDO v : positionMapDOS) {
                if (!robot.getFloorAreaJson().contains(v.getId())) {
                    TaskLimitationAreaDTO taskLimitationAreaDTO = new TaskLimitationAreaDTO(v.getFloor(), v.getArea());
                    limitationAreaList.add(taskLimitationAreaDTO);
                }
            }

            robotNoLimitationAreaDTO.setLimitationAreaList(limitationAreaList);
            robotNoLimitationAreaDTOS.add(robotNoLimitationAreaDTO);
        }
        return robotNoLimitationAreaDTOS;
    }

    /**
     * 目前一个点位/线库， 只能派一辆车去做
     *
     * @param taskDetailDOS
     * @return
     */
    private List<RobotTaskDetailDO> getSingleTask(List<RobotTaskDetailDO> taskDetailDOS) {
        //剔除目前正在做的线库和点位
        if (!znConfigConstant.getTaskNeedSingle()) {
            return taskDetailDOS;
        }

        log.info("---------处理:一个点位/线库 只派一台车 开始-------------");

        //不能分配任务的线库id
        Set<Long> laneIds = new HashSet<>();
        //不能分配任务的点位id
        Set<Long> mapIds = new HashSet<>();
        List<RobotTaskDetailDO> doingTaskDetails = robotTaskDetailMapper.selectList(new LambdaQueryWrapperX<RobotTaskDetailDO>()
                .eq(RobotTaskDetailDO::getTaskStatus, RobotTaskDetailStatusEnum.DOING.getType()));
        if (ObjectUtil.isNotEmpty(doingTaskDetails)) {
            for (RobotTaskDetailDO detail : doingTaskDetails) {
                if (RobotTaskTypeEnum.TAKE_RELEASE.getType().equals(detail.getTaskType())
                        || RobotTaskTypeEnum.MOVE.getType().equals(detail.getTaskType())
                        || RobotTaskTypeEnum.TAKE.getType().equals(detail.getTaskType())
                        || RobotTaskTypeEnum.RELEASE.getType().equals(detail.getTaskType())) {
                    if (ObjectUtil.isNotEmpty(detail.getFromLaneId())) {
                        laneIds.add(detail.getFromLaneId());
                    }
                    if (ObjectUtil.isNotEmpty(detail.getToLaneId())) {
                        laneIds.add(detail.getToLaneId());
                    }
                    if (ObjectUtil.isNotEmpty(detail.getFromMapItemId())) {
                        mapIds.add(detail.getFromMapItemId());
                    }
                    if (ObjectUtil.isNotEmpty(detail.getToMapItemId())) {
                        mapIds.add(detail.getToMapItemId());
                    }
                }
            }
        }

        //过滤处理中的, 不能分配的任务;
        List<RobotTaskDetailDO> detailDOS = taskDetailDOS.stream()
                .filter(v -> !laneIds.contains(v.getFromLaneId())
                        && !laneIds.contains(v.getToLaneId())
                        && !mapIds.contains(v.getFromMapItemId())
                        && !mapIds.contains(v.getToMapItemId()))
                .collect(Collectors.toList());

        if (ObjectUtil.isEmpty(detailDOS)) {
            log.info("过滤处理中的任务对应的线库id和点位id后，任务数据为空");
            return detailDOS;
        }

        //取货线库
        Map<Long, RobotTaskDetailDO> fromLaneMap = getFromLaneMap(detailDOS);

        //放货线库
        Map<Long, RobotTaskDetailDO> toLaneMap = getToLaneMap(detailDOS);

        //取货点位--最小
        Map<Long, Long> fromLocationMap = getFromLocationMap(detailDOS);

        //放货点位--最大
        Map<Long, Long> toLocationMap = getToLocationMap(detailDOS);

        //返回的数据
        List<RobotTaskDetailDO> list = new ArrayList<>();
        for (RobotTaskDetailDO v : detailDOS) {
            RobotTaskDetailDO fromLane = ObjectUtil.isNotEmpty(v.getFromLaneId()) ? fromLaneMap.get(v.getFromLaneId()) : null;
            RobotTaskDetailDO toLane = ObjectUtil.isNotEmpty(v.getToLaneId()) ? toLaneMap.get(v.getToLaneId()) : null;
            Long fromLocation = ObjectUtil.isNotEmpty(v.getFromMapItemId()) ? fromLocationMap.get(v.getFromMapItemId()) : null;
            Long toLocation = ObjectUtil.isNotEmpty(v.getToMapItemId()) ? toLocationMap.get(v.getToMapItemId()) : null;
            //校验放货线库，有取的任务
            RobotTaskDetailDO fromLaneDoRelease = ObjectUtil.isNotEmpty(v.getToLaneId()) ? fromLaneMap.get(v.getToLaneId()) : null;
            //校验放的普通库位，有取的任务
            Long toHaveFrom = ObjectUtil.isNotEmpty(v.getToMapItemId()) ? fromLocationMap.get(v.getToMapItemId()) : null;
            //校验取货库位，有放的任务
            Long fromHaveTo = ObjectUtil.isNotEmpty(v.getFromMapItemId()) ? toLocationMap.get(v.getFromMapItemId()) : null;

            //取放模式
            if (RobotTaskTypeEnum.TAKE_RELEASE.getType().equals(v.getTaskType())) {
                takeReleaseSetTask(list, v, fromLane, toLane, fromLocation, toLocation, fromLaneDoRelease, toHaveFrom, fromHaveTo);
            } else if (RobotTaskTypeEnum.MOVE.getType().equals(v.getTaskType())
                    || RobotTaskTypeEnum.RELEASE.getType().equals(v.getTaskType())) {
                //移动和仅放货
                moveSetTask(list, v, fromLane, toLane, fromLocation, toLocation, toHaveFrom, fromHaveTo, fromLaneDoRelease);
            } else if (RobotTaskTypeEnum.TAKE.getType().equals(v.getTaskType())) {
                //仅取货
                takeSetTask(list, v, fromLane, toLane, fromLocation, toLocation, toHaveFrom, fromHaveTo);
            } else if (RobotTaskTypeEnum.PARK.getType().equals(v.getTaskType())
                    || RobotTaskTypeEnum.MOVE_TO_POINT.getType().equals(v.getTaskType())) {
                list.add(v);
            }
        }

        log.info("---------处理:一个点位/线库 只派一台车 结束------------- :{}", JSON.toJSONString(list));

        return list;
    }

    private Map<Long, Long> getToLocationMap(List<RobotTaskDetailDO> detailDOS) {
        List<RobotTaskDetailDO> collect = detailDOS.stream()
                .filter(v -> ObjectUtil.isNotEmpty(v.getToMapItemId()))
                .collect(Collectors.toList());
        if (ObjectUtil.isEmpty(collect)) {
            return new HashMap<>();
        }
        return collect.stream().collect(Collectors.groupingBy(RobotTaskDetailDO::getToMapItemId,
                Collectors.collectingAndThen(Collectors.maxBy(Comparator.comparingLong(RobotTaskDetailDO::getToLocationNumber)),
                        a -> a.isPresent() ? a.get().getToLocationNumber() : null)));
    }

    private Map<Long, Long> getFromLocationMap(List<RobotTaskDetailDO> detailDOS) {
        List<RobotTaskDetailDO> collect = detailDOS.stream()
                .filter(v -> ObjectUtil.isNotEmpty(v.getFromMapItemId()))
                .collect(Collectors.toList());
        if (ObjectUtil.isEmpty(collect)) {
            return new HashMap<>();
        }
        return collect.stream().collect(Collectors.groupingBy(RobotTaskDetailDO::getFromMapItemId,
                Collectors.collectingAndThen(Collectors.minBy(Comparator.comparingLong(RobotTaskDetailDO::getFromLocationNumber)),
                        a -> a.isPresent() ? a.get().getFromLocationNumber() : null)));
    }

    private Map<Long, RobotTaskDetailDO> getToLaneMap(List<RobotTaskDetailDO> detailDOS) {

        List<RobotTaskDetailDO> collect = detailDOS.stream()
                .filter(v -> ObjectUtil.isNotEmpty(v.getToLaneId()))
                .collect(Collectors.toList());
        if (ObjectUtil.isEmpty(collect)) {
            return new HashMap<>();
        }
        return collect.stream().collect(Collectors.toMap(RobotTaskDetailDO::getToLaneId, Function.identity(), BinaryOperator.maxBy(Comparator.comparingLong(RobotTaskDetailDO::getToLocationNumber))));
    }

    private Map<Long, RobotTaskDetailDO> getFromLaneMap(List<RobotTaskDetailDO> detailDOS) {
        List<RobotTaskDetailDO> collect = detailDOS.stream()
                .filter(v -> ObjectUtil.isNotEmpty(v.getFromLaneId()))
                .collect(Collectors.toList());
        if (ObjectUtil.isEmpty(collect)) {
            return new HashMap<>();
        }

        return collect.stream()
                .collect(Collectors.toMap(RobotTaskDetailDO::getFromLaneId, Function.identity(), BinaryOperator.minBy(Comparator.comparingLong(RobotTaskDetailDO::getFromLocationNumber))));
    }


    /**
     * 仅取货设置任务
     *
     * @param list
     * @param v
     * @param fromLane
     * @param toLane
     * @param fromLocation
     * @param toLocation
     */
    private void takeSetTask(List<RobotTaskDetailDO> list, RobotTaskDetailDO v, RobotTaskDetailDO fromLane,
                             RobotTaskDetailDO toLane, Long fromLocation, Long toLocation,
                             Long toHaveFrom, Long fromHaveTo) {

        if (ObjectUtil.isNotEmpty(fromHaveTo) && fromHaveTo < v.getFromLocationNumber()) {
            log.info("下发任务给PP--仅取货任务--取货点有放货任务 :{}", v.getId());
            return;
        }

        if (!(ObjectUtil.isNotEmpty(fromLocation) && fromLocation.equals(v.getFromLocationNumber()))) {
            log.info("下发任务给PP--仅取货--排序不符合 :{}", v.getId());
            return;
        }

        //终点是线库
        if (ObjectUtil.isNotEmpty(v.getFromLaneId()) && ObjectUtil.isNotEmpty(fromLane)
                && fromLane.getFromLocationNumber().equals(v.getFromLocationNumber())) {
            list.add(v);

            //终点是普通库位
        } else if (ObjectUtil.isEmpty(v.getFromLaneId())) {
            list.add(v);
        }

    }

    /**
     * 移动设置任务
     *
     * @param list
     * @param v
     */
    private void moveSetTask(List<RobotTaskDetailDO> list, RobotTaskDetailDO v, RobotTaskDetailDO fromLane,
                             RobotTaskDetailDO toLane,
                             Long fromLocation, Long toLocation, Long toHaveFrom,
                             Long fromHaveTo, RobotTaskDetailDO fromLaneDoRelease) {

        if (ObjectUtil.isNotEmpty(toHaveFrom) && toHaveFrom < v.getToLocationNumber()) {
            log.info("下发任务给PP--移动/仅放任务--放货点有取货任务 :{}", v.getId());
            return;
        }

        if (!(ObjectUtil.isNotEmpty(toLocation) && toLocation.equals(v.getToLocationNumber()))) {
            log.info("下发任务给PP--移动任务--排序不符合 :{}", v.getId());
            return;
        }

        //终点是线库
        if (ObjectUtil.isNotEmpty(v.getToLaneId()) && ObjectUtil.isNotEmpty(toLane)
                && toLane.getToLocationNumber().equals(v.getToLocationNumber())
                && ObjectUtil.isEmpty(fromLaneDoRelease)) {
            list.add(v);

            //终点是普通库位
        } else if (ObjectUtil.isEmpty(v.getToLaneId())) {
            list.add(v);
        }
    }


    /**
     * 取放，设置任务
     */
    private void takeReleaseSetTask(List<RobotTaskDetailDO> list, RobotTaskDetailDO v, RobotTaskDetailDO fromLane,
                                    RobotTaskDetailDO toLane, Long fromLocation, Long toLocation,
                                    RobotTaskDetailDO fromLaneDoRelease, Long toHaveFrom,
                                    Long fromHaveTo) {

        if (ObjectUtil.isNotEmpty(toHaveFrom) && toHaveFrom < v.getToLocationNumber()) {
            log.info("下发任务给PP--取放任务--放货点有取货任务 :{}", v.getId());
            return;

        } else if (ObjectUtil.isNotEmpty(fromHaveTo) && fromHaveTo < v.getFromLocationNumber()) {
            log.info("下发任务给PP--取放任务--取货点有放货任务 :{}", v.getId());
            return;
        }

        if (!(ObjectUtil.isNotEmpty(fromLocation) && fromLocation.equals(v.getFromLocationNumber())
                && ObjectUtil.isNotEmpty(toLocation) && toLocation.equals(v.getToLocationNumber()))) {
            log.info("下发任务给PP--取放任务--取/放的排序不符合 :{}", v.getId());
            return;
        }
        //取放都是线库
        if (ObjectUtil.isNotEmpty(v.getFromLaneId()) && ObjectUtil.isNotEmpty(v.getToLaneId())
                && ObjectUtil.isNotEmpty(fromLane)
                && fromLane.getFromLocationNumber().equals(v.getFromLocationNumber())
                && ObjectUtil.isNotEmpty(toLane)
                && toLane.getToLocationNumber().equals(v.getToLocationNumber())) {

            if (v.getFromLaneId().equals(v.getToLaneId())) {
                list.add(v);
            } else if (ObjectUtil.isEmpty(fromLaneDoRelease)) {
                //放货的线库，有取的任务，先不进行放
                list.add(v);
            }

            //取是线库，放是点位
        } else if (ObjectUtil.isNotEmpty(v.getFromLaneId()) && ObjectUtil.isEmpty(v.getToLaneId())
                && ObjectUtil.isNotEmpty(fromLane)
                && fromLane.getFromLocationNumber().equals(v.getFromLocationNumber())) {
            list.add(v);

            //取是点位，放是线库
        } else if (ObjectUtil.isEmpty(v.getFromLaneId()) && ObjectUtil.isNotEmpty(v.getToLaneId())
                && ObjectUtil.isNotEmpty(toLane)
                && toLane.getToLocationNumber().equals(v.getToLocationNumber())
                && ObjectUtil.isEmpty(fromLaneDoRelease)) {
            list.add(v);

            //取放都是点位
        } else if (ObjectUtil.isEmpty(v.getFromLaneId()) && ObjectUtil.isEmpty(v.getToLaneId())) {
            list.add(v);
        }
    }


}
