#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计算
volatile uint8_t obstacle_level = 0; // 0 - 无障碍 , 1 = 远 ,2 = 中 , 3 = 近
/*******
对于 超声波的配置
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 TEST
void ultrasonic_task_entry(ULONG thread_input) {
    HAL_TIM_IC_Start_IT(&htim5, TIM_CHANNEL_1);
	DWT_Init();
	
    while (1) {
        HCSR04_Trigger();

        ULONG events;
        if (tx_event_flags_get(&ultrasonic_event, EVENT_ECHO_DONE, TX_OR_CLEAR,
                               &events, TX_WAIT_FOREVER) == TX_SUCCESS) {
            if (distance_cm < 30) {
                obstacle_level = 3; // 很近
            }else if(distance_cm > 30 && distance_cm < 50)
			{
				obstacle_level = 2; // 中
			}else if(distance_cm > 50 && distance_cm < 80)
			{
				obstacle_level = 1;
			}else 
			{
				obstacle_level = 0; // 无障碍
			}
			
			if(obstacle_level > 0)
			{
				tx_event_flags_set(&response_events,EVENT_OBSTACLE_DETECTED,TX_OR); // 发送事件
			}
        }

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






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

******/
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) {
    if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
        if (is_first_capture == 0) {
            ic_val1 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_FALLING);
            is_first_capture = 1;
        } else {
            ic_val2 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, 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);
        }
		   HAL_TIM_IC_Start_IT(&htim5,TIM_CHANNEL_1);
    }
}