matrix-spray/src/main/java/com/qyft/ms/app/service/CMDService.java

package com.qyft.ms.app.service;

import cn.hutool.json.JSONUtil;
import com.alibaba.fastjson.JSON;
import com.qyft.ms.app.common.constant.WebSocketMessageType;
import com.qyft.ms.app.common.generator.PathGenerator;
import com.qyft.ms.app.common.result.CMDResultCode;
import com.qyft.ms.app.model.entity.OperationLog;
import com.qyft.ms.app.model.entity.SysSettings;
import com.qyft.ms.app.model.form.CMDForm;
import com.qyft.ms.app.model.vo.ExecutionResult;
import com.qyft.ms.device.controller.DeviceController;
import com.qyft.ms.device.model.bo.DeviceStatus;
import com.qyft.ms.device.service.DeviceStatusService;
import com.qyft.ms.device.service.DeviceTcpCMDService;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

import java.util.*;
import java.util.List;
import java.util.function.Supplier;

import static com.qyft.ms.app.common.generator.PathGenerator.generatePathPoints;

@Slf4j
@RequiredArgsConstructor
@Service
public class CMDService {

    private final WebSocketService webSocketService;
    private final DeviceTcpCMDService deviceTcpCMDService;
    private final OperationLogService operationLogService;
    private final MatrixCraftService matrixCraftService;
    private final ISysSettingsService sysSettingsService;
    private final DeviceStatusService deviceStatusService;
    private volatile boolean running = true;


    private void initExecutorThread(List<Supplier<Boolean>> cmdList, CMDForm form) {
        new Thread(() -> run(cmdList, form)).start();
    }

// 电机移动
    public boolean moveMotorToPosition(CMDForm form) {
        Map<String, Object> params = form.getParams();
        if (params == null || !params.containsKey("axis") || !params.containsKey("position")) {
            return false;
        }
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        String axis = (String) params.get("axis");
        double position = Optional.ofNullable(params.get("position"))
                .filter(Number.class::isInstance)
                .map(Number.class::cast)
                .map(Number::doubleValue)
                .orElse(0.0);
        cmdList.add(() -> deviceTcpCMDService.moveMotorToPosition(axis, position));
        initExecutorThread(cmdList, form);
        return true;
    };

    // 切换清洗管路/喷涂管路
    public boolean switchThreeWayValve(CMDForm form) {
        Map<String, Object> params = form.getParams();
        if (params == null || !params.containsKey("type")) {
            return false;
        }
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        String type = (String) params.get("type");
        if (type.equals("clean")) {
            cmdList.add(deviceTcpCMDService::switchThreeWayValveToSubstrate);
        } else if (type.equals("spray")) {
            cmdList.add(deviceTcpCMDService::switchThreeWayValveToSpray);
        }else {
            return false;
        }
        initExecutorThread(cmdList, form);
        return true;
    }

    // 除湿阀、清洗阀、喷嘴阀控制方法
    public boolean controlValve(CMDForm form) {
        Map<String, Object> params = form.getParams();
        if (params == null || !params.containsKey("valveType") || !params.containsKey("isOpen")) {
           return false;
        }
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
       if(params.get("valveType") !=null && params.get("isOpen") !=null) {
           String valveType = (String) params.get("valveType");
           boolean isOpen = (boolean) params.get("isOpen");
           cmdList.add(() -> deviceTcpCMDService.controlValve(valveType,isOpen));
       }
        initExecutorThread(cmdList, form);
        return true;
    }

    // 以指定电压值开启高压电
    public boolean turnOnHighVoltage(CMDForm form) {
        Map<String, Object> params = form.getParams();
        if (params == null || !params.containsKey("voltage")) {
            return false;
        }
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        int voltage = (int) params.get("voltage");
        if(voltage> 5000) {
            return false;
        }
            cmdList.add(() -> deviceTcpCMDService.turnOnHighVoltage(voltage));
        initExecutorThread(cmdList, form);
        return true;
    }

    // 关闭高压电
    public boolean turnOffHighVoltage(CMDForm form) {
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        cmdList.add(deviceTcpCMDService::turnOffHighVoltage);
        initExecutorThread(cmdList, form);
        return true;
    }

    // 以指定转速开启注射泵
    public boolean turnOnSyringePump(CMDForm form) {
        Map<String, Object> params = form.getParams();
        if (params == null || !params.containsKey("rotationSpeed")) {
            return false;
        }
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        double rotationSpeed = Optional.ofNullable(params.get("rotationSpeed"))
                .filter(Number.class::isInstance)
                .map(Number.class::cast)
                .map(Number::doubleValue)
                .orElse(0.0);
        cmdList.add(() -> deviceTcpCMDService.turnOnSyringePump(rotationSpeed));
        initExecutorThread(cmdList, form);
        return true;
    }

    // 停止注射泵
    public boolean turnOffSyringePump(CMDForm form) {
        Map<String, Object> params = form.getParams();
        if (params == null) {
            return false;
        }
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        cmdList.add(deviceTcpCMDService::turnOffSyringePump);
        initExecutorThread(cmdList, form);
        return true;
    }

    // 设置指定轴的电机运行时电流
    public boolean setMotorRunningCurrent(CMDForm form) {
        Map<String, Object> params = form.getParams();
        if (params == null || !params.containsKey("axis") || !params.containsKey("current")) {
            return false;
        }
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        String axis = (String) params.get("axis");
        double current = Optional.ofNullable(params.get("current"))
                .filter(Number.class::isInstance)
                .map(Number.class::cast)
                .map(Number::doubleValue)
                .orElse(0.0);
        cmdList.add(() -> deviceTcpCMDService.setMotorRunningCurrent(axis, current));
        initExecutorThread(cmdList, form);
        return true;
    }

    // 开始喷涂
    public String startWork(CMDForm form) {
        Map<String, Object> params = form.getParams();
        if (
                params == null ||
                !params.containsKey("position") ||
                !params.containsKey("space") ||
                !params.containsKey("routeType") ||
                !params.containsKey("movementSpeed") ||
                !params.containsKey("height") ||
                !params.containsKey("matrixFlowVelocity")
        ) {
            return "参数错误";
        }

        if ( operationLogService.getIng() != null) {
            return "当前有正在运行的任务";
        }
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        // 托盘位置
        List<SysSettings> slidePositionList = sysSettingsService.getSlidePositionList();
        List<Map<String, Integer>> position = (List<Map<String, Integer>>) params.get("position");
        if (position == null) {
            return "position参数错误";
        }

        // 设置指定轴的电机的运行速度
        int movementSpeed = (Integer) params.get("movementSpeed");
        cmdList.add(() -> deviceTcpCMDService.setMotorSpeed("x", movementSpeed));
        cmdList.add(() -> deviceTcpCMDService.setMotorSpeed("y", movementSpeed));
        cmdList.add(() -> deviceTcpCMDService.setMotorSpeed("z", 10));

        // 回到原点
        cmdList.add(() -> deviceTcpCMDService.moveMotorToPosition("Z", 5));
        cmdList.add(() -> deviceTcpCMDService.moveMotorToPosition("X", 5));
        cmdList.add(() -> deviceTcpCMDService.moveMotorToPosition("Y", 5));

        cmdList.add(() -> deviceTcpCMDService.motorMoveToHome("Z"));
        cmdList.add(() -> deviceTcpCMDService.motorMoveToHome("X"));
        cmdList.add(() -> deviceTcpCMDService.motorMoveToHome("Y"));



        for (int i = 0; i < position.size(); i++) {
            Map<String, Integer> p = position.get(i);

            if (p.get("x1") == null || p.get("y1") == null ||  p.get("x2") == null ||  p.get("y2") == null || p.get("index") == null) {
                return "position参数错误";
            }
            // 玻片的位置
            String p1 = slidePositionList.get(p.get("index")).getValue();
            String[] p1s = p1.split(",");
            double x = Double.parseDouble(p1s[0]);
            double y = Double.parseDouble(p1s[1]);
            double z = Double.parseDouble(p1s[2]);

            // 玻片范围的实际位置
            // 托盘点位 x y z
            double left =x+ p.get("x1");
            double right = x+ p.get("x2");
            double top = y+ p.get("y1");
            double bottom = y+ p.get("y2");

            int space = (Integer) params.get("space");
            int routeType = (Integer) params.get("routeType");
            List<PathGenerator.Points> horizontalPath = generatePathPoints(
                    left, right, top, bottom, space, routeType == 1 ? PathGenerator.MoveMode.HORIZONTAL_ZIGZAG_TOP_DOWN : PathGenerator.MoveMode.VERTICAL_ZIGZAG_LEFT_RIGHT
            );
            log.info("horizontalPath:{}", horizontalPath);
            if(horizontalPath.isEmpty()) {
                return "路径规划失败";
            }

            // 将三通阀转至喷涂管路
            cmdList.add(() -> deviceTcpCMDService.switchThreeWayValve("clear_spray"));

            // 移动到指定高度(位置)
            int height  = (Integer) params.get("height");

            if(z - height < 15) {
                return "高度设置太低,有撞针的风险";
            }
            cmdList.add(() -> deviceTcpCMDService.moveMotorToPosition("Z",z - height ));
            cmdList.add(() -> deviceTcpCMDService.moveMotorToPosition("X", left));
            cmdList.add(() -> deviceTcpCMDService.moveMotorToPosition("Y", top));

            // 是否加电 电压
            Object voltage = params.get("voltage");
            if (voltage instanceof Integer) {
                cmdList.add(() -> deviceTcpCMDService.turnOnHighVoltage( (Integer) voltage));
            }

            // 开启喷嘴阀
            cmdList.add(() -> deviceTcpCMDService.controlValve("Nozzle", true));
            // 推注射泵

            int matrixFlowVelocity = (int) params.get("matrixFlowVelocity");
            cmdList.add(() -> deviceTcpCMDService.turnOnSyringePump(matrixFlowVelocity));

            // 插入日志
            if(i == 0) {
                cmdList.add( () -> {
                    OperationLog operationLog = new OperationLog();
                    operationLog.setStatus(0);
                    operationLog.setMatrixId((Long) params.get("matrixCraftId"));
                    operationLog.setMatrixInfo(JSON.toJSONString(params));
                    operationLogService.add(operationLog);
                    return true;
                });
                cmdList.add(() -> {
                    sysSettingsService.updateWorkStatus("spraying");
                    return true;
                });
            }

            // 执行轨迹
            for (PathGenerator.Points point : horizontalPath) {

                double nextX = (int) point.getX();
                double nextY = (int) point.getY();
                log.info("当前喷针位置x:{}, y:{}", nextX, nextY );
                cmdList.add(() -> deviceTcpCMDService.moveMotorToPosition("X", nextX));
                cmdList.add(() -> deviceTcpCMDService.moveMotorToPosition("Y", nextY));
            }
            // 停止喷涂
            cmdList.add(deviceTcpCMDService::syringePumpStop);
            // 关闭喷嘴阀
            cmdList.add(() -> deviceTcpCMDService.controlValve("Nozzle", true));
        }
        // 关闭高压
        cmdList.add(deviceTcpCMDService::turnOffHighVoltage);
        // 回到原点
        cmdList.add(() -> deviceTcpCMDService.motorMoveToHome("X"));
        cmdList.add(() -> deviceTcpCMDService.motorMoveToHome("Y"));
        cmdList.add(() -> deviceTcpCMDService.motorMoveToHome("Z"));

        // 结束日志
        cmdList.add( () -> {
            OperationLog operationLog = operationLogService.getIng();
            operationLog.setStatus(1);
            operationLogService.updateById(operationLog);
            return true;
        });
        cmdList.add(() -> {
            sysSettingsService.updateWorkStatus("idle");
            Map<String, String> result = new HashMap<>();
            result.put("code", "spray_complete");
            result.put("msg", "喷涂任务已完成");
            webSocketService.pushMsg(WebSocketMessageType.WARN, result);
            return true;
        });
        new Thread(() -> {
            running = true;
            ExecutionResult executionResult = new ExecutionResult();
            executionResult.setCommandId(form.getCommandId());
            executionResult.setCommandName(form.getCommandName());

            // 执行所有命令
            for (Supplier<Boolean> command : cmdList) {

                boolean result = command.get();
                if(!running) {
                    log.error("指令线程停止: {}", JSONUtil.toJsonStr(form));
                    executionResult.setStatus(CMDResultCode.FAILURE.getCode());
                    executionResult.setMessage(CMDResultCode.FAILURE.getMsg());
                    webSocketService.pushMsg(WebSocketMessageType.CMD, executionResult);
                    return;
                }
//            try {
//                Thread.sleep(5000);
//            } catch (InterruptedException e) {
//                throw new RuntimeException(e);
//            }
                if (!result) {
                    log.error("指令执行异常: {}", JSONUtil.toJsonStr(form));
                    executionResult.setStatus(CMDResultCode.FAILURE.getCode());
                    executionResult.setMessage(CMDResultCode.FAILURE.getMsg());
                    webSocketService.pushMsg(WebSocketMessageType.CMD, executionResult);
                    return;
                }
            }
            log.info("指令执行成功: {}", JSONUtil.toJsonStr(form));
            executionResult.setStatus(CMDResultCode.SUCCESS.getCode());
            executionResult.setMessage(CMDResultCode.SUCCESS.getMsg());
            webSocketService.pushMsg(WebSocketMessageType.CMD, executionResult);
        }).start();
        return "ok";
    }

    // 结束喷涂
    public boolean stopWork(CMDForm form) {
        List<Supplier<Boolean>> cmdList = new ArrayList<>();

        cmdList.add(() -> {
            running = false;
            return true;
        });
        // 停止喷涂
        cmdList.add(deviceTcpCMDService::turnOffSyringePump);
        // 关闭高压
        cmdList.add(deviceTcpCMDService::turnOffHighVoltage);
        // 关闭喷嘴阀
        cmdList.add(() -> deviceTcpCMDService.controlValve("Nozzle", true));
        // 回到原点
        cmdList.add(() -> deviceTcpCMDService.motorMoveToHome("X"));
        cmdList.add(() -> deviceTcpCMDService.motorMoveToHome("Z"));
        // 结束日志
        cmdList.add( () -> {
            OperationLog operationLog = operationLogService.getIng();
            operationLog.setStatus(1);
            operationLogService.updateById(operationLog);
            return true;
        });
        cmdList.add(() -> {
            sysSettingsService.updateWorkStatus("idle");
            return true;
        });
        initExecutorThread(cmdList, form);
        return true;
    }

    // 测试轴转动
    public boolean rotate(CMDForm form) {
        Map<String, Object> params = form.getParams();
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        String axis = (String) params.get("axis");
        double rotationSpeed = Optional.ofNullable(params.get("rotationSpeed"))
                .filter(Number.class::isInstance)
                .map(Number.class::cast)
                .map(Number::doubleValue)
                .orElse(0.0);
        int time = (Integer) params.get("time");
        cmdList.add(() -> deviceTcpCMDService.rotateMotor(axis, rotationSpeed, time));
        initExecutorThread(cmdList, form);
        return true;
    }

    // 轴停止转动
    public boolean stopMotor(CMDForm form) {
        Map<String, Object> params = form.getParams();
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        String axis = (String) params.get("axis");
        cmdList.add(() -> deviceTcpCMDService.stopMotor(axis));
        initExecutorThread(cmdList, form);
        return true;
    }

    // 开始清洗
    public boolean startWash(CMDForm form) {
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        Map<String, Object> params = form.getParams();
        String type = (String) params.get("type");

//        type: "injector" | "nozzle"
        if(Objects.equals(type, "injector")) {
            cmdList.add(() -> deviceTcpCMDService.switchThreeWayValve("clear_spray"));
            cmdList.add(() -> deviceTcpCMDService.controlValve("Cleaning", true));
        } else if (Objects.equals(type, "nozzle")) {
            cmdList.add(() -> deviceTcpCMDService.moveMotorToPosition("X", 173.08));
            cmdList.add(() -> deviceTcpCMDService.moveMotorToPosition("Y", 75.2));
            cmdList.add(() -> deviceTcpCMDService.moveMotorToPosition("Z", 70));
            cmdList.add(() -> deviceTcpCMDService.switchThreeWayValve("clear_nozzle"));
            cmdList.add(() -> deviceTcpCMDService.controlValve("Nozzle", true));
        }
        cmdList.add(() -> {
            sysSettingsService.updateWorkStatus("washing");
            return true;
        });
        initExecutorThread(cmdList, form);
        return true;
    }

    // 结束清洗
    public boolean stopWash(CMDForm form) {
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        cmdList.add(() -> deviceTcpCMDService.switchThreeWayValve("close_all"));
        cmdList.add(() -> deviceTcpCMDService.controlValve("Cleaning", false));
        cmdList.add(() -> deviceTcpCMDService.controlValve("Nozzle", false));
        cmdList.add(() -> {
            sysSettingsService.updateWorkStatus("idle");
            return true;
        });
        initExecutorThread(cmdList, form);
        return true;
    }

    // 照明灯板控制方法
    /**
     * 以指定亮度开启照明灯板
     *  brightness 亮度值，范围 0 - 100
     * @return 操作是否成功
     */
    public boolean turnOnLightPanel(CMDForm form) {
        Map<String, Object> params = form.getParams();
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        int brightness = (int) params.get("brightness");
        cmdList.add(() -> deviceTcpCMDService.turnOnLightPanel(brightness));
        initExecutorThread(cmdList, form);
        return true;
    }
    /**
     * 关闭照明灯板
     * @return 操作是否成功
     */
    public boolean turnOffLightPanel(CMDForm form) {
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        cmdList.add(deviceTcpCMDService::turnOffLightPanel);
        initExecutorThread(cmdList, form);
        return true;
    }

    // 开始预充
    public boolean startPrefill(CMDForm form) {
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        Map<String, Object> params = form.getParams();

        cmdList.add(() -> deviceTcpCMDService.switchThreeWayValve("clear_spray"));

        cmdList.add(() -> deviceTcpCMDService.controlValve("Nozzle", true));
        cmdList.add(() -> deviceTcpCMDService.turnOnSyringePump((Double) params.get("rotationSpeed")));
        cmdList.add(() -> {
            sysSettingsService.updateWorkStatus("prefilling");
            return true;
        });
        initExecutorThread(cmdList, form);
        return true;
    }
// 停止预充
    public boolean stopPrefill(CMDForm form) {
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        cmdList.add(deviceTcpCMDService::turnOffSyringePump);
        cmdList.add(() -> deviceTcpCMDService.controlValve("Nozzle", false));

        cmdList.add(() -> {
            sysSettingsService.updateWorkStatus("idle");
            return true;
        });
        initExecutorThread(cmdList, form);
        return true;
    }

    // 电机回到原点
    public boolean motorMoveToHome(CMDForm form) {
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        Map<String, Object> params = form.getParams();
        String axis = (String) params.get("axis");
        cmdList.add(() -> deviceTcpCMDService.motorMoveToHome(axis));
        initExecutorThread(cmdList, form);
        return true;
    }

    // 开始除湿
    public boolean startDehumidify(CMDForm form) {
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        Map<String, Object> params = form.getParams();
        cmdList.add(() -> {
            sysSettingsService.updateWorkStatus("dehumidifying");
            return true;
        });
        double humidity = Optional.ofNullable(params.get("humidity"))
                .filter(Number.class::isInstance)
                .map(Number.class::cast)
                .map(Number::doubleValue)
                .orElse(0.0);
        cmdList.add(() -> deviceTcpCMDService.startDehumidify(humidity));
        cmdList.add(() -> {
            sysSettingsService.updateTargetHumidity(humidity);
            while (true) {
                double targetHumidity = sysSettingsService.getTargetHumidity();
                DeviceStatus deviceStatus = deviceStatusService.getDeviceStatus();
                if (deviceStatus.getHumidity()<=targetHumidity ) {
                    deviceTcpCMDService.controlValve("Dehumidification", false);
                    sysSettingsService.updateWorkStatus("idle");
                    Map<String, String> result = new HashMap<>();
                    result.put("code", "dehumidify_complete");
                    result.put("msg", "已达到目标湿度");
                    webSocketService.pushMsg(WebSocketMessageType.WARN, result);
                    break;
                }
             }
            return true;
        });

        initExecutorThread(cmdList, form);
        return true;
    }

    // 结束除湿
    public boolean stopDehumidify(CMDForm form) {
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        cmdList.add(() -> deviceTcpCMDService.controlValve("Dehumidification", false));
        cmdList.add(() -> {
            sysSettingsService.updateWorkStatus("idle");
            return true;
        });
        initExecutorThread(cmdList, form);
        return true;
    }

    // 设置电机速度
    public boolean setMotorSpeed(CMDForm form) {
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        Map<String, Object> params = form.getParams();
        String axis = (String) params.get("axis");
        double speed = Optional.ofNullable(params.get("speed"))
                .filter(Number.class::isInstance)
                .map(Number.class::cast)
                .map(Number::doubleValue)
                .orElse(0.0);
        cmdList.add(() -> deviceTcpCMDService.setMotorSpeed(axis, speed));
        initExecutorThread(cmdList, form);
        return true;
    }

    public boolean trayOut(CMDForm form) {
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        cmdList.add(() -> deviceTcpCMDService.moveMotorToPosition("Y", 150));
        initExecutorThread(cmdList, form);
        return true;
    }

    public boolean trayIn(CMDForm form) {
        List<Supplier<Boolean>> cmdList = new ArrayList<>();
        cmdList.add(() -> deviceTcpCMDService.moveMotorToPosition("Y", 0));
        initExecutorThread(cmdList, form);
        return true;
    }


    public void run(List<Supplier<Boolean>> cmdList, CMDForm form) {
        ExecutionResult executionResult = new ExecutionResult();
        executionResult.setCommandId(form.getCommandId());
        executionResult.setCommandName(form.getCommandName());

        // 执行所有命令
        for (Supplier<Boolean> command : cmdList) {

            boolean result = command.get();
//            try {
//                Thread.sleep(5000);
//            } catch (InterruptedException e) {
//                throw new RuntimeException(e);
//            }
            if (!result) {
                log.error("指令执行异常: {}", JSONUtil.toJsonStr(form));
                executionResult.setStatus(CMDResultCode.FAILURE.getCode());
                executionResult.setMessage(CMDResultCode.FAILURE.getMsg());
                webSocketService.pushMsg(WebSocketMessageType.CMD, executionResult);
                return;
            }
        }
        log.info("指令执行成功: {}", JSONUtil.toJsonStr(form));
        executionResult.setStatus(CMDResultCode.SUCCESS.getCode());
        executionResult.setMessage(CMDResultCode.SUCCESS.getMsg());
        webSocketService.pushMsg(WebSocketMessageType.CMD, executionResult);
    }



}