#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 <math.h>
#include <string.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"
#include "./sensor/WF282A.h"
#include "asm/rtc.h"
#include "system/timer.h"
#include "adv_time_stamp_setting.h"
#include "btstack/le/le_user.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 200 // 定义缓冲区可以存储XXX个sensor_data_t元素

#define MPU_FIFO_INTERVAL 4  //隔多久读取六轴fifo，单位10ms
#define MPU_FIFO_LEN    16 //(MPU_FIFO_INTERVAL*10/2.5)  //400hz采用频率，那每40ms的时间，fifo就有16组六轴数据

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

//////////////////////////////////////////////////////////////////////////////////////////////////
//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);
void BLE_send_fuc(void);
//END -- 函数定义
//////////////////////////////////////////////////////////////////////////////////////////////////


//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 变量定义
static u32 timer_offset_ms = 0;

typedef struct{
    signed short SC7U22_data[6];   //12字节
    int mmc5603nj_buffer[3];   //12字节
    int16_t temperature; //2
    uint32_t pressure; //4
}sensor_package_t __attribute__((packed));

typedef struct{
    uint8_t checkout_1;
    uint8_t checkout_2;  
    uint8_t foot; //1：左脚，2：右脚
    uint8_t package_index;
    sensor_package_t sensor_package[MPU_FIFO_LEN];//一次蓝牙发送MPU_FIFO_LEN组传感器数据
}ble_send_data_t;  //一次蓝牙发送的数据内容

// --- 环形缓冲区 ---
static circle_buffer_t g_ble_send_cb; // 环形缓冲区管理结构体
static ble_send_data_t g_sensor_data_storage[SENSOR_DATA_BUFFER_SIZE];  //缓冲区

extern u8 foot_init;
static OS_SEM receiver_ready_sem; // 用于启动同步的信号量

static const uart_bus_t *uart_bus = NULL;

static u16 test_id = 0;
//END -- 变量定义
//////////////////////////////////////////////////////////////////////////////////////////////////


// 重置计时器
void reset_ms_timer(void) {
    timer_offset_ms = sys_timer_get_ms();
    xlog("Timer has been reset.\n");
}

// 获取从上次重置后经过的毫秒数
u32 get_ms_timer(void) {
    return sys_timer_get_ms() - timer_offset_ms;
}


/**
 * @brief 传感器采集任务
 * 
 */
void sensor_collect_task(void){
    static ble_send_data_t send_data;
    mmc5603nj_mag_data_t mmc5603nj_buffer;
    float temperature = 0;
    float pressure = 0;
    int interval = 0;
    signed short accx_buf[100];
    signed short accy_buf[100];
    signed short accz_buf[100];
    signed short gyrx_buf[100];
    signed short gyry_buf[100];
    signed short gyrz_buf[100];
    int fifo_num = 0;
    static int SL_data_index = 0;
    u8 package_index = 1;
    int tmp_index = 0;

    while(1){//4组地磁数据、16组六轴数据、1组气压计数据
        interval++;
        mmc5603nj_read_mag_data(&mmc5603nj_buffer);
        for(int i = (interval-1)*4; i < interval*4; i++){
            send_data.sensor_package[i].mmc5603nj_buffer[0] = (int32_t)(mmc5603nj_buffer.x * 1000.0f);
            send_data.sensor_package[i].mmc5603nj_buffer[1] = (int32_t)(mmc5603nj_buffer.y * 1000.0f);
            send_data.sensor_package[i].mmc5603nj_buffer[2] = (int32_t)(mmc5603nj_buffer.z * 1000.0f);
        }
        // xlog("MAG x: %.2f,y: %.2f,z: %.2f\n",mmc5603nj_buffer.x,mmc5603nj_buffer.y,mmc5603nj_buffer.z);

        SL_SC7U22_FIFO_Read(accx_buf,accy_buf,accz_buf,gyrx_buf,gyry_buf,gyrz_buf); //一次性读取内置fifo的数据
        for(int i = 0; i < MPU_FIFO_LEN/4; i++){
            tmp_index = SL_data_index + i;
            // if(tmp_index >= MPU_FIFO_LEN-1) tmp_index = MPU_FIFO_LEN-1;
            send_data.sensor_package[tmp_index].SC7U22_data[0] = accx_buf[i];  //acc_x
            send_data.sensor_package[tmp_index].SC7U22_data[1] = accy_buf[i];  //acc_y
            send_data.sensor_package[tmp_index].SC7U22_data[2] = accz_buf[i];  //acc_z
            send_data.sensor_package[tmp_index].SC7U22_data[3] = gyrx_buf[i];  //gyr_x
            send_data.sensor_package[tmp_index].SC7U22_data[4] = gyry_buf[i];  //gyr_y
            send_data.sensor_package[tmp_index].SC7U22_data[5] = gyrz_buf[i];  //gyr_z

            // xlog(" Acc_x : %4d, Acc_y : %4d, Acc_z : %4d,\r\n", send_data.sensor_package[tmp_index].SC7U22_data[0], send_data.sensor_package[tmp_index].SC7U22_data[1], send_data.sensor_package[tmp_index].SC7U22_data[2]);	
            #if 0
                float acc_g[3];
                float gyr_dps[3];
                acc_g[0] =   (float)send_data.sensor_package[tmp_index].SC7U22_data[0] / 2048.0f;
                acc_g[1] =   (float)send_data.sensor_package[tmp_index].SC7U22_data[1] / 2048.0f;
                acc_g[2] =   (float)send_data.sensor_package[tmp_index].SC7U22_data[2] / 2048.0f;
                gyr_dps[0] = (float)send_data.sensor_package[tmp_index].SC7U22_data[3] * 0.061f;
                gyr_dps[1] = (float)send_data.sensor_package[tmp_index].SC7U22_data[4] * 0.061f;
                gyr_dps[2] = (float)send_data.sensor_package[tmp_index].SC7U22_data[5] * 0.061f;
                printf("  ACC(g):  x=%.3f, y=%.3f, z=%.3f\n", acc_g[0], acc_g[1], acc_g[2]);
                printf("  GYR(dps):x=%.3f, y=%.3f, z=%.3f\n", gyr_dps[0], gyr_dps[1], gyr_dps[2]);
            #endif
        }
        SL_data_index += MPU_FIFO_LEN/4;


        if(interval >= 4){ 
            interval = 0;
            SL_data_index = 0;
            #if BMP280
            bmp280_read_data(&temperature, &pressure);//每40ms读取一次
            #else
            WF_GET_Temperature_Pressure(&temperature, &pressure);
            #endif
            for(int i = 0;i<MPU_FIFO_LEN;i++){
                send_data.sensor_package[i].temperature = (int16_t)(temperature * 1000.0f);
                send_data.sensor_package[i].pressure = (int32_t)(pressure * 1000.0f);      
            }

            // xlog("temperature: %.2f,pressure: %.2f\n",temperature,pressure);
            // xlog("fifo_num:%d\n",fifo_num);
            
            send_data.checkout_1 = 0xBE;
            send_data.checkout_2 = 0xBB;
            send_data.foot = foot_init;
            send_data.package_index = package_index;
            circle_buffer_write(&g_ble_send_cb, &send_data);
            os_sem_post(&receiver_ready_sem); //通知另一个发送任务

            memset(&send_data, 0, sizeof(ble_send_data_t));
            memset(&accx_buf, 0, sizeof(accx_buf));
            memset(&accy_buf, 0, sizeof(accy_buf));
            memset(&accz_buf, 0, sizeof(accz_buf));
            memset(&gyrx_buf, 0, sizeof(gyrx_buf));
            memset(&gyry_buf, 0, sizeof(gyry_buf));
            memset(&gyrz_buf, 0, sizeof(gyrz_buf));

            package_index++;
            if(package_index >= 0xFF) package_index = 1;
            // xlog("=====================%d============================\n",get_ms_timer());   
        }
        os_time_dly(1); //10ms为单位
    }
}


void data_log(uint8_t* data){
    static u8 imu_airplane[MPU_FIFO_LEN][12];
    // 检查数据包头部
    if (data[0] != 0xBE || data[1] != 0xBB) {
        printf("Error: Invalid data packet header.\n");
        return;
    }

    //左右脚
     uint8_t package_foot = data[2];
    
    // 解析包索引
    uint8_t package_index = data[3];
    printf("--- Parsing Data Packet Index: %d ---\n", package_index);

    uint8_t* p = &data[4]; // 指向数据负载的起始位置

    // 循环解析16组数据
    for (int i = 0; i < MPU_FIFO_LEN; i++) {
        // 1. 解析六轴传感器数据 (12 bytes)
        int16_t imu_raw[6];
        for (int j = 0; j < 6; j++) {
            imu_airplane[i][2*j] = p[0];
            imu_airplane[i][2*j+1] = p[1];
            // 小端模式: 低字节在前, 高字节在后
            imu_raw[j] = (int16_t)(((uint16_t)p[1] << 8) | (uint16_t)p[0]);
            p += 2;
        }
        float acc_g[3];
        float gyr_dps[3];
        acc_g[0] = (float)imu_raw[0] / 2048.0f;
        acc_g[1] = (float)imu_raw[1] / 2048.0f;
        acc_g[2] = (float)imu_raw[2] / 2048.0f;
        gyr_dps[0] = (float)imu_raw[3] * 0.061f;
        gyr_dps[1] = (float)imu_raw[4] * 0.061f;
        gyr_dps[2] = (float)imu_raw[5] * 0.061f;

        // 2. 解析地磁传感器数据 (12 bytes)
        int32_t mag_raw[3];
        for (int j = 0; j < 3; j++) {
            // 小端模式
            mag_raw[j] = (int32_t)(((uint32_t)p[3] << 24) | ((uint32_t)p[2] << 16) | ((uint32_t)p[1] << 8) | (uint32_t)p[0]);
            p += 4;
        }
        float mag_gauss[3];
        mag_gauss[0] = (float)mag_raw[0] / 1000.0f;
        mag_gauss[1] = (float)mag_raw[1] / 1000.0f;
        mag_gauss[2] = (float)mag_raw[2] / 1000.0f;

        // 3. 解析温度数据 (2 bytes)
        int16_t temp_raw = (int16_t)(((uint16_t)p[1] << 8) | (uint16_t)p[0]);
        p += 2;
        float temperature = (float)temp_raw / 1000.0f;

        // 4. 解析气压数据 (4 bytes)
        uint32_t press_raw = (uint32_t)(((uint32_t)p[3] << 24) | ((uint32_t)p[2] << 16) | ((uint32_t)p[1] << 8) | (uint32_t)p[0]);
        p += 4;
        float pressure = (float)press_raw / 1000.0f;

        // 打印解析后的数据
        if(i % 8 == 0){
            printf(" ==================ble index: %d\n", *p);
            printf("Package[%d]:\n", i);
            printf("  ACC(g):  x=%.3f, y=%.3f, z=%.3f\n", acc_g[0], acc_g[1], acc_g[2]);
            printf("  GYR(dps):x=%.3f, y=%.3f, z=%.3f\n", gyr_dps[0], gyr_dps[1], gyr_dps[2]);
            printf("  MAG(Gs): x=%.3f, y=%.3f, z=%.3f\n", mag_gauss[0], mag_gauss[1], mag_gauss[2]);
            printf("  TEMP(C): %.3f, PRESS(Pa): %.3f\n", temperature, pressure);
        }
        
    }
    // printf("--- End of Packet ---\n\n");

    extern void uartSendData(void *buf, u16 len) ; // 确保u16是uint16_t或unsigned short
    // uartSendData(imu_airplane, sizeof(imu_airplane)); 
    uartSendData(data, 484); // 发送总共17字节
}

/**
 * @brief ble数据发送函数
 * 
 */
void BLE_send_fuc(void){
    ble_send_data_t send_data;
    uint8_t send_buffer[484];
    while(1){   
        os_sem_pend(&receiver_ready_sem, 0);  //阻塞等待
        circle_buffer_read(&g_ble_send_cb, &send_data);
        
        // 逐字节打包数据到 send_buffer, 采用小端模式
        uint8_t *p = send_buffer;
        *p++ = send_data.checkout_1;
        *p++ = send_data.checkout_2;
        *p++ = send_data.foot;
        *p++ = send_data.package_index;

        for (int i = 0; i < MPU_FIFO_LEN; i++) {
            sensor_package_t *pkg = &send_data.sensor_package[i];

            // 1. 打包六轴数据 (6 * int16_t)
            for (int j = 0; j < 6; j++) {
                *p++ = (uint8_t)(pkg->SC7U22_data[j] & 0xFF);
                *p++ = (uint8_t)((pkg->SC7U22_data[j] >> 8) & 0xFF);
            }

            // 2. 打包地磁数据 (3 * int32_t)
            for (int j = 0; j < 3; j++) {
                *p++ = (uint8_t)(pkg->mmc5603nj_buffer[j] & 0xFF);
                *p++ = (uint8_t)((pkg->mmc5603nj_buffer[j] >> 8) & 0xFF);
                *p++ = (uint8_t)((pkg->mmc5603nj_buffer[j] >> 16) & 0xFF);
                *p++ = (uint8_t)((pkg->mmc5603nj_buffer[j] >> 24) & 0xFF);
            }

            // 3. 打包温度数据 (int16_t)
            *p++ = (uint8_t)(pkg->temperature & 0xFF);
            *p++ = (uint8_t)((pkg->temperature >> 8) & 0xFF);

            // 4. 打包气压数据 (uint32_t)
            *p++ = (uint8_t)(pkg->pressure & 0xFF);
            *p++ = (uint8_t)((pkg->pressure >> 8) & 0xFF);
            *p++ = (uint8_t)((pkg->pressure >> 16) & 0xFF);
            *p++ = (uint8_t)((pkg->pressure >> 24) & 0xFF);

            #if 0
            float acc_g[3];
            float gyr_dps[3];
            acc_g[0] = (float)send_data.sensor_package[i].SC7U22_data[0] / 2048.0f;
            acc_g[1] = (float)send_data.sensor_package[i].SC7U22_data[1] / 2048.0f;
            acc_g[2] = (float)send_data.sensor_package[i].SC7U22_data[2] / 2048.0f;
            gyr_dps[0] = (float)send_data.sensor_package[i].SC7U22_data[3] * 0.061f;
            gyr_dps[1] = (float)send_data.sensor_package[i].SC7U22_data[4] * 0.061f;
            gyr_dps[2] = (float)send_data.sensor_package[i].SC7U22_data[5] * 0.061f;
            printf("  ACC(g):  x=%.3f, y=%.3f, z=%.3f\n", acc_g[0], acc_g[1], acc_g[2]);
            printf("  GYR(dps):x=%.3f, y=%.3f, z=%.3f\n", gyr_dps[0], gyr_dps[1], gyr_dps[2]);
            #endif
        }


        extern void uartSendData(void *buf, u16 len) ; // 确保u16是uint16_t或unsigned short
        uartSendData(send_buffer, 484); // 发送总共17字节
        send_data_to_ble_client(send_buffer, 484); // 发送数据
        
        // data_log(send_buffer);
    }
}



// ------------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------------------------------------------



/**
 * @brief 开始采集传感器数据并通过ble发送
 * 
 */
void start_clloct(void){
    os_task_create(sensor_collect_task,NULL,5,1024,32,"sensor_collect_task");
    os_task_create(BLE_send_fuc,NULL,5,1024,32,"BLE_send_fuc");
}

/**
 * @brief 停止采集和ble发送
 * 
 */
void stop_clloct(void){
    os_task_del("sensor_collect_task");
    os_task_del("BLE_send_fuc");
}

/**
 * @brief 初始化，在app_main.c的app_main函数被调用
 * 
 */
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(&g_ble_send_cb, g_sensor_data_storage, SENSOR_DATA_BUFFER_SIZE, sizeof(ble_send_data_t));

    os_sem_create(&receiver_ready_sem, 0);

    extern void test_uart_init(void);
    test_uart_init();

}


/**
 * @brief 发给上位机
 * 
 */
void test_uart_init(void){
    #if TCFG_UART0_ENABLE == 0
    static u8 buff[40];
    struct uart_platform_data_t u_arg = {0};
    u_arg.tx_pin = IO_PORT_DP;
    u_arg.rx_cbuf = buff;
    u_arg.rx_cbuf_size = 32;
    u_arg.frame_length = 6;
    u_arg.rx_timeout = 100;
    u_arg.isr_cbfun = NULL;
    u_arg.baud = 1000000;
    u_arg.is_9bit = 0;
    uart_bus = uart_dev_open(&u_arg);
    #endif
}

void uartSendData(void *buf, u16 len) 			//发送数据的接口。
{
    #if TCFG_UART0_ENABLE == 0
    if (uart_bus) {
        uart_bus->write(buf, len);		//把数据写到DMA
    }
    #endif
}
//////////////////////////////////////////////////////////////////////////////
//test
//

#define BUFF_LEN 500
static signed char acc_data_buf[BUFF_LEN] = {0};
// 1. 定义一个全局的信号量
static OS_SEM ble_send_sem;


int j = 0;
void data_send_task(void){
    signed short accx_buf[100];
    signed short accy_buf[100];
    signed short accz_buf[100];
    signed short gyrx_buf[100];
    signed short gyry_buf[100];
    signed short gyrz_buf[100];
    SL_SC7U22_FIFO_Read(accx_buf,accy_buf,accz_buf,gyrx_buf,gyry_buf,gyrz_buf); //一次性读取内置fifo的数据

#if 1
    // 定义新的Packet ID和数据长度
    #define PACKET_ID_RAW_IMU 0x04
    #define PACKET_LENGTH_RAW_IMU 12 // 6个传感器值，每个2字节

    // 声明一个发送缓冲区，用于包含帧头、ID、长度、数据和校验和
    // 帧头 (2) + ID (1) + 长度 (1) + 数据 (12) + 校验和 (1) = 17 字节
    uint8_t tx_buffer[2 + 1 + 1 + PACKET_LENGTH_RAW_IMU + 1];
    uint8_t checksum = 0;
    int i; // 用于循环计算校验和

    // 填充帧头
    tx_buffer[0] = 0xAA;
    tx_buffer[1] = 0xFF;

    // 填充Packet ID和长度
    tx_buffer[2] = PACKET_ID_RAW_IMU;
    tx_buffer[3] = PACKET_LENGTH_RAW_IMU;

    // 填充原始传感器数据 (与你原先的processing_data内容相同)
    tx_buffer[4] = (uint8_t)(accx_buf[0] & 0xFF);         // accX LSB
    tx_buffer[5] = (uint8_t)(accx_buf[0] >> 8 & 0xFF);    // accX MSB
    tx_buffer[6] = (uint8_t)(accy_buf[0] & 0xFF);         // accY LSB
    tx_buffer[7] = (uint8_t)(accy_buf[0] >> 8 & 0xFF);    // accY MSB
    tx_buffer[8] = (uint8_t)(accz_buf[0] & 0xFF);         // accZ LSB
    tx_buffer[9] = (uint8_t)(accz_buf[0] >> 8 & 0xFF);    // accZ MSB
    tx_buffer[10] = (uint8_t)(gyrx_buf[0] & 0xFF);        // gyrX LSB
    tx_buffer[11] = (uint8_t)(gyrx_buf[0] >> 8 & 0xFF);   // gyrX MSB
    tx_buffer[12] = (uint8_t)(gyry_buf[0] & 0xFF);        // gyrY LSB
    tx_buffer[13] = (uint8_t)(gyry_buf[0] >> 8 & 0xFF);   // gyrY MSB
    tx_buffer[14] = (uint8_t)(gyrz_buf[0] & 0xFF);        // gyrZ LSB
    tx_buffer[15] = (uint8_t)(gyrz_buf[0] >> 8 & 0xFF);   // gyrZ MSB

    // 计算校验和 (从 Packet ID 到所有数据字节的和)
    checksum = tx_buffer[2] + tx_buffer[3]; // ID + Length
    for (i = 0; i < PACKET_LENGTH_RAW_IMU; i++) {
        checksum += tx_buffer[4 + i]; // 加上所有数据字节
    }
    tx_buffer[4 + PACKET_LENGTH_RAW_IMU] = checksum; // 校验和是最后一个字节

    // 发送整个缓冲区
    extern void uartSendData(void *buf, u16 len) ; // 确保u16是uint16_t或unsigned short
    uartSendData(tx_buffer, sizeof(tx_buffer)); // 发送总共17字节
#endif

}
static u16 gtest_id = 0;
void test_func(void){
    // a. 初始化信号量，初始值为0
    // os_sem_create(&ble_send_sem, 0);

    // b. 注册回调函数，让协议栈知道在准备好时该调用谁
    // struct ble_server_operation_t *ble_ops;
    // ble_get_server_operation_table(&ble_ops);
    // ble_ops->regist_wakeup_send(NULL, on_ble_can_send);
    for(int i = 0;i<BUFF_LEN;i++){
        acc_data_buf[i] = i;
    }

    SL_SC7U22_Config();
    mmc5603nj_init();
    #if BMP280
    BMP280_init();
    #else
    WF_Init();
    #endif
    os_task_create(BLE_send_fuc,NULL,5,1024,32,"BLE_send_fuc");
    os_task_create(sensor_collect_task,NULL,5,1024,32,"sensor_collect_task");
    // create_process(&test_id, "sensor_test",NULL,data_send_task ,3);
    // data_send_task();
}