package com.iflytop.sgs.app.cmd;

import com.iflytop.sgs.app.core.BaseCommandHandler;
import com.iflytop.sgs.app.model.bo.Point3D;
import com.iflytop.sgs.app.model.dto.CmdDTO;
import com.iflytop.sgs.app.service.DeviceCommandUtilService;
import com.iflytop.sgs.app.service.DevicePositionService;
import com.iflytop.sgs.app.service.DeviceStateService;
import com.iflytop.sgs.app.service.GantryArmService;
import com.iflytop.sgs.common.annotation.CommandMapping;
import com.iflytop.sgs.common.enums.HeatModuleCode;
import com.iflytop.sgs.common.enums.data.DevicePositionCode;
import com.iflytop.sgs.common.exception.AppException;
import com.iflytop.sgs.common.result.ResultCode;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Component;

import java.util.concurrent.CompletableFuture;

/**
 * 移至加热 todo wmy 需要改
 */
@Slf4j
@Component
@RequiredArgsConstructor
@CommandMapping("move_to_heat_area")//业务指令注解
public class MoveToHeatAreaCommand extends BaseCommandHandler {
    private final DeviceCommandUtilService deviceCommandUtilService;
    private final DevicePositionService devicePositionService;
    private final GantryArmService gantryArmService;
    private final DeviceStateService deviceStateService;

    @Override
    public CompletableFuture<Void> handle(CmdDTO cmdDTO) throws Exception {
        if (deviceStateService.getCommandState().get().isMoveToHeatAreaCommandExecuting()) {
            throw new AppException(ResultCode.COMMAND_ALREADY_EXECUTING);
        }
        if (deviceStateService.getCommandState().get().isMoveToSolutionAreaCommandExecuting()) {
            throw new AppException(ResultCode.CMD_BUSY);
        }
        try {
            deviceStateService.getCommandState().get().setMoveToHeatAreaCommandExecuting(true);
        } catch (Exception e) {
            deviceStateService.getCommandState().get().setMoveToHeatAreaCommandExecuting(false);
            throw e;
        }
        String heatId = cmdDTO.getStringParam("heatId");
        HeatModuleCode heatModuleId = HeatModuleCode.valueOf(heatId);
        //加液电机升起的安全高度
        double liquidMotorSafeDistance = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance();
        //获取机械臂夹取托盘的横向距离
        double clawTrayGrip = devicePositionService.getPosition(DevicePositionCode.clawTrayGrip).getDistance();
        //获取机械臂夹取托盘的纵向高度
        double clawTrayHeight = devicePositionService.getPosition(DevicePositionCode.clawTrayHeight).getDistance();
        //获取指定加热模块上方点位
        Point3D heatAreaTrayClawPoint3D = deviceCommandUtilService.getHeatAreaTrayClawPoint3D(heatModuleId);
        return runAsync(() -> {
            try {
                //升高加液电机高度
                deviceCommandUtilService.motorLiquidMove(cmdDTO.getCommandId(),cmdDTO.getCommand(),liquidMotorSafeDistance);
                //移动机械臂到加热区上方 此时机械臂夹着托盘
                deviceCommandUtilService.gantryMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatAreaTrayClawPoint3D); //将机械臂移动至加热模块上方
                //下降z轴,夹爪脱离机械臂
                deviceCommandUtilService.gantryZMove(clawTrayHeight);
                //移动x轴向右脱离托盘
                deviceCommandUtilService.gantryXMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -clawTrayGrip);
                //z轴返回原点
                deviceCommandUtilService.motorLiquidMove(0);
            } finally {
                deviceStateService.setGantryArmStateIdle(true);
                deviceStateService.getCommandState().get().setMoveToHeatAreaCommandExecuting(false);
            }
        });
    }
}