graphite-digester-service/src/main/java/com/iflytop/gd/app/service/DeviceCommandService.java

package com.iflytop.gd.app.service;


import cn.hutool.json.JSONObject;
import com.iflytop.gd.app.core.DebugGenerator;
import com.iflytop.gd.common.cmd.CommandFuture;
import com.iflytop.gd.common.cmd.CyclicNumberGenerator;
import com.iflytop.gd.common.cmd.DeviceCommandBundle;
import com.iflytop.gd.common.constant.CommandStatus;
import com.iflytop.gd.hardware.HardwareService;
import jakarta.annotation.PostConstruct;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.BlockingQueue;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.ConcurrentMap;
import java.util.concurrent.LinkedBlockingQueue;

@Slf4j
@Service
@RequiredArgsConstructor
public class DeviceCommandService {
    private final HardwareService hardwareService;
    private final WebSocketService webSocketService;
    private final DeviceStateService deviceStateService;
    /**
     * 需要等待加液区空闲的龙门架机械臂指令
     */
    private final ConcurrentMap<Integer, CommandFuture> sendCommandFutureMap = new ConcurrentHashMap<>();
    private final BlockingQueue<CommandFuture[]> gantryCommandQueue = new LinkedBlockingQueue<>();

    @PostConstruct
    private void initExecutorThread() {
        new Thread(this::executeCommands).start();
    }

    private void executeCommands() {
        while (true) {
            try {
                deviceStateService.setGantryArmStateIdle(true);
                CommandFuture[] commandFutureArray = gantryCommandQueue.take();
                for (CommandFuture commandFuture : commandFutureArray) {
                    executeCommand(commandFuture);
                }
            } catch (Exception e) {
                Thread.currentThread().interrupt();
            } finally {
                deviceStateService.setGantryArmStateIdle(false);
            }
        }
    }

    public synchronized CommandFuture[] sendCommandGantryQueue(DeviceCommandBundle... deviceCommandBundles) {
        return sendCommandGantryQueue(null, null, deviceCommandBundles);
    }

    public synchronized CommandFuture[] sendCommandGantryQueue(String cmdId, String cmdCode, DeviceCommandBundle... deviceCommandBundles) {
        List<CommandFuture> commandFutureList = new ArrayList<>();
        for (DeviceCommandBundle deviceCommandBundle : deviceCommandBundles) {
            commandFutureList.add(createDeviceCommandFuture(cmdId, cmdCode, deviceCommandBundle));
        }
        CommandFuture[] commandFutureArray = commandFutureList.toArray(new CommandFuture[0]);
        try {
            gantryCommandQueue.put(commandFutureArray);
        } catch (Exception e) {
            log.error("设备指令入队列失败", e);
            throw new RuntimeException(e);
        }
        return commandFutureArray;
    }

    /**
     * 根据 DeviceCommand 创建 CommandFuture
     */
    private CommandFuture createDeviceCommandFuture(String cmdId, String cmdCode, DeviceCommandBundle deviceCommandBundle) {
        CommandFuture commandFuture = createDeviceCommandFuture(deviceCommandBundle);
        commandFuture.setCmdId(cmdId);
        commandFuture.setCmdCode(cmdCode);
        return commandFuture;
    }

    /**
     * 根据 DeviceCommand 创建 CommandFuture
     */
    private CommandFuture createDeviceCommandFuture(DeviceCommandBundle deviceCommandBundle) {
        CommandFuture commandFuture = new CommandFuture();
        commandFuture.setDeviceCommandBundle(deviceCommandBundle);
        commandFuture.getResponseFuture().whenComplete((result, ex) -> {
            sendCommandFutureMap.remove(deviceCommandBundle.getDeviceCommand().getCmdId());
        });
        return commandFuture;
    }

    public void executeCommand(CommandFuture commandFuture) {
        int cmdId = CyclicNumberGenerator.getInstance().generateNumber();
        commandFuture.getDeviceCommandBundle().getDeviceCommand().setCmdId(cmdId);
        sendCommandFutureMap.put(cmdId, commandFuture);
        commandFuture.setStartSendTime(System.currentTimeMillis());
        if (!deviceStateService.getDeviceState().isVirtual()) {
            if (!hardwareService.sendCommand(commandFuture.getDeviceCommandBundle().getDeviceCommand())) {
                sendCommandFutureMap.remove(commandFuture.getDeviceCommandBundle().getDeviceCommand().getCmdId());
                throw new RuntimeException("向设备发送指令失败");
            }
        } else {
            //虚拟模式
            new Thread(() -> {
                try {
                    String actionName = commandFuture.getDeviceCommandBundle().getDeviceCommand().getAction().name();
                    if (actionName.contains("move") || actionName.contains("origin")) {
                        Thread.sleep(300);
                    }
                    JSONObject jsonObject = new JSONObject();
                    jsonObject.putOnce("cmdId", cmdId);
                    jsonObject.putOnce("success", true);
                    completeCommandResponse(jsonObject);
                } catch (InterruptedException e) {
                    // 处理中断异常
                    Thread.currentThread().interrupt();
                }
            }).start();
        }
        if (commandFuture.getCmdId() != null) {
            webSocketService.pushDebug(DebugGenerator.generateJson(commandFuture.getCmdId(), commandFuture.getCmdCode(), CommandStatus.DEVICE_SEND, commandFuture.getDeviceCommandBundle().getCmdName() + "指令，已发给设备", commandFuture.getDeviceCommandBundle()));
        }

    }

    public CommandFuture sendCommand(DeviceCommandBundle deviceCommand) {
        CommandFuture commandFuture = createDeviceCommandFuture(deviceCommand);
        executeCommand(commandFuture);
        return commandFuture;
    }

    public CommandFuture sendCommand(String cmdId, String cmdCode, DeviceCommandBundle deviceCommandBundle) {
        CommandFuture commandFuture = createDeviceCommandFuture(cmdId, cmdCode, deviceCommandBundle);
        executeCommand(commandFuture);
        return commandFuture;
    }

    public void completeCommandResponse(JSONObject deviceResult) {
        Integer cmdId = deviceResult.getInt("cmdId");
        if (cmdId != null) {
            CommandFuture future = sendCommandFutureMap.get(cmdId);
            if (future != null) {
                future.setEndSendTime(System.currentTimeMillis());
                Boolean success = deviceResult.getBool("success"); //数据验证
                if (success == null || !success) { //response失败
                    if (future.getCmdId() != null) {
                        webSocketService.pushDebug(DebugGenerator.generateJson(future.getCmdId(), future.getCmdCode(), CommandStatus.DEVICE_ERROR,
                                future.getDeviceCommandBundle().getCmdName() + "指令，设备response错误，耗时:" + (future.getEndSendTime() - future.getStartSendTime()), deviceResult));
                    }
                    future.completeResponseExceptionally(new RuntimeException("response失败:" + deviceResult));
                } else {
                    if (future.getCmdId() != null) {
                        webSocketService.pushDebug(DebugGenerator.generateJson(future.getCmdId(), future.getCmdCode(), CommandStatus.DEVICE_RESULT,
                                future.getDeviceCommandBundle().getCmdName() + "指令，设备response正常，耗时:" + (future.getEndSendTime() - future.getStartSendTime()), deviceResult));
                    }
                    future.completeResponse(deviceResult);
                }
            }
        }
    }

    /**
     * 取消等待中的future并从map中移除
     */
    public synchronized void releaseAllCommandFutures() {
        for (Integer key : sendCommandFutureMap.keySet()) {
            CommandFuture future = sendCommandFutureMap.remove(key);
            if (future != null) {
                future.getResponseFuture().cancel(true);
            }
        }
    }

}