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

import cn.hutool.core.util.ObjectUtil;
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.path.PathPlanningApi;
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.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.mqtt.enums.path.TaskTypeEnum;
import cn.iocoder.yudao.module.system.constant.path.PathPlanningTopicConstant;
import cn.iocoder.yudao.module.system.constant.robot.RobotTaskChcheConstant;
import cn.iocoder.yudao.module.system.constant.robot.RobotTopicConstant;
import cn.iocoder.yudao.module.system.dal.dataobject.houselocation.WareHouseLocationDO;
import cn.iocoder.yudao.module.system.dal.dataobject.positionmap.PositionMapDO;
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.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.positionmap.PositionMapMapper;
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.robot.*;
import cn.iocoder.yudao.module.system.enums.robot.charge.ChargeModelEnum;
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.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.Function;
import java.util.stream.Collectors;

@Service
@Validated
@Slf4j
public class RobotPathPlanningServiceImpl implements RobotPathPlanningService {
    @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.task_need_single:true}")
    private Boolean taskNeedSingle;

    @Value("${zn.location_number_reduce:100000000}")
    private Long locationNumberReduce;

    @Resource
    private CommonConfigMapper configMapper;


    @Resource
    private WareHouseLocationMapper locationMapper;

    @Resource
    private RedisUtil redisUtil;

    @Autowired
    private DistributeTasksService distributeTasksService;

    @Resource
    private PositionMapMapper positionMapMapper;

    @Resource
    private PathPlanningApi pathPlanningApi;

    @Value("${zn.robot_chearg.release_location_number_config:50}")
    private Long releaseLocationNumberConfig;

    @Value("${zn.robot_chearg.priority_config:50}")
    private Long priorityConfig;

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

        log.info("-------开始查找车子和任务------");
        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("--不存在需要执行的任务--");
            return;
        }

        log.info("-------查找到车子和任务------");
        distributeTasksToPP(robots, taskDetailDOS);

    }

    @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<Long, RobotTaskDetailDO> taskDetailMap =
                taskDetailDOS.stream().collect(Collectors.toMap(v -> v.getId(), Function.identity()));

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

        for (RobotChargeLogDO v : logs) {

            Long priority = priorityConfig;
            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();
            }

            TaskToPathPlanningDTO pathPlanning = TaskToPathPlanningDTO.builder()
                    .id(String.valueOf(v.getId()))
                    .type(TaskTypeEnum.CHARGE.getType())
                    .priority(priority)
                    .createTime(createTime)
                    .build();

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

            pathPlanning.setReleaseGroupId("POINT_" + v.getPositionMapItemId());
            pathPlanning.setReleaseLocationNumber(releaseLocationNumberConfig);
            pathPlanning.setReleasePointId(v.getPositionMapItemId());
            pathPlanningList.add(pathPlanning);
        }

        log.info("充电任务下发给PP :{}", JSON.toJSONString(pathPlanningList));
        pathPlanningApi.synchronousLineObject(pathPlanningList, PathPlanningTopicConstant.TASK_ASSIGNMENT_REQUEST);
    }

    @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 = taskDetailDOS.stream().map(RobotTaskDetailDO::getFromLocationId).collect(Collectors.toSet());
        Set<Long> toLocationIds = taskDetailDOS.stream().map(RobotTaskDetailDO::getToLocationId).collect(Collectors.toSet());
        locationIds.addAll(fromLocationIds);
        locationIds.addAll(toLocationIds);

        List<WareHouseLocationDO> locations = locationMapper.selectList(new LambdaQueryWrapperX<WareHouseLocationDO>()
                .in(WareHouseLocationDO::getId, locationIds));


        Map<Long, WareHouseLocationDO> locationDOMap =
                locations.stream().collect(Collectors.toMap(v -> v.getId(), Function.identity()));

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

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

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

            TaskToPathPlanningDTO pathPlanning = TaskToPathPlanningDTO.builder()
                    .id(String.valueOf(taskDetailDO.getId()))
                    .type(TaskTypeEnum.TASK.getType())
                    .priority(taskDetailDO.getPriority())
                    .createTime(taskDetailDO.getCreateTime())
                    .build();

            List<TaskRobotNoLimittationAreaDTO> robotNoLimitions = null;
            if (ObjectUtil.isNotEmpty(taskDetailDO.getRobotNo()) && ObjectUtil.isNotEmpty(robotNoLimittationAreaDTOMap)) {
                TaskRobotNoLimittationAreaDTO taskRobotNoLimittationAreaDTO = robotNoLimittationAreaDTOMap.get(taskDetailDO.getRobotNo());
                robotNoLimitions = Arrays.asList(taskRobotNoLimittationAreaDTO);
            } else if (ObjectUtil.isEmpty(taskDetailDO.getRobotNo())) {
                robotNoLimitions = robotNoLimitationArea;
            }

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

            //取是线库
            if (ObjectUtil.isNotEmpty(taskDetailDO.getFromLocationId())
                    && ObjectUtil.isNotEmpty(taskDetailDO.getFromLaneId())) {

                pathPlanning.setTakeGroupId("LINE_" + taskDetailDO.getFromLaneId());
                pathPlanning.setTakeLocationNumber(Math.abs(locationNumberReduce - taskDetailDO.getFromLocationNumber()));
                pathPlanning.setTakePointId(locationDOMap.get(taskDetailDO.getFromLocationId()).getMapItemId());

            } else if (ObjectUtil.isNotEmpty(taskDetailDO.getFromLocationId())
                    && ObjectUtil.isEmpty(taskDetailDO.getFromLaneId())) {
                //取的是普通点位
                pathPlanning.setTakeGroupId("POINT_" + taskDetailDO.getFromMapItemId());
                pathPlanning.setTakeLocationNumber(Math.abs(locationNumberReduce - taskDetailDO.getFromLocationNumber()));
                pathPlanning.setTakePointId(locationDOMap.get(taskDetailDO.getFromLocationId()).getMapItemId());
            }

            //放是线库
            if (ObjectUtil.isNotEmpty(taskDetailDO.getToLocationId())
                    && ObjectUtil.isNotEmpty(taskDetailDO.getToLaneId())) {

                pathPlanning.setReleaseGroupId("LINE_" + taskDetailDO.getToLaneId());
                pathPlanning.setReleaseLocationNumber(taskDetailDO.getToLocationNumber());
                pathPlanning.setReleasePointId(locationDOMap.get(taskDetailDO.getToLocationId()).getMapItemId());

            } else if (ObjectUtil.isNotEmpty(taskDetailDO.getToLocationId())
                    && ObjectUtil.isEmpty(taskDetailDO.getToLaneId())) {
                //放的是普通点位
                pathPlanning.setReleaseGroupId("POINT_" + taskDetailDO.getToMapItemId());
                pathPlanning.setReleaseLocationNumber(taskDetailDO.getToLocationNumber());
                pathPlanning.setReleasePointId(locationDOMap.get(taskDetailDO.getToLocationId()).getMapItemId());
            }

            pathPlanningList.add(pathPlanning);
        }

        if (ObjectUtil.isNotEmpty(pathPlanningList)) {
            log.info("作业任务下发给PP :{}", JSON.toJSONString(pathPlanningList));
            pathPlanningApi.synchronousLineObject(pathPlanningList, PathPlanningTopicConstant.TASK_ASSIGNMENT_REQUEST);
        }
    }

    /**
     * 设置机器人编号及不能行走的区域
     *
     * @param robots
     */
    private 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 (!taskNeedSingle) {
            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())) {
                    laneIds.add(detail.getFromLaneId());
                    laneIds.add(detail.getToLaneId());
                    mapIds.add(detail.getFromMapItemId());
                    mapIds.add(detail.getToMapItemId());
                }
            }
        }

        //过滤处理中的, 不能分配的任务;
        List<RobotTaskDetailDO> detailDOS = taskDetailDOS.stream()
                .filter(v -> !laneIds.contains(v.getFromLaneId())
                        && !laneIds.contains(v.getToLaneId())
                        && !RobotTaskTypeEnum.PARK.getType().equals(v.getTaskType()) //停车在充电的任务做
                        && !RobotTaskTypeEnum.CHARGE.getType().equals(v.getTaskType()) //停车在充电的任务做
                        && !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 = detailDOS.stream()
                .filter(v -> ObjectUtil.isNotEmpty(v.getFromLaneId()))
                .collect(Collectors.groupingBy(RobotTaskDetailDO::getFromLaneId,
                        Collectors.collectingAndThen(Collectors.minBy(Comparator.comparingLong(RobotTaskDetailDO::getFromLocationNumber)), Optional::get)));

        //放货线库
        Map<Long, RobotTaskDetailDO> toLaneMap = detailDOS.stream()
                .filter(v -> ObjectUtil.isNotEmpty(v.getToLaneId()))
                .collect(Collectors.groupingBy(RobotTaskDetailDO::getToLaneId,
                        Collectors.collectingAndThen(Collectors.maxBy(Comparator.comparingLong(RobotTaskDetailDO::getToLocationNumber)), Optional::get)));

        //取货点位--最小
        Map<Long, Long> fromLocationMap = detailDOS.stream()
                .filter(v -> ObjectUtil.isNotEmpty(v.getFromMapItemId()))
                .collect(Collectors.groupingBy(RobotTaskDetailDO::getFromMapItemId,
                        Collectors.collectingAndThen(Collectors.minBy(Comparator.comparingLong(RobotTaskDetailDO::getFromLocationNumber)),
                                a -> a.isPresent() ? a.get().getFromLocationNumber() : null)));

        //放货点位--最大
        Map<Long, Long> toLocationMap = detailDOS.stream()
                .filter(v -> ObjectUtil.isNotEmpty(v.getToMapItemId()))
                .collect(Collectors.groupingBy(RobotTaskDetailDO::getToMapItemId,
                        Collectors.collectingAndThen(Collectors.maxBy(Comparator.comparingLong(RobotTaskDetailDO::getToLocationNumber)),
                                a -> a.isPresent() ? a.get().getToLocationNumber() : null)));

        //返回的数据
        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);
            }
        }

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

        return list;
    }


    /**
     * 仅取货设置任务
     *
     * @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);
        }
    }

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


    /**
     * 取放货
     *
     * @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, Map<Long, WareHouseLocationDO> locationMp,
                                        List<TaskToPathPlanningDTO> taskToPathPlanningDTOS) {

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

        TaskToPathPlanningDTO taskToPathPlanningDTO = new TaskToPathPlanningDTO();

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


}
