CMVR-IOT/cmvr-iot-resource/src/main/java/com/cmvr/resource/service/ResourceRobotControlService.java

package com.cmvr.resource.service;

import cmvr.msgs.Agv;
import cmvr.quic_edge.v1.QuicEdge;
import com.cmvr.common.exception.GlobalException;
import com.cmvr.edge.client.model.EdgeCommonVO;
import com.cmvr.edge.client.service.EdgeAgvService;
import com.cmvr.resource.domain.ResourceDevice;
import com.cmvr.resource.domain.ResourceMap;
import com.cmvr.resource.domain.ResourceRobot;
import com.cmvr.resource.mapper.ResourceDeviceMapper;
import com.cmvr.resource.mapper.ResourceMapMapper;
import com.cmvr.resource.mapper.ResourceRobotMapper;
import lombok.RequiredArgsConstructor;
import org.apache.commons.lang3.StringUtils;
import org.springframework.stereotype.Service;

import java.util.List;
import java.util.Locale;
import java.util.stream.Collectors;

@Service
@RequiredArgsConstructor
public class ResourceRobotControlService {
    private final ResourceRobotMapper robotMapper;
    private final ResourceDeviceMapper deviceMapper;
    private final ResourceMapMapper mapMapper;
    private final ResourceRobotMapService robotMapService;
    private final EdgeAgvService edgeAgvService;

    public List<String> listRemoteMaps(String robotRef) {
        return edgeAgvService.listMaps(buildAgvRequest(requireRobot(robotRef)));
    }

    public int bindMap(String robotRef, String mapId) {
        ResourceRobot robot = requireRobot(robotRef);
        if (mapMapper.selectById(mapId) == null) {
            throw new GlobalException("地图不存在：" + mapId);
        }
        return robotMapService.bind(robot.getRobotId(), mapId, true);
    }

    public String downloadMap(String robotRef, String mapName) {
        return edgeAgvService.downloadMap(buildAgvRequest(requireRobot(robotRef)), mapName);
    }

    public String uploadActiveMap(String robotRef) {
        ResourceRobot target = requireRobot(robotRef);
        ResourceMap map = mapMapper.selectById(target.getCurrentMapId());
        if (map == null) {
            throw new GlobalException("机器人未绑定有效地图");
        }
        String sourceRobotId = StringUtils.trimToNull(map.getSourceRobotId());
        String sourceMapName = StringUtils.trimToNull(map.getSourceMapName());
        if (sourceRobotId == null || sourceMapName == null) {
            throw new GlobalException("地图未配置来源机器人或来源地图名称");
        }
        ResourceRobot source = requireRobot(sourceRobotId);
        String content = edgeAgvService.downloadMap(buildAgvRequest(source), sourceMapName);
        if (StringUtils.isBlank(content)) {
            throw new GlobalException("下载的地图内容为空");
        }
        edgeAgvService.uploadMap(buildAgvRequest(target), sourceMapName, content);
        return "上传成功";
    }

    public ResourceRobot syncRuntimeState(String robotRef) {
        ResourceRobot robot = requireRobot(robotRef);
        try {
            Agv.AgvRuntimeState runtime = edgeAgvService.getRuntimeState(buildAgvRequest(robot));
            if (runtime == null) {
                throw new GlobalException("AGV未返回运行状态");
            }
            if (runtime.hasBattery()) {
                int percentage = (int) Math.round(runtime.getBattery().getPercentage() * 100D);
                robot.setBatteryLevel(Math.max(0, Math.min(100, percentage)));
            }
            if (runtime.hasPose()) {
                Agv.AgvPose2d pose = runtime.getPose();
                robot.setCurrentPosition(String.format(Locale.ROOT, "%.3f,%.3f,%.3f",
                        pose.getX(), pose.getY(), pose.getTheta()));
            }
            robot.setStatus(String.valueOf(runtime.getMode()));
            robotMapper.updateAgvRuntimeStatus(robot);
            return robot;
        } catch (GlobalException error) {
            throw error;
        } catch (Exception error) {
            robot.setStatus("0");
            robotMapper.updateAgvRuntimeStatus(robot);
            String detail = StringUtils.isBlank(error.getMessage())
                    ? error.getClass().getSimpleName() : error.getMessage();
            throw new GlobalException("获取机器人运行状态失败：" + detail, error);
        }
    }

    public Agv.AgvPose2d getRealtimeLocation(String robotRef) {
        Agv.AgvRuntimeState runtime = edgeAgvService.getRuntimeState(
                buildAgvRequest(requireRobot(robotRef)));
        return runtime.hasPose() ? runtime.getPose() : null;
    }

    public void navigateToPosition(String robotRef, Double x, Double y, Double theta) {
        if (x == null || y == null) {
            throw new GlobalException("X和Y坐标不能为空");
        }
        Agv.AgvPose2d pose = Agv.AgvPose2d.newBuilder()
                .setX(x)
                .setY(y)
                .setTheta(theta == null ? 0D : theta)
                .build();
        edgeAgvService.navigateToPose(buildAgvRequest(requireRobot(robotRef)), pose);
    }

    public String startMapping(String robotRef, int dimension, String mapName, boolean realTime) {
        Agv.AgvMapDimension mapDimension = Agv.AgvMapDimension.forNumber(dimension);
        if (mapDimension == null) {
            throw new GlobalException("无效的建图维度：" + dimension);
        }
        return edgeAgvService.startMapping(
                buildAgvRequest(requireRobot(robotRef)), mapDimension, mapName, realTime);
    }

    public void stopMapping(String robotRef) {
        edgeAgvService.stopMapping(buildAgvRequest(requireRobot(robotRef)));
    }

    private ResourceRobot requireRobot(String robotRef) {
        String normalized = StringUtils.trimToNull(robotRef);
        if (normalized == null) {
            throw new GlobalException("机器人标识不能为空");
        }
        ResourceRobot robot = robotMapper.selectById(normalized);
        if (robot == null) {
            robot = robotMapper.selectByRobotIdentity(normalized);
        }
        if (robot == null || "1".equals(robot.getArchivedStatus())) {
            throw new GlobalException("机器人不存在：" + normalized);
        }
        return robot;
    }

    private EdgeCommonVO buildAgvRequest(ResourceRobot robot) {
        List<ResourceDevice> agvDevices = deviceMapper.selectByRobotId(
                robot.getRobotId(), QuicEdge.DeviceKind.DEVICE_KIND_AGV_VALUE, true);
        if (agvDevices.isEmpty()) {
            List<ResourceDevice> onlineDevices = deviceMapper.selectByRobotId(robot.getRobotId(), null, true);
            if (onlineDevices.isEmpty()) {
                throw new GlobalException("机器人当前没有在线设备：" + robot.getRobotId());
            }
            String summary = onlineDevices.stream()
                    .map(device -> device.getDeviceId() + "(" + deviceKindName(device.getDeviceKind()) + ")")
                    .collect(Collectors.joining("、"));
            throw new GlobalException("机器人在线设备中没有AGV，当前设备：" + summary);
        }
        if (agvDevices.size() > 1) {
            String ids = agvDevices.stream().map(ResourceDevice::getDeviceId)
                    .collect(Collectors.joining("、"));
            throw new GlobalException("机器人存在多个在线AGV，无法确定默认设备：" + ids);
        }
        EdgeCommonVO request = new EdgeCommonVO();
        request.setRobotId(robot.getRobotId());
        request.setDeviceId(agvDevices.get(0).getDeviceId());
        return request;
    }

    private String deviceKindName(Integer deviceKind) {
        if (deviceKind == null) {
            return "未知类型";
        }
        QuicEdge.DeviceKind kind = QuicEdge.DeviceKind.forNumber(deviceKind);
        return kind == null ? "未知类型" + deviceKind : kind.name().replace("DEVICE_KIND_", "");
    }
}