#include <stdio.h>
#include "main.h"
#include "cmsis_os.h"
#include <string.h>
#include <stdbool.h>

/* Demo includes. */
#include "fdcan_bus.h"
#include "commonMsg.h"

#define INTERNAL_CAN_IT_FLAGS          (  CAN_IT_TX_MAILBOX_EMPTY |\
                                          CAN_IT_RX_FIFO0_MSG_PENDING |\
                                          CAN_IT_ERROR_WARNING |\
                                          CAN_IT_ERROR_PASSIVE |\
                                          CAN_IT_LAST_ERROR_CODE |\
                                          CAN_IT_ERROR )

/*-----------------------------------------------------------*/
extern FDCAN_HandleTypeDef hfdcan1;

FDCAN_TxHeaderTypeDef   TxHeader;
FDCAN_RxHeaderTypeDef   RxHeader;
uint8_t               TxData[8];
uint8_t               RxData[8];
uint32_t              TxMailbox;

uint16_t ISpritzStationID = 0x310;
uint8_t tab_data[8] = { 0 };

extern volatile uint16_t calibrationStep;
extern volatile bool resetFlag;
extern osMessageQId CANTxQueueHandle;
extern osThreadId CANbusTaskHandle;

/* USER CODE BEGIN PFP */

//Set Transmit parameters
HAL_StatusTypeDef FDCAN_Transmit(uint32_t identifier, uint32_t dataLen, uint32_t data_size, uint8_t * tab_data)
{
uint8_t tx_data[8] = { 0 };

	// Détermine l'adresse du périphérique auquel la trame est destiné.
	TxHeader.DataLength = dataLen;
	TxHeader.TxFrameType = FDCAN_DATA_FRAME;
	TxHeader.BitRateSwitch = FDCAN_BRS_OFF;
	TxHeader.ErrorStateIndicator = FDCAN_ESI_PASSIVE;
	TxHeader.FDFormat = FDCAN_FD_CAN;
	TxHeader.IdType = FDCAN_STANDARD_ID;
	TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
	TxHeader.MessageMarker = 0;
	TxHeader.Identifier = identifier;

	memcpy(tx_data,tab_data,data_size);

    HAL_StatusTypeDef ret = HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, tx_data);
    return ret;
}

void FDCAN_filterConfig(void)
{
	FDCAN_FilterTypeDef sFilterConfig;

    sFilterConfig.IdType = FDCAN_STANDARD_ID;
    sFilterConfig.FilterIndex = 0;
    sFilterConfig.FilterType = FDCAN_FILTER_MASK;
    sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
    sFilterConfig.FilterID1 = 0x0FE;
    sFilterConfig.FilterID2 = 0x7FF;
    if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }

    /* Configure global filter on both FDCAN instances:
    Filter all remote frames with STD and EXT ID
    Reject non matching frames with STD ID and EXT ID */
    if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE) != HAL_OK)
    {
        Error_Handler();
    }

    /* Activate Rx FIFO 0 new message notification on both FDCAN instances */
    if (HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) != HAL_OK)
    {
        Error_Handler();
    }

    if (HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_BUS_OFF, 0) != HAL_OK)
    {
        Error_Handler();
    }

	//Start listening for CAN traffic with IT
	HAL_FDCAN_Start(&hfdcan1);
}

void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
{
osMessageQId destQueue;

	if((RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) != 0)
	{
		/* Retrieve Rx messages from RX FIFO0 */
		if( HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &RxHeader, RxData) != HAL_OK )
		{
			return;
		}
		if ( RxHeader.Identifier == MCU_R_COMMAND )
		{
			dev_command_request request;

			memcpy(request.data,RxData,3);
			// Read device address and process calibration if matches
			if( request.RQ_Device == MCU_S_WATERTANK )
			{
				switch( request.RQ_OpCode ) {
					case 3: calibrationStep++;	// Calibration
						break;
					case 2: calibrationStep = 0;	// Reset
						break;
				}

				destQueue = (osMessageQId)getQueueId(WATER_TANK);
				MSG_STRUCT *p_sendMsg = allocMemoryPoolMessage(); // receiver must free
				if (calibrationStep > 0) {
					p_sendMsg->opcode = OP_CALIBRATION;
					p_sendMsg->parameter = calibrationStep;
				} else {
					p_sendMsg->opcode = OP_RESET;
					p_sendMsg->parameter = 0;
				}
				osMessagePut(destQueue, (uint32_t)p_sendMsg, osWaitForever);
			}

		}
    }
}

// --- Optional: FDCAN Transmit Complete Callback (ISR) ---
// This ISR is typically called by the HAL driver when a message has been successfully transmitted
// and the corresponding hardware Tx buffer is free.
void HAL_FDCAN_TxBufferCompleteCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t BufferIndexes)
{
	UNUSED(hfdcan);
	UNUSED(BufferIndexes);

    // Ensure this callback is for the correct FDCAN instance if you have multiple
    if (hfdcan == &hfdcan1)
    {
        BaseType_t xHigherPriorityTaskWoken = pdFALSE;
        // Notify the CAN Tx task that a buffer is now free.
        // This unblocks vCanBusTxTask if it's waiting for a notification.
        vTaskNotifyGiveFromISR(CANbusTaskHandle, &xHigherPriorityTaskWoken);
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    }
}

BaseType_t CAN_EnqueueFrame(const CAN_Frame_t *pxFrame)
{
    return xQueueSend(CANTxQueueHandle, pxFrame, portMAX_DELAY); // Bloque jusqu'à ce que la file soit disponible
}

void vCanBusTask( void *pvParameters )
{
/* Remove warning for unused parameter */
    (void)pvParameters;
    CAN_Frame_t xFrameToSend;
    HAL_StatusTypeDef hal_status;

    FDCAN_filterConfig();

    while (1)
    {
        // 1. Wait for a CAN frame to be available in the queue.
        //    This task will block here until a frame is available.
        //    portMAX_DELAY means wait indefinitely.
        if (xQueueReceive(CANTxQueueHandle, &xFrameToSend, portMAX_DELAY) == pdPASS)
        {
            // A frame has been successfully retrieved from the queue.
            // Now, attempt to transmit it via FDCAN hardware.

            // 2. Try to add the message to an FDCAN transmit buffer.
            //    This is where you interact with the hardware.
            //    HAL_FDCAN_AddMessageToTxBuffer attempts to add the message.
            //    It returns HAL_OK on success, HAL_BUSY if no buffer is free, or HAL_ERROR.
        	hal_status = FDCAN_Transmit(xFrameToSend.identifier, xFrameToSend.dataLength,
        			xFrameToSend.dataSize, xFrameToSend.data);

            // If using FDCAN_TX_FIFO_IT, the HAL will manage which buffer to use.
            // If using a specific buffer (FDCAN_TX_BUFFER0, etc.), you might need to
            // manage which buffer index is free (e.g., using the BufferIndexes from callback).
            // For simplicity, let's assume FDCAN_TX_FIFO_IT for automatic buffer selection.
            // If you intend to use specific buffers, the ISR and task logic needs to be more complex
            // to pass which buffer is free. Using the FIFO is generally simpler.

            if (hal_status != HAL_OK)
            {
                // Transmission failed (e.g., all hardware Tx buffers are full).
                // This means the TxBufferCompleteCallback hasn't freed a buffer yet,
                // or another higher priority task/ISR filled it immediately.
                // You need to decide how to handle this:
                // Option A: Re-queue the frame to try again. (Be careful of infinite loops if always full)
                //           xQueueSendToFront(CANTxQueueHandle, &xFrameToSend, 0); // Try to put it back
                // Option B: Log an error or increment a counter.
                //           printf("CAN Tx failed: buffer full or error.\r\n");
                // Option C: Implement a short delay before trying again (careful with real-time).
                //           vTaskDelay(pdMS_TO_TICKS(1));

                // For this example, we'll just log and drop it to prevent potential infinite blocking.
                // In a real application, you might want a retry mechanism or error propagation.
                // printf("Warning: Failed to add CAN frame to hardware buffer, dropping frame.\n");

                // If you are relying on the TxBufferCompleteCallback to unblock,
                // and you get HAL_BUSY, it means the buffer isn't actually free *right now*.
                // In this case, you might need to go back and wait for another notification.
                // This highlights why the `ulTaskNotifyTake` should be the primary unblock mechanism.
            }
            else
            {
                // Frame successfully handed over to FDCAN hardware for transmission.
                // The FDCAN hardware will generate the TxBufferCompleteCallback when done.
            }
        }
    }
}

