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

import cn.hutool.core.util.ObjectUtil;
import cn.iocoder.yudao.framework.mybatis.core.query.LambdaQueryWrapperX;
import cn.iocoder.yudao.module.mqtt.api.path.PathPlanningApi;
import cn.iocoder.yudao.module.mqtt.api.path.dto.*;
import cn.iocoder.yudao.module.system.constant.path.PathPlanningTopicConstant;
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.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.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.line.DirectionEnum;
import cn.iocoder.yudao.module.system.enums.path.PathTypeEnum;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

import javax.annotation.Resource;
import java.util.ArrayList;
import java.util.Arrays;
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;

//    public static ExecutorService executor = Executors.newFixedThreadPool(5);

    @Resource
    private PathPlanningApi pathPlanningApi;

    @Resource
    private PositionMapItemMapper positionMapItemMapper;

    @Resource
    private RobotInformationMapper informationMapper;

    /**
     * 同步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);

//        List<PositionMapLineDO> positionLines = positionMapLineMapper.selectList(new LambdaQueryWrapperX<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);
        }

        pathPlanningApi.synchronousLineObject(new PositionMapLineDTO(), PathPlanningTopicConstant.SYNCHRONOUS_ALL_MAP_LINE_END);
    }

    /**
     * 同步ware_position_map_item的点位信息
     *
     * @param data
     */
    @Override
    public void synchronousAllItem(PositionMapSaveReqVO data) {
        log.info("synchronousAllItem----start");
        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;
            }
            PositionMapItemPathDTO relatedPathNode = new PositionMapItemPathDTO();
            relatedPathNode.setFloor(positionMapDO.getFloor());
            relatedPathNode.setArea(positionMapDO.getArea());
            relatedPathNode.setType(PathTypeEnum.INIT.getType());
            relatedPathNode.setControl_nodes(positionMapItemSynDTOS);

            log.info("synchronousAllItem----doing");
            pathPlanningApi.synchronousLineObject(relatedPathNode, PathPlanningTopicConstant.SYNCHRONOUS_ALL_MAP_NODE);
        }

        log.info("synchronousAllItem----end");
    }

    /**
     * 添加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) {
            list.add(positionMapLineDO);
            if (DirectionEnum.ONE_WAY.getType().equals(positionMapLineDO.getDirection())) {
                positionMapLineDO.setPpToward(positionMapLineDO.changeToward(positionMapLineDO.getToward()));
            } else {
                list.add(setPositionMapLineDTOData(positionMapLineDO));
            }
        }

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

        pathPlanningApi.synchronousLineObject(relatedPathNode, PathPlanningTopicConstant.ADD_MAP_LINE);
    }

    /**
     * 删除 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);

        pathPlanningApi.synchronousLineObject(relatedPathNode, PathPlanningTopicConstant.DELETE_MAP_LINE);

    }

    /**
     * 更新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) {
            list.add(positionMapLineDO);
            if (DirectionEnum.ONE_WAY.getType().equals(positionMapLineDO.getDirection())) {
                positionMapLineDO.setPpToward(positionMapLineDO.changeToward(positionMapLineDO.getToward()));
            } else {
                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);

        pathPlanningApi.synchronousLineObject(relatedPathNode, PathPlanningTopicConstant.UPDATE_MAP_LINE);
    }

    /**
     * 添加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));

        pathPlanningApi.synchronousLineObject(PositionMapItemPathDTO, PathPlanningTopicConstant.ADD_MAP_NODE);
    }

    /**
     * 删除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()));

        pathPlanningApi.synchronousLineObject(PositionMapItemPathDTO, PathPlanningTopicConstant.DELETE_MAP_NODE);
    }

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

        pathPlanningApi.synchronousLineObject(PositionMapItemPathDTO, PathPlanningTopicConstant.UPDATE_MAP_NODE);
    }

    @Override
    public void robotDimensions() {
        List<RobotDimensionsDTO> list = informationMapper.selectRobotDimensions();
        pathPlanningApi.synchronousLineObject(list, PathPlanningTopicConstant.SEND_ROBOT_DIMENSIONS);
    }

    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.getReverseSpeedLimit())
                .reverseSpeedLimit(positionMapLineDO.getForwardSpeedLimit())
                .beginControlX(positionMapLineDO.getEndControlX())
                .beginControlY(positionMapLineDO.getEndControlY())
                .endControlX(positionMapLineDO.getBeginControlX())
                .endControlY(positionMapLineDO.getBeginControlY())
                .expansionZoneFront(positionMapLineDO.getExpansionZoneAfter())
                .expansionZoneAfter(positionMapLineDO.getExpansionZoneFront())
                .expansionZoneLeft(positionMapLineDO.getExpansionZoneRight())
                .expansionZoneRight(positionMapLineDO.getExpansionZoneLeft())
                .toward(positionMapLineDO.changeToward(positionMapLineDO.getToward()))
                .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) {
            list.add(positionMapLineDO);
            if (DirectionEnum.ONE_WAY.getType().equals(positionMapLineDO.getDirection())) {
                positionMapLineDO.setPpToward(positionMapLineDO.changeToward(positionMapLineDO.getToward()));
            } else {
                list.add(setPositionMapLineDTOData(positionMapLineDO));
            }
        }

        relatedPathNode.setControl_nodes(list);

        pathPlanningApi.synchronousPointToPP(relatedPathNode, PathPlanningTopicConstant.SYNCHRONOUS_ALL_MAP_LINE);

    }


}
