package cn.iocoder.yudao.module.system.service.path;

import cn.hutool.core.util.ObjectUtil;
import cn.hutool.json.JSONUtil;
import cn.iocoder.yudao.framework.mybatis.core.query.LambdaQueryWrapperX;
import cn.iocoder.yudao.module.infra.api.websocket.WebSocketSenderApi;
import cn.iocoder.yudao.module.mqtt.api.common.CommonApi;
import cn.iocoder.yudao.module.mqtt.api.path.dto.*;
import cn.iocoder.yudao.module.mqtt.api.path.task.TaskPathPlanningDTO;
import cn.iocoder.yudao.module.system.api.robot.dto.FloorZoneDTO;
import cn.iocoder.yudao.module.system.api.robot.vo.PathPlanningMovePoseVO;
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.CommonConstant;
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.RobotTaskChcheConstant;
import cn.iocoder.yudao.module.system.constant.robot.RobotTopicConstant;
import cn.iocoder.yudao.module.system.constant.webSocket.WebSocketConstant;
import cn.iocoder.yudao.module.system.controller.admin.log.vo.RobotTaskDetailActionLogSaveReqVO;
import cn.iocoder.yudao.module.system.controller.admin.path.dto.GraphMatchDataDTO;
import cn.iocoder.yudao.module.system.controller.admin.path.dto.SimulationRobotPoseDTO;
import cn.iocoder.yudao.module.system.controller.admin.positionmap.vo.PositionMapItemSaveReqVO;
import cn.iocoder.yudao.module.system.controller.admin.positionmap.vo.PositionMapLineSaveReqVO;
import cn.iocoder.yudao.module.system.controller.admin.positionmap.vo.PositionMapSaveReqVO;
import cn.iocoder.yudao.module.system.dal.dataobject.config.CommonConfigDO;
import cn.iocoder.yudao.module.system.dal.dataobject.log.RobotTaskDetailActionLogDO;
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.positionmap.PositionMapLineDO;
import cn.iocoder.yudao.module.system.dal.dataobject.robot.RobotInformationDO;
import cn.iocoder.yudao.module.system.dal.dataobject.robot.RobotModelDO;
import cn.iocoder.yudao.module.system.dal.mysql.positionmap.PositionMapItemMapper;
import cn.iocoder.yudao.module.system.dal.mysql.positionmap.PositionMapLineMapper;
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.enums.common.ZeroOneEnum;
import cn.iocoder.yudao.module.system.enums.config.CommandConfigTypeEnum;
import cn.iocoder.yudao.module.system.enums.item.PositionMapItemEnum;
import cn.iocoder.yudao.module.system.enums.line.DirectionEnum;
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.path.PathTypeEnum;
import cn.iocoder.yudao.module.system.enums.robot.actionlog.ActionStatusEnum;
import cn.iocoder.yudao.module.system.enums.robot.task.RobotCommandTypeEnum;
import cn.iocoder.yudao.module.system.service.config.CommonConfigService;
import cn.iocoder.yudao.module.system.service.log.RobotTaskDetailActionLogService;
import cn.iocoder.yudao.module.system.service.positionmap.PositionChangePointBindingService;
import cn.iocoder.yudao.module.system.service.positionmap.PositionMapItemService;
import cn.iocoder.yudao.module.system.service.positionmap.PositionMapService;
import cn.iocoder.yudao.module.system.service.remote.RemoteControllerInformationService;
import cn.iocoder.yudao.module.system.service.robot.RobotInformationService;
import cn.iocoder.yudao.module.system.service.robot.RobotModelService;
import cn.iocoder.yudao.module.system.util.redis.RedisUtil;
import com.alibaba.fastjson.JSON;
import com.google.common.collect.Lists;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.BeanUtils;
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 javax.annotation.Resource;
import java.math.BigDecimal;
import java.math.MathContext;
import java.time.Duration;
import java.time.LocalDateTime;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.LinkedList;
import java.util.List;
import java.util.Map;
import java.util.stream.Collectors;

import static cn.iocoder.yudao.framework.common.exception.util.ServiceExceptionUtil.exception;
import static cn.iocoder.yudao.framework.common.exception.util.ServiceExceptionUtil.exception0;
import static cn.iocoder.yudao.module.system.enums.ErrorCodeConstants.PATH_PLANNING_NOT_EXISTS;

/**
 * 同步信息给车辆
 */
@Slf4j
@Service
public class PathPlanningServiceImpl implements PathPlanningService {

    @Resource
    private PositionMapLineMapper positionMapLineMapper;

    @Resource
    private PositionMapMapper positionMapMapper;

    @Resource
    private PositionMapItemMapper positionMapItemMapper;

    @Resource
    private RobotInformationMapper informationMapper;

    @Autowired
    private RobotInformationService robotInformationService;

    /*@Resource
    private CommonApi commonApi;*/

    @Autowired
    private MqttUtils mqttUtils;

    @Resource
    private RedisUtil redisUtil;

    @Resource
    public WebSocketSenderApi webSocketSenderApi;

    @Resource
    private CommonConfigService configService;

    @Resource
    @Lazy
    private PositionMapService positionMapService;

    @Resource
    private PositionMapItemService positionMapItemService;

    @Autowired
    private ZnConfigConstant znConfigConstant;

    @Resource
    private PositionChangePointBindingService positionChangePointBindingService;

    @Resource
    @Lazy
    private RemoteControllerInformationService controllerInformationService;

    @Resource
    private RobotTaskDetailActionLogService taskDetailActionLogService;

    @Resource
    private RobotModelService modelService;

    /**
     * 同步ware_position_map_line的点位信息
     */
    @Override
    public void synchronousPoint(PositionMapSaveReqVO data) {

        List<PositionMapDO> positionMapDOS = positionMapMapper.selectList(new LambdaQueryWrapperX<PositionMapDO>()
                .eq(ObjectUtil.isNotEmpty(data.getFloor()), PositionMapDO::getFloor, data.getFloor())
                .eq(ObjectUtil.isNotEmpty(data.getArea()), PositionMapDO::getArea, data.getArea()));

        if (ObjectUtil.isEmpty(positionMapDOS)) {
            throw exception(PATH_PLANNING_NOT_EXISTS);
        }

        PositionMapLineDO positionMapLineDO = new PositionMapLineDO();
        List<PositionMapLineDTO> positionLines = positionMapLineMapper.getAllPositionMapLine(positionMapLineDO);

        if (ObjectUtil.isEmpty(positionLines)) {
            throw exception(PATH_PLANNING_NOT_EXISTS);
        }

        Map<String, List<PositionMapLineDTO>> positionMapLineMap = positionLines.stream()
                .collect(Collectors.groupingBy(PositionMapLineDTO::getPositionMapId));
        for (PositionMapDO positionMapDO : positionMapDOS) {
            PositionMapLinePathDTO relatedPathNode = new PositionMapLinePathDTO();
            relatedPathNode.setFloor(positionMapDO.getFloor());
            relatedPathNode.setArea(positionMapDO.getArea());
            relatedPathNode.setType(PathTypeEnum.INIT.getType());
            List<PositionMapLineDTO> positionMapLineDOS = positionMapLineMap.get(positionMapDO.getId() + "");
            if (ObjectUtil.isEmpty(positionMapLineDOS)) {
                continue;
            }

            assembleDataToPP(relatedPathNode, positionMapLineDOS);
        }

        try {
            Thread.sleep(2000);
        } catch (InterruptedException e) {
            throw new RuntimeException(e);
        }

//        commonApi.commonMethod(new PositionMapLineDTO(), PathPlanningTopicConstant.SYNCHRONOUS_ALL_MAP_LINE_END);
        mqttUtils.pub(PathPlanningTopicConstant.SYNCHRONOUS_ALL_MAP_LINE_END,JSON.toJSONString(new PositionMapLineDO()));
    }

    /**
     * 同步ware_position_map_item的点位信息
     *
     * @param data
     */
    @Override
    public void synchronousAllItem(PositionMapSaveReqVO data) {
        log.info("开始同步点位信息");
        List<PositionMapDO> positionMapDOS = positionMapMapper.selectList(new LambdaQueryWrapperX<PositionMapDO>()
                .eq(ObjectUtil.isNotEmpty(data.getFloor()), PositionMapDO::getFloor, data.getFloor())
                .eq(ObjectUtil.isNotEmpty(data.getArea()), PositionMapDO::getArea, data.getArea()));
        if (ObjectUtil.isEmpty(positionMapDOS)) {
            throw exception(PATH_PLANNING_NOT_EXISTS);
        }

        for (PositionMapDO positionMapDO : positionMapDOS) {

            List<PositionMapItemSynDTO> positionMapItemSynDTOS = positionMapItemMapper.getAllByMapId(positionMapDO.getId());
            if (ObjectUtil.isEmpty(positionMapItemSynDTOS)) {
                continue;
            }

            List<List<PositionMapItemSynDTO>> partition = Lists.partition(positionMapItemSynDTOS, znConfigConstant.getSynchronousAllMapNode());
            for (List<PositionMapItemSynDTO> nodes : partition) {
                PositionMapItemPathDTO relatedPathNode = new PositionMapItemPathDTO();
                relatedPathNode.setFloor(positionMapDO.getFloor());
                relatedPathNode.setArea(positionMapDO.getArea());
                relatedPathNode.setType(PathTypeEnum.INIT.getType());
                relatedPathNode.setControl_nodes(nodes);
//                commonApi.commonMethod(relatedPathNode, PathPlanningTopicConstant.SYNCHRONOUS_ALL_MAP_NODE);
                mqttUtils.pub(PathPlanningTopicConstant.SYNCHRONOUS_ALL_MAP_NODE,JSON.toJSONString(relatedPathNode));
            }
        }

        log.info("同步点位信息结束");
    }

    /**
     * 添加ware_position_map_line的点位信息
     *
     * @param data
     */
    @Override
    public void synchronousLineAdd(PositionMapLineSaveReqVO data) {
        if (ObjectUtil.isEmpty(data.getPositionMapId())) {
            throw exception0(PATH_PLANNING_NOT_EXISTS.getCode(), "请传positionMapId");
        }

        PositionMapDO positionMapDO = positionMapMapper.selectOne(new LambdaQueryWrapperX<PositionMapDO>()
                .eq(ObjectUtil.isNotEmpty(data.getPositionMapId()), PositionMapDO::getId, data.getPositionMapId()));

        if (ObjectUtil.isEmpty(positionMapDO)) {
            throw exception(PATH_PLANNING_NOT_EXISTS);
        }

        PositionMapLineDO positionMapLine = new PositionMapLineDO();
        positionMapLine.setId(data.getId());
        List<PositionMapLineDTO> positionMapLineDOS = positionMapLineMapper.getAllPositionMapLine(positionMapLine);

        if (ObjectUtil.isEmpty(positionMapLineDOS)) {
            throw exception(PATH_PLANNING_NOT_EXISTS);
        }

        List<PositionMapLineDTO> list = new ArrayList<>();
        for (PositionMapLineDTO positionMapLineDO : positionMapLineDOS) {
            if (DirectionEnum.ONE_WAY.getType().equals(positionMapLineDO.getDirection())) {
                positionMapLineDO.setPpToward(positionMapLineDO.getToward());
                list.add(positionMapLineDO);
            } else {
                positionMapLineDO.setPpToward(positionMapLineDO.changeTowardFirst(positionMapLineDO.getToward()));
                list.add(positionMapLineDO);
                list.add(setPositionMapLineDTOData(positionMapLineDO));
            }
        }

        PositionMapLinePathDTO relatedPathNode = new PositionMapLinePathDTO();
        relatedPathNode.setFloor(positionMapDO.getFloor());
        relatedPathNode.setArea(positionMapDO.getArea());
        relatedPathNode.setControl_nodes(list);

//        commonApi.commonMethod(relatedPathNode, PathPlanningTopicConstant.ADD_MAP_LINE);
        mqttUtils.pub(PathPlanningTopicConstant.ADD_MAP_LINE,JSON.toJSONString(relatedPathNode));
    }

    /**
     * 删除 ware_position_map_line的点位信息
     *
     * @param data
     */
    @Override
    public void synchronousLineDelete(PositionMapLineSaveReqVO data) {
        if (ObjectUtil.isEmpty(data.getPositionMapId())) {
            throw exception0(PATH_PLANNING_NOT_EXISTS.getCode(), "请传positionMapId");
        }

        PositionMapDO positionMapDO = positionMapMapper.selectOne(new LambdaQueryWrapperX<PositionMapDO>()
                .eq(ObjectUtil.isNotEmpty(data.getPositionMapId()), PositionMapDO::getId, data.getPositionMapId()));

        if (ObjectUtil.isEmpty(positionMapDO)) {
            throw exception(PATH_PLANNING_NOT_EXISTS);
        }

        PositionMapLinePathDTO relatedPathNode = new PositionMapLinePathDTO();
        relatedPathNode.setFloor(positionMapDO.getFloor());
        relatedPathNode.setArea(positionMapDO.getArea());
        relatedPathNode.setType(PathTypeEnum.DELETE.getType());

        List<Long> ids = new ArrayList<>();
        ids.add(data.getId());
        relatedPathNode.setIds(ids);

//        commonApi.commonMethod(relatedPathNode, PathPlanningTopicConstant.DELETE_MAP_LINE);
        mqttUtils.pub(PathPlanningTopicConstant.DELETE_MAP_LINE,JSON.toJSONString(relatedPathNode));
    }

    /**
     * 更新ware_position_map_line
     *
     * @param data
     */
    @Override
    public void synchronousLineUpdate(PositionMapLineSaveReqVO data) {
        if (ObjectUtil.isEmpty(data.getPositionMapId())) {
            throw exception0(PATH_PLANNING_NOT_EXISTS.getCode(), "请传positionMapId");
        }

        PositionMapDO positionMapDO = positionMapMapper.selectOne(new LambdaQueryWrapperX<PositionMapDO>()
                .eq(ObjectUtil.isNotEmpty(data.getPositionMapId()), PositionMapDO::getId, data.getPositionMapId()));

        if (ObjectUtil.isEmpty(positionMapDO)) {
            throw exception(PATH_PLANNING_NOT_EXISTS);
        }

        PositionMapLineDO positionMapLine = new PositionMapLineDO();
        positionMapLine.setId(data.getId());
        List<PositionMapLineDTO> positionMapLineDOS = positionMapLineMapper.getAllPositionMapLine(positionMapLine);

        if (ObjectUtil.isEmpty(positionMapLineDOS)) {
            throw exception(PATH_PLANNING_NOT_EXISTS);
        }

        List<PositionMapLineDTO> list = new ArrayList<>();
        for (PositionMapLineDTO positionMapLineDO : positionMapLineDOS) {
            if (DirectionEnum.ONE_WAY.getType().equals(positionMapLineDO.getDirection())) {
                positionMapLineDO.setPpToward(positionMapLineDO.getToward());
                list.add(positionMapLineDO);
            } else {
                positionMapLineDO.setPpToward(positionMapLineDO.changeTowardFirst(positionMapLineDO.getToward()));
                list.add(positionMapLineDO);
                list.add(setPositionMapLineDTOData(positionMapLineDO));
            }
        }

        PositionMapLinePathDTO relatedPathNode = new PositionMapLinePathDTO();
        relatedPathNode.setFloor(positionMapDO.getFloor());
        relatedPathNode.setArea(positionMapDO.getArea());
        relatedPathNode.setType(PathTypeEnum.UPDATE.getType());
        relatedPathNode.setControl_nodes(list);

//        commonApi.commonMethod(relatedPathNode, PathPlanningTopicConstant.UPDATE_MAP_LINE);
        mqttUtils.pub(PathPlanningTopicConstant.UPDATE_MAP_LINE,JSON.toJSONString(relatedPathNode));
    }

    /**
     * 添加ware_position_map_item
     *
     * @param data
     */
    @Override
    public void synchronousAllItemAdd(PositionMapItemSaveReqVO data) {
        if (ObjectUtil.isEmpty(data.getId())) {
            throw exception0(PATH_PLANNING_NOT_EXISTS.getCode(), "请传ware_position_map_item的id");
        }

        PositionMapItemDO positionMapItemDO = positionMapItemMapper.selectOne(new LambdaQueryWrapperX<PositionMapItemDO>()
                .eq(PositionMapItemDO::getId, data.getId()));

        if (ObjectUtil.isEmpty(positionMapItemDO)) {
            throw exception(PATH_PLANNING_NOT_EXISTS);
        }

        PositionMapDO positionMapDO = positionMapMapper.selectOne(new LambdaQueryWrapperX<PositionMapDO>()
                .eq(PositionMapDO::getId, positionMapItemDO.getPositionMapId()));

        if (ObjectUtil.isEmpty(positionMapDO)) {
            throw exception(PATH_PLANNING_NOT_EXISTS);
        }

        PositionMapItemSynDTO build = PositionMapItemSynDTO.builder()
                .type(Double.valueOf(positionMapItemDO.getType()))
                .x(Double.valueOf(positionMapItemDO.getActualLocationX()))
                .y(Double.valueOf(positionMapItemDO.getActualLocationY()))
                .sortNum(positionMapItemDO.getSortNum())
                .yaw(Double.parseDouble(positionMapItemDO.getLocationYaw()))
                .id(positionMapItemDO.getId()+"").build();

        PositionMapItemPathDTO PositionMapItemPathDTO = new PositionMapItemPathDTO();
        PositionMapItemPathDTO.setFloor(positionMapDO.getFloor());
        PositionMapItemPathDTO.setArea(positionMapDO.getArea());
        PositionMapItemPathDTO.setType(PathTypeEnum.ADD.getType());
        PositionMapItemPathDTO.setControl_nodes(Arrays.asList(build));

//        commonApi.commonMethod(PositionMapItemPathDTO, PathPlanningTopicConstant.ADD_MAP_NODE);
        mqttUtils.pub(PathPlanningTopicConstant.ADD_MAP_NODE,JSON.toJSONString(PositionMapItemPathDTO));
    }

    /**
     * 删除ware_position_map_item
     *
     * @param data
     */
    @Override
    public void synchronousItemDelete(PositionMapItemSaveReqVO data) {
        if (ObjectUtil.isEmpty(data.getId())) {
            throw exception0(PATH_PLANNING_NOT_EXISTS.getCode(), "请传ware_position_map_item的id");
        }

        PositionMapItemDO positionMapItemDO = positionMapItemMapper.selectOne(new LambdaQueryWrapperX<PositionMapItemDO>()
                .eq(PositionMapItemDO::getId, data.getId()));

        if (ObjectUtil.isEmpty(positionMapItemDO)) {
            throw exception(PATH_PLANNING_NOT_EXISTS);
        }

        PositionMapDO positionMapDO = positionMapMapper.selectOne(new LambdaQueryWrapperX<PositionMapDO>()
                .eq(PositionMapDO::getId, positionMapItemDO.getPositionMapId()));

        if (ObjectUtil.isEmpty(positionMapDO)) {
            throw exception(PATH_PLANNING_NOT_EXISTS);
        }

        PositionMapItemPathDTO PositionMapItemPathDTO = new PositionMapItemPathDTO();
        PositionMapItemPathDTO.setFloor(positionMapDO.getFloor());
        PositionMapItemPathDTO.setArea(positionMapDO.getArea());
        PositionMapItemPathDTO.setType(PathTypeEnum.DELETE.getType());
        PositionMapItemPathDTO.setIds(Arrays.asList(data.getId()));

//        commonApi.commonMethod(PositionMapItemPathDTO, PathPlanningTopicConstant.DELETE_MAP_NODE);
        mqttUtils.pub( PathPlanningTopicConstant.DELETE_MAP_NODE,JSON.toJSONString(PositionMapItemPathDTO));
    }

    @Override
    public void synchronousItemUpdate(PositionMapItemSaveReqVO data) {
        if (ObjectUtil.isEmpty(data.getId())) {
            throw exception0(PATH_PLANNING_NOT_EXISTS.getCode(), "请传ware_position_map_item的id");
        }

        PositionMapItemDO positionMapItemDO = positionMapItemMapper.selectOne(new LambdaQueryWrapperX<PositionMapItemDO>()
                .eq(PositionMapItemDO::getId, data.getId()));

        if (ObjectUtil.isEmpty(positionMapItemDO)) {
            throw exception(PATH_PLANNING_NOT_EXISTS);
        }

        PositionMapDO positionMapDO = positionMapMapper.selectOne(new LambdaQueryWrapperX<PositionMapDO>()
                .eq(PositionMapDO::getId, positionMapItemDO.getPositionMapId()));

        if (ObjectUtil.isEmpty(positionMapDO)) {
            throw exception(PATH_PLANNING_NOT_EXISTS);
        }

        PositionMapItemSynDTO build = PositionMapItemSynDTO.builder()
                .type(Double.valueOf(positionMapItemDO.getType()))
                .x(Double.valueOf(positionMapItemDO.getActualLocationX()))
                .y(Double.valueOf(positionMapItemDO.getActualLocationY()))
                .sortNum(positionMapItemDO.getSortNum())
                .yaw(Double.parseDouble(positionMapItemDO.getLocationYaw()))
                .id(positionMapItemDO.getId()+"").build();

        PositionMapItemPathDTO PositionMapItemPathDTO = new PositionMapItemPathDTO();
        PositionMapItemPathDTO.setFloor(positionMapDO.getFloor());
        PositionMapItemPathDTO.setArea(positionMapDO.getArea());
        PositionMapItemPathDTO.setType(PathTypeEnum.UPDATE.getType());
        PositionMapItemPathDTO.setControl_nodes(Arrays.asList(build));

//        commonApi.commonMethod(PositionMapItemPathDTO, PathPlanningTopicConstant.UPDATE_MAP_NODE);
        mqttUtils.pub(PathPlanningTopicConstant.UPDATE_MAP_NODE,JSON.toJSONString(PositionMapItemPathDTO));
    }

    @Override
    public void robotDimensions() {
        robotInformationService.sendRobotConfigToPP();
    }

    /**
     * 发送路径给车机
     *
     * @param message
     */
    @Override
    public void sendPosedsToRobot(String message) {
        PathToRobotDTO pathRobotDTO = JSON.parseObject(message, PathToRobotDTO.class);
        log.info("任务类型 :{}",pathRobotDTO.getTaskType());
        String mac = robotInformationService.getMacByRobotNo(pathRobotDTO.getRobotNo());
        String topic = RobotTopicConstant.ROBOT_TASK_MOVE_TOPIC + mac;
        if(RobotCommandTypeEnum.SWITCH_MAP.getType().equals(pathRobotDTO.getTaskType())) {
            switchMap(pathRobotDTO,topic);
        } /*else if (PathTaskTypeToRobotEnum.DROP_OFF_GOODS.getType().equals(pathRobotDTO.getTaskType())) {
            wordDropOffGoods(message,topic,pathRobotDTO.getRobotNo());
        } */else {
//            commonApi.commonMethodStr(message, topic);
            mqttUtils.pub(topic,message);
        }
    }

    /**
     * 适配不同车型的放货点
     * @param message
     * @param topic
     */
    private void wordDropOffGoods(String message, String topic, String robotNo) {
        RobotInformationDO robotInformation = robotInformationService.getInformationByRobotNo(robotNo);
        RobotModelDO model = modelService.getModel(robotInformation.getRobotModelId());
        if (String.valueOf(ZeroOneEnum.ZERO.getType()).equals(model.getRobotXOffset())
                && String.valueOf(ZeroOneEnum.ZERO.getType()).equals(model.getRobotYOffset())) {
//            commonApi.commonMethodStr(message, topic);
            mqttUtils.pub(topic,message);
            return;
        }
        PathToRobotChangeXYDTO pathToRobotChangeXY = JSON.parseObject(message, PathToRobotChangeXYDTO.class);
        List<PathToRobotChangeXYDataDTO> data = pathToRobotChangeXY.getData();
        boolean needOffset = false;
        PathToRobotChangeXYArgPoseDTO cargoPose = new PathToRobotChangeXYArgPoseDTO();
        for (PathToRobotChangeXYDataDTO item : data) {
            if (RobotCommandTypeEnum.WORK_DROP_OFF_GOODS.getType().equals(item.getCommandType()) && ZeroOneEnum.ONE.getType().equals(item.getIsCommandEnd())) {
                needOffset = true;
                cargoPose = item.getArg().getCargoPose();
            }
        }

        if (!needOffset) {
//            commonApi.commonMethodStr(message, topic);
            mqttUtils.pub(topic,message);
            return;
        }

        BigDecimal robotXOffset = new BigDecimal(model.getRobotXOffset());
        BigDecimal robotYOffset = new BigDecimal(model.getRobotYOffset());
        double sin = Math.sin(cargoPose.getYaw());
        double cos = Math.cos(cargoPose.getYaw());
        BigDecimal sino = new BigDecimal(String.valueOf(sin));
        BigDecimal coso = new BigDecimal(String.valueOf(cos));
        BigDecimal x1 = new BigDecimal(String.valueOf(cargoPose.getX()));
        BigDecimal y1 = new BigDecimal(String.valueOf(cargoPose.getY()));
        BigDecimal x = (coso.multiply(robotXOffset, MathContext.DECIMAL32)).subtract(sino.multiply(robotYOffset, MathContext.DECIMAL32)).add(x1);
        BigDecimal y = (sino.multiply(robotXOffset, MathContext.DECIMAL32)).add(coso.multiply(robotYOffset, MathContext.DECIMAL32)).add(y1);
        cargoPose.setX(x.doubleValue());
        cargoPose.setY(y.doubleValue());
        log.info("111111转换前数据 :{}",message);
        log.info("111111转换后 :{}",JSON.toJSONString(pathToRobotChangeXY));
//        commonApi.commonMethodStr(JSON.toJSONString(pathToRobotChangeXY), topic);
        mqttUtils.pub(topic,JSON.toJSONString(pathToRobotChangeXY));
    }

    /**
     * 统计时间使用
     * @param pathRobotDTO
     * @param topic
     */
    public void switchMap(PathToRobotDTO pathRobotDTO,String topic ) {
        String mapFileName = positionMapService.getMapFileNameByFloorAndAreaName(pathRobotDTO.getArg().getFloor(),pathRobotDTO.getArg().getAreaId());
        pathRobotDTO.getArg().setMapName(mapFileName);
        controllerInformationService.robotChangeSpeed(pathRobotDTO);
//        commonApi.commonMethodStr(JSON.toJSONString(pathRobotDTO), topic);
        mqttUtils.pub(topic,JSON.toJSONString(pathRobotDTO));
        RobotTaskDetailActionLogDO actionLog = taskDetailActionLogService.getLastTaskByRobotNo(pathRobotDTO.getRobotNo(),null);
        if (ObjectUtil.isNotEmpty(actionLog)) {
            //为了统计各个地图的工作时间
            RobotTaskDetailActionLogSaveReqVO data = new RobotTaskDetailActionLogSaveReqVO();
            data.setActionMsg(actionLog.getRobotNo() + " 切换为 " + pathRobotDTO.getArg().getFloor()+"层 " + pathRobotDTO.getArg().getAreaId());
            data.setRobotNo(actionLog.getRobotNo());
            data.setTaskDetailId(actionLog.getTaskDetailId());
            data.setTaskStage(actionLog.getTaskStage());
            data.setCommandType(actionLog.getCommandType());
            data.setTaskNo(actionLog.getTaskNo());
            data.setActionStatus(actionLog.getActionStatus());
            data.setStartTime(LocalDateTime.now());
            Long mapId = robotInformationService.getRobotMapIdByRobotNo(actionLog.getRobotNo());
            if (ObjectUtil.isNotEmpty(mapId)) {
                data.setPositionMapId(mapId);
            }
            taskDetailActionLogService.createTaskDetailActionLog(data);
        }
        if (ObjectUtil.isNotEmpty(actionLog) && ObjectUtil.isNotEmpty(actionLog.getStartTime())) {
            Duration duration = Duration.between(actionLog.getCreateTime(), LocalDateTime.now());
            actionLog.setDuration(duration.toMinutes());
            taskDetailActionLogService.updateTaskDetailActionLogs(Collections.singletonList(actionLog));
        }
    }

    /**
     * 实时行为上报
     * @param orderId
     * @param robotNo
     * @param behaviorId
     * @param isReach
     */
    @Override
    public void updateBehavior(String orderId, String robotNo, String behaviorId, String isReach) {
        TaskPathPlanningDTO build = TaskPathPlanningDTO.builder().orderId(orderId).robotNo(robotNo)
                .behaviorId(behaviorId).isReach(isReach).build();
        log.info("111111发给PP的数据 :{}",JSON.toJSONString(build));
//        commonApi.commonMethod(build, PathPlanningTopicConstant.UPDATE_BEHAVIOR);
        mqttUtils.pub(PathPlanningTopicConstant.UPDATE_BEHAVIOR,JSON.toJSONString(build));
    }

    /**
     * 车辆即将走的点位
     * @param message
     */
    @Override
    public void pathPlanningMovePose(String message) {
        log.info("111111路径规划车辆即将行走的点位 :{}",message);
        PathPlanningMovePoseVO robotStatusData = JSON.parseObject(message, PathPlanningMovePoseVO.class);
        String mac = robotInformationService.getMacByRobotNo(robotStatusData.getRobotNo());
        String floorAreaKey = RobotTaskChcheConstant.ROBOT_FLOOR_AREA + mac;
        Object floorAreaObject = redisUtil.get(floorAreaKey);
        FloorZoneDTO floorZoneDTO = JSONUtil.toBean((String) floorAreaObject, FloorZoneDTO.class);
        webSocketSenderApi.sendObject(floorZoneDTO.getFloor() + CommonConstant.SYMBOL + floorZoneDTO.getArea(),
                WebSocketConstant.PLANNING_MOVE_POSE, message);
    }

    /**
     * 仿真点位信息
     */
    @Override
    public void simulationRobotPoseRequest() {

        if (!znConfigConstant.getSendRobotInitPose()) {
            log.info("不同步初始点位信息");
            return;
        }

        CommonConfigDO config = configService.getConfig(CommandConfigTypeEnum.SIMULATION_CONFIG.getType().longValue());
        if (ObjectUtil.isEmpty(config)) {
            log.info("未配置默认地图");
            return;
        }
        PositionMapDO positionMap = null;
        if (ObjectUtil.isEmpty(config) || ObjectUtil.isEmpty(config.getConfigStr())) {
            List<PositionMapDO> maps = positionMapService.getAllMap();
            if (ObjectUtil.isEmpty(maps)) {
                return;
            }
            positionMap = maps.get(0);
        }else {
            positionMap = positionMapService.getPositionMap(Long.valueOf(config.getConfigStr()));
        }
        if (ObjectUtil.isEmpty(positionMap)) {
            log.info("仿真环境没有地图信息");
            return;
        }

        List<RobotInformationDO> robots = robotInformationService.getAllRobot();

        if (ObjectUtil.isEmpty(robots)) {
            log.info("仿真环境没有机器人信息");
            return;
        }

        List<PositionMapItemDO> itemDOList = new LinkedList<>();
        List<PositionMapItemDO> items = positionMapItemService.getPositionMapItemByMapAndType(positionMap.getId(), PositionMapItemEnum.STOP.getType());
        if (ObjectUtil.isNotEmpty(items)) {
            itemDOList.addAll(items);
        }
        if ( itemDOList.size() < robots.size()) {
            List<PositionMapItemDO> itemPoses = positionMapItemService.getPositionMapItemByMapAndType(positionMap.getId(), PositionMapItemEnum.PATH.getType());
            itemDOList.addAll(itemPoses);
        }

        List<SimulationRobotPoseDTO> simulationList = new ArrayList<>();
        int i = 0;
        for (RobotInformationDO robot : robots) {
            SimulationRobotPoseDTO simulationRobotPoseDTO = new SimulationRobotPoseDTO();
            simulationRobotPoseDTO.setMac(robot.getMacAddress());
            if (itemDOList.size() <= i) {
                continue;
            }
            simulationRobotPoseDTO.setLocationX(itemDOList.get(i).getLocationX());
            simulationRobotPoseDTO.setLocationY(itemDOList.get(i).getLocationY());
            simulationRobotPoseDTO.setActualLocationX(itemDOList.get(i).getActualLocationX());
            simulationRobotPoseDTO.setActualLocationY(itemDOList.get(i).getActualLocationY());
            simulationRobotPoseDTO.setLocationYaw(itemDOList.get(i).getLocationYaw());
            simulationRobotPoseDTO.setFloor(positionMap.getFloor()+"");
            simulationRobotPoseDTO.setArea(positionMap.getArea());

            String floorAreaKey = RobotTaskChcheConstant.ROBOT_FLOOR_AREA + robot.getMacAddress();
            FloorZoneDTO floorZoneDTO = new FloorZoneDTO();
            floorZoneDTO.setFloor(simulationRobotPoseDTO.getFloor());
            floorZoneDTO.setArea(simulationRobotPoseDTO.getArea());
            redisUtil.set(floorAreaKey, JSON.toJSONString(floorZoneDTO));

            simulationList.add(simulationRobotPoseDTO);
            i++;
        }
//        commonApi.commonMethod(simulationList, PathPlanningTopicConstant.INIT_ROBOT_POSE);
        mqttUtils.pub( PathPlanningTopicConstant.INIT_ROBOT_POSE,JSON.toJSONString(simulationList));
    }

    /**
     * 路网匹配实时数据
     * @param message
     */
    @Override
    public void graphMatchData(String message) {
        GraphMatchDataDTO graphMatchData = JSON.parseObject(message, GraphMatchDataDTO.class);
        String key = PathPlanningChcheConstant.PATH_PLANNING_GRAPH_MATCH + graphMatchData.getRobotNo();
        redisUtil.set(key,graphMatchData.getIsMatched());
    }

    @Override
    public void synchronousStart() {
//        commonApi.commonMethodStr("开始同步点位信息", PathPlanningTopicConstant.SYNCHRONOUS_ALL_MAP_INIT);
        mqttUtils.pub(PathPlanningTopicConstant.SYNCHRONOUS_ALL_MAP_INIT,"开始同步点位信息");
        try {
            Thread.sleep(1000);
        } catch (InterruptedException e) {
            throw new RuntimeException(e);
        }
    }

    @Override
    public void sendPositionChangePointBinding() {
        List<PositionAllChangePointBindingDTO> bindingDTOS = positionChangePointBindingService.getAllPositionChangePointBinding();
        if (ObjectUtil.isNotEmpty(bindingDTOS)) {
//            commonApi.commonMethod(bindingDTOS, PathPlanningTopicConstant.SYNCHRONOUS_ALL_SWITCH_POINT_BINDING);
            mqttUtils.pub(PathPlanningTopicConstant.SYNCHRONOUS_ALL_SWITCH_POINT_BINDING,JSON.toJSONString(bindingDTOS));
        }
    }

    public PositionMapLineDTO setPositionMapLineDTOData(PositionMapLineDTO positionMapLineDO) {
        PositionMapLineDTO build = PositionMapLineDTO.builder()
                .id(positionMapLineDO.getId())
                .positionMapId(positionMapLineDO.getPositionMapId())
                .startingPointId(positionMapLineDO.getEndPointId())
                .endPointId(positionMapLineDO.getStartingPointId())
                .direction(1)
                .forwardSpeedLimit(positionMapLineDO.getForwardSpeedLimit())
                .reverseSpeedLimit(positionMapLineDO.getReverseSpeedLimit())
                .beginControlX(positionMapLineDO.getEndControlX())
                .beginControlY(positionMapLineDO.getEndControlY())
                .endControlX(positionMapLineDO.getBeginControlX())
                .endControlY(positionMapLineDO.getBeginControlY())
                .expansionZoneFront(positionMapLineDO.getExpansionZoneAfter())
                .expansionZoneAfter(positionMapLineDO.getExpansionZoneFront())
                .expansionZoneLeft(positionMapLineDO.getExpansionZoneRight())
                .expansionZoneRight(positionMapLineDO.getExpansionZoneLeft())
                .ppToward(positionMapLineDO.changeToward(positionMapLineDO.getToward()))
                .method(positionMapLineDO.getMethod())
                .actualEndControlX(positionMapLineDO.getActualBeginControlX())
                .actualEndControlY(positionMapLineDO.getActualBeginControlY())
                .actualBeginControlX(positionMapLineDO.getActualEndControlX())
                .actualBeginControlY(positionMapLineDO.getActualEndControlY())
                .actualEndPointX(positionMapLineDO.getActualStartPointX())
                .actualEndPointY(positionMapLineDO.getActualStartPointY())
                .actualStartPointX(positionMapLineDO.getActualEndPointX())
                .actualStartPointY(positionMapLineDO.getActualEndPointY())
                .collisionAvoidanceFlag(positionMapLineDO.getCollisionAvoidanceFlag())
                .build();
        return build;
    }

    /**
     * 组装数据发送到PP
     *
     * @param relatedPathNode
     * @param positionMapLineDOS
     */
    private void assembleDataToPP(PositionMapLinePathDTO relatedPathNode, List<PositionMapLineDTO> positionMapLineDOS) {

        List<PositionMapLineDTO> list = new ArrayList<>();
        for (PositionMapLineDTO positionMapLineDO : positionMapLineDOS) {
            if (DirectionEnum.ONE_WAY.getType().equals(positionMapLineDO.getDirection())) {
                positionMapLineDO.setPpToward(positionMapLineDO.getToward());
                list.add(positionMapLineDO);
            } else {
                positionMapLineDO.setPpToward(positionMapLineDO.changeTowardFirst(positionMapLineDO.getToward()));
                list.add(positionMapLineDO);
                list.add(setPositionMapLineDTOData(positionMapLineDO));
            }
        }

        relatedPathNode.setControl_nodes(list);

//        commonApi.commonMethod(relatedPathNode, PathPlanningTopicConstant.SYNCHRONOUS_ALL_MAP_LINE);
        mqttUtils.pub(PathPlanningTopicConstant.SYNCHRONOUS_ALL_MAP_LINE,JSON.toJSONString(relatedPathNode));
    }


}
