#include "can_controller.h"
#include <cstring>
#include <main.h>
#include <base/zlog.h>
#define TAG "CAN"

#define CAN_MESSAGE_POOL_SIZE 20
// 使用 CMSIS - RTOS v2 的内存池句柄类型
osMemoryPoolId_t can_message_pool;
// 使用 CMSIS - RTOS v2 的信号量句柄类型
osSemaphoreId_t g_can_rx0_semaphore = NULL;
// 使用 CMSIS - RTOS v2 的消息队列句柄类型
osMessageQueueId_t g_can_rx0_queue = NULL;

CanMessage g_can_recv_msg;

CANSystemResourceManager canSystemResourceManager;

// 新的线程函数
void canMessageProcessingTask(void *argument) {
	while (1) {
		// 等待信号量
		osSemaphoreAcquire(g_can_rx0_semaphore, osWaitForever);

		CanMessage* msgPtr = (CanMessage*)osMemoryPoolAlloc(can_message_pool, osWaitForever);
		if (msgPtr == NULL) {
			continue;
		}

		msgPtr->id = g_can_recv_msg.id;
		msgPtr->length = g_can_recv_msg.length;
		memcpy(msgPtr->data, g_can_recv_msg.data, msgPtr->length);
#if 1
		// 将消息放入接收队列
		osStatus_t status = osMessageQueuePut(g_can_rx0_queue, &msgPtr, 0U, 0U);
		if (status != osOK) {
			osMemoryPoolFree(can_message_pool, msgPtr);
		}
#endif
	}
}

CANSystemResourceManager::CANSystemResourceManager() {
	// 初始化信号量
	osSemaphoreAttr_t semaphore_attr = {0};
	semaphore_attr.name = "g_can_rx0_semaphore";
	g_can_rx0_semaphore = osSemaphoreNew(1, 1, &semaphore_attr);
	if (g_can_rx0_semaphore == NULL) {
		// 处理信号量创建失败的情况
	}

	// 创建对象池
	osMemoryPoolAttr_t pool_attr = {0};
	pool_attr.name = "can_message_pool";
	can_message_pool = osMemoryPoolNew(CAN_MESSAGE_POOL_SIZE, sizeof(CanMessage), &pool_attr);
	if (can_message_pool == NULL) {
		// 处理对象池创建失败的情况
		if (g_can_rx0_semaphore != NULL) {
			osSemaphoreDelete(g_can_rx0_semaphore);
		}
	}

	// 创建接收队列
	osMessageQueueAttr_t queue_attr = {0};
	queue_attr.name = "g_can_rx0_queue";

	g_can_rx0_queue = osMessageQueueNew(20, sizeof(CanMessage*), &queue_attr);
	if (g_can_rx0_queue == NULL) {
		// 处理消息队列创建失败的情况
		if (g_can_rx0_semaphore != NULL) {
			osSemaphoreDelete(g_can_rx0_semaphore);
		}
		if (can_message_pool != NULL) {
			osMemoryPoolDelete(can_message_pool);
		}
	}

	// 创建 CAN 消息处理线程
	osThreadAttr_t canProcessingTaskAttr = {0};
	canProcessingTaskAttr.name = "CanMessageProcessingTask";
	canProcessingTaskAttr.stack_size = 128 * 8;
	canProcessingTaskAttr.priority = osPriorityNormal;
	osThreadNew(canMessageProcessingTask, NULL, &canProcessingTaskAttr);
}

CANSystemResourceManager::~CANSystemResourceManager() {
	// 释放信号量
	if (g_can_rx0_semaphore != NULL) {
		osSemaphoreDelete(g_can_rx0_semaphore);
	}

	// 释放消息队列
	if (g_can_rx0_queue != NULL) {
		osMessageQueueDelete(g_can_rx0_queue);
	}

	// 释放对象池
	if (can_message_pool != NULL) {
		osMemoryPoolDelete(can_message_pool);
	}
}

/**
 * @brief     CAN RX0 中断服务函数
 * @note      处理CAN FIFO0的接收中断
 */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
	if (hcan->Instance == CAN1) {
		CAN_RxHeaderTypeDef rx_msg_header;

		if (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) == 0)     /* 没有接收到数据 */
		{
			return;
		}

		if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_msg_header, g_can_recv_msg.data) != HAL_OK)  /* 读取数据 */
		{
			return;
		}
#if 0
		if (rx_msg_header.StdId != (CAN_BASE_FRAME_ID + CAN_CONFIG->getCanFrameId()) ||
				rx_msg_header.IDE != CAN_ID_STD || rx_msg_header.RTR != CAN_RTR_DATA)       /* 接收到的ID不对 / 不是标准帧 / 不是数据帧 */
		{
			return;
		}
#else
		if ((rx_msg_header.StdId != (CAN_BASE_FRAME_ID + CAN_CONFIG->getCanFrameId()) &&
				rx_msg_header.StdId != (CAN_X_MOTOR_FRAME_ID + CAN_CONFIG->getCanFrameId()) &&
				rx_msg_header.StdId != (CAN_Y_MOTOR_FRAME_ID + CAN_CONFIG->getCanFrameId())&&
				rx_msg_header.StdId != (CAN_Z_MOTOR_FRAME_ID + CAN_CONFIG->getCanFrameId()))||
						rx_msg_header.IDE != CAN_ID_STD || rx_msg_header.RTR != CAN_RTR_DATA)       /* 接收到的ID不对 / 不是标准帧 / 不是数据帧 */
			{
				return;
			}
#endif
		g_can_recv_msg.id = rx_msg_header.StdId;
		g_can_recv_msg.length = rx_msg_header.DLC;

		osSemaphoreRelease(g_can_rx0_semaphore);
	}
}

CanController::CanController()
: tx_queue(NULL),
  tx_task_handle(NULL) {
}

CanController::~CanController() {
	stop();
}

bool CanController::start(CAN_HandleTypeDef* hcan) {
	this->hcan = hcan;
	// 启动CAN外围设备
	CAN_FilterTypeDef sFilterConfig;

	/* 配置CAN过滤器 */
	sFilterConfig.FilterBank = 0;                             /* 过滤器0 */
	sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
	sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
	sFilterConfig.FilterIdHigh = 0x0000;                      /* 32位ID */
	sFilterConfig.FilterIdLow = 0x0000;
	sFilterConfig.FilterMaskIdHigh = 0x0000;                  /* 32位MASK */
	sFilterConfig.FilterMaskIdLow = 0x0000;
	sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0;    /* 过滤器0关联到FIFO0 */
	sFilterConfig.FilterActivation = CAN_FILTER_ENABLE;       /* 激活滤波器0 */
	sFilterConfig.SlaveStartFilterBank = 14;

	/* 过滤器配置 */
	if (HAL_CAN_ConfigFilter(hcan, &sFilterConfig) != HAL_OK) {
		Error_Handler();
	}

	if (HAL_CAN_Start(hcan) != HAL_OK) {
		Error_Handler();
	}
	if (HAL_CAN_ActivateNotification(hcan, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) {
		Error_Handler();
	}
	mutex_tx_queue_ = osMutexNew(NULL);

	// 创建发送队列
	osMessageQueueAttr_t tx_queue_attr = {0};
	tx_queue_attr.name = "tx_queue";
	tx_queue = osMessageQueueNew(20, sizeof(CanMessage*), &tx_queue_attr);
	if (tx_queue == NULL) {
		return false;
	}

	// 创建发送任务
	osThreadAttr_t tx_task_attr = {0};
	tx_task_attr.name = "CanTxTask";
	tx_task_attr.stack_size = 256 * 8;
	tx_task_attr.priority = osPriorityNormal;
	tx_task_handle = osThreadNew(canTxTask, this, &tx_task_attr);
	if (tx_task_handle == NULL) {
		return false;
	}

	// 创建协议解析任务
	osThreadAttr_t parser_task_attr = {0};
	parser_task_attr.name = "CanParserTask";
	parser_task_attr.stack_size = 256 * 8;
	parser_task_attr.priority = osPriorityNormal;
	parser_task_handle = osThreadNew(canParserTask, this, &parser_task_attr);
	if (parser_task_handle == NULL) {
		osThreadTerminate(tx_task_handle);
		return false;
	}
	return true;
}

void CanController::stop() {
	if (tx_task_handle != NULL) {
		osThreadTerminate(tx_task_handle);
	}
	if (parser_task_handle != NULL) {
		osThreadTerminate(parser_task_handle);
	}
	if (tx_queue != NULL) {
		osMessageQueueDelete(tx_queue);
	}
}

bool CanController::sendMessage(const CanMessage& msg) {
#if CAN_MODULE_ENABLE
	CanMessage* msgPtr = (CanMessage*)osMemoryPoolAlloc(can_message_pool, osWaitForever);
	if (msgPtr == NULL) {
		return false;
	}
	msg.cloneTo(msgPtr);
	osMutexAcquire(mutex_tx_queue_, osWaitForever);
	osStatus_t status = osMessageQueuePut(tx_queue, &msgPtr, 0U, 0U);
	osMutexRelease(mutex_tx_queue_);
	if (status != osOK) {
		osMemoryPoolFree(can_message_pool, msgPtr);
		return false;
	}
	return true;
#endif
}

void CanController::canTxTask(void *argument) {
	CanController* controller = static_cast<CanController*>(argument);
	CanMessage *msgPtr = NULL;
	CAN_TxHeaderTypeDef can_msg_header;

	while (1) {
		osStatus_t status = osMessageQueueGet(controller->tx_queue, &msgPtr, NULL, osWaitForever);
		if (status == osOK) {
			if(msgPtr == NULL)
			{
				return;
			}
			uint16_t t = 0;
			uint32_t TxMailbox = CAN_TX_MAILBOX0;

			can_msg_header.StdId = msgPtr->id;
			can_msg_header.ExtId = msgPtr->id;
			can_msg_header.IDE = CAN_ID_STD;        // 标准帧
			can_msg_header.RTR = CAN_RTR_DATA;    // 数据帧
			can_msg_header.DLC = msgPtr->length;

			if (HAL_CAN_AddTxMessage(controller->hcan, &can_msg_header, msgPtr->data, &TxMailbox) != HAL_OK) /* 发送消息 */
			{
				osMemoryPoolFree(can_message_pool, msgPtr);
				continue;
			}

			while (HAL_CAN_GetTxMailboxesFreeLevel(controller->hcan) != 3)   /* 等待发送完成,所有邮箱为空 */
			{
				t++;
				if (t > 0xFFF) {
					HAL_CAN_AbortTxRequest(controller->hcan, TxMailbox);     /* 超时，直接中止邮箱的发送请求 */
					break;
				}
			}
			osMemoryPoolFree(can_message_pool, msgPtr);
			osDelay(2);
		}
	}
}

void CanController::canParserTask(void *argument) {
#if 1
	CanController* controller = static_cast<CanController*>(argument);
	CanMessage *msgPtr = NULL;
	while (1) {
		osStatus_t status = osMessageQueueGet(g_can_rx0_queue, &msgPtr, NULL, osWaitForever);
		if (status == osOK) {
			if(msgPtr != NULL) {
				controller->parser.parseMessage(msgPtr);
				osMemoryPoolFree(can_message_pool, msgPtr);
			}
		}
	}
#endif
}