#include "SimMsg.h"
#include "GncS.h"

#include <iostream>
#include <string>
#include <vector>
#include <mutex>


using namespace std;

static mutex telemetry_mutex;

void GncSWriteLog(const std::string &msg)
{
	std::cout << msg<<std::endl;
}

string topic_name_cmd = "Command";
string topic_name_tlm = "Telemetry";

SimMsg* GncS_part = nullptr;
string servername = "Gnc_Service";

//初始化
void GncS_init(uint8_t domainid, std::string appname)
{
    std::vector<std::string> parameters;
    string  expression = "dest = '"+ servername + "'";
	if (nullptr == GncS_part)
	{
		GncS_part=new SimMsg(domainid, 3000, appname, GncSWriteLog);
        GncS_part->create_pub(topic_name_cmd);
		GncS_part->create_pub(topic_name_tlm);
		
        GncS_part->create_sub(topic_name_cmd, command_callback, expression, parameters);
        GncS_part->create_sub(topic_name_tlm, telemetry_callback, expression, parameters);
	}
}

// 遥控接收回调函数
void command_callback(std::string src, std::string dest, std::string type,
                      std::string reserve1, std::string reserve2,
                      std::vector<uint8_t>& data) {
    // 打印接收到的数据
    std::cout << "Command received from " << src << " to " << dest << std::endl;
    std::cout << "Type: " << type << std::endl;
    std::cout << "Data (" << data.size() << " bytes): ";
    for (size_t i = 0; i < data.size(); ++i) {
        std::cout << std::hex << std::uppercase << (int)data[i];
        if (i < data.size() - 1) {
            std::cout << ", ";
        }
    }
    std::cout << std::dec << std::endl;
}

// 遥测接收回调函数
void telemetry_callback(std::string src, std::string dest, std::string type,
                      std::string reserve1, std::string reserve2,
                      std::vector<uint8_t>& data) {
    lock_guard<mutex> lock(telemetry_mutex);
    
    // 根据源设备解析不同的数据结构
    if (src == "Star_sensor_Hardware_Service") {
        // 星敏感器数据
        if (data.size() >= sizeof(Star_sensorHS_Frame)) {
            Star_sensorHS_Frame frame;
            memcpy(&frame, data.data(), sizeof(Star_sensorHS_Frame));
            
            // 打印星敏感器数据
            std::cout << "\n========== Star Sensor Telemetry ==========" << std::endl;


            std::cout << "类型(0为纳型,1为皮型): " << static_cast<int>(frame.star_type) << std::endl;
            std::cout << "标号: " << static_cast<int>(frame.star_num) << std::endl;

            // 四元数数据（十进制浮点数）
            std::cout << "q0: " << frame.q0 << std::endl;
            std::cout << "q1: " << frame.q1 << std::endl;
            std::cout << "q2: " << frame.q2 << std::endl;
            std::cout << "q3: " << frame.q3 << std::endl;
            
            std::cout << "wx: " << frame.wx << std::endl;
            std::cout << "wy: " << frame.wy << std::endl;
            std::cout << "wz: " << frame.wz << std::endl;
            
            // 状态信息（十进制整数）
            std::cout << "Star Number (detected): " << (int)frame.star_num_d << std::endl;
            
            std::cout << "Header Errors: " << frame.header_err_cnt << std::endl;
            std::cout << "Checksum Errors: " << frame.check_err_cnt << std::endl;
            std::cout << "Length Errors: " << frame.length_err_cnt << std::endl;
            std::cout << "UART Resets: " << frame.uart_reset_cnt << std::endl;
            std::cout << "Total Faults: " << frame.fault_cnt << std::endl;
            
            std::cout << "=====================================\n" << std::endl;
        } else {
            std::cout << "[ERROR] Star sensor data size mismatch. Expected: " 
                      << sizeof(Star_sensorHS_Frame) << ", Received: " << data.size() << std::endl;
        }
    }
    else if (src == "Gyro_Hardware_Service") {
        // 陀螺仪数据
        if (data.size() >= sizeof(GYRO_Info_Frame)) {
            GYRO_Info_Frame frame;
            memcpy(&frame, data.data(), sizeof(GYRO_Info_Frame));
            
            // 打印陀螺仪数据
            std::cout << "\n========== Gyroscope Telemetry ==========" << std::endl;
            
            std::cout << "X-axis: " << frame.x_angle_delta << std::endl;
            std::cout << "Y-axis: " << frame.y_angle_delta << std::endl;
            std::cout << "Z-axis: " << frame.z_angle_delta << std::endl;

            // 错误计数
            std::cout << "Header Errors: " << frame.header_err_cnt << std::endl;
            std::cout << "Checksum Errors: " << frame.check_err_cnt << std::endl;
            std::cout << "Length Errors: " << frame.length_err_cnt << std::endl;
            std::cout << "UART Resets: " << frame.uart_reset_cnt << std::endl;
            std::cout << "Total Faults: " << frame.fault_cnt << std::endl;
            
            std::cout << "=====================================\n" << std::endl;
        } else {
            std::cout << "[ERROR] Gyro data size mismatch. Expected: " 
                      << sizeof(GYRO_Info_Frame) << ", Received: " << data.size() << std::endl;
        }
    }
    else if (src == "Mems_Hardware_Service") {
        // MEMS数据
        if (data.size() >= sizeof(MEMS_Info_Frame)) {
            MEMS_Info_Frame frame;
            memcpy(&frame, data.data(), sizeof(MEMS_Info_Frame));
            
            // 打印MEMS数据
            std::cout << "\n========== MEMS Telemetry ==========" << std::endl;
            
            // 角增量数据（弧度）
            std::cout << "X-axis: " << frame.x_angle_delta << std::endl;
            std::cout << "Y-axis: " << frame.y_angle_delta << std::endl;
            std::cout << "Z-axis: " << frame.z_angle_delta << std::endl;
            
            // 错误计数
            std::cout << "Header Errors: " << frame.header_err_cnt << std::endl;
            std::cout << "Checksum Errors: " << frame.check_err_cnt << std::endl;
            std::cout << "Length Errors: " << frame.length_err_cnt << std::endl;
            std::cout << "UART Resets: " << frame.uart_reset_cnt << std::endl;
            std::cout << "Total Faults: " << frame.fault_cnt << std::endl;
            
            std::cout << "=====================================\n" << std::endl;
        } else {
            std::cout << "[ERROR] MEMS data size mismatch. Expected: " 
                      << sizeof(MEMS_Info_Frame) << ", Received: " << data.size() << std::endl;
        }
    }
    else if (src == "Flywheel_Hardware_Service") {
        // FLYWHEEL数据
        if (data.size() >= sizeof(FLYWHEEL_Info_Frame)) {
            FLYWHEEL_Info_Frame frame;
            memcpy(&frame, data.data(), sizeof(FLYWHEEL_Info_Frame));
            
            // 打印FLYWHEEL数据
            std::cout << "\n========== FLYWHEEL Telemetry ==========" << std::endl;
            
            
            std::cout << "飞轮ID: " << frame.wheel_id << std::endl;
            std::cout << "硬件ID: " << frame.hardware_id << std::endl;
            std::cout << "转速: " << frame.rotate_speed << std::endl;
            std::cout << "电流: " << frame.current << std::endl;
            std::cout << "加速度矩: " << frame.acc_torque << std::endl;

            std::cout << "命令计数器: " << frame.cmd_cnt << std::endl;
            std::cout << "软件错误码: " << frame.soft_err << std::endl;
            std::cout << "软件版本: " << frame.soft_ver << std::endl;
            
            std::cout << "Power Status: " << (frame.on_off_status ? "ON" : "OFF") 
                      << " (" << (int)frame.on_off_status << ")" << std::endl;
            
            // 错误计数
            std::cout << "Header Errors: " << frame.header_err_cnt << std::endl;
            std::cout << "Checksum Errors: " << frame.check_err_cnt << std::endl;
            std::cout << "Length Errors: " << frame.length_err_cnt << std::endl;
            std::cout << "UART Resets: " << frame.uart_reset_cnt << std::endl;
            std::cout << "Total Faults: " << frame.fault_cnt << std::endl;

            std::cout << "请求类型: " << frame.req_type << std::endl;
            std::cout << "错误码: " << frame.error_code << std::endl;
            
            std::cout << "=====================================\n" << std::endl;
        } else {
            std::cout << "[ERROR] FLYWHEEL data size mismatch. Expected: " 
                      << sizeof(FLYWHEEL_Info_Frame) << ", Received: " << data.size() << std::endl;
        }
    }
    else {
        // 未知源设备或命令数据
        std::cout << "[WARNING] Unknown source device or command data: " << src << std::endl;
        std::cout << "Data (" << data.size() << " bytes): ";
        for (size_t i = 0; i < data.size(); ++i) {
            std::cout << std::hex << std::uppercase << (int)data[i];
            if (i < data.size() - 1) {
                std::cout << ", ";
            }
        }
        std::cout << std::dec << std::endl;
    }
}


// AD硬件服务化遥控发布
void GncS_command_Pub(uint8_t* data, string dest, uint16_t len)
{
	GncS_part->publish(topic_name_cmd, "Gnc_Service", dest, "command", data, len);
}

// AD硬件服务化遥测发布
void GncS_telemetry_Pub(uint8_t* data, string dest, uint16_t len)
{
	GncS_part->publish(topic_name_tlm, "Gnc_Service", dest, "command", data, len);
}