#include "GncS.h"
#include <iostream>
#include <cstring>
#include <chrono>
#include <thread>

int main() {
    // 初始化AD硬件服务化
    GncS_init(0, "Gnc_Service");

    uint8_t request_star0_data[] = {0, 1};
    uint8_t request_star1_data[] = {0, 2};
    uint8_t request_flywheel1_data[] = {0, 0xFF};

    Flywheel_Command cmd;
    cmd.wheel_id = 0;     // 飞轮1
    cmd.mode = 0x58;      // 速度模式
    cmd.reserved = 0x00;
    cmd.torque = 1.5f;    // 1.5 Nm
    cmd.speed = 2000.0f;  // 2000 rpm

    uint8_t* command_data = (uint8_t*)&cmd;


    uint8_t request_data[] = {0xFF};

    
    
    int count = 0;
    while (true) {
        count++;

        // 发送遥控数据
        GncS_telemetry_Pub(request_star0_data, "Star_sensor_Hardware_Service", sizeof(request_star0_data));
        GncS_telemetry_Pub(request_star1_data, "Star_sensor_Hardware_Service", sizeof(request_star1_data));
        GncS_command_Pub(command_data, "Flywheel_Hardware_Service", sizeof(Flywheel_Command));
        GncS_telemetry_Pub(request_flywheel1_data, "Flywheel_Hardware_Service", sizeof(request_flywheel1_data));


        GncS_telemetry_Pub(request_data, "Gyro_Hardware_Service", sizeof(request_data));
        GncS_telemetry_Pub(request_data, "Mems_Hardware_Service", sizeof(request_data));
        
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
    
    return 0;
}