package com.cmvr.test.flow.runtime.operator.edge;

import com.alibaba.fastjson2.JSONObject;
import com.cmvr.common.exception.GlobalException;
import com.cmvr.edge.client.model.EdgeCommonVO;
import com.cmvr.edge.client.service.EdgeArmService;
import com.cmvr.test.enums.ActionEnum;
import com.cmvr.test.flow.runtime.message.TaskNodeExecuteMessage;
import com.cmvr.test.flow.runtime.message.TaskNodeExecuteResult;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

import java.util.Objects;

/**
 * 机械臂边缘操作服务
 * 处理机械臂相关的设备行为，如末端运动到指定位置
 *
 * @author cmvr-iot
 * @since 2026-07-01
 */
@Slf4j
@Service
@RequiredArgsConstructor
public class EdgeArmOperateService implements EdgeOperateService {

    private final EdgeArmService edgeArmService;

    @Override
    public boolean supports(ActionEnum action) {
        return action.name().startsWith("ARM_");
    }

    @Override
    public TaskNodeExecuteResult execute(TaskNodeExecuteMessage message) {
        ActionEnum action = message.getAction();
        JSONObject inputParams = message.getInputParams();
        String terminalId = message.getTerminalId();
        
        // 从输入参数中获取设备ID
        String deviceId = inputParams.getString("deviceId");
        
        if (deviceId == null || deviceId.isEmpty()) {
            throw new GlobalException("设备ID(deviceId)不能为空");
        }
        // 构建边缘端通用参数
        EdgeCommonVO edgeCommonVO = new EdgeCommonVO();
        edgeCommonVO.setTerminalId(terminalId);
        edgeCommonVO.setDeviceId(deviceId);

        // 先调用开启力矩
        edgeArmService.torqueOn(edgeCommonVO);

        if (Objects.requireNonNull(action) == ActionEnum.ARM_MOVE_TO_POINT) {
            log.info("执行机械臂末端运动到指定点操作，设备ID: {}", deviceId);

            // 从输入参数中获取坐标信息
            Double x = inputParams.getDouble("x");
            Double y = inputParams.getDouble("y");
            Double z = inputParams.getDouble("z");
            Double rx = inputParams.getDouble("rx");
            Double ry = inputParams.getDouble("ry");
            Double rz = inputParams.getDouble("rz");
            String frame = inputParams.getString("frame");
            Double velocity = inputParams.getDouble("velocity");
            Double acceleration = inputParams.getDouble("acceleration");
            Double blendRadius = inputParams.getDouble("blendRadius");

            // 参数验证
            if (x == null || y == null || z == null) {
                throw new GlobalException("X、Y、Z坐标不能为空");
            }
            if (rx == null || ry == null || rz == null) {
                throw new GlobalException("RX、RY、RZ旋转角度不能为空");
            }

            // 直接调用边缘端机械臂服务执行笛卡尔空间直线运动
            edgeArmService.moveL(
                    edgeCommonVO,
                    x,
                    y,
                    z,
                    rx,
                    ry,
                    rz,
                    frame,
                    velocity,
                    acceleration,
                    blendRadius
            );

            log.info("机械臂末端运动任务下发成功");
        } else {
            throw new GlobalException("不支持的机械臂操作类型: " + action);
        }
        
        return TaskNodeExecuteResult.success();
    }
}
