package com.ruoyi.websocket.service;

import com.alibaba.fastjson2.JSON;
import com.alibaba.fastjson2.JSONArray;
import com.alibaba.fastjson2.JSONObject;
import com.ruoyi.mqtt.constant.MqttTopicConstants;
import com.ruoyi.mqtt.util.MqttTopicUtil;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.messaging.simp.SimpMessagingTemplate;
import org.springframework.stereotype.Service;

/**
 * WebSocket消息推送服务
 * 将MQTT接收到的机器人数据实时推送给WebSocket客户端
 */
@Service
public class WebSocketPushService {

    private static final Logger log = LoggerFactory.getLogger(WebSocketPushService.class);

    @Autowired
    private SimpMessagingTemplate messagingTemplate;

    /**
     * 推送机器人位姿数据到WebSocket
     * @param deviceId 设备ID
     * @param payload MQTT消息体
     */
    public void pushRobotPose(String deviceId, String payload) {
        try {
            JSONObject json = JSON.parseObject(payload);
            JSONArray argsArray = json.getJSONArray("args");
            if (argsArray != null && !argsArray.isEmpty()) {
                JSONObject poseData = argsArray.getJSONObject(0);
                JSONObject pose = poseData.getJSONObject("pose");
                JSONObject vel = poseData.getJSONObject("vel");
                JSONObject rtk = poseData.getJSONObject("rtk");
                
                JSONObject pushData = new JSONObject();
                pushData.put("deviceId", deviceId);
                pushData.put("type", "pose");
                pushData.put("timestamp", System.currentTimeMillis());
                
                JSONObject data = new JSONObject();
                if (pose != null) {
                    data.put("xyz", pose.getJSONArray("xyz"));
                    data.put("rpy", pose.getJSONArray("rpy"));
                    data.put("heading", pose.getDouble("heading"));
                }
                if (vel != null) {
                    data.put("velocity", vel.getDouble("enu"));
                    data.put("heading", vel.getDouble("heading"));
                }
                data.put("confidence", poseData.getDouble("score"));
                data.put("runState", poseData.getInteger("runState"));
                data.put("odometer", poseData.getDouble("odometer"));
                data.put("replan", poseData.getDouble("replan"));

                if (rtk != null) {
                    JSONObject rtkInfo = new JSONObject();
                    rtkInfo.put("star", rtk.getInteger("star"));
                    rtkInfo.put("status", rtk.getInteger("status"));
                    data.put("rtk", rtkInfo);
                }
                
                pushData.put("data", data);
                
                // 推送到设备专属频道
                messagingTemplate.convertAndSend("/topic/robot/" + deviceId + "/pose", pushData);
                // 也推送到通用频道
                messagingTemplate.convertAndSend("/topic/robot/all/pose", pushData);
            }
        } catch (Exception e) {
            log.error("推送机器人位姿数据失败", e);
        }
    }

    /**
     * 推送任务实时信息
     * @param deviceId 设备ID
     * @param payload MQTT消息体
     */
    public void pushTaskRealtimeInfo(String deviceId, String payload) {
        try {
            JSONObject json = JSON.parseObject(payload);
            JSONArray argsArray = json.getJSONArray("args");
            if (argsArray != null && !argsArray.isEmpty()) {
                JSONObject taskInfo = argsArray.getJSONObject(0);
                
                JSONObject pushData = new JSONObject();
                pushData.put("deviceId", deviceId);
                pushData.put("type", "task_realtime");
                pushData.put("timestamp", System.currentTimeMillis());
                pushData.put("data", taskInfo);
                
                messagingTemplate.convertAndSend("/topic/robot/" + deviceId + "/task", pushData);
            }
        } catch (Exception e) {
            log.error("推送任务实时信息失败", e);
        }
    }

    /**
     * 推送轨迹数据
     * @param deviceId 设备ID
     * @param shortTopic 主题类型（trajectory 或 trajectory/2d/compact）
     * @param payload MQTT消息体
     */
    public void pushTrajectory(String deviceId, String shortTopic, String payload) {
        try {
            JSONObject pushData = new JSONObject();
            pushData.put("deviceId", deviceId);
            pushData.put("type", "trajectory");
            pushData.put("subType", shortTopic);
            pushData.put("timestamp", System.currentTimeMillis());
            pushData.put("data", payload);
            
            messagingTemplate.convertAndSend("/topic/robot/" + deviceId + "/trajectory", pushData);
        } catch (Exception e) {
            log.error("推送轨迹数据失败", e);
        }
    }

    /**
     * 推送到达事件
     * @param deviceId 设备ID
     * @param payload MQTT消息体
     */
    public void pushArriveEvent(String deviceId, String payload) {
        try {
            JSONObject json = JSON.parseObject(payload);
            JSONArray argsArray = json.getJSONArray("args");
            if (argsArray != null && !argsArray.isEmpty()) {
                JSONObject arriveInfo = argsArray.getJSONObject(0);
                
                JSONObject pushData = new JSONObject();
                pushData.put("deviceId", deviceId);
                pushData.put("type", "arrive");
                pushData.put("timestamp", System.currentTimeMillis());
                pushData.put("data", arriveInfo);
                
                messagingTemplate.convertAndSend("/topic/robot/" + deviceId + "/event", pushData);
            }
        } catch (Exception e) {
            log.error("推送到达事件失败", e);
        }
    }

    /**
     * 推送地图列表响应
     * @param deviceId 设备ID
     * @param payload MQTT消息体
     */
    public void pushMapListResponse(String deviceId, String payload) {
        try {
            JSONObject pushData = new JSONObject();
            pushData.put("deviceId", deviceId);
            pushData.put("type", "map_list");
            pushData.put("timestamp", System.currentTimeMillis());
            pushData.put("data", payload);
            
            messagingTemplate.convertAndSend("/topic/robot/" + deviceId + "/map", pushData);
        } catch (Exception e) {
            log.error("推送地图列表响应失败", e);
        }
    }

    /**
     * 推送当前地图
     * @param deviceId 设备ID
     * @param payload MQTT消息体
     */
    public void pushCurrentMap(String deviceId, String payload) {
        try {
            JSONObject pushData = new JSONObject();
            pushData.put("deviceId", deviceId);
            pushData.put("type", "current_map");
            pushData.put("timestamp", System.currentTimeMillis());
            pushData.put("data", payload);
            
            messagingTemplate.convertAndSend("/topic/robot/" + deviceId + "/map", pushData);
        } catch (Exception e) {
            log.error("推送当前地图失败", e);
        }
    }

    /**
     * 推送导航操作响应
     * @param deviceId 设备ID
     * @param shortTopic 操作类型
     * @param payload MQTT消息体
     */
    public void pushNavigationReply(String deviceId, String shortTopic, String payload) {
        try {
            JSONObject pushData = new JSONObject();
            pushData.put("deviceId", deviceId);
            pushData.put("type", "navigation_reply");
            pushData.put("action", shortTopic);
            pushData.put("timestamp", System.currentTimeMillis());
            pushData.put("data", payload);
            
            messagingTemplate.convertAndSend("/topic/robot/" + deviceId + "/navigation", pushData);
        } catch (Exception e) {
            log.error("推送导航响应失败", e);
        }
    }

    /**
     * 推送任务控制响应
     * @param deviceId 设备ID
     * @param shortTopic 操作类型
     * @param payload MQTT消息体
     */
    public void pushTaskProcedureReply(String deviceId, String shortTopic, String payload) {
        try {
            JSONObject pushData = new JSONObject();
            pushData.put("deviceId", deviceId);
            pushData.put("type", "task_procedure_reply");
            pushData.put("action", shortTopic);
            pushData.put("timestamp", System.currentTimeMillis());
            pushData.put("data", payload);
            
            messagingTemplate.convertAndSend("/topic/robot/" + deviceId + "/task", pushData);
        } catch (Exception e) {
            log.error("推送任务控制响应失败", e);
        }
    }

    /**
     * 推送规划响应
     * @param deviceId 设备ID
     * @param shortTopic 主题
     * @param payload MQTT消息体
     */
    public void pushPlanningResponse(String deviceId, String shortTopic, String payload) {
        try {
            JSONObject pushData = new JSONObject();
            pushData.put("deviceId", deviceId);
            pushData.put("type", "planning_reply");
            pushData.put("action", shortTopic);
            pushData.put("timestamp", System.currentTimeMillis());
            pushData.put("data", payload);
            
            messagingTemplate.convertAndSend("/topic/robot/" + deviceId + "/planning", pushData);
        } catch (Exception e) {
            log.error("推送规划响应失败", e);
        }
    }

    /**
     * 推送原始MQTT消息（用于调试）
     * @param deviceId 设备ID
     * @param topic MQTT主题
     * @param payload MQTT消息体
     */
    public void pushRawMessage(String deviceId, String topic, String payload) {
        try {
            JSONObject pushData = new JSONObject();
            pushData.put("deviceId", deviceId);
            pushData.put("topic", topic);
            pushData.put("timestamp", System.currentTimeMillis());
            pushData.put("data", payload);

            messagingTemplate.convertAndSend("/topic/robot/" + deviceId + "/raw", pushData);
        } catch (Exception e) {
            log.error("推送原始消息失败", e);
        }
    }

    /**
     * 推送ASM远程调用响应
     * @param deviceId 设备ID
     * @param payload MQTT消息体
     */
    public void pushAsmReply(String deviceId, String payload) {
        try {
            JSONObject json = JSON.parseObject(payload);
            JSONObject pushData = new JSONObject();
            pushData.put("deviceId", deviceId);
            pushData.put("type", "asm_reply");
            pushData.put("function", json.getString("function"));
            pushData.put("status", json.getString("status"));
            pushData.put("pubTimestamp", json.getLong("pub_timestamp"));
            pushData.put("timestamp", System.currentTimeMillis());
            pushData.put("data", payload);

            messagingTemplate.convertAndSend("/topic/robot/" + deviceId + "/asm", pushData);
        } catch (Exception e) {
            log.error("推送ASM响应失败", e);
        }
    }

    /**
     * 推送ASM远程调用进度反馈
     * @param deviceId 设备ID
     * @param payload MQTT消息体
     */
    public void pushAsmProgress(String deviceId, String payload) {
        try {
            JSONObject json = JSON.parseObject(payload);
            JSONArray argsArray = json.getJSONArray("args");
            if (argsArray != null && !argsArray.isEmpty()) {
                JSONObject argObj = argsArray.getJSONObject(0);

                JSONObject pushData = new JSONObject();
                pushData.put("deviceId", deviceId);
                pushData.put("type", "asm_progress");
                pushData.put("function", argObj.getString("function"));
                pushData.put("progress", argObj.get("progress"));
                pushData.put("timestamp", System.currentTimeMillis());
                pushData.put("data", payload);

                messagingTemplate.convertAndSend("/topic/robot/" + deviceId + "/asm", pushData);
            }
        } catch (Exception e) {
            log.error("推送ASM进度失败", e);
        }
    }

    /**
     * 推送ASM远程调用状态反馈
     * @param deviceId 设备ID
     * @param payload MQTT消息体
     */
    public void pushAsmState(String deviceId, String payload) {
        try {
            JSONObject json = JSON.parseObject(payload);
            JSONArray argsArray = json.getJSONArray("args");
            if (argsArray != null && !argsArray.isEmpty()) {
                JSONObject argObj = argsArray.getJSONObject(0);

                JSONObject pushData = new JSONObject();
                pushData.put("deviceId", deviceId);
                pushData.put("type", "asm_state");
                pushData.put("function", argObj.getString("function"));
                pushData.put("state", argObj.getInteger("state"));
                pushData.put("timestamp", System.currentTimeMillis());
                pushData.put("data", payload);

                messagingTemplate.convertAndSend("/topic/robot/" + deviceId + "/asm", pushData);
            }
        } catch (Exception e) {
            log.error("推送ASM状态失败", e);
        }
    }

    /**
     * 推送电池信息
     * @param deviceId 设备ID
     * @param payload MQTT消息体
     */
    public void pushBatteryInfo(String deviceId, String payload) {
        try {
            JSONObject json = JSON.parseObject(payload);
            JSONArray argsArray = json.getJSONArray("args");
            if (argsArray != null && !argsArray.isEmpty()) {
                JSONObject batteryData = argsArray.getJSONObject(0);

                JSONObject pushData = new JSONObject();
                pushData.put("deviceId", deviceId);
                pushData.put("type", "battery");
                pushData.put("timestamp", System.currentTimeMillis());

                // 提取电池信息
                JSONObject battery = new JSONObject();
                battery.put("capacity", batteryData.getDouble("capacity"));      // 电量百分比 0.0~1.0
                battery.put("temperature", batteryData.getDouble("temperature")); // 温度
                battery.put("voltage", batteryData.getDouble("voltage"));      // 电压
                battery.put("current", batteryData.getDouble("current"));      // 电流
                battery.put("charging", batteryData.getBoolean("charging"));   // 充电状态
                battery.put("other", batteryData.getJSONArray("other"));      // 其他设备电量

                pushData.put("data", battery);

                messagingTemplate.convertAndSend("/topic/robot/" + deviceId + "/battery", pushData);
                log.debug("电池信息已推送: {}", battery);
            }
        } catch (Exception e) {
            log.error("推送电池信息失败", e);
        }
    }
}
