/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file    app_threadx.c
  * @author  MCD Application Team
  * @brief   ThreadX applicative file
  ******************************************************************************
    * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "app_threadx.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

// BLE define 
#define BLE_RX_THREAD_STACK_SIZE 2048
#define BLE_RX_THREAD_PRIORITY    9
#define BLE_TX_THREAD_STACK_SIZE 2048
#define BLE_TX_THREAD_PRIORITY    10
// IMU thread config
// IMU thread config
#define IMU_ANGLE_THREAD_STACK_SIZE  1024
#define IMU_ANGLE_THREAD_PRIORITY    11



/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* ȫ<>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */

TX_EVENT_FLAGS_GROUP system_events;
TX_EVENT_FLAGS_GROUP sensor_events;
TX_EVENT_FLAGS_GROUP response_events; // 用于避障事件实现

MotorCommand current_motor_cmd = {0,0};
_GPSData gps_data;

// Ble thread vd
TX_THREAD ble_rx_thread;
TX_THREAD ble_tx_thread;
UCHAR ble_rx_stack[BLE_RX_THREAD_STACK_SIZE];
UCHAR ble_tx_stack[BLE_TX_THREAD_STACK_SIZE];

TX_QUEUE ble_tx_queue;
#define BLE_TX_QUEUE_LEN 10
//ULONG ble_tx_queue_buffer[BLE_TX_QUEUE_LEN]; // 一定要是ULONG类型 之前使用的是BLE_Message

#define BLE_TX_MSG_LEN 64
#define BLE_TX_QUEUE_LEN 10

__attribute__((aligned(4)))
ULONG ble_tx_queue_buffer[BLE_TX_QUEUE_LEN * ((BLE_TX_MSG_LEN + sizeof(ULONG) - 1) / sizeof(ULONG))];

// imu
TX_THREAD imu_angle_thread;
UCHAR imu_angle_stack[IMU_ANGLE_THREAD_STACK_SIZE];

#define IM948_RX_QUEUE_SIZE 64
ULONG im948_rx_queue_buffer[IM948_RX_QUEUE_SIZE];
TX_QUEUE im948_uart_rx_queue;
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
#define GPS_TASK_STACK_SIZE   2048
#define GPS_TASK_PRIORITY     11

TX_THREAD gps_task;
ULONG gps_task_stack[GPS_TASK_STACK_SIZE / sizeof(ULONG)];

static TX_THREAD obstacle_thread;
static UCHAR obstacle_stack[512];
//超声波
static UCHAR ultrasonic_stack[512];
static void obstacle_thread_entry(ULONG);
TX_THREAD ultrasonic_task_thread;

/* USER CODE END 1 */
/* USER CODE END 1 */

/* USER CODE END PFP */

/**
  * @brief  Application ThreadX Initialization.
  * @param memory_ptr: memory pointer
  * @retval int
  */
UINT App_ThreadX_Init(VOID *memory_ptr)
{
    HCBle_SendData("进入 App_ThreadX_Init\r\n");
//	HCBle_SendData("%d",sizeof(ULONG));
	UINT status;

	// === 创建 BLE RX 线程 ===
    status = tx_thread_create(&ble_rx_thread, "BLE RX Thread",
                              ble_rx_task_entry, 0,
                              ble_rx_stack, BLE_RX_THREAD_STACK_SIZE,
                              BLE_RX_THREAD_PRIORITY, BLE_RX_THREAD_PRIORITY,
                              TX_NO_TIME_SLICE, TX_AUTO_START);
	
	if(status != TX_SUCCESS)
	{
		return status;
	}
	
    // === 创建 BLE TX 线程 ===
    status = tx_thread_create(&ble_tx_thread, "BLE TX Thread",
                              ble_tx_task_entry, 0,
                              ble_tx_stack, BLE_TX_THREAD_STACK_SIZE,
                              BLE_TX_THREAD_PRIORITY, BLE_TX_THREAD_PRIORITY,
                              TX_NO_TIME_SLICE, TX_AUTO_START);
	
    if (status != TX_SUCCESS) {
        HCBle_SendData("❌ BLE TX 线程创建失败，错误码=%d\r\n", status);
        return status;
    }
	
	    // === 创建 BLE TX 消息队列 ===
	status = tx_queue_create(&ble_tx_queue, "BLE TX Queue",
							 (BLE_TX_MSG_LEN + sizeof(ULONG) - 1) / sizeof(ULONG),
							 ble_tx_queue_buffer,
							 sizeof(ble_tx_queue_buffer));


     if (status != TX_SUCCESS) {
        HCBle_SendData("❌ BLE TX 消息队列创建失败，错误码=%d\r\n", status);
        return status;
    }

	status = tx_thread_create(&imu_angle_thread, "IMU Angle Thread",
                          imu_angle_ble_task_entry, 0,
                          imu_angle_stack, IMU_ANGLE_THREAD_STACK_SIZE,
                          IMU_ANGLE_THREAD_PRIORITY, IMU_ANGLE_THREAD_PRIORITY,
                          TX_NO_TIME_SLICE, TX_AUTO_START);

	if (status != TX_SUCCESS) {
		return status;
	}

	
	status = tx_queue_create(&im948_uart_rx_queue, "IM948_RX_QUEUE",
                               TX_1_ULONG, // sizeof(ULONG) bytes per entry
                               im948_rx_queue_buffer,
                               IM948_RX_QUEUE_SIZE * sizeof(ULONG));
	if(status != TX_SUCCESS)
	{
		return status;
	}
	
	status = tx_thread_create(&gps_task,
                  "GPS Task",
                  gps_thread_entry,
                  0,
                  gps_task_stack,
                  GPS_TASK_STACK_SIZE,
                  GPS_TASK_PRIORITY,
                  GPS_TASK_PRIORITY,
                  TX_NO_TIME_SLICE,
                  TX_AUTO_START);
	
	if(status != TX_SUCCESS)
	{
		return status;
	}
	
	// obstacle_thread create
	status = tx_thread_create(&obstacle_thread,
							"obstacle",
							obstacle_thread_entry,
							0,obstacle_stack,sizeof(obstacle_stack),8,8,TX_NO_TIME_SLICE,TX_AUTO_START);
	
	if(status != TX_SUCCESS)
	{
		return status;
	}
	
	 status = tx_thread_create(&ultrasonic_task_thread,
                     "Ultrasonic",
                     ultrasonic_task_entry,
                     0,
                     ultrasonic_stack,
                     sizeof(ultrasonic_stack),
                     7, 7,    // 这里的优先级
                     TX_NO_TIME_SLICE,
                     TX_AUTO_START);
		
		if(status != TX_SUCCESS)
	{
		return status;
	}
	
	//
//    HCBle_SendData("✅ BLE RX/TX 线程和队列初始化完成\r\n");
					 
//	status = ControlThreadCreate();
//	if(status != TX_SUCCESS)
//	{
//		return status;
//	}			 
//	status = Encoder_ThreadCreate();	
//	if(status != TX_SUCCESS)
//	{
//		return status;
//	}	
	
    return TX_SUCCESS;
}


  /**
  * @brief  Function that implements the kernel's initialization.
  * @param  None
  * @retval None
  */
void MX_ThreadX_Init(void)
{
  /* USER CODE BEGIN Before_Kernel_Start */
	HCBle_InitEventFlags();  // 这必须在任何使用前调用一次
  /* USER CODE END Before_Kernel_Start */
  tx_kernel_enter();

  /* USER CODE BEGIN Kernel_Start_Error */

  /* USER CODE END Kernel_Start_Error */
}

/* USER CODE BEGIN 1 */
//新加入的 obstacle 
static void obstacle_thread_entry(ULONG arg)
{
	while(1)
	{
		ULONG evt; // 这个应该是用来接收数据的
		tx_event_flags_get(&response_events,EVENT_OBSTACLE_DETECTED,TX_OR_CLEAR,&evt,TX_WAIT_FOREVER);
		
		switch(obstacle_level)
		{
			case 1: // 远
				Buzzer_Open();
				Shake_Motor_Open();
				tx_thread_sleep(30);
				Buzzer_Close();
				Shake_Motor_Close();
				break;
			
			case 2:
				for(int i = 0; i < 2;i++)
				{
					Buzzer_Open();
					Shake_Motor_Open();
					tx_thread_sleep(50);
					Buzzer_Close();
                    Shake_Motor_Close();
                    tx_thread_sleep(30);
				}
				break;
				
			case 3: // 近
				Buzzer_Open();
				Shake_Motor_Open();
				tx_thread_sleep(150);
				Buzzer_Close();
				Shake_Motor_Close();
				break;
			default:
				break;
		}
		tx_thread_sleep(20);
	}
}

/* USER CODE END 1 */
