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

import cmvr.api.Common;
import cmvr.api.HumanoidRobotCommand;
import cmvr.api.HumanoidRobotServiceGrpc;
import com.alibaba.fastjson2.JSON;
import com.cmvr.edge.client.manage.GrpcServiceManager;
import com.cmvr.edge.client.model.EdgeCommonVO;
import com.cmvr.edge.client.model.humanoid.EdgeMoveJVO;
import com.cmvr.edge.client.model.humanoid.EdgeSpeedJVO;
import com.cmvr.edge.client.service.EdgeHumanoidRobotService;
import com.cmvr.edge.client.utils.EdgeCommonUtil;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

@Slf4j
@Service
@RequiredArgsConstructor
public class EdgeHumanoidRobotServiceImpl implements EdgeHumanoidRobotService {

    private final GrpcServiceManager grpcServiceManager;

    @Override
    public String moveJ(EdgeMoveJVO vo) {
        HumanoidRobotServiceGrpc.HumanoidRobotServiceBlockingStub stub =
                grpcServiceManager.getGrpcClient(vo.getTerminalId(),
                        HumanoidRobotServiceGrpc.HumanoidRobotServiceBlockingStub.class);

        Common.CommandHeader.Request header = EdgeCommonUtil.buildRequest(vo.getDeviceId());

        HumanoidRobotCommand.MoveJ.Request.Builder builder = HumanoidRobotCommand.MoveJ.Request.newBuilder()
                .setHeader(header)
                .setVel(vo.getVel())
                .setAcc(vo.getAcc());

        if (vo.getCmds() != null) {
            for (EdgeMoveJVO.JointCmd j : vo.getCmds()) {
                builder.addCmds(HumanoidRobotCommand.JointCmd.newBuilder()
                        .setJointName(j.getJointName())
                        .setRad(j.getRad())
                        .setVel(j.getVel())
                        .build());
            }
        }

//        log.info("movej start time={}", System.currentTimeMillis());

        HumanoidRobotCommand.MoveJ.Response response = stub.moveJ(builder.build());
//        log.info("movej success time={}", System.currentTimeMillis());
        return JSON.toJSONString(response.getHeader());
    }

    @Override
    public String getJointState(EdgeCommonVO vo) {
        HumanoidRobotServiceGrpc.HumanoidRobotServiceBlockingStub stub =
                grpcServiceManager.getGrpcClient(vo.getTerminalId(), HumanoidRobotServiceGrpc.HumanoidRobotServiceBlockingStub.class);

        Common.CommandHeader.Request header = EdgeCommonUtil.buildRequest(vo.getDeviceId());
        HumanoidRobotCommand.JointRequest build = HumanoidRobotCommand.JointRequest.newBuilder()
                .setHeader(header)
                .build();
        HumanoidRobotCommand.JointResponse response = stub.getJointState(build);
        return JSON.toJSONString(response.getStateList());
    }

    @Override
    public String speedJ(EdgeSpeedJVO vo) {
        HumanoidRobotServiceGrpc.HumanoidRobotServiceBlockingStub stub =
                grpcServiceManager.getGrpcClient(vo.getTerminalId(), HumanoidRobotServiceGrpc.HumanoidRobotServiceBlockingStub.class);

        Common.CommandHeader.Request header = EdgeCommonUtil.buildRequest(vo.getDeviceId());
        HumanoidRobotCommand.SpeedJ.Request request = HumanoidRobotCommand.SpeedJ.Request.newBuilder()
                .setHeader(header)
                .setAcc(vo.getAcc())
                .setVel(vo.getVel())
                .setDir(vo.getDir())
                .build();
        HumanoidRobotCommand.SpeedJ.Response response = stub.speedJ(request);
        return JSON.toJSONString(response.getHeader());
    }

    @Override
    public String torqueOn(EdgeCommonVO vo) {
        HumanoidRobotServiceGrpc.HumanoidRobotServiceBlockingStub stub =
                grpcServiceManager.getGrpcClient(vo.getTerminalId(), HumanoidRobotServiceGrpc.HumanoidRobotServiceBlockingStub.class);

        Common.CommandHeader.Request header = EdgeCommonUtil.buildRequest(vo.getDeviceId());
        Common.CommandHeader.Feedback response = stub.torqueOn(header);
        return JSON.toJSONString(response);
    }

    @Override
    public String torqueOff(EdgeCommonVO vo) {
        HumanoidRobotServiceGrpc.HumanoidRobotServiceBlockingStub stub =
                grpcServiceManager.getGrpcClient(vo.getTerminalId(), HumanoidRobotServiceGrpc.HumanoidRobotServiceBlockingStub.class);

        Common.CommandHeader.Request header = EdgeCommonUtil.buildRequest(vo.getDeviceId());
        Common.CommandHeader.Feedback response = stub.torqueOff(header);
        return JSON.toJSONString(response);
    }
}
