#include "bsp_sr04.h"
#include "cmsis_os.h"

/* 
 * 声音在 20℃ 时的速度约为 340m/s
 * 距离 (cm) = (微秒时间 * 0.0343) / 2
 */
#define SOUND_SPEED_CM_US 0.0343f

static sr04_data_t sr04_dev = {0};

/**
 * @brief 使用 DWT (Cortex-M 内部计数器) 实现精准微秒延迟
 * @note  72MHz 下计数 72 次即为 1us，这是最高精度的延时/计时方式，不占用通用定时器
 */
static void delay_us(uint32_t us)
{
    uint32_t start_tick = DWT->CYCCNT;
    uint32_t delay_ticks = us * (SystemCoreClock / 1000000);
    
    while ((DWT->CYCCNT - start_tick) < delay_ticks);
}

void sr04_init(void)
{
    /* 1. 初始化 DWT 计数器 (Cortex-M3 内核自带) */
    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; 
    DWT->CYCCNT = 0; 
    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;

    /* 2. 引脚初始状态 */
    HAL_GPIO_WritePin(TRIG_GPIO_Port, TRIG_Pin, GPIO_PIN_RESET);
    HAL_Delay(50); // 等待模块内部稳定
    sr04_dev.distance = 0.0f;
}

float sr04_measure(void)
{
    uint32_t start_time = 0;
    uint32_t end_time = 0;
    uint32_t timeout_cnt = 0;

    /* 0. 确保 Trig 电平已经提前拉低，防止状态异常 */
    HAL_GPIO_WritePin(TRIG_GPIO_Port, TRIG_Pin, GPIO_PIN_RESET);
    delay_us(5);

    /* 1. 发送触发信号：给 20us 宽脉冲（新版 HC-SR04 常需更稳的触发） */
    HAL_GPIO_WritePin(TRIG_GPIO_Port, TRIG_Pin, GPIO_PIN_SET);
    delay_us(25); 
    HAL_GPIO_WritePin(TRIG_GPIO_Port, TRIG_Pin, GPIO_PIN_RESET);

    /* 2. 等待 Echo 引脚变高 (开始计时) */
    // 超时判断改为 10ms (Echo 通常在 Trig 后很快变高)
    timeout_cnt = HAL_GetTick();
    while (HAL_GPIO_ReadPin(ECHO_GPIO_Port, ECHO_Pin) == GPIO_PIN_RESET) {
        if ((HAL_GetTick() - timeout_cnt) > 10) { 
            // 如果死活等不到高电平，说明模块没被触发或 ECHO 引脚接线/电平不对
            sr04_dev.distance = 0.0f; 
            return 0.0f;
        }
    }
    start_time = DWT->CYCCNT;

    /* 3. 等待 Echo 引脚变低 (结束计时) */
    while (HAL_GPIO_ReadPin(ECHO_GPIO_Port, ECHO_Pin) == GPIO_PIN_SET) {
        // 简单保护以防挂死，超过 30ms (5米) 强退
        if ((DWT->CYCCNT - start_time) > (SystemCoreClock / 33)) break; 
    }
    end_time = DWT->CYCCNT;

    /* 4. 精准计算距离 */
    // 振荡次数 / (频率 / 1000000) = 微秒数
    float duration_us = (float)(end_time - start_time) / (SystemCoreClock / 1000000);
    
    // 距离 (cm) = (时间 * 声速) / 2
    // 在这里我们加一个微小的补偿，如果 duration_us 过小也视为 0
    if (duration_us < 5.0f) {
        sr04_dev.distance = 0.0f;
    } else {
        sr04_dev.distance = (duration_us * SOUND_SPEED_CM_US) / 2.0f;
    }
    sr04_dev.last_tick = HAL_GetTick();

    return sr04_dev.distance;
}

float sr04_get_distance(void)
{
    return sr04_dev.distance;
}