#include <stdio.h>
#include <inttypes.h>
#include "../config.h"

#if !EXAMPLE_USE_TOUCH
#error "SC7U22 requires EXAMPLE_USE_TOUCH (IMU shares the touch I2C bus)."
#endif

#include "driver/gpio.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include <math.h>
#include "esp_timer.h"
#include "../ui/generated/gui_guider.h"  // 包含GUI引导器头文件
#include "lvgl.h"  // 包含LVGL头文件
#include "LVGL/lvgl_init.h"
#include "ui_cmd.h"
#include "main.h"


static bool event_cooldown_active = false;   // 是否处于冷却屏蔽中
static int64_t cooldown_end_time = 0;        // 冷却结束时间戳
static const char *TAG_IMU = "SCU722";
i2c_master_dev_handle_t SC7U22_handle;
/** 与触摸共用的 I2C 总线句柄（SC7U22_init 写入，供扫描/查询） */
i2c_master_bus_handle_t s_i2c_bus;
sensor_data_t data_buffer[DATA_BUFFER_SIZE];
SC7U22_DataType sensor_data = {0};
static int buffer_index = 0;
static int buffer_count = 0;

struct acc_offset offset_data = {0};          
noise_datatype noise_data = {0};

float ax=0,ay=0,az=0,gx=0,gy=0,gz=0;
//float acc_pre_x=0,acc_pre_y=0,acc_pre_z=0;

float acc_sum = 0;
float acc_sign_sum = 0;

float acc_hp = 0;
uint32_t bum_energy = 0;
float iir_state = 0;   

uint16_t start_stop_cnt = 0;
uint16_t stop_cnt = 0;


uint8_t sharp_turn_flag = 0,pre_st_flag = 0;
bool sharpturn_ret_L = false,sharpturn_ret_R = false;


uint8_t rapid_acc_flag = 0,pre_ra_flag = 0,rapid_acc_ret = 0;
uint8_t rapid_dec_flag = 0,pre_rd_flag = 0,rapid_dec_ret = 0;
bool brake_fir_flag = false, rapid_fir_flag = false;

const char* MYTAG = "SC7U22";

extern int lottie_id;
extern int current_page_idx;
extern lv_ui guider_ui;  // 外部声明GUI引导器

/*
 * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
 * range 2G, 4G, 8G or 16G.
 */
float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width)
{
    double power = 2;

    float half_scale = (float)((pow((double)power, (double)bit_width) / 2.0f));

    return (GRAVITY_EARTH * val * g_range) / half_scale;
}



/*
 * @brief This function converts lsb to degree per second for 16 bit gyro at
 * range 125, 250, 500, 1000 or 2000dps.
 */
float lsb_to_dps(int16_t val, float dps, uint8_t bit_width)
{
    double power = 2;

    float half_scale = (float)((pow((double)power, (double)bit_width) / 2.0f));

    return (dps / (half_scale)) * (val);
}

void i2c_scan_devices(void)
{
    if (s_i2c_bus == NULL) {
        ESP_LOGW(MYTAG, "i2c_scan: bus not ready (call SC7U22_init first)");
        return;
    }

    uint8_t address;
    esp_err_t ret;
    bool found = false;

    ESP_LOGI(MYTAG, "Scanning I2C devices on shared bus...");

    for (address = 1; address < 127; address++) {
        ret = i2c_master_probe(s_i2c_bus, (uint16_t)address, 100);

        if (ret == ESP_OK) {
            ESP_LOGI(MYTAG, "Device found at address: 0x%02X", address);
            found = true;
        }
        vTaskDelay(50 / portTICK_PERIOD_MS);
    }

    if (!found) {
        ESP_LOGI(MYTAG, "No I2C devices found");
    }
}

void SC7U22_init(i2c_master_bus_handle_t i2c_bus)
{
    
    if (i2c_bus == NULL) {
        ESP_LOGE(MYTAG, "SC7U22_init: I2C bus handle is NULL");
        return;
    }

    s_i2c_bus = i2c_bus;
    ESP_LOGI(MYTAG, "SC7U22 on shared I2C bus: %p", (void *)s_i2c_bus);

    i2c_device_config_t dev_cfg = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = SC7U22_I2C_ADDR,
        .scl_speed_hz = I2C_MASTER_FREQ_HZ,
    };

    esp_err_t ret = i2c_master_bus_add_device(s_i2c_bus, &dev_cfg, &SC7U22_handle);

    if (ret != ESP_OK) {
        ESP_LOGE(MYTAG, "添加设备失败，错误码: %s", esp_err_to_name(ret));
        return;
    }

    SC7U22_Sensor_enable();

    ESP_LOGI(MYTAG, "SC7U22 初始化完成（硬件就绪，校准将在后台进行）");
}
esp_err_t SC7U22_Reg_W(uint8_t reg_addr, uint8_t data)
{
    uint8_t write_buf[2] = {reg_addr, data};
    return i2c_master_transmit(SC7U22_handle, write_buf, sizeof(write_buf), 1000 / portTICK_PERIOD_MS);
}

esp_err_t SC7U22_Reg_R(uint8_t reg_addr, uint8_t *data)
{
    return i2c_master_transmit_receive(SC7U22_handle, &reg_addr, 1, data, 1, 1000 / portTICK_PERIOD_MS);
}

void SC7U22_Sensor_enable(void)
{
    SC7U22_Reg_W(0x7F, 0x00);   // Bank / page（如需要）
    vTaskDelay(pdMS_TO_TICKS(2));
    SC7U22_Reg_W(0x7D, 0x0E);   // PWR_CTRL enable acc + gyro
    vTaskDelay(pdMS_TO_TICKS(5));

    // 加速度
    SC7U22_Reg_W(0x40, 0xA8);   // ACC_CONF（用缺省）
    SC7U22_Reg_W(0x41, 0x02);   // ±4g

    // 陀螺仪（关键）
    SC7U22_Reg_W(0x42, 0xA9);   // GYR_CONF（缺省、安全）
    SC7U22_Reg_W(0x43, 0x00);   // ±2000 dps

}


void SC7U22_DaMYTAGet(SC7U22_DataType* SC7U22_DataStucture)
{
    esp_err_t ret;
    uint8_t raw_acc[6]; // 用于存放 AX_L, AX_H, AY_L, AY_H, AZ_L, AZ_H
    uint8_t raw_gyr[6]; // 用于存放 GX_L, GX_H, GY_L, GY_H, GZ_L, GZ_H
    
    // ================= 加速度计读取 (一次读6字节) =================
    // 假设 XOUT_L 是起始地址。如果你的宏定义 XOUT_H 比 XOUT_L 小，请改用 XOUT_H
    uint8_t acc_reg_addr = SC7U22_ACCEL_XOUT_H; 
    
    ret = i2c_master_transmit_receive(SC7U22_handle, &acc_reg_addr, 1, raw_acc, 6, 1000 / portTICK_PERIOD_MS);
    
    if (ret == ESP_OK) {
        // 拼接数据。注意：通常 I2C 传感器先发低位(L)，后发高位(H)。
        // 具体的 buffer 顺序取决于寄存器地址顺序。
        // 假设地址顺序是: X_L(0), X_H(1), Y_L(2), Y_H(3), Z_L(4), Z_H(5)
        SC7U22_DataStucture->Ax = (int16_t)((raw_acc[0] << 8) | raw_acc[1]);
        SC7U22_DataStucture->Ay = (int16_t)((raw_acc[2] << 8) | raw_acc[3]);
        SC7U22_DataStucture->Az = (int16_t)((raw_acc[4] << 8) | raw_acc[5]);

    } else {
        ESP_LOGE(MYTAG, "Accel read failed: %s", esp_err_to_name(ret));
    }

    // ================= 陀螺仪读取 (一次读6字节) =================
    // 假设 XOUT_L 是起始地址
    uint8_t gyr_reg_addr = SC7U22_GYRO_XOUT_H;
    //暂时注释掉
    ret = i2c_master_transmit_receive(SC7U22_handle, &gyr_reg_addr, 1, raw_gyr, 6, 1000 / portTICK_PERIOD_MS);
    
    if (ret == ESP_OK) {
        SC7U22_DataStucture->Gx = (int16_t)((raw_gyr[0] << 8) | raw_gyr[1]);
        SC7U22_DataStucture->Gy = (int16_t)((raw_gyr[2] << 8) | raw_gyr[3]);
        SC7U22_DataStucture->Gz = (int16_t)((raw_gyr[4] << 8) | raw_gyr[5]);
    } else {
        ESP_LOGE(MYTAG, "Gyro read failed");
    }
}

void baseline_remove(void)
{
    // 加速度计去偏
    ax = lsb_to_mps2(sensor_data.Ax, (float)4, 16) - offset_data.acc_x_offset;
    ay = lsb_to_mps2(sensor_data.Ay, (float)4, 16) - offset_data.acc_y_offset;
    az = lsb_to_mps2(sensor_data.Az, (float)4, 16) - offset_data.acc_z_offset;
    
    // 陀螺仪去偏 (新增)
    gx = lsb_to_dps(sensor_data.Gx, (float)2000, 16) - offset_data.gyr_x_offset;
    gy = lsb_to_dps(sensor_data.Gy, (float)2000, 16) - offset_data.gyr_y_offset;
    gz = lsb_to_dps(sensor_data.Gz, (float)2000, 16) - offset_data.gyr_z_offset;
}

void noise_threshold_get(void)
{
    uint8_t ax_cnt = 0,ay_cnt = 0,az_cnt = 0,gx_cnt = 0,gy_cnt = 0,gz_cnt = 0;
    SC7U22_DaMYTAGet(&sensor_data);
    baseline_remove();
    for(int i = 0;i<50;i++)
    {
       if(ax!=0) {noise_data.accx_noise+=fabs(ax); ax_cnt++;}
       if(ay!=0) {noise_data.accy_noise+=fabs(ay); ay_cnt++;}
       if(az!=0) {noise_data.accz_noise+=fabs(az); az_cnt++;}
       if(gx!=0) {noise_data.gyrx_noise+=fabs(gx); gx_cnt++;}
       if(gy!=0) {noise_data.gyry_noise+=fabs(gy); gy_cnt++;}
       if(gz!=0) {noise_data.gyrz_noise+=fabs(gz); gz_cnt++;}
        vTaskDelay(pdMS_TO_TICKS(5));
    }
    noise_data.accx_noise/=(float)ax_cnt;
    noise_data.accy_noise/=(float)ay_cnt;
    noise_data.accz_noise/=(float)az_cnt;
    noise_data.gyrx_noise/=(float)gx_cnt;
    noise_data.gyry_noise/=(float)gy_cnt;
    noise_data.gyrz_noise/=(float)gz_cnt;
}

void data_deal_with_noise(void)
{
    if(fabs(ax)<fabs(noise_data.accx_noise)*1.5f+0.05) ax = 0;
    if(fabs(ay)<fabs(noise_data.accy_noise)*1.5f+0.05) ay = 0;
    if(fabs(az)<fabs(noise_data.accz_noise)*1.5f+0.05) az = 0;
    if(fabs(gx)<fabs(noise_data.gyrx_noise)*1.5f+1) gx = 0;
    if(fabs(gy)<fabs(noise_data.gyry_noise)*1.5f+1) gy = 0;
    if(fabs(gz)<fabs(noise_data.gyrz_noise)*1.5f+1) gz = 0;
}

void mySC7_getdata(void)
{
    
    SC7U22_DaMYTAGet(&sensor_data);
    baseline_remove();  //去零偏处理
    data_deal_with_noise();//去噪声处理

    
    data_buffer[buffer_index].accel_x = ax;
    data_buffer[buffer_index].accel_y = ay;
    data_buffer[buffer_index].accel_z = az;
    data_buffer[buffer_index].timestamp = esp_timer_get_time();

    buffer_index = (buffer_index + 1) % DATA_BUFFER_SIZE;
    if (buffer_count < DATA_BUFFER_SIZE) {
        buffer_count++;
    }
}

void accel_Getoffset(void)
{
    float ax_sum=0, ay_sum=0, az_sum=0;
    float gx_sum=0, gy_sum=0, gz_sum=0; // 新增
    
    ESP_LOGI(MYTAG, "正在校准传感器，请保持静止...");

    for(int i = 0; i < 100; i++) // 采样100次
    {
        SC7U22_DaMYTAGet(&sensor_data);
        
        // 累加加速度
        ax_sum += lsb_to_mps2(sensor_data.Ax, (float)4, 16);
        ay_sum += lsb_to_mps2(sensor_data.Ay, (float)4, 16);
        az_sum += lsb_to_mps2(sensor_data.Az, (float)4, 16);
        
        // 累加陀螺仪 (注意：这里使用 2000dps 量程)
        gx_sum += lsb_to_dps(sensor_data.Gx, (float)2000, 16);
        gy_sum += lsb_to_dps(sensor_data.Gy, (float)2000, 16);
        gz_sum += lsb_to_dps(sensor_data.Gz, (float)2000, 16);
        
        vTaskDelay(pdMS_TO_TICKS(10));
    }
    
    // 计算平均值
    offset_data.acc_x_offset = ax_sum / 100.0f;
    offset_data.acc_y_offset = ay_sum / 100.0f;
    offset_data.acc_z_offset = (az_sum / 100.0f) - GRAVITY_EARTH; // Z轴要扣除重力

    // 计算陀螺仪偏移 (理论上静止时全是0，所以读数即为偏移)
    offset_data.gyr_x_offset = gx_sum / 100.0f;
    offset_data.gyr_y_offset = gy_sum / 100.0f;
    offset_data.gyr_z_offset = gz_sum / 100.0f; 

    int gz_milli = (int)(offset_data.gyr_z_offset * 1000.0f);
    ESP_LOGI(MYTAG, "校准完成! Gz Offset: %d.%03d", gz_milli / 1000, abs(gz_milli % 1000));
}
void bum_energy_get(void)
{
    acc_sum += fabs(az);
    acc_sign_sum += az;
    iir_state = ALPHA * iir_state + (1 - ALPHA) * acc_sum;
    acc_hp = acc_sum -iir_state;                  //高通滤波
    bum_energy += (uint32_t)(acc_hp*acc_hp*1000);                 //处理颠簸能量 
}

bool is_bump(void)
{   
    uint32_t temp  = bum_energy;
    static bool wasbump = false;
    float variance = 0; 
    bum_energy = 0;
    acc_sum = 0;
    acc_sign_sum /= DATA_BUFFER_SIZE;
    for(int i = 0;i<DATA_BUFFER_SIZE;i++)
    {
        variance += fabs(data_buffer[i].accel_z - acc_sign_sum);     //平均绝对值误差
    }
    acc_sign_sum = 0;
    bool isbump = (temp>=BUM_THRESHOLD&&variance>=400);
    if(isbump&&!wasbump) 
    {
        wasbump = isbump;
        return true;
    }
    wasbump = isbump;
    return false;
}

void sharp_turn_detect(void)
{
    if(fabs(gz) > turn_gyro_threshold && fabs(ay) > turn_accel_threshold) 
    {
        sharp_turn_flag++;
    }
    else 
    {
        sharp_turn_flag = 0;
    }

    if(sharp_turn_flag == 0 && pre_st_flag >= 5) // 连续触发次数稍微减少一点，灵敏些
    {
        pre_st_flag = 0;
        // 左转通常 gyrz > 0 (逆时针)，右转 gyrz < 0 (顺时针)
        // 具体正负取决于芯片安装面，你测试时如果不准，互换一下即可
        if(gz > 0) sharpturn_ret_L = true;
        else if(gz < 0) sharpturn_ret_R = true;
    }
    pre_st_flag = sharp_turn_flag;
}

driving_event_t is_sharp_turn(void)
{
    if(sharpturn_ret_L) 
    {
        sharpturn_ret_L = 0;
        return EVENT_SHARP_LTURN;
    }
    else if(sharpturn_ret_R)
    {
        sharpturn_ret_R = 0;
        return EVENT_SHARP_RTURN;
    }
    return EVENT_NONE;
}

static float calculate_acceleration_magnitude(void)
{
    return sqrtf(ax * ax + ay * ay + az * az);
}

void start_stop_state(void)
{
    static uint8_t st_cnt = 0;
    static uint8_t sp_cnt = 0;
    static bool stop_flag = false;
    bool is_stop  = calculate_acceleration_magnitude()<= is_stop_magnitude;
    if(is_stop&&!stop_flag) sp_cnt++;
    if(sp_cnt>=3)
    {
        stop_flag = true;
        sp_cnt = 0;
    }
    if(is_stop&&stop_flag)
    {
        stop_cnt++;
    }
    
    if(stop_flag&&!is_stop)
    {
        st_cnt++;
    }
    else{
        st_cnt = 0;
    }
    if(st_cnt>=3)
    {
        start_stop_cnt++;
        st_cnt = 0;
        stop_flag = false;
    }    
}

// 判断急停急加速
void detect_driving_events(void)
{
        //获取数据
        mySC7_getdata();
         //颠簸能量累加
        bum_energy_get(); 
        //急转弯检测
        sharp_turn_detect();
         //急加速检测  急刹车检测

    if (buffer_count < 2) return ;

    int current_idx = (buffer_index - 1 + DATA_BUFFER_SIZE) % DATA_BUFFER_SIZE;
    int prev_idx = (buffer_index - 2 + DATA_BUFFER_SIZE) % DATA_BUFFER_SIZE;

    // 计算时间差（秒）
    float time_diff = (data_buffer[current_idx].timestamp - 
                      data_buffer[prev_idx].timestamp) / 1000000.0f;
    
    if (time_diff <= 0) return ;


    float accel_diff_x = data_buffer[current_idx].accel_x - data_buffer[prev_idx].accel_x;
    float jerk_x = accel_diff_x / time_diff; // 急动度

    // 急刹车/减速检测
    // 逻辑：当前有一个很大的负向加速度(刹车)，且变化率很大
    // 注意：如果是“前倾 accx 变大”，说明 +X 是车头向下或向前。
    // 假设 +X 是向前加速，那么 -X 就是刹车。
    if ((jerk_x < -rapid_accel_threshold/0.02) && (data_buffer[current_idx].accel_x < -rapid_accel_threshold)) 
        rapid_dec_flag = 1;
    
    if ((data_buffer[current_idx].accel_x < -rapid_accel_threshold) && rapid_dec_flag) 
        rapid_dec_flag++;
    else 
        rapid_dec_flag = 0;

    if (rapid_dec_flag == 0 && pre_rd_flag >= 4) rapid_dec_ret = 1; // 阈值改小一点容易触发
    pre_rd_flag = rapid_dec_flag;

    // 急加速检测
    // 逻辑：当前有一个很大的正向加速度
    if ((jerk_x > rapid_accel_threshold/0.02) && (data_buffer[current_idx].accel_x > rapid_accel_threshold)) 
        rapid_acc_flag = 1;
    
    if ((data_buffer[current_idx].accel_x > rapid_accel_threshold) && rapid_acc_flag) 
        rapid_acc_flag++;
    else 
        rapid_acc_flag = 0;

    if (rapid_acc_flag == 0 && pre_ra_flag >= 4) rapid_acc_ret = 1;
    pre_ra_flag = rapid_acc_flag;

}
driving_event_t is_rapid_acc_or_dec(void)
{
    if(rapid_acc_ret)
    {
        rapid_acc_ret = 0;
        return EVENT_SUDDEN_ACCEL;
    }
    if(rapid_dec_ret)
    {
        rapid_dec_ret = 0;
        return EVENT_SUDDEN_BRAKE;
    }
     return EVENT_NONE;
}   

bool is_jam(void)
{   

    float stop_proportion = (float)stop_cnt/600.0;
    uint16_t temp_cnt = start_stop_cnt;
    stop_cnt = 0;
    start_stop_cnt = 0;
    if((stop_proportion>=stop_proportion_threshold&&temp_cnt>=start_stop_threshold)||stop_proportion>=0.8f)
    {
        return true;
    }
     return false;
}
// 在IMU任务函数外定义（全局区），避免栈溢出，同时供回调函数访问
// 驾驶事件冷却检查：冷却中返回true，允许触发返回false
bool is_event_in_cooldown(void)
{
    if (!event_cooldown_active) {
        return false;
    }

    // 当前时间 < 结束时间 = 还在冷却
    if (esp_timer_get_time() < cooldown_end_time) {
        return true;
    } else {
        // 冷却结束，复位
        event_cooldown_active = false;
        ESP_LOGI(MYTAG, "冷却时间已结束，可以再次触发驾驶事件");
        return false;
    }
}

// 触发一次事件后，启动50秒冷却
void start_event_cooldown(void)
{
    event_cooldown_active = true;
    cooldown_end_time = esp_timer_get_time() + (EVENT_COOLDOWN_MS * 1000); // 转微秒
    ESP_LOGI(MYTAG, "已启动冷却，%d 毫秒内不再触发", EVENT_COOLDOWN_MS);
}

/** 诊断：因 g_ble_play_sticker_active 抑制幸运表情时打印（节流，避免刷屏） */
#define BLE_GUARD_LOG_THROTTLE_MS 3000
static uint32_t s_ble_guard_last_log_tick;
static void ble_guard_log_lucky_suppressed(const char *reason)
{
    uint32_t t = lv_tick_get();
    if (s_ble_guard_last_log_tick != 0 && (t - s_ble_guard_last_log_tick) < BLE_GUARD_LOG_THROTTLE_MS)
        return;
    s_ble_guard_last_log_tick = t;
    ESP_LOGW("BLE_STICKER_GUARD",
             "Lucky suppressed: %s (g_ble_play_sticker_active still true)",
             reason);
}

void imu_task(void *param)
{
    ESP_LOGI(MYTAG, "Starting IMU task");
    driving_event_t drive_even = EVENT_NONE;
    uint16_t i = 0;

    /* 保持原校准逻辑不变，只调整执行时机：由主初始化路径改为后台任务执行。 */
    uint32_t calib_start_ms = (uint32_t)(esp_timer_get_time() / 1000ULL);
    ESP_LOGI(MYTAG, "IMU 后台校准开始");
    vTaskDelay(pdMS_TO_TICKS(20));
    accel_Getoffset();
    noise_threshold_get();
    uint32_t calib_elapsed_ms = (uint32_t)(esp_timer_get_time() / 1000ULL) - calib_start_ms;
    ESP_LOGI(MYTAG, "IMU 后台校准完成，耗时: %" PRIu32 " ms", calib_elapsed_ms);

    while(1)
    {
        // ========== 原有逻辑保持不变 ==========
        mySC7_getdata(); // 永远读取传感器数据
        ESP_LOGW(MYTAG,"正在读取IMU数据...");
        if (current_page_idx == 0) 
        {
            detect_driving_events();
            drive_even = is_sharp_turn();
            if(drive_even == EVENT_SHARP_LTURN)     //急转弯判断 结束一个急转弯后判定
            {
                if (is_event_in_cooldown()) continue;  // 冷却中直接跳过
                if (g_ble_play_sticker_active)
                {
                    ble_guard_log_lucky_suppressed("left sharp turn");
                    continue; /* BLE 正在播表情时不插播 IMU 幸运 */
                }
                start_event_cooldown();              // 启动50秒冷却


                /*左急转弯反馈入口*/
                lottie_id = 29;
                ESP_LOGE(MYTAG,"左急转!");
                /*左急转弯反馈结束*/
            }else if(drive_even == EVENT_SHARP_RTURN)
            {

                if (is_event_in_cooldown()) continue;  // 冷却中直接跳过
                if (g_ble_play_sticker_active)
                {
                    ble_guard_log_lucky_suppressed("right sharp turn");
                    continue;
                }
                start_event_cooldown();              // 启动50秒冷却


                /*右急转弯反馈入口*/
                lottie_id = 29;
                ESP_LOGE(MYTAG,"右急转!");
                /*右急转弯反馈入口*/
            }
           
            drive_even = is_rapid_acc_or_dec();
            if(drive_even==EVENT_SUDDEN_ACCEL)      
            {
                if (is_event_in_cooldown()) continue;  // 冷却中直接跳过
                if (g_ble_play_sticker_active)
                {
                    ble_guard_log_lucky_suppressed("sudden accel");
                    continue;
                }
                start_event_cooldown();              // 启动50秒冷却


                /*急加速反馈入口*/
                lottie_id = 29;
                ESP_LOGE(MYTAG,"急加速!");
                /*急加速反馈结束*/
            }
            else if(drive_even==EVENT_SUDDEN_BRAKE)
            {
                if (is_event_in_cooldown()) continue;  // 冷却中直接跳过
                if (g_ble_play_sticker_active)
                {
                    ble_guard_log_lucky_suppressed("sudden brake");
                    continue;
                }
                start_event_cooldown();              // 启动50秒冷却


                /*急刹车反馈入口*/
                lottie_id = 29;
                ESP_LOGE(MYTAG,"急刹车!");
                /*急刹车反馈结束*/
            }
        }


        // ========== 4. 通用逻辑（堵车/颠簸检测） ==========
        vTaskDelay(pdMS_TO_TICKS(20));
        i++;

        if(i%25==0 && current_page_idx == 0)
        {
            //堵车数据检测
            start_stop_state();
            // printf("accx:%4.2f, accy:%4.2f, accz:%4.2f, gyrx:%4.2f, gyry:%4.2f, gyrz:%4.2f\n",
            //                 ax,ay,az,gx,gy,gz);
        } 

        if(i%200==0 && current_page_idx == 0)                                       //2秒累加检测  反馈程序入口
        {   
            if(is_bump())                           //判断路面颠簸
            {
                ESP_LOGE(MYTAG,"路面颠簸!");
            }
        }  

        if(i>=15000 && current_page_idx == 0)                     //5分钟判定一次堵车
        {
            if(is_jam())
            {
                ESP_LOGE(MYTAG,"堵车!");
            }
           i = 0;
        }      
        vTaskDelay(pdMS_TO_TICKS(30));
    }
}


i2c_master_bus_handle_t get_i2c_master_bus_handle(void)
{
    return s_i2c_bus;
}
i2c_master_bus_handle_t touch_bus_handle_BMI;
i2c_master_bus_handle_t touch_bus_handle_NST112;
i2c_master_bus_handle_t touch_bus_handle_MS523;
i2c_master_bus_handle_t n_i2c_bus;
//i2c_master_bus_config_t i2c_bus_config = {0};
esp_err_t BMI_init(void)
{
    ESP_LOGI(TAG_IMU, "开始初始化 SC7U22...");
    vTaskDelay(pdMS_TO_TICKS(100));
    
#if !EXAMPLE_USE_TOUCH
#error "BMI_init requires EXAMPLE_USE_TOUCH (SC7U22 shares touch I2C bus)."
#endif

    //extern i2c_master_bus_handle_t touch_bus_handle;    I2C主总线
    i2c_master_bus_config_t i2c_bus_config = {0};
    i2c_bus_config.clk_source = I2C_CLK_SRC_DEFAULT;
    i2c_bus_config.i2c_port = 0;
    i2c_bus_config.scl_io_num = TEST_BOARD_I2C_SCL_PIN;
    i2c_bus_config.sda_io_num = TEST_BOARD_I2C_SDA_PIN;
    i2c_bus_config.glitch_ignore_cnt = 7;
    i2c_bus_config.flags.enable_internal_pullup = true;
    //i2c_new_master_bus(&i2c_bus_config, &i2c_bus_handle);
    ESP_ERROR_CHECK(i2c_new_master_bus(&i2c_bus_config, &touch_bus_handle_BMI));
    s_i2c_bus = touch_bus_handle_BMI;
    i2c_scan_devices();
    SC7U22_init(touch_bus_handle_BMI);
    bmi_task_stack = heap_caps_malloc(BMI_TASK_STACK_SIZE, MALLOC_CAP_SPIRAM);
    if (bmi_task_stack == NULL)
    {
        ESP_LOGE("LVGL", "Failed to allocate BMI task stack");
        return ESP_FAIL;
    }

    TaskHandle_t task_handle = xTaskCreateStaticPinnedToCore(imu_task, "BMI", BMI_TASK_STACK_SIZE, NULL, BMI_TASK_PRIORITY, bmi_task_stack, &bmi_task_tcb, 0);
    if (task_handle == NULL)
    {
        ESP_LOGE("LVGL", "Failed to create BMI task");
        heap_caps_free(bmi_task_stack);
        bmi_task_stack = NULL;
        return ESP_FAIL;
    }
    ESP_LOGI("LVGL", "BMI task created successfully");
    return ESP_OK;
}

i2c_master_dev_handle_t NST112_handle;
uint8_t temp_data[2] ={0};
uint8_t config_data[2] = {0};
uint8_t H_tmp_data[2] = {0};
uint8_t L_tmp_data[2] = {0};
esp_err_t NST112_Reg_W(uint8_t reg_addr, uint8_t data)
{
    uint8_t write_buf[] = {reg_addr, data};
    return i2c_master_transmit(NST112_handle, write_buf, sizeof(write_buf), 1000 / portTICK_PERIOD_MS);
}

esp_err_t NST112_Reg_R(uint8_t reg_addr)
{
    esp_err_t ret = i2c_master_transmit_receive(NST112_handle,&reg_addr,1,temp_data,2,1000 / portTICK_PERIOD_MS);
    if(ret != ESP_OK){
        ESP_LOGE("NST112", "读取 NST112 寄存器失败，错误码: %s", esp_err_to_name(ret));
        return ret;
    }
    else{
        return ESP_OK;
    }
}

extern StaticTask_t nst112_task_tcb;
extern StackType_t *nst112_task_stack;
void nst112_task(void *param)
{
    ESP_LOGI("NST112", "Starting NST112 task");
    uint8_t read_addr = 0x00;
    float temp_c = 0;
    while(1)
    {
        i2c_master_transmit_receive(NST112_handle, &read_addr, 1, temp_data, 2, 1000 / portTICK_PERIOD_MS);
        int16_t raw = (int16_t)((temp_data[0] << 8) | temp_data[1]);
        temp_c = raw / 256.0f;
        ESP_LOGI("NST112", "TEMP: %02X %02X", temp_data[0], temp_data[1]);
        ESP_LOGI("NST112", "TEMP (C): %f", temp_c);
        if(temp_c > 70){
            ESP_LOGI("NST112", "TEMP: WARNING");
        }
        if(temp_c < (-20)){
            ESP_LOGI("NST112", "TEMP: WARNING");
        }
        vTaskDelay(pdMS_TO_TICKS(2000));
    }
}

esp_err_t NST112_init(void)
{
    
    //extern i2c_master_bus_handle_t touch_bus_handle_touch;
    
    i2c_master_bus_config_t i2c_bus_config = {0};
    i2c_bus_config.clk_source = I2C_CLK_SRC_DEFAULT;
    i2c_bus_config.i2c_port = 1;
    i2c_bus_config.scl_io_num = NST112_I2C_SCL_PIN;
    i2c_bus_config.sda_io_num = NST112_I2C_SDA_PIN;
    i2c_bus_config.glitch_ignore_cnt = 7;
    i2c_bus_config.flags.enable_internal_pullup = true;
    //i2c_new_master_bus(&i2c_bus_config, &i2c_bus_handle);
    ESP_ERROR_CHECK(i2c_new_master_bus(&i2c_bus_config, &touch_bus_handle_NST112));   //此处单传感器调试用，正式使用时改为传入共享总线句柄

    n_i2c_bus = touch_bus_handle_NST112;//touch_bus_handle_touch
    s_i2c_bus = touch_bus_handle_NST112;
    i2c_scan_devices();
    ESP_LOGI("NST112", "NST112 on shared I2C bus: %p", (void *)n_i2c_bus);

    i2c_device_config_t dev_cfg = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = NST112_I2C_ADDR,
        .scl_speed_hz = I2C_MASTER_FREQ_HZ,
    };
    esp_err_t ret = i2c_master_bus_add_device(n_i2c_bus, &dev_cfg, &NST112_handle);
    if (ret != ESP_OK) {
        ESP_LOGE("NST112", "添加 NST112 设备失败");
        return ret;
    }
    uint8_t write_buf[] = {0x01,0x60,0xA0}; // 0x01 寄存器地址，0x60 和 0xA0 是要写入的数据
    uint8_t high_temp_config_reg[] = {0x03,0x05,0x50};
    uint8_t low_temp_config_reg[] = {0x02,0x00,0xA0};
    uint8_t read_config_addr = 0x01;
    uint8_t read_H_TMP_addr = 0x02;
    uint8_t read_L_TMP_addr = 0x03;
    i2c_master_transmit(NST112_handle, write_buf, sizeof(write_buf), 1000 / portTICK_PERIOD_MS);
    vTaskDelay(pdMS_TO_TICKS(1000));
    ret = i2c_master_transmit_receive(NST112_handle,&read_config_addr,1 ,config_data,2,pdMS_TO_TICKS(100));
    vTaskDelay(pdMS_TO_TICKS(1000));
    ret = i2c_master_transmit_receive(NST112_handle,&read_L_TMP_addr,1 ,H_tmp_data,2,pdMS_TO_TICKS(100));
    vTaskDelay(pdMS_TO_TICKS(1000));
    ret = i2c_master_transmit_receive(NST112_handle,&read_H_TMP_addr,1 ,L_tmp_data,2,pdMS_TO_TICKS(100));
    vTaskDelay(pdMS_TO_TICKS(1000));
    ESP_LOGI("NST112", "TEMP: %02X %02X L:%02X %02X H:%02X %02X ", config_data[0], config_data[1],L_tmp_data[0],L_tmp_data[1],H_tmp_data[0],L_tmp_data[1]);
    
    //i2c_master_transmit(NST112_handle, high_temp_config_reg, sizeof(high_temp_config_reg), 1000 / portTICK_PERIOD_MS);
    //i2c_master_transmit(NST112_handle, low_temp_config_reg, sizeof(low_temp_config_reg), 1000 / portTICK_PERIOD_MS);
    vTaskDelay(pdMS_TO_TICKS(100));
    uint8_t read_addr = 0x00;
    //i2c_master_transmit_receive(NST112_handle, &read_addr, 1, temp_data, 2, 1000 / portTICK_PERIOD_MS);
    nst112_task_stack = heap_caps_malloc(NST112_TASK_STACK_SIZE, MALLOC_CAP_SPIRAM);
    if (nst112_task_stack == NULL)
    {
        ESP_LOGE("LVGL", "Failed to allocate NST112 task stack");
        return ESP_FAIL;
    }
    xTaskCreateStaticPinnedToCore(nst112_task, "NST112", NST112_TASK_STACK_SIZE, NULL, NST112_TASK_PRIORITY, nst112_task_stack, &nst112_task_tcb,0);
    return ESP_OK;
}

#define MS523_CTRL_GPIO         GPIO_NUM_48
i2c_master_dev_handle_t MS523_handle;
extern StaticTask_t MS523_task_tcb;
extern StackType_t *MS523_task_stack;
esp_err_t ms523_write_byte(uint8_t reg_addr, uint8_t data) {
    uint8_t write_buf[2] = {reg_addr, data};
    esp_err_t ret = i2c_master_transmit(MS523_handle, write_buf, sizeof(write_buf), pdMS_TO_TICKS(100));
    if (ret != ESP_OK) {
        ESP_LOGE("MS523", "Write reg 0x%02X failed: %s", reg_addr, esp_err_to_name(ret));
    }
    return ret;
}

esp_err_t ms523_read_byte(uint8_t reg_addr, uint8_t *data) {
    esp_err_t ret = i2c_master_transmit_receive(MS523_handle, &reg_addr, 1, data, 1, pdMS_TO_TICKS(100));
    if (ret == ESP_OK) {
        ESP_LOGI("MS523", "Read reg 0x%02X = 0x%02X", reg_addr, *data);
    } else {
        ESP_LOGE("MS523", "Read reg 0x%02X failed: %s", reg_addr, esp_err_to_name(ret));
    }
    return ret;
}




#define GPIO_OUTPUT_PIN  GPIO_NUM_48  // 以GPIO2为例
void MS523_ctrl_init(void){

    gpio_config_t io_conf = {
        .pin_bit_mask = (1ULL << GPIO_OUTPUT_PIN), // 选择要配置的引脚[reference:5]
        .mode = GPIO_MODE_OUTPUT,                 // 设置为输出模式[reference:6]
        .pull_up_en = GPIO_PULLUP_DISABLE,        // 禁用内部上拉电阻[reference:7]
        .pull_down_en = GPIO_PULLDOWN_DISABLE,    // 禁用内部下拉电阻[reference:8]
        .intr_type = GPIO_INTR_DISABLE            // 禁用中断[reference:9]
    };
    
    gpio_config(&io_conf); // 应用配置[reference:10]


    // 2. 设置GPIO输出高电平
    gpio_set_level(GPIO_OUTPUT_PIN, 1);
    vTaskDelay(pdMS_TO_TICKS(1000));
    ESP_LOGI("MS523", "当前输出高电平");
}






void ms523_detect_card(void) {
    // 1-4：发送寻卡指令
    ms523_write_byte(0x0A, 0x80);
    ms523_write_byte(0x09, 0x52); 
    ms523_write_byte(0x0D, 0x87);
    ms523_write_byte(0x01, 0x0C);

    // 5：轮询等待 RxIRq（超时 50ms）
    uint8_t irq = 0;
    int timeout_ms = 50;
    TickType_t start = xTaskGetTickCount();
    while ((xTaskGetTickCount() - start) < pdMS_TO_TICKS(timeout_ms)) {
        ms523_read_byte(0x04, &irq);
        if (irq & 0x20) {
            ESP_LOGI("MS523", "✅ Card detected!");
            return; // 检测到卡，退出函数
        }
        vTaskDelay(pdMS_TO_TICKS(1));
    }

    // 检查碰撞
    uint8_t err;
    ms523_read_byte(0x06, &err);
    if (err & 0x08) {
        ESP_LOGI("MS523", "✅ Card detected (Collision)!");
        return;
    }

    // 无卡
    ESP_LOGW("MS523", "No card detected.");
}

void MS523_task(void *param){
    while (1)
    {
        ms523_detect_card();
        ESP_LOGW("MS523", "运行中");
        vTaskDelay(500);
    }
}



esp_err_t MS523_init(void)
{
    MS523_ctrl_init();
    ESP_LOGI(TAG_IMU, "开始初始化 MS523");
    vTaskDelay(pdMS_TO_TICKS(100));
    
#if !EXAMPLE_USE_TOUCH
#error "BMI_init requires EXAMPLE_USE_TOUCH (SC7U22 shares touch I2C bus)."
#endif

    //extern i2c_master_bus_handle_t touch_bus_handle;    I2C主总线
    i2c_master_bus_config_t i2c_bus_config = {0};
    i2c_bus_config.clk_source = I2C_CLK_SRC_DEFAULT;
    i2c_bus_config.i2c_port = 0;
    i2c_bus_config.scl_io_num = TEST_BOARD_I2C_SCL_PIN;
    i2c_bus_config.sda_io_num = TEST_BOARD_I2C_SDA_PIN;
    i2c_bus_config.glitch_ignore_cnt = 7;
    i2c_bus_config.flags.enable_internal_pullup = true;
    //i2c_new_master_bus(&i2c_bus_config, &i2c_bus_handle);
    ESP_ERROR_CHECK(i2c_new_master_bus(&i2c_bus_config, &touch_bus_handle_MS523));
    s_i2c_bus = touch_bus_handle_MS523;
    //i2c_scan_devices();
    
    i2c_device_config_t dev_cfg = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = MS523_I2C_ADDR,
        .scl_speed_hz = I2C_MASTER_FREQ_HZ,
    };
    esp_err_t ret = i2c_master_bus_add_device(s_i2c_bus, &dev_cfg, &MS523_handle);
    uint8_t reg_addr = 0x37;
    uint8_t version = 0;
    uint8_t data = 0;
    ms523_read_byte(reg_addr,&version);
    ms523_write_byte(0x14, 0x83);  // 开启 TX1/TX2 天线驱动

    ms523_write_byte(0x12, 0x00);  // 发送速率 106k
    ms523_write_byte(0x13, 0x08);  // 接收速率 106k
    ms523_write_byte(0x26, 0x48);  // 接收增益 48dB
    if (ret == ESP_OK) {
        ESP_LOGI("MS523", "NFC version:0x%02X", version);
    } else {
        ESP_LOGE("MS523", "Read version failed: %s", esp_err_to_name(ret));
    }
    
    
    MS523_task_stack = heap_caps_malloc(MS523_TASK_STACK_SIZE, MALLOC_CAP_SPIRAM);
    if (MS523_task_stack == NULL)
    {
        ESP_LOGE("LVGL", "Failed to allocate NST112 task stack");
        return ESP_FAIL;
    }
    

    //ms523_write_byte(0x0A, 0x80);
    //ms523_read_byte(0x0A,&data);
    //ms523_write_byte(0x09, 0x52); 
    //ms523_read_byte(0x09,&data);
    //ms523_write_byte(0x0D, 0x87);
    //ms523_read_byte(0x0D,&data);
    //ms523_write_byte(0x01, 0x0C);
    //ms523_read_byte(0x01,&data);
    xTaskCreateStaticPinnedToCore(MS523_task, "MS523", MS523_TASK_STACK_SIZE, NULL, MS523_TASK_PRIORITY, MS523_task_stack, &MS523_task_tcb,1);
    return ESP_OK;
}