package com.cmvr.web.controller.api;

import cmvr.api.ArmCommand;
import com.cmvr.common.core.domain.AjaxResult;
import com.cmvr.edge.client.model.EdgeCommonVO;
import com.cmvr.edge.client.model.arm.*;
import com.cmvr.edge.client.service.EdgeArmService;
import io.swagger.annotations.Api;
import io.swagger.annotations.ApiOperation;
import lombok.RequiredArgsConstructor;
import org.springframework.validation.annotation.Validated;
import org.springframework.web.bind.annotation.GetMapping;
import org.springframework.web.bind.annotation.PostMapping;
import org.springframework.web.bind.annotation.RequestBody;
import org.springframework.web.bind.annotation.RequestMapping;
import org.springframework.web.bind.annotation.RestController;

/**
 * 边缘系统机械臂控制器
 *
 * @author cmvr-iot
 * @since 2026-07-01
 */
@Api(tags = "边缘--机械臂")
@RestController
@RequestMapping("/api/arm")
@RequiredArgsConstructor
public class EdgeArmController {

    private final EdgeArmService edgeArmService;

    @ApiOperation("关闭力矩")
    @GetMapping("/torqueOff")
    public AjaxResult torqueOff(EdgeCommonVO vo) {
        edgeArmService.torqueOff(vo);
        return AjaxResult.ok();
    }

    @ApiOperation("开启力矩")
    @GetMapping("/torqueOn")
    public AjaxResult torqueOn(EdgeCommonVO vo) {
        edgeArmService.torqueOn(vo);
        return AjaxResult.ok();
    }

    /**
     * 清除机械臂故障状态。
     */
    @ApiOperation("清除故障")
    @GetMapping("/clearFault")
    public AjaxResult clearFault(EdgeCommonVO vo) {
        edgeArmService.clearFault(vo);
        return AjaxResult.ok();
    }

    @ApiOperation("关节空间运动")
    @PostMapping("/moveJ")
    public AjaxResult moveJ(@RequestBody EdgeArmMoveJVO moveJVO) {
        edgeArmService.moveJ(
            moveJVO,
            moveJVO.getTarget(),
            moveJVO.getVelocity(),
            moveJVO.getAcceleration(),
            moveJVO.getBlendRadius(),
            moveJVO.getJointVelocityLimits(),
            moveJVO.getAsynchronous()
        );
        return AjaxResult.ok();
    }

    @ApiOperation("笛卡尔空间直线运动")
    @PostMapping("/moveL")
    public AjaxResult moveL(@RequestBody EdgeArmMoveLVO moveLVO) {
        edgeArmService.moveL(
            moveLVO,
            moveLVO.getX(),
            moveLVO.getY(),
            moveLVO.getZ(),
            moveLVO.getRx(),
            moveLVO.getRy(),
            moveLVO.getRz(),
            moveLVO.getFrame(),
            moveLVO.getVelocity(),
            moveLVO.getAcceleration(),
            moveLVO.getBlendRadius()
        );
        return AjaxResult.ok();
    }

    @ApiOperation("关节速度控制")
    @PostMapping("/speedJ")
    public AjaxResult speedJ(@RequestBody EdgeArmSpeedJVO speedJVO) {
        edgeArmService.speedJ(
            speedJVO,
            speedJVO.getVelocities(),
            speedJVO.getAcceleration(),
            speedJVO.getDuration()
        );
        return AjaxResult.ok();
    }

    @ApiOperation("笛卡尔速度控制")
    @PostMapping("/speedL")
    public AjaxResult speedL(@RequestBody EdgeArmSpeedLVO speedLVO) {
        edgeArmService.speedL(
            speedLVO,
            speedLVO.getVx(),
            speedLVO.getVy(),
            speedLVO.getVz(),
            speedLVO.getWx(),
            speedLVO.getWy(),
            speedLVO.getWz(),
            speedLVO.getFrame(),
            speedLVO.getAcceleration(),
            speedLVO.getDuration()
        );
        return AjaxResult.ok();
    }

    @ApiOperation("伺服关节控制")
    @PostMapping("/servoJ")
    public AjaxResult servoJ(@RequestBody EdgeArmServoJVO servoJVO) {
        edgeArmService.servoJ(servoJVO, servoJVO.getTarget());
        return AjaxResult.ok();
    }

    @ApiOperation("停止运动")
    @GetMapping("/stopMotion")
    public AjaxResult stopMotion(EdgeCommonVO vo) {
        edgeArmService.stopMotion(vo);
        return AjaxResult.ok();
    }

    @ApiOperation("获取关节状态")
    @GetMapping("/getJointState")
    public AjaxResult getJointState(EdgeCommonVO vo) {
        ArmCommand.JointResponse result = edgeArmService.getJointState(vo);
        ArmCommand.JointState state = result.getState();
        
        // 转换为VO
        EdgeArmJointStateVO jointStateVO = new EdgeArmJointStateVO();
        jointStateVO.setName(state.getNameList());
        jointStateVO.setPosition(state.getPositionList());
        jointStateVO.setVelocity(state.getVelocityList());
        jointStateVO.setEffort(state.getEffortList());
        jointStateVO.setTimestamp(state.getTimestamp());
        
        return AjaxResult.ok(jointStateVO);
    }

    @ApiOperation("获取末端位姿")
    @GetMapping("/getPose")
    public AjaxResult getPose(@Validated EdgeArmPoseQueryVO vo) {
        ArmCommand.GetPose.Response result = edgeArmService.getPose(vo, vo.getBaseLink(), vo.getEeLink());
        ArmCommand.CartesianPose pose = result.getPose();
        
        // 转换为VO
        EdgeArmCartesianPoseVO poseVO = new EdgeArmCartesianPoseVO();
        poseVO.setX(pose.getX());
        poseVO.setY(pose.getY());
        poseVO.setZ(pose.getZ());
        poseVO.setRx(pose.getRx());
        poseVO.setRy(pose.getRy());
        poseVO.setRz(pose.getRz());
        
        return AjaxResult.ok(poseVO);
    }

    @ApiOperation("标定零点")
    @GetMapping("/calibrateZeroQ")
    public AjaxResult calibrateZeroQ(@Validated EdgeArmCalibrateZeroQVO vo) {
        edgeArmService.calibrateZeroQ(vo, vo.getJointName());
        return AjaxResult.ok();
    }

    @ApiOperation("获取位姿矩阵")
    @GetMapping("/getPoseMatrix")
    public AjaxResult getPoseMatrix(@Validated EdgeArmPoseQueryVO vo) {
        ArmCommand.GetPoseMatrix.Response result = edgeArmService.getPoseMatrix(vo, vo.getBaseLink(), vo.getEeLink());
        // 将TransformMatrix4x4转换为Map
        ArmCommand.TransformMatrix4x4 matrix = result.getMatrix();
        java.util.Map<String, Object> matrixMap = new java.util.HashMap<>();
        matrixMap.put("m00", matrix.getM00());
        matrixMap.put("m01", matrix.getM01());
        matrixMap.put("m02", matrix.getM02());
        matrixMap.put("m03", matrix.getM03());
        matrixMap.put("m10", matrix.getM10());
        matrixMap.put("m11", matrix.getM11());
        matrixMap.put("m12", matrix.getM12());
        matrixMap.put("m13", matrix.getM13());
        matrixMap.put("m20", matrix.getM20());
        matrixMap.put("m21", matrix.getM21());
        matrixMap.put("m22", matrix.getM22());
        matrixMap.put("m23", matrix.getM23());
        matrixMap.put("m30", matrix.getM30());
        matrixMap.put("m31", matrix.getM31());
        matrixMap.put("m32", matrix.getM32());
        matrixMap.put("m33", matrix.getM33());
        return AjaxResult.ok(matrixMap);
    }

    @ApiOperation("计算正向运动学")
    @PostMapping("/computeForwardKinematics")
    public AjaxResult computeForwardKinematics(@RequestBody EdgeArmForwardKinematicsVO fkVO) {
        ArmCommand.ComputeForwardKinematics.Response result = edgeArmService.computeForwardKinematics(
            fkVO,
            fkVO.getJoints(),
            fkVO.getBaseLink(),
            fkVO.getEeLink()
        );
        // 将TransformMatrix4x4转换为Map
        ArmCommand.TransformMatrix4x4 matrix = result.getMatrix();
        java.util.Map<String, Object> matrixMap = new java.util.HashMap<>();
        matrixMap.put("m00", matrix.getM00());
        matrixMap.put("m01", matrix.getM01());
        matrixMap.put("m02", matrix.getM02());
        matrixMap.put("m03", matrix.getM03());
        matrixMap.put("m10", matrix.getM10());
        matrixMap.put("m11", matrix.getM11());
        matrixMap.put("m12", matrix.getM12());
        matrixMap.put("m13", matrix.getM13());
        matrixMap.put("m20", matrix.getM20());
        matrixMap.put("m21", matrix.getM21());
        matrixMap.put("m22", matrix.getM22());
        matrixMap.put("m23", matrix.getM23());
        matrixMap.put("m30", matrix.getM30());
        matrixMap.put("m31", matrix.getM31());
        matrixMap.put("m32", matrix.getM32());
        matrixMap.put("m33", matrix.getM33());
        return AjaxResult.ok(matrixMap);
    }
}
