#include "Ultrasound.h"
TX_THREAD ultrasonic_task_handle;
TX_EVENT_FLAGS_GROUP ultrasonic_event;


extern TIM_HandleTypeDef htim2;

volatile uint32_t ic_val1 = 0; // 捕获值1
volatile uint32_t ic_val2 = 0; // 捕获值2
volatile uint8_t  is_first_capture = 0; // 是否为第一次捕获
volatile uint32_t distance_cm = 0;  // 距离按照 cm计算

/*******
对于 超声波的配置
Psc: 250 - 1  以达到   1tick = 1us的效果
*******/

/****

DWT的初始化  待确认

放到main.c中进行初始化
****/
void DWT_Init(void)
{
	CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
	DWT->CYCCNT = (uint32_t)0u; //新加入的
	DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
}




/********
delay_us() 函数实现 (使用DWT)  待确认
*********/
void delay_us(uint32_t us)
{
	uint32_t start = DWT->CYCCNT;
	uint32_t ticks = us * (SystemCoreClock / 1000000U); // us * 64
	while((DWT->CYCCNT - start) < ticks);
}




/******
Trig 触发

 You only need to supply a short 10uS 
pulse to the trigger input to start the ranging

尤其在周期性测距中，如果前一个测距周期残留了 Trig 为高电平，可能导致错误测距或模块死机。因此加上 HAL_GPIO_WritePin(..., RESET); HAL_Delay(); 是一种保险写法。
******/
void HCSR04_Trigger(void)
{
	HAL_GPIO_WritePin(HC_Trig_GPIO_Port,HC_Trig_Pin,GPIO_PIN_RESET);
	delay_us(2);  //拉低2~5us
	HAL_GPIO_WritePin(HC_Trig_GPIO_Port,HC_Trig_Pin,GPIO_PIN_SET);
	delay_us(10); // 保持高电平10us 
	HAL_GPIO_WritePin(HC_Trig_GPIO_Port,HC_Trig_Pin,GPIO_PIN_RESET);
}


#ifdef HCSR_TEST
void ultrasonic_task_entry(ULONG thread_input) {
    HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_2);

    while (1) {
        HCSR04_Trigger();

        ULONG actual_flags;
        if (tx_event_flags_get(&ultrasonic_event, EVENT_ECHO_DONE, TX_OR_CLEAR,
                               &actual_flags, TX_WAIT_FOREVER) == TX_SUCCESS) {
            if (distance_cm < 30) {
                // 执行避障处理：如振动、停机、语音提示等
            }
        }

        tx_thread_sleep(50);  // 每次测距间隔 50 ticks
    }
}
#endif




/*******
Echo 输入捕获回调函数

******/
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) {
    if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_2) {
        if (is_first_capture == 0) {
            ic_val1 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_2);
            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_2, TIM_INPUTCHANNELPOLARITY_FALLING);
            is_first_capture = 1;
        } else {
            ic_val2 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_2);
            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_2, TIM_INPUTCHANNELPOLARITY_RISING);
            is_first_capture = 0;

            uint32_t delta = (ic_val2 > ic_val1) ? (ic_val2 - ic_val1) : (0xFFFF - ic_val1 + ic_val2);
            distance_cm = delta / 58;

            // 通知任务
//            tx_event_flags_set(&ultrasonic_event, EVENT_ECHO_DONE, TX_OR);
        }
    }
}