package com.cmvr.edge.client.service.impl;

import cmvr.api.ArmCommand;
import cmvr.api.ArmServiceGrpc;
import cmvr.api.Common;
import cn.hutool.core.util.StrUtil;
import com.cmvr.common.exception.GlobalException;
import com.cmvr.edge.client.manage.GrpcServiceManager;
import com.cmvr.edge.client.model.EdgeCommonVO;
import com.cmvr.edge.client.service.EdgeArmService;
import com.cmvr.edge.client.utils.EdgeCommonUtil;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

import java.util.Arrays;

/**
 * 机械臂服务实现类
 *
 * @author cmvr-iot
 * @since 2026-07-01
 */
@Slf4j
@Service
@RequiredArgsConstructor
public class EdgeArmServiceImpl implements EdgeArmService {

    private final GrpcServiceManager grpcServiceManager;

    @Override
    public Common.CommandHeader.Feedback torqueOff(EdgeCommonVO edgeCommonVO) {
        ArmServiceGrpc.ArmServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
            edgeCommonVO.getTerminalId(), ArmServiceGrpc.ArmServiceBlockingStub.class);
        Common.CommandHeader.Request request = EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId());
        return executeGrpcCall(() -> stub.torqueOff(request));
    }

    @Override
    public Common.CommandHeader.Feedback torqueOn(EdgeCommonVO edgeCommonVO) {
        ArmServiceGrpc.ArmServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
            edgeCommonVO.getTerminalId(), ArmServiceGrpc.ArmServiceBlockingStub.class);
        Common.CommandHeader.Request request = EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId());
        return executeGrpcCall(() -> stub.torqueOn(request));
    }

    @Override
    public Common.CommandHeader.Feedback clearFault(EdgeCommonVO edgeCommonVO) {
        ArmServiceGrpc.ArmServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
            edgeCommonVO.getTerminalId(), ArmServiceGrpc.ArmServiceBlockingStub.class);
        Common.CommandHeader.Request request = EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId());
        return executeGrpcCall(() -> stub.clearFault(request));
    }

    @Override
    public ArmCommand.MoveJ.Response moveJ(EdgeCommonVO edgeCommonVO, double[] target, Double velocity,
                                            Double acceleration, Double blendRadius,
                                            double[] jointVelocityLimits, Boolean asynchronous) {
        if (target == null || target.length == 0) {
            throw new GlobalException("目标关节位置不能为空");
        }

        ArmServiceGrpc.ArmServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
            edgeCommonVO.getTerminalId(), ArmServiceGrpc.ArmServiceBlockingStub.class);

        // 构建关节位置命令
        ArmCommand.JointPositionCommand targetCommand = ArmCommand.JointPositionCommand.newBuilder()
            .addAllPosition(Arrays.asList(
                Arrays.stream(target).boxed().toArray(Double[]::new)
            ))
            .build();

        // 构建运动选项
        ArmCommand.MotionOptions.Builder optionsBuilder = ArmCommand.MotionOptions.newBuilder();
        if (velocity != null) {
            optionsBuilder.setVelocity(velocity);
        }
        if (acceleration != null) {
            optionsBuilder.setAcceleration(acceleration);
        }
        if (blendRadius != null) {
            optionsBuilder.setBlendRadius(blendRadius);
        }
        if (jointVelocityLimits != null && jointVelocityLimits.length > 0) {
            optionsBuilder.addAllJointVelocityLimits(Arrays.asList(
                Arrays.stream(jointVelocityLimits).boxed().toArray(Double[]::new)
            ));
        }
        if (asynchronous != null) {
            optionsBuilder.setAsynchronous(asynchronous);
        }

        ArmCommand.MoveJ.Request request = ArmCommand.MoveJ.Request.newBuilder()
            .setHeader(EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId()))
            .setTarget(targetCommand)
            .setOptions(optionsBuilder.build())
            .build();

        return executeGrpcCall(() -> stub.moveJ(request));
    }

    @Override
    public ArmCommand.MoveL.Response moveL(EdgeCommonVO edgeCommonVO, double x, double y, double z,
                                            double rx, double ry, double rz, String frame,
                                            Double velocity, Double acceleration, Double blendRadius) {
        ArmServiceGrpc.ArmServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
            edgeCommonVO.getTerminalId(), ArmServiceGrpc.ArmServiceBlockingStub.class);

        // 构建笛卡尔位姿
        ArmCommand.CartesianPose targetPose = ArmCommand.CartesianPose.newBuilder()
            .setX(x)
            .setY(y)
            .setZ(z)
            .setRx(rx)
            .setRy(ry)
            .setRz(rz)
            .build();

        // 构建运动选项
        ArmCommand.MotionOptions.Builder optionsBuilder = ArmCommand.MotionOptions.newBuilder();
        if (velocity != null) {
            optionsBuilder.setVelocity(velocity);
        }
        if (acceleration != null) {
            optionsBuilder.setAcceleration(acceleration);
        }
        if (blendRadius != null) {
            optionsBuilder.setBlendRadius(blendRadius);
        }

        // 确定坐标系类型
        ArmCommand.ArmFrameType frameType = parseFrameType(frame);

        ArmCommand.MoveL.Request request = ArmCommand.MoveL.Request.newBuilder()
            .setHeader(EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId()))
            .setTarget(targetPose)
            .setOptions(optionsBuilder.build())
            .setFrame(frameType)
            .build();

        return executeGrpcCall(() -> stub.moveL(request));
    }

    @Override
    public ArmCommand.SpeedJ.Response speedJ(EdgeCommonVO edgeCommonVO, double[] velocities,
                                              Double acceleration, Double duration) {
        if (velocities == null || velocities.length == 0) {
            throw new GlobalException("关节速度数组不能为空");
        }

        ArmServiceGrpc.ArmServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
            edgeCommonVO.getTerminalId(), ArmServiceGrpc.ArmServiceBlockingStub.class);

        // 构建关节速度命令
        ArmCommand.JointVelocityCommand velocityCommand = ArmCommand.JointVelocityCommand.newBuilder()
            .addAllVelocity(Arrays.asList(
                Arrays.stream(velocities).boxed().toArray(Double[]::new)
            ))
            .build();

        ArmCommand.SpeedJ.Request.Builder requestBuilder = ArmCommand.SpeedJ.Request.newBuilder()
            .setHeader(EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId()))
            .setVelocity(velocityCommand);

        if (acceleration != null) {
            requestBuilder.setAcceleration(acceleration);
        }
        if (duration != null) {
            requestBuilder.setDuration(duration);
        }

        return executeGrpcCall(() -> stub.speedJ(requestBuilder.build()));
    }

    @Override
    public ArmCommand.SpeedL.Response speedL(EdgeCommonVO edgeCommonVO, double vx, double vy, double vz,
                                              double wx, double wy, double wz, String frame,
                                              Double acceleration, Double duration) {
        ArmServiceGrpc.ArmServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
            edgeCommonVO.getTerminalId(), ArmServiceGrpc.ArmServiceBlockingStub.class);

        // 构建笛卡尔速度
        ArmCommand.CartesianVelocity velocity = ArmCommand.CartesianVelocity.newBuilder()
            .setVx(vx)
            .setVy(vy)
            .setVz(vz)
            .setWx(wx)
            .setWy(wy)
            .setWz(wz)
            .build();

        ArmCommand.SpeedL.Request.Builder requestBuilder = ArmCommand.SpeedL.Request.newBuilder()
            .setHeader(EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId()))
            .setVelocity(velocity);

        if (acceleration != null) {
            requestBuilder.setAcceleration(acceleration);
        }
        if (duration != null) {
            requestBuilder.setDuration(duration);
        }

        // 确定坐标系类型
        ArmCommand.ArmFrameType frameType = parseFrameType(frame);
        requestBuilder.setFrame(frameType);

        return executeGrpcCall(() -> stub.speedL(requestBuilder.build()));
    }

    @Override
    public ArmCommand.ServoJ.Response servoJ(EdgeCommonVO edgeCommonVO, double[] target) {
        if (target == null || target.length == 0) {
            throw new GlobalException("目标关节位置不能为空");
        }

        ArmServiceGrpc.ArmServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
            edgeCommonVO.getTerminalId(), ArmServiceGrpc.ArmServiceBlockingStub.class);

        // 构建关节位置命令
        ArmCommand.JointPositionCommand targetCommand = ArmCommand.JointPositionCommand.newBuilder()
            .addAllPosition(Arrays.asList(
                Arrays.stream(target).boxed().toArray(Double[]::new)
            ))
            .build();

        ArmCommand.ServoJ.Request request = ArmCommand.ServoJ.Request.newBuilder()
            .setHeader(EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId()))
            .setTarget(targetCommand)
            .build();

        return executeGrpcCall(() -> stub.servoJ(request));
    }

    @Override
    public Common.CommandHeader.Feedback stopMotion(EdgeCommonVO edgeCommonVO) {
        ArmServiceGrpc.ArmServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
            edgeCommonVO.getTerminalId(), ArmServiceGrpc.ArmServiceBlockingStub.class);
        Common.CommandHeader.Request request = EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId());
        return executeGrpcCall(() -> stub.stopMotion(request));
    }

    @Override
    public ArmCommand.JointResponse getJointState(EdgeCommonVO edgeCommonVO) {
        ArmServiceGrpc.ArmServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
            edgeCommonVO.getTerminalId(), ArmServiceGrpc.ArmServiceBlockingStub.class);
        ArmCommand.JointRequest request = ArmCommand.JointRequest.newBuilder()
            .setHeader(EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId()))
            .build();
        return executeGrpcCall(() -> stub.getJointState(request));
    }

    @Override
    public ArmCommand.GetPose.Response getPose(EdgeCommonVO edgeCommonVO, String baseLink, String eeLink) {
        ArmServiceGrpc.ArmServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
            edgeCommonVO.getTerminalId(), ArmServiceGrpc.ArmServiceBlockingStub.class);

        ArmCommand.GetPose.Request.Builder requestBuilder = ArmCommand.GetPose.Request.newBuilder()
            .setHeader(EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId()));

        if (StrUtil.isNotBlank(baseLink)) {
            requestBuilder.setBaseLink(baseLink);
        }
        if (StrUtil.isNotBlank(eeLink)) {
            requestBuilder.setEeLink(eeLink);
        }

        return executeGrpcCall(() -> stub.getPose(requestBuilder.build()));
    }

    @Override
    public ArmCommand.CalibrateZeroQ.Response calibrateZeroQ(EdgeCommonVO edgeCommonVO, String jointName) {
        ArmServiceGrpc.ArmServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
            edgeCommonVO.getTerminalId(), ArmServiceGrpc.ArmServiceBlockingStub.class);

        ArmCommand.CalibrateZeroQ.Request.Builder requestBuilder = ArmCommand.CalibrateZeroQ.Request.newBuilder()
            .setHeader(EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId()));

        if (StrUtil.isNotBlank(jointName)) {
            requestBuilder.setJointName(jointName);
        }

        return executeGrpcCall(() -> stub.calibrateZeroQ(requestBuilder.build()));
    }

    @Override
    public ArmCommand.GetPoseMatrix.Response getPoseMatrix(EdgeCommonVO edgeCommonVO, String baseLink, String eeLink) {
        ArmServiceGrpc.ArmServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
            edgeCommonVO.getTerminalId(), ArmServiceGrpc.ArmServiceBlockingStub.class);

        ArmCommand.GetPoseMatrix.Request.Builder requestBuilder = ArmCommand.GetPoseMatrix.Request.newBuilder()
            .setHeader(EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId()));

        if (StrUtil.isNotBlank(baseLink)) {
            requestBuilder.setBaseLink(baseLink);
        }
        if (StrUtil.isNotBlank(eeLink)) {
            requestBuilder.setEeLink(eeLink);
        }

        return executeGrpcCall(() -> stub.getPoseMatrix(requestBuilder.build()));
    }

    @Override
    public ArmCommand.ComputeForwardKinematics.Response computeForwardKinematics(EdgeCommonVO edgeCommonVO,
                                                                                  double[] joints,
                                                                                  String baseLink,
                                                                                  String eeLink) {
        if (joints == null || joints.length == 0) {
            throw new GlobalException("关节位置数组不能为空");
        }

        ArmServiceGrpc.ArmServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
            edgeCommonVO.getTerminalId(), ArmServiceGrpc.ArmServiceBlockingStub.class);

        // 构建关节位置命令
        ArmCommand.JointPositionCommand jointsCommand = ArmCommand.JointPositionCommand.newBuilder()
            .addAllPosition(Arrays.asList(
                Arrays.stream(joints).boxed().toArray(Double[]::new)
            ))
            .build();

        ArmCommand.ComputeForwardKinematics.Request.Builder requestBuilder = ArmCommand.ComputeForwardKinematics.Request.newBuilder()
            .setHeader(EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId()))
            .setJoints(jointsCommand);

        if (StrUtil.isNotBlank(baseLink)) {
            requestBuilder.setBaseLink(baseLink);
        }
        if (StrUtil.isNotBlank(eeLink)) {
            requestBuilder.setEeLink(eeLink);
        }

        return executeGrpcCall(() -> stub.computeForwardKinematics(requestBuilder.build()));
    }

    /**
     * 解析坐标系类型
     *
     * @param frame 坐标系字符串
     * @return 坐标系枚举
     */
    private ArmCommand.ArmFrameType parseFrameType(String frame) {
        if (StrUtil.isBlank(frame)) {
            return ArmCommand.ArmFrameType.ARM_FRAME_BASE;
        }

        switch (frame.toUpperCase()) {
            case "BASE":
                return ArmCommand.ArmFrameType.ARM_FRAME_BASE;
            case "TOOL":
                return ArmCommand.ArmFrameType.ARM_FRAME_TOOL;
            case "WORLD":
                return ArmCommand.ArmFrameType.ARM_FRAME_WORLD;
            case "USER":
                return ArmCommand.ArmFrameType.ARM_FRAME_USER;
            default:
                log.warn("未知的坐标系类型: {}, 使用默认值 BASE", frame);
                return ArmCommand.ArmFrameType.ARM_FRAME_BASE;
        }
    }

    /**
     * 执行gRPC调用并处理异常
     *
     * @param call gRPC调用
     * @param <T> 返回类型
     * @return 调用结果
     */
    private <T> T executeGrpcCall(java.util.function.Supplier<T> call) {
        try {
            return call.get();
        } catch (Exception e) {
            log.error("机械臂 gRPC调用失败", e);
            throw new GlobalException("机械臂通信失败: " + e.getMessage());
        }
    }
}
