package com.iflytop.gd.app.service;

import com.iflytop.gd.app.model.bo.Point2D;
import com.iflytop.gd.app.model.bo.Point3D;
import com.iflytop.gd.app.model.entity.DevicePosition;
import com.iflytop.gd.common.cmd.CommandFuture;
import com.iflytop.gd.common.cmd.DeviceCommandBundle;
import com.iflytop.gd.common.cmd.DeviceCommandGenerator;
import com.iflytop.gd.common.enums.AcidPumpDeviceCode;
import com.iflytop.gd.common.enums.HeatModuleCode;
import com.iflytop.gd.common.enums.cmd.CmdAxis;
import com.iflytop.gd.common.enums.data.DevicePositionCode;
import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver;
import com.iflytop.gd.hardware.drivers.StepMotorDriver.StepMotorCtrlDriver;
import com.iflytop.gd.hardware.exception.HardwareException;
import com.iflytop.gd.hardware.service.GDDeviceStatusService;
import com.iflytop.gd.hardware.type.IO.InputIOMId;
import com.iflytop.gd.hardware.type.IO.OutputIOMId;
import com.iflytop.gd.hardware.drivers.LiquidDistributionArmDriver;
import com.iflytop.gd.hardware.type.Servo.LiquidArmMId;
import com.iflytop.gd.hardware.type.StepMotor.StepMotorMId;
import lombok.RequiredArgsConstructor;
import org.springframework.stereotype.Component;

import java.util.Arrays;
import java.util.concurrent.CompletableFuture;
import java.util.concurrent.TimeUnit;

@Component
@RequiredArgsConstructor
public class DeviceCommandUtilService {
    private final DeviceCommandService deviceCommandService;
    private final DevicePositionService devicePositionService;
    private final LiquidDistributionArmDriver liquidDistributionArmDriver;
    private final GDDeviceStatusService gdDeviceStatusService;
    private final StepMotorCtrlDriver stepMotorCtrlDriver;
    private final OutputIOCtrlDriver outputIOCtrlDriver;
    /**
     * 门电机回原点
     */
    public void doorOrigin() throws Exception {
        doorOrigin(null, null);
    }

    /**
     * 门电机回原点
     */
    public void doorOrigin(String cmdId, String cmdCode) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.doorOrigin();
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 门电机移动
     */
    public void doorMove(double position) throws Exception {
        doorMove(null, null, position);
    }

    /**
     * 门电机移动
     */
    public void doorMove(String cmdId, String cmdCode, double position) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.doorMove(position);
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 龙门架机械臂移动到指定点
     */
    public void gantryMove(Point3D point) throws Exception {
        gantryMove(null, null, point);
    }

    /**
     * 龙门架机械臂移动到指定点
     */
    public void gantryMove(String cmdId, String cmdCode, Point3D point) throws Exception {
        outputIOCtrlDriver.open(OutputIOMId.DO_TRAY_MOTOR_CLAMP);
        DeviceCommandBundle gantryXMoveDeviceCommand = DeviceCommandGenerator.gantryXMove(point.getX());
        CommandFuture gantryXMoveDeviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, gantryXMoveDeviceCommand);

        DeviceCommandBundle gantryYMoveDeviceCommand = DeviceCommandGenerator.gantryYMove(point.getY());
        CommandFuture gantryYMoveDeviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, gantryYMoveDeviceCommand);

        DeviceCommandBundle gantryZMoveDeviceCommand = DeviceCommandGenerator.gantryZMove(point.getZ());
        CommandFuture gantryZMoveDeviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, gantryZMoveDeviceCommand);
        commandWait(gantryXMoveDeviceCommandFuture, gantryYMoveDeviceCommandFuture, gantryZMoveDeviceCommandFuture);
        outputIOCtrlDriver.close(OutputIOMId.DO_TRAY_MOTOR_CLAMP);
    }

    /**
     * 龙门架机械臂移动到指定点
     */
    public void gantryMoveQueue(Point3D point) throws Exception {
        gantryMoveQueue(null, null, point);
    }

    /**
     * 龙门架机械臂移动到指定点
     */
    public void gantryMoveQueue(String cmdId, String cmdCode, Point3D point) throws Exception {
        DeviceCommandBundle gantryXMoveDeviceCommand = DeviceCommandGenerator.gantryXMove(point.getX());
        DeviceCommandBundle gantryYMoveDeviceCommand = DeviceCommandGenerator.gantryYMove(point.getY());
        DeviceCommandBundle gantryZMoveDeviceCommand = DeviceCommandGenerator.gantryZMove(point.getZ());
        CommandFuture[] deviceCommandFutureArr = deviceCommandService.sendCommandGantryQueue(cmdId, cmdCode, gantryXMoveDeviceCommand, gantryYMoveDeviceCommand, gantryZMoveDeviceCommand);
        commandWait(deviceCommandFutureArr);
    }


    /**
     * 龙门架机械臂X轴回原点
     */
    public void gantryXMoveOrigin() throws Exception {
        gantryXMoveOrigin(null, null);
    }

    /**
     * 龙门架机械臂X轴回原点
     */
    public void gantryXMoveOrigin(String cmdId, String cmdCode) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryXOrigin();
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 龙门架机械臂X轴回原点
     */
    public void gantryXMoveOriginQueue() throws Exception {
        gantryXMoveOriginQueue(null, null);
    }

    /**
     * 龙门架机械臂X轴回原点
     */
    public void gantryXMoveOriginQueue(String cmdId, String cmdCode) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryXOrigin();
        CommandFuture[] deviceCommandFutureArr = deviceCommandService.sendCommandGantryQueue(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFutureArr);
    }


    /**
     * 龙门架机械臂Y轴回原点
     */
    public void gantryYMoveOrigin() throws Exception {
        gantryYMoveOrigin(null, null);
    }

    /**
     * 龙门架机械臂Y轴回原点
     */
    public void gantryYMoveOrigin(String cmdId, String cmdCode) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryYOrigin();
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }


    /**
     * 龙门架机械臂Y轴回原点
     */
    public void gantryYMoveOriginQueue() throws Exception {
        gantryYMoveOriginQueue(null, null);
    }

    /**
     * 龙门架机械臂Y轴回原点
     */
    public void gantryYMoveOriginQueue(String cmdId, String cmdCode) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryYOrigin();
        CommandFuture[] deviceCommandFutureArr = deviceCommandService.sendCommandGantryQueue(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFutureArr);
    }


    /**
     * 龙门架机械臂Z轴回原点
     */
    public void gantryZMoveOrigin() throws Exception {
        gantryZMoveOrigin(null, null);
    }

    /**
     * 龙门架机械臂Z轴回原点
     */
    public void gantryZMoveOrigin(String cmdId, String cmdCode) throws Exception {
        outputIOCtrlDriver.open(OutputIOMId.DO_TRAY_MOTOR_CLAMP);
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryZOrigin();
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
        outputIOCtrlDriver.close(OutputIOMId.DO_TRAY_MOTOR_CLAMP);
    }


    /**
     * 龙门架机械臂Z轴回原点
     */
    public void gantryZMoveOriginQueue() throws Exception {
        gantryZMoveOriginQueue(null, null);
    }

    /**
     * 龙门架机械臂Z轴回原点
     */
    public void gantryZMoveOriginQueue(String cmdId, String cmdCode) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryZOrigin();
        CommandFuture[] deviceCommandFutureArr = deviceCommandService.sendCommandGantryQueue(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFutureArr);
    }

    /**
     * 龙门架机械臂X轴移动到指定点
     */
    public void gantryXMove(double position) throws Exception {
        gantryXMove(null, null, position);
    }

    /**
     * 龙门架机械臂X轴移动到指定点
     */
    public void gantryXMove(String cmdId, String cmdCode, double position) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryXMove(position);
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 龙门架机械臂X轴移动到指定点
     */
    public void gantryXMoveQueue(double position) throws Exception {
        gantryXMoveQueue(null, null, position);
    }

    /**
     * 龙门架机械臂X轴移动到指定点
     */
    public void gantryXMoveQueue(String cmdId, String cmdCode, double position) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryXMove(position);
        CommandFuture[] deviceCommandFutureArr = deviceCommandService.sendCommandGantryQueue(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFutureArr);
    }

    /**
     * 龙门架机械臂Y轴移动到指定点
     */
    public void gantryYMove(double position) throws Exception {
        gantryYMove(null, null, position);
    }

    /**
     * 龙门架机械臂Y轴移动到指定点
     */
    public void gantryYMove(String cmdId, String cmdCode, double position) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryYMove(position);
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 龙门架机械臂Y轴移动到指定点
     */
    public void gantryYMoveQueue(double position) throws Exception {
        gantryYMoveQueue(null, null, position);
    }

    /**
     * 龙门架机械臂Y轴移动到指定点
     */
    public void gantryYMoveQueue(String cmdId, String cmdCode, double position) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryYMove(position);
        CommandFuture[] deviceCommandFutureArr = deviceCommandService.sendCommandGantryQueue(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFutureArr);
    }

    /**
     * 龙门架机械臂Z轴移动到指定点
     */
    public void gantryZMove(double position) throws Exception {
        gantryZMove(null, null, position);
    }

    /**
     * 龙门架机械臂Z轴移动到指定点
     */
    public void gantryZMove(String cmdId, String cmdCode, double position) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryZMove(position);
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 龙门架机械臂Z轴移动到指定点
     */
    public void gantryZMoveQueue(double position) throws Exception {
        gantryZMoveQueue(null, null, position);
    }

    /**
     * 龙门架机械臂Z轴移动到指定点
     */
    public void gantryZMoveQueue(String cmdId, String cmdCode, double position) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryZMove(position);
        CommandFuture[] deviceCommandFutureArr = deviceCommandService.sendCommandGantryQueue(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFutureArr);
    }

    /**
     * 龙门架机械臂X轴相对移动到指定点
     */
    public void gantryXMoveBy(double position) throws Exception {
        gantryXMoveBy(null, null, position);
    }

    /**
     * 龙门架机械臂X轴相对移动到指定点
     */
    public void gantryXMoveBy(String cmdId, String cmdCode, double position) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryXMoveBy(position);
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 龙门架机械臂X轴相对移动到指定点
     */
    public void gantryXMoveByQueue(double position) throws Exception {
        gantryXMoveByQueue(null, null, position);
    }

    /**
     * 龙门架机械臂X轴相对移动到指定点
     */
    public void gantryXMoveByQueue(String cmdId, String cmdCode, double position) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryXMoveBy(position);
        CommandFuture[] deviceCommandFutureArr = deviceCommandService.sendCommandGantryQueue(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFutureArr);
    }

    /**
     * 龙门架机械臂Y轴相对移动到指定点
     */
    public void gantryYMoveBy(double position) throws Exception {
        gantryYMoveBy(null, null, position);
    }

    /**
     * 龙门架机械臂Y轴相对移动到指定点
     */
    public void gantryYMoveBy(String cmdId, String cmdCode, double position) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryYMoveBy(position);
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 龙门架机械臂Y轴相对移动到指定点
     */
    public void gantryYMoveByQueue(double position) throws Exception {
        gantryYMoveByQueue(null, null, position);
    }

    /**
     * 龙门架机械臂Y轴相对移动到指定点
     */
    public void gantryYMoveByQueue(String cmdId, String cmdCode, double position) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryYMoveBy(position);
        CommandFuture[] deviceCommandFutureArr = deviceCommandService.sendCommandGantryQueue(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFutureArr);
    }

    /**
     * 龙门架机械臂Z轴相对移动到指定点
     */
    public void gantryZMoveBy(double position) throws Exception {
        gantryZMoveBy(null, null, position);
    }

    /**
     * 龙门架机械臂Z轴相对移动到指定点
     */
    public void gantryZMoveBy(String cmdId, String cmdCode, double position) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryZMoveBy(position);
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 龙门架机械臂Z轴相对移动到指定点
     */
    public void gantryZMoveByQueue(double position) throws Exception {
        gantryZMoveByQueue(null, null, position);
    }

    /**
     * 龙门架机械臂Z轴相对移动到指定点
     */
    public void gantryZMoveByQueue(String cmdId, String cmdCode, double position) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryZMoveBy(position);
        CommandFuture[] deviceCommandFutureArr = deviceCommandService.sendCommandGantryQueue(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFutureArr);
    }

    /**
     * 移动夹爪
     */
    public void clawMove(double position) throws Exception {
        clawMove(null, null, position);
    }

    /**
     * 移动夹爪
     */
    public void clawMove(String cmdId, String cmdCode, double position) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.clawMove(position);
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 调整拍子升降电机位置
     *
     * @param num 1为上升1格，-1为下降1格
     */
    public void capMotorMove(int num) throws Exception {
        capMotorMove(null, null, num);
    }

    /**
     * 将拍子升降区抬升至可以获取的位置
     */
    public void capUpBalance() throws Exception {
        capUpBalance(null, null);
    }

    /**
     * 将拍子升降区抬升至可以获取的位置
     */
    public void capUpBalance(String cmdId, String cmdCode) throws Exception {
        int capNum = 0;
        for (InputIOMId inputIOMId : InputIOMId.values()) {
            Boolean exist = gdDeviceStatusService.getInputState(inputIOMId);
            if (exist) {
                capNum++;
            }
        }
        double capLiftingHeight = devicePositionService.getPosition(DevicePositionCode.capLiftingHeight).getDistance();
        double capLiftingHeightSum = (6 - capNum) * capLiftingHeight;
        outputIOCtrlDriver.open(OutputIOMId.DO_TRAY_MOTOR_CLAMP);
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.trayMotorMove(capLiftingHeightSum);
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
        outputIOCtrlDriver.close(OutputIOMId.DO_TRAY_MOTOR_CLAMP);
    }

    /**
     * 调整拍子升降电机位置
     *
     * @param num 1为上升1格，-1为下降1格
     */
    public void capMotorMove(String cmdId, String cmdCode, int num) throws Exception {
        outputIOCtrlDriver.open(OutputIOMId.DO_TRAY_MOTOR_CLAMP);
        double capLiftingHeight = devicePositionService.getPosition(DevicePositionCode.capLiftingHeight).getDistance();
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.trayMotorMove(num * capLiftingHeight);
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
        outputIOCtrlDriver.close(OutputIOMId.DO_TRAY_MOTOR_CLAMP);
    }


    /**
     * 加热模块升降电机移动到指定位置
     */
    public void heaterMotorMoveOrigin(HeatModuleCode heatModuleId) throws Exception {
        heaterMotorMoveOrigin(null, null, heatModuleId);
    }

    /**
     * 加热模块升降电机回原点
     */
    public void heaterMotorMoveOrigin(String cmdId, String cmdCode, HeatModuleCode heatModuleId) throws Exception {
        DeviceCommandBundle deviceCommand = switch (heatModuleId) {
            case heat_module_01 -> DeviceCommandGenerator.heaterMotor1Origin();
            case heat_module_02 -> DeviceCommandGenerator.heaterMotor2Origin();
            case heat_module_03 -> DeviceCommandGenerator.heaterMotor3Origin();
            case heat_module_04 -> DeviceCommandGenerator.heaterMotor4Origin();
            case heat_module_05 -> DeviceCommandGenerator.heaterMotor5Origin();
            case heat_module_06 -> DeviceCommandGenerator.heaterMotor6Origin();
        };
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 加热模块升降电机移动到指定位置
     */
    public void heaterMotorMove(HeatModuleCode heatModuleId, double position) throws Exception {
        heaterMotorMove(null, null, heatModuleId, position);
    }

    /**
     * 加热模块升降电机移动到指定位置
     */
    public void heaterMotorMove(String cmdId, String cmdCode, HeatModuleCode heatModuleId, double position) throws Exception {
        DeviceCommandBundle deviceCommand = switch (heatModuleId) {
            case heat_module_01 -> DeviceCommandGenerator.heaterMotor1Move(position);
            case heat_module_02 -> DeviceCommandGenerator.heaterMotor2Move(position);
            case heat_module_03 -> DeviceCommandGenerator.heaterMotor3Move(position);
            case heat_module_04 -> DeviceCommandGenerator.heaterMotor4Move(position);
            case heat_module_05 -> DeviceCommandGenerator.heaterMotor5Move(position);
            case heat_module_06 -> DeviceCommandGenerator.heaterMotor6Move(position);
        };
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

//    /**
//     * 双轴械臂 移动至指定点
//     */
//    public void dualRobotMovePoint(Point2D point2D) throws Exception {
//        dualRobotMovePoint(null, null, point2D);
//    }

//    /**
//     * 双轴械臂 移动至指定点
//     */
//    public void dualRobotMovePoint(String cmdId, String cmdCode, Point2D point2D) throws Exception {
//        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.dualRobotMovePoint(point2D.getX(), point2D.getY());
//        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
//        commandWait(deviceCommandFuture);
//    }

    /**
     * 双轴械臂 移动至指定试管
     */
    public void dualRobotMovePoint(int index) throws Exception {
        liquidDistributionArmDriver.liquidDistributionArmMoveTo(LiquidArmMId.LiquidDistributionArm, index);
    }

    /**
     * 加液机械臂回原点
     */
    public void dualRobotOrigin() throws Exception {
        liquidDistributionArmDriver.liquidDistributionArmMoveTo(LiquidArmMId.LiquidDistributionArm, 0);
    }

//    /**
//     * 加液机械臂回原点
//     */
//    public void dualRobotOrigin(String cmdId, String cmdCode) throws Exception {
//        DeviceCommandBundle dualRobotJoint1OriginDeviceCommand = DeviceCommandGenerator.dualRobotJoint1Origin();
//        DeviceCommandBundle dualRobotJoint2OriginDeviceCommand = DeviceCommandGenerator.dualRobotJoint2Origin();
//        CommandFuture dualRobotJoint1OriginDeviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, dualRobotJoint1OriginDeviceCommand);
//        CommandFuture dualRobotJoint2OriginDeviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, dualRobotJoint2OriginDeviceCommand);
//        commandWait(dualRobotJoint1OriginDeviceCommandFuture, dualRobotJoint2OriginDeviceCommandFuture);
//    }

    /**
     * 加液机械臂回原点
     */
    public void dualRobotOrigin(String cmdId, String cmdCode, CmdAxis cmdAxis) throws Exception {
        if (cmdAxis == CmdAxis.joint1) {
            DeviceCommandBundle deviceCommand = DeviceCommandGenerator.dualRobotJoint1Origin();
            CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
            commandWait(deviceCommandFuture);
        } else {
            DeviceCommandBundle deviceCommand = DeviceCommandGenerator.dualRobotJoint2Origin();
            CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
            commandWait(deviceCommandFuture);
        }
    }

    /**
     * 添加溶液
     */
    public void acidPumpMoveBy(AcidPumpDeviceCode acidPumpDevice, double position) throws Exception {
        acidPumpMoveBy(null, null, acidPumpDevice, position);
    }

    /**
     * 添加溶液
     */
    public void acidPumpMoveBy(String cmdId, String cmdCode, AcidPumpDeviceCode acidPumpDevice, double position) throws Exception {
        DeviceCommandBundle deviceCommand;
        switch (acidPumpDevice) {
            case acid_pump_01 -> {
                stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_1_MOTOR_MID, 1);
                deviceCommand = DeviceCommandGenerator.acidPump1MoveBy(position);
            }
            case acid_pump_02 -> {
                stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_2_MOTOR_MID, 1);
                deviceCommand = DeviceCommandGenerator.acidPump2MoveBy(position);
            }
            case acid_pump_03 -> {
                stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_3_MOTOR_MID, 1);
                deviceCommand = DeviceCommandGenerator.acidPump3MoveBy(position);
            }
            case acid_pump_04 -> {
                stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_4_MOTOR_MID, 1);
                deviceCommand = DeviceCommandGenerator.acidPump4MoveBy(position);
            }
            case acid_pump_05 -> {
                stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_5_MOTOR_MID, 1);
                deviceCommand = DeviceCommandGenerator.acidPump5MoveBy(position);
            }
            case acid_pump_06 -> {
                stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_6_MOTOR_MID, 1);
                deviceCommand = DeviceCommandGenerator.acidPump6MoveBy(position);
            }
            case acid_pump_07 -> {
                stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_7_MOTOR_MID, 1);
                deviceCommand = DeviceCommandGenerator.acidPump7MoveBy(position);
            }
            case acid_pump_08 -> {
                stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_8_MOTOR_MID, 1);
                deviceCommand = DeviceCommandGenerator.acidPump8MoveBy(position);
            }
            default -> throw new RuntimeException("index 未找到");
        }
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
        switch (acidPumpDevice) {
            case acid_pump_01 -> stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_1_MOTOR_MID, 0);
            case acid_pump_02 -> stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_2_MOTOR_MID, 0);
            case acid_pump_03 -> stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_3_MOTOR_MID, 0);
            case acid_pump_04 -> stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_4_MOTOR_MID, 0);
            case acid_pump_05 -> stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_5_MOTOR_MID, 0);
            case acid_pump_06 -> stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_6_MOTOR_MID, 0);
            case acid_pump_07 -> stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_7_MOTOR_MID, 0);
            case acid_pump_08 -> stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.ACID_PUMP_8_MOTOR_MID, 0);
            default -> throw new RuntimeException("index 未找到");
        }
    }

    /**
     * 开始摇匀
     */
    public void shakeStart() throws Exception {
        shakeStart(null, null);
    }

    /**
     * 开始摇匀
     */
    public void shakeStart(String cmdId, String cmdCode) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.shakeMotorStart();
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 停止摇匀
     */
    public void shakeStop() throws Exception {
        shakeStop(null, null);
    }

    /**
     * 停止摇匀
     */
    public void shakeStop(String cmdId, String cmdCode) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.shakeMotorStop();
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 指定加热模块开始加热
     */
    public void heatRodOpen(HeatModuleCode heatModuleId, double temperature) throws Exception {
        heatRodOpen(null, null, heatModuleId, temperature);
    }

    /**
     * 指定加热模块开始加热
     */
    public void heatRodOpen(String cmdId, String cmdCode, HeatModuleCode heatModuleId, double temperature) throws Exception {
        DeviceCommandBundle deviceCommand = switch (heatModuleId) {
            case heat_module_01 -> DeviceCommandGenerator.heatRod1Open(temperature);
            case heat_module_02 -> DeviceCommandGenerator.heatRod2Open(temperature);
            case heat_module_03 -> DeviceCommandGenerator.heatRod3Open(temperature);
            case heat_module_04 -> DeviceCommandGenerator.heatRod4Open(temperature);
            case heat_module_05 -> DeviceCommandGenerator.heatRod5Open(temperature);
            case heat_module_06 -> DeviceCommandGenerator.heatRod6Open(temperature);
        };
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 指定加热模块停止加热
     */
    public void heatRodClose(HeatModuleCode heatModuleId) throws Exception {
        heatRodClose(null, null, heatModuleId);
    }

    /**
     * 指定加热模块停止加热
     */
    public void heatRodClose(String cmdId, String cmdCode, HeatModuleCode heatModuleId) throws Exception {
        DeviceCommandBundle deviceCommand = switch (heatModuleId) {
            case heat_module_01 -> DeviceCommandGenerator.heatRod1Close();
            case heat_module_02 -> DeviceCommandGenerator.heatRod2Close();
            case heat_module_03 -> DeviceCommandGenerator.heatRod3Close();
            case heat_module_04 -> DeviceCommandGenerator.heatRod4Close();
            case heat_module_05 -> DeviceCommandGenerator.heatRod5Close();
            case heat_module_06 -> DeviceCommandGenerator.heatRod6Close();
        };
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 开启补光灯
     */
    public void fillLightOpen(Double brightness) throws Exception {
        fillLightOpen(null, null, brightness);
    }

    /**
     * 开启补光灯
     */
    public void fillLightOpen(String cmdId, String cmdCode, Double brightness) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.fillLightOpen(brightness);
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 关闭补光灯
     */
    public void fillLightClose() throws Exception {
        fillLightClose(null, null);
    }

    /**
     * 关闭补光灯
     */
    public void fillLightClose(String cmdId, String cmdCode) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.fillLightClose();
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 拍照
     */
    public void takePhoto() throws Exception {
        takePhoto(null, null);
    }

    /**
     * 拍照
     */
    public void takePhoto(String cmdId, String cmdCode) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.takePhoto();
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 拍子升降回原点
     */
    public void trayMotorOrigin(String cmdId, String cmdCode) throws Exception {
        DeviceCommandBundle deviceCommand = DeviceCommandGenerator.trayMotorOrigin();
        CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand);
        commandWait(deviceCommandFuture);
    }

    /**
     * 获取指定加热模块托盘检测传感器数值
     */
    public Boolean heatModuleSensorState(HeatModuleCode heatModuleCode) throws HardwareException {
        return switch (heatModuleCode) {
            case heat_module_01 -> gdDeviceStatusService.getInputState(InputIOMId.HEAT_MODULE01_EXIST);
            case heat_module_02 -> gdDeviceStatusService.getInputState(InputIOMId.HEAT_MODULE02_EXIST);
            case heat_module_03 -> gdDeviceStatusService.getInputState(InputIOMId.HEAT_MODULE03_EXIST);
            case heat_module_04 -> gdDeviceStatusService.getInputState(InputIOMId.HEAT_MODULE04_EXIST);
            case heat_module_05 -> gdDeviceStatusService.getInputState(InputIOMId.HEAT_MODULE05_EXIST);
            case heat_module_06 -> gdDeviceStatusService.getInputState(InputIOMId.HEAT_MODULE06_EXIST);
        };

    }


    public void commandWait(CommandFuture... futures) throws Exception {
        CompletableFuture<?>[] responseFutures = Arrays.stream(futures)
                .map(CommandFuture::getResponseFuture)
                .toArray(CompletableFuture[]::new);
        CompletableFuture.allOf(responseFutures)
                .get(120, TimeUnit.SECONDS);
    }

    /////////////////////////////////////////////////获取位置

    /**
     * 获取指定加热区托盘夹爪点位
     */
    public Point3D getHeatAreaTrayClawPoint3D(HeatModuleCode heatModuleId) {
        DevicePosition devicePosition = switch (heatModuleId) {
            case heat_module_01 -> devicePositionService.getPosition(DevicePositionCode.heatArea1TrayClawPoint);
            case heat_module_02 -> devicePositionService.getPosition(DevicePositionCode.heatArea2TrayClawPoint);
            case heat_module_03 -> devicePositionService.getPosition(DevicePositionCode.heatArea3TrayClawPoint);
            case heat_module_04 -> devicePositionService.getPosition(DevicePositionCode.heatArea4TrayClawPoint);
            case heat_module_05 -> devicePositionService.getPosition(DevicePositionCode.heatArea5TrayClawPoint);
            case heat_module_06 -> devicePositionService.getPosition(DevicePositionCode.heatArea6TrayClawPoint);
        };
        return devicePosition.getPoint3D();
    }


    /**
     * 获取指定加热区拍子夹爪点位
     */
    public Point3D getHeatAreaCapClawPointPoint3D(HeatModuleCode heatModuleId) {
        DevicePosition devicePosition = switch (heatModuleId) {
            case heat_module_01 -> devicePositionService.getPosition(DevicePositionCode.heatArea1CapClawPoint);
            case heat_module_02 -> devicePositionService.getPosition(DevicePositionCode.heatArea2CapClawPoint);
            case heat_module_03 -> devicePositionService.getPosition(DevicePositionCode.heatArea3CapClawPoint);
            case heat_module_04 -> devicePositionService.getPosition(DevicePositionCode.heatArea4CapClawPoint);
            case heat_module_05 -> devicePositionService.getPosition(DevicePositionCode.heatArea5CapClawPoint);
            case heat_module_06 -> devicePositionService.getPosition(DevicePositionCode.heatArea6CapClawPoint);
        };
        return devicePosition.getPoint3D();
    }

    /**
     * 根据试管需要获取点位
     */
    public Point2D getPointByTubeNum(int tubeNum) {
        return new Point2D(tubeNum * 60.0, tubeNum * 60.0);//TODO 算法需要完善
    }

}