#include "system/includes.h"
#include "btstack/btstack_task.h"
#include "app_config.h"
#include "app_action.h"
#include "asm/pwm_led.h"
#include "tone_player.h"
#include "ui_manage.h"
#include "gpio.h"
#include "app_main.h"
#include "asm/charge.h"
#include "update.h"
#include "app_power_manage.h"
#include "audio_config.h"
#include "app_charge.h"
#include "bt_profile_cfg.h"
#include "dev_manager/dev_manager.h"
#include "update_loader_download.h"
#include "./sensor/SC7U22.h"
#include "./buffer/circle_buffer.h"
#include "btstack/avctp_user.h"
#include "calculate/skiing_tracker.h"
#include "xtell.h"
#include "./ano/ano_protocol.h"
#include "./sensor/MMC56.h"
#include "./sensor/BMP280.h"
#include "./sensor/AK8963.h"
///////////////////////////////////////////////////////////////////////////////////////////////////
//宏定义
#define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if ENABLE_XLOG
    #define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
#else
    #define xlog(format, ...) ((void)0)
#endif

#define SENSOR_DATA_BUFFER_SIZE 500 // 定义缓冲区可以存储XXX个sensor_data_t元素

//
///////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 函数定义
void send_data_to_ble_client(const u8* data, u16 length);
extern void create_process(u16* pid, const char* name, void *priv, void (*func)(void *priv), u32 msec);
extern void close_process(u16* pid,char* name);
//END -- 函数定义
//////////////////////////////////////////////////////////////////////////////////////////////////


//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 变量定义


typedef struct {
    // -- 六轴 -- 
    signed short acc_data[3];
    signed short gyr_data[3];
    // -- 磁力计 --
    uint8_t mmc5603nj_buffer[9];
    // -- 速度 --
    uint16_t speed_cms;
    // -- 气压计 --
    //...
} BLE_send_data_t;

static int count = 0;

// --- 环形缓冲区 ---
static circle_buffer_t BLE_send_buff; // 环形缓冲区管理结构体
BLE_send_data_t BLE_send_data[SENSOR_DATA_BUFFER_SIZE];

//END -- 变量定义
//////////////////////////////////////////////////////////////////////////////////////////////////

/**
 * @brief 六轴静态校准
 * 
 */
void SC7U22_static_calibration(void){
    signed short acc_data_buf[3];
    signed short gyr_data_buf[3];
    float angle[3];
    float quaternion_output[3];

    static signed short combined_raw_data[6];
    static int calibration_done = 0;
    char status = 0;
    set_SC7U22_Error_Flag(0);
    SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
    memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
    memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
    status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, angle,NULL, 0, quaternion_output);

    if(status == 1){ //校准完成
        extern u16 SC7U22_calibration_id;
        extern u8 SC7U22_init;
        SC7U22_init = 1;
        close_process(&SC7U22_calibration_id, "SC7U22_calibration");
        u8 send2_1[5] = {0xBB,0xBE,0x02,0x00,0x01};
        send_data_to_ble_client(&send2_1,strlen(send2_1));
    }
    // if(count > 100){
    //     count = 0;
    //     char log_buffer[100]; 
    //     // snprintf( log_buffer, sizeof(log_buffer),"status:%d\n",status);  
    //     // send_data_to_ble_client(&log_buffer,strlen(log_buffer));
    //     xlog("status:%d\n", status);
    //         xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",combined_raw_data[0],combined_raw_data[1],combined_raw_data[2],combined_raw_data[3],combined_raw_data[4],combined_raw_data[5]);
    // }
    // count++;
}

/**
 * @brief 开始采集传感器数据和计算速度
 * 
 */
void start_collect_fuc(void){
    // xlog("=======sensor_read_data START\n");
    
    static signed short combined_raw_data[6];
    static int initialized = 0;
    static int calibration_done = 0;
    char status = 0;
    BLE_send_data_t BLE_send_data_tmp;
    uint8_t mmc5603nj_buffer[9];
    signed short acc_data_buf[3];
    signed short gyr_data_buf[3];
    float angle[3];
    float quaternion_output[3];
    SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
    mmc5603nj_read_origin_data(mmc5603nj_buffer);

    memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
    memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));

    status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, angle,NULL, 0, quaternion_output);

    memcpy(acc_data_buf, &combined_raw_data[0], 3 * sizeof(signed short));
    memcpy(gyr_data_buf, &combined_raw_data[3], 3 * sizeof(signed short));
    uint16_t speed = sensor_processing_task(acc_data_buf,gyr_data_buf,angle, quaternion_output);
    
    // -- 数据包装进结构体 --
    memcpy(BLE_send_data_tmp.acc_data, acc_data_buf, 3 * sizeof(signed short));
    memcpy(BLE_send_data_tmp.gyr_data, gyr_data_buf, 3 * sizeof(signed short));
    memcpy(BLE_send_data_tmp.mmc5603nj_buffer, mmc5603nj_buffer, 9);
    BLE_send_data_tmp.speed_cms = speed;

    // -- 放进缓冲区 --
    if(circle_buffer_is_full(&BLE_send_buff) == 0){
        circle_buffer_write(&BLE_send_buff, &BLE_send_data_tmp);
    }
    

}

/**
 * @brief ble数据发送
 * 
 */
void BLE_send_fuc(void){
    BLE_send_data_t BLE_send_data_tmp;
    if(circle_buffer_is_empty(&BLE_send_buff) == 0){
        circle_buffer_read(&BLE_send_buff, &BLE_send_data_tmp);
    }
    


}


void xtell_task_create(void){

    #if TCFG_GSENOR_USER_IIC_TYPE
    int ret = hw_iic_init(0);
    xlog("init iic result:%d\n", ret);  //返回0成功
    #else
    int ret = soft_iic_init(0);
    int num_chars_written = snprintf(log_buffer_1, sizeof(log_buffer_1),"init iic: %d\n", ret);  
    
    #endif
    
    // MPU9250_Mag_Init();
    //iic总线设备扫描
    extern void i2c_scanner_probe(void);
    i2c_scanner_probe();

    xlog("xtell_task_create\n");

    circle_buffer_init(&BLE_send_buff, BLE_send_data, SENSOR_DATA_BUFFER_SIZE, sizeof(BLE_send_data_t));

}