/*
 * uart_tasks.c
 *
 *  Created on: Aug 14, 2024
 *      Author: jakubski
 */

#include "cmsis_os.h"
#include "main.h"

#include <stdio.h>
#include <string.h>

#include "interprocess_data.h"
#include "measurements.h"
#include "mock_tasks.h"
#include "uart_tasks.h"
#include "meas_tasks.h"

enum SerialReceiverStates { srWaitForHeader, srCheckCrc, srRecieveData, srExecuteCmd, srFail, srFinish, srLast };

// osThreadId_t uart8RecieveTaskHandle;
// osThreadId_t uart8TransmitTaskHandle;
//  osSemaphoreDef_t uart8RxSemaphoreDef;
//  osSemaphoreId_t uart8RxSemaphore = NULL;

// osMessageQueueId_t uart8DecodedFrameDataTaskQueue;
// osMessageQueueId_t uart8SendCmdToSlaveQueue;

extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart8;
extern DMA_HandleTypeDef hdma_uart8_rx;
extern CRC_HandleTypeDef hcrc;

uint8_t uart1RxBuffer[UART1_RX_BUFF_SIZE]        = { 0 };
uint8_t uart1TxBuffer[UART1_TX_BUFF_SIZE]        = { 0 };
uint8_t uart1TaskFrameData[INPUT_DATA_BUFF_SIZE] = { 0 };

uint8_t uart8RxBuffer[UART8_RX_BUFF_SIZE]        = { 0 };
uint8_t uart8TxBuffer[UART8_TX_BUFF_SIZE]        = { 0 };
uint8_t uart8TaskFrameData[INPUT_DATA_BUFF_SIZE] = { 0 };

uint8_t boardToUartNumberMap[SLAVES_COUNT] = { /*1*/ 8, 3, 6, 2 };

UartTaskData uart1TaskData = { 0 }; // Board 1
UartTaskData uart3TaskData = { 0 }; // Board 2
UartTaskData uart6TaskData = { 0 }; // Board 3
UartTaskData uart2TaskData = { 0 }; // Board 4
UartTaskData uart8TaskData = { 0 }; // Debug

UartTaskData* uartTasks[] = { &uart8TaskData, NULL };

uint8_t outputDataBuffer[OUTPUT_DATA_BUFF_SIZE];
uint16_t outputDataBufferPos = 0;


//extern uint32_t slaveLastSeen[SLAVES_COUNT];
//extern RESMeasurements resMeasurements[SLAVES_COUNT];
//extern SesnorsInfo sensorsInfo[SLAVES_COUNT];
//extern osMutexId_t resMeasurementsMutex;
//extern osMutexId_t sensorsInfoMutex;
//RESMeasurements resMeasurements = { 0 };
//SesnorsInfo sensorsInfo         = { 0 };
uint32_t slaveLastSeen[SLAVES_COUNT] = { 0 };
//osMutexId_t resMeasurementsMutex;
//osMutexId_t sensorsInfoMutex;

extern RNG_HandleTypeDef hrng;

void UartTasksInit(void) {
  uart1TaskData.uartRxBuffer = uart1RxBuffer;
  uart1TaskData.uartRxBufferLen = UART1_RX_BUFF_SIZE;
  uart1TaskData.uartTxBuffer = uart1TxBuffer;
  uart1TaskData.uartRxBufferLen = UART1_TX_BUFF_SIZE;
  uart1TaskData.frameData = uart1TaskFrameData;
  uart1TaskData.frameDataLen = UART1_RX_BUFF_SIZE;
  uart1TaskData.huart = &huart1;
  uart1TaskData.uartNumber = 1;
  uart1TaskData.processDataCb = Uart1ReceivedDataProcessCallback;
  uart1TaskData.processRxDataMsgBuffer = NULL;

//  uart8TaskData.uartRxBuffer = uart8RxBuffer;
//  uart8TaskData.uartRxBufferLen = UART8_RX_BUFF_SIZE;
//  uart8TaskData.uartTxBuffer = uart8TxBuffer;
//  uart8TaskData.uartRxBufferLen = UART8_TX_BUFF_SIZE;
//  uart8TaskData.frameData = uart8TaskFrameData;
//  uart8TaskData.frameDataLen = UART8_RX_BUFF_SIZE;
//  uart8TaskData.huart = &huart8;
//  uart8TaskData.uartNumber = 8;
//  uart8TaskData.processDataCb = Uart8ReceivedDataProcessCallback;
//  uart8TaskData.processRxDataMsgBuffer = NULL;

  UartTaskCreate(&uart1TaskData);
//  UartTaskCreate(&uart8TaskData);
}

void UartTaskCreate (UartTaskData* uartTaskData) {
    osThreadAttr_t osThreadAttrRxUart = { 0 };
//    osThreadAttr_t osThreadAttrTxUart = { 0 };

    osThreadAttrRxUart.stack_size = configMINIMAL_STACK_SIZE * 2;
    osThreadAttrRxUart.priority   = (osPriority_t)osPriorityHigh;
    uartTaskData->uartRecieveTaskHandle = osThreadNew (UartRxTask, uartTaskData, &osThreadAttrRxUart);

//    osMessageQueueAttr_t uartTxMsgQueueAttr = { 0 };
//    uartTaskData->sendCmdToSlaveQueue = osMessageQueueNew (16, sizeof (InterProcessData), &uartTxMsgQueueAttr);

//    osThreadAttrTxUart.stack_size        = configMINIMAL_STACK_SIZE * 4;
//    osThreadAttrTxUart.priority          = (osPriority_t)osPriorityNormal;
//    uartTaskData->uartTransmitTaskHandle = osThreadNew (UartTxTask, uartTaskData, &osThreadAttrTxUart);
}

void Uart8TasksInit (void) {
    osThreadAttr_t osThreadAttrRxUart = { 0 };
//    osThreadAttr_t osThreadAttrTxUart = { 0 };
#if 0
	osMessageQueueAttr_t uartRxMsgQueueAttr = { 0 };
	uartRxMsgQueueAttr.name = "uart8RxMsgQueue";
	uart8DecodedFrameDataTaskQueue = osMessageQueueNew(4, sizeof(SerialProtocolFrameData), &uartRxMsgQueueAttr);
	uart8TaskData.processDataQueue = uart8DecodedFrameDataTaskQueue;
#else
//    uart8TaskData.processDataQueue = NULL;
#endif

//	uart8TaskData.processRxDataMsgBuffer = xMessageBufferCreate( INPUT_DATA_BUFF_SIZE );
//    uart8TaskData.processDataCb = NULL;
    uart8TaskData.processDataCb = Uart8ReceivedDataProcessCallback;

//    resMeasurementsMutex = osMutexNew (NULL);
//    sensorsInfoMutex     = osMutexNew (NULL);

    osThreadAttrRxUart.name       = "os_thread_uart8_rx";
    osThreadAttrRxUart.stack_size = configMINIMAL_STACK_SIZE * 2;
    osThreadAttrRxUart.priority   = (osPriority_t)osPriorityHigh;

    uart8TaskData.uartRxBuffer          = uart8RxBuffer;
    uart8TaskData.uartRxBufferLen       = UART8_RX_BUFF_SIZE;
    uart8TaskData.uartTxBuffer          = uart8TxBuffer;
    uart8TaskData.uartRxBufferLen       = UART8_TX_BUFF_SIZE;
    uart8TaskData.frameData             = uart8TaskFrameData;
    uart8TaskData.frameDataLen          = UART8_RX_BUFF_SIZE;
    uart8TaskData.huart                 = &huart8;
    uart8TaskData.uartNumber            = 8;
    uart8TaskData.uartRecieveTaskHandle = osThreadNew (UartRxTask, &uart8TaskData, &osThreadAttrRxUart);


//    osMessageQueueAttr_t uartTxMsgQueueAttr = { 0 };
//    uartTxMsgQueueAttr.name                 = "uart8TxMsgQueue";
//    uart8TaskData.sendCmdToSlaveQueue       = osMessageQueueNew (16, sizeof (InterProcessData), &uartTxMsgQueueAttr);
//
//    osThreadAttrTxUart.name              = "os_thread_uart8_tx";
//    osThreadAttrTxUart.stack_size        = configMINIMAL_STACK_SIZE * 4;
//    osThreadAttrTxUart.priority          = (osPriority_t)osPriorityNormal;
//    uart8TaskData.uartTransmitTaskHandle = osThreadNew (UartTxTask, &uart8TaskData, &osThreadAttrTxUart);
}

void HAL_UART_RxCpltCallback (UART_HandleTypeDef* huart) {
    //	osSemaphoreRelease(uart8RxSemaphore);
}

void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef* huart, uint16_t Size) {
  if (huart->Instance == USART1) {
    HandleUartRxCallback(&uart1TaskData, huart, Size);
  } else if (huart->Instance == UART8) {
    HandleUartRxCallback(&uart8TaskData, huart, Size);
  }
}

void HAL_UART_TxCpltCallback (UART_HandleTypeDef* huart) {
    if (huart->Instance == UART8) {
    }
}

void HandleUartRxCallback (UartTaskData* uartTaskData, UART_HandleTypeDef* huart, uint16_t Size) {
    BaseType_t pxHigherPriorityTaskWoken = pdFALSE;
    osMutexAcquire (uartTaskData->rxDataBufferMutex, osWaitForever);
    memcpy (&(uartTaskData->frameData[uartTaskData->frameBytesCount]), uartTaskData->uartRxBuffer, Size);
    uartTaskData->frameBytesCount += Size;
    osMutexRelease (uartTaskData->rxDataBufferMutex);
    xTaskNotifyFromISR (uartTaskData->uartRecieveTaskHandle, Size, eSetValueWithOverwrite, &pxHigherPriorityTaskWoken);
    //	HAL_UARTEx_ReceiveToIdle_DMA(huart, uart8RxBuffer, UART8_RX_BUFF_SIZE);
    //	__HAL_DMA_DISABLE_IT(&hdma_uart8_rx, DMA_IT_HT);
    HAL_UARTEx_ReceiveToIdle_IT (uartTaskData->huart, uartTaskData->uartRxBuffer, uartTaskData->uartRxBufferLen);
    portEND_SWITCHING_ISR (pxHigherPriorityTaskWoken);
}

void UartRxTask (void* argument) {
    UartTaskData* uartTaskData             = (UartTaskData*)argument;
    SerialProtocolFrameData spFrameData    = { 0 };
    uint32_t bytesRec                      = 0;
    uint32_t crc                           = 0;
    uint16_t frameCommandRaw               = 0x0000;
    uint16_t frameBytesCount               = 0;
    uint16_t frameCrc                      = 0;
    uint16_t frameTotalLength              = 0;
    uint16_t dataToSend                    = 0;
    portBASE_TYPE crcPass                  = pdFAIL;
    portBASE_TYPE proceed                  = pdFALSE;
    portBASE_TYPE frameTimeout             = pdFAIL;
    enum SerialReceiverStates receverState = srWaitForHeader;

    uartTaskData->rxDataBufferMutex = osMutexNew (NULL);
    HAL_UARTEx_ReceiveToIdle_IT (uartTaskData->huart, uartTaskData->uartRxBuffer, uartTaskData->uartRxBufferLen);
    //	HAL_UARTEx_ReceiveToIdle_DMA(&huart8, uart8RxBuffer, 32);
    while (pdTRUE) {
        //		HAL_UART_Receive_IT(&huart8, uart8RxBuffer, 1);
        //		if(osSemaphoreAcquire(uart8RxSemaphore, pdMS_TO_TICKS(1000)) !=
        // osOK) 		if(xTaskNotifyWait(0, 0, &bytesRec, portMAX_DELAY) == pdTrue)
        frameTimeout = !(xTaskNotifyWait (0, 0, &bytesRec, pdMS_TO_TICKS (FRAME_TIMEOUT_MS)));

        osMutexAcquire (uartTaskData->rxDataBufferMutex, osWaitForever);
        frameBytesCount = uartTaskData->frameBytesCount;
        osMutexRelease (uartTaskData->rxDataBufferMutex);
        if ((frameTimeout == pdTRUE) && (frameBytesCount > 0)) {
            receverState = srFail;
            proceed      = pdTRUE;
        } else {
            if (frameTimeout == pdFALSE) {
                proceed = pdTRUE;
#if UART_TASK_LOGS
                printf ("Uart%d: RX bytes received: %ld\n", uartTaskData->uartNumber, bytesRec);
#endif
            } else {
                if (uartTaskData->huart->RxState == HAL_UART_STATE_READY) {
                    HAL_UARTEx_ReceiveToIdle_IT (uartTaskData->huart, uartTaskData->uartRxBuffer, uartTaskData->uartRxBufferLen);
                }
            }
        }

        while (proceed) {
            switch (receverState) {
            case srWaitForHeader:
                osMutexAcquire (uartTaskData->rxDataBufferMutex, osWaitForever);
                if (uartTaskData->frameData[0] == FRAME_INDICATOR) {
                    if (frameBytesCount > FRAME_ID_LENGTH) {
                        spFrameData.frameHeader.frameId =
                        CONVERT_BYTES_TO_SHORT_WORD (&(uartTaskData->frameData[FRAME_HEADER_LENGTH - FRAME_RESP_STAT_LENGTH - FRAME_DATALEN_LENGTH - FRAME_ID_LENGTH - FRAME_COMMAND_LENGTH]));
                    }
                    if (frameBytesCount > FRAME_ID_LENGTH + FRAME_COMMAND_LENGTH) {
                        frameCommandRaw = CONVERT_BYTES_TO_SHORT_WORD (&(uartTaskData->frameData[FRAME_HEADER_LENGTH - FRAME_RESP_STAT_LENGTH - FRAME_DATALEN_LENGTH - FRAME_COMMAND_LENGTH]));
                        spFrameData.frameHeader.frameCommand    = (SerialProtocolCommands)(frameCommandRaw & 0x7FFF);
                        spFrameData.frameHeader.isResponseFrame = (frameCommandRaw & 0x8000) != 0 ? pdTRUE : pdFALSE;
                    }
                    if ((frameBytesCount > FRAME_ID_LENGTH + FRAME_COMMAND_LENGTH + FRAME_RESP_STAT_LENGTH) && ((spFrameData.frameHeader.frameCommand & 0x8000) != 0)) {
                        spFrameData.frameHeader.respStatus = (SerialProtocolRespStatus)(uartTaskData->frameData[FRAME_ID_LENGTH + FRAME_COMMAND_LENGTH + FRAME_RESP_STAT_LENGTH]);
                    }
                    if (frameBytesCount >= FRAME_HEADER_LENGTH) {
                        spFrameData.frameHeader.frameDataLength = CONVERT_BYTES_TO_SHORT_WORD (&(uartTaskData->frameData[FRAME_HEADER_LENGTH - FRAME_RESP_STAT_LENGTH - FRAME_DATALEN_LENGTH]));
                        frameTotalLength                        = FRAME_HEADER_LENGTH + spFrameData.frameHeader.frameDataLength + FRAME_CRC_LENGTH;
                        receverState                            = srRecieveData;
                    } else {
                        proceed = pdFALSE;
                    }
                } else {
                    if (frameBytesCount > 0) {
                        receverState = srFail;
                    } else {
                        proceed = pdFALSE;
                    }
                }
                osMutexRelease (uartTaskData->rxDataBufferMutex);
                break;
            case srRecieveData:
                if (frameBytesCount >= frameTotalLength) {
                    receverState = srCheckCrc;
                } else {
                    proceed = pdFALSE;
                }
                break;
            case srCheckCrc:
                osMutexAcquire (uartTaskData->rxDataBufferMutex, osWaitForever);
                frameCrc = CONVERT_BYTES_TO_SHORT_WORD (&(uartTaskData->frameData[frameTotalLength - FRAME_CRC_LENGTH]));
                crc      = HAL_CRC_Calculate (&hcrc, (uint32_t*)(uartTaskData->frameData), frameTotalLength - FRAME_CRC_LENGTH);
                osMutexRelease (uartTaskData->rxDataBufferMutex);
                crcPass = frameCrc == crc;
                if (crcPass) {
#if UART_TASK_LOGS
                    printf ("Uart%d: Frame CRC PASS\n", uartTaskData->uartNumber);
#endif
                    receverState = srExecuteCmd;
                } else {
                    receverState = srFail;
                }
                break;
            case srExecuteCmd:
                if ((uartTaskData->processDataCb != NULL) || (uartTaskData->processRxDataMsgBuffer != NULL)) {
                    osMutexAcquire (uartTaskData->rxDataBufferMutex, osWaitForever);
                    memcpy (spFrameData.dataBuffer, &(uartTaskData->frameData[FRAME_HEADER_LENGTH]), spFrameData.frameHeader.frameDataLength);
                    osMutexRelease (uartTaskData->rxDataBufferMutex);
                }

                if (uartTaskData->processRxDataMsgBuffer != NULL) {
                    if(xMessageBufferSend (uartTaskData->processRxDataMsgBuffer, &spFrameData, sizeof (SerialProtocolFrameHeader) + spFrameData.frameHeader.frameDataLength, pdMS_TO_TICKS (200)) == pdFALSE)
                    {
                    	receverState = srFail;
                    	break;
                    }
                }
                if (uartTaskData->processDataCb != NULL) {
                    uartTaskData->processDataCb (uartTaskData, &spFrameData);
                }
                receverState = srFinish;
                break;
            case srFail:
                dataToSend = 0;
                if ((frameTimeout == pdTRUE) && (frameBytesCount > 2)) {
                    dataToSend = PrepareRespFrame (uartTaskData->uartTxBuffer, spFrameData.frameHeader.frameId, spFrameData.frameHeader.frameCommand, spTimeout, NULL, 0);
#if UART_TASK_LOGS
                    printf ("Uart%d: RX data receiver timeout!\n", uartTaskData->uartNumber);
#endif
                } else if (!crcPass) {
                    dataToSend = PrepareRespFrame (uartTaskData->uartTxBuffer, spFrameData.frameHeader.frameId, spFrameData.frameHeader.frameCommand, spCrcFail, NULL, 0);
#if UART_TASK_LOGS
                    printf ("Uart%d: Frame CRC FAIL\n", uartTaskData->uartNumber);
#endif
                }
                else
                {
                	dataToSend = PrepareRespFrame (uartTaskData->uartTxBuffer, spFrameData.frameHeader.frameId, spFrameData.frameHeader.frameCommand, spInternalError, NULL, 0);
                }
                if (dataToSend > 0) {
                    HAL_UART_Transmit_IT (uartTaskData->huart, uartTaskData->uartTxBuffer, dataToSend);
                }
#if UART_TASK_LOGS
                printf ("Uart%d: TX bytes sent: %d\n", dataToSend, uartTaskData->uartNumber);
#endif
                receverState = srFinish;
                break;
            case srFinish:
            default:
                osMutexAcquire (uartTaskData->rxDataBufferMutex, osWaitForever);
                uartTaskData->frameBytesCount = 0;
                osMutexRelease (uartTaskData->rxDataBufferMutex);
                spFrameData.frameHeader.frameCommand = spUnknown;
                frameTotalLength                     = 0;
                outputDataBufferPos                  = 0;
                receverState                         = srWaitForHeader;
                proceed                              = pdFALSE;
                break;
            }
        }
    }
}

void Uart8ReceivedDataProcessCallback (void* arg, SerialProtocolFrameData* spFrameData)
{
	Uart1ReceivedDataProcessCallback(arg, spFrameData);
}

void Uart1ReceivedDataProcessCallback (void* arg, SerialProtocolFrameData* spFrameData) {
    UartTaskData* uartTaskData = (UartTaskData*)arg;
    uint16_t dataToSend        = 0;
    outputDataBufferPos = 0;
    SerialProtocolRespStatus respStatus = spUnknownCommand;
    switch (spFrameData->frameHeader.frameCommand) {
    case spGetElectricalMeasurments:
    	osMutexAcquire (resMeasurementsMutex, osWaitForever);
		for(int i = 0; i < 3; i++)
		{
			WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &resMeasurements.voltageRMS[i], sizeof(float));
		}
		for(int i = 0; i < 3; i++)
		{
			WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &resMeasurements.voltagePeak[i], sizeof(float));
		}
		for(int i = 0; i < 3; i++)
		{
			WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &resMeasurements.currentRMS[i], sizeof(float));
		}
		for(int i = 0; i < 3; i++)
		{
			WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &resMeasurements.currentPeak[i], sizeof(float));
		}
		for(int i = 0; i < 3; i++)
		{
			WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &resMeasurements.power[i], sizeof(float));
		}
        osMutexRelease(resMeasurementsMutex);
        respStatus = spOK;
        break;
    case spGetSensorMeasurments:
    	osMutexAcquire (sensorsInfoMutex, osWaitForever);
    	WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &sensorsInfo.pvTemperature[0], sizeof(float));
		WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &sensorsInfo.pvTemperature[1], sizeof(float));
		WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &sensorsInfo.fanVoltage, sizeof(float));
		WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &sensorsInfo.pvEncoder, sizeof(float));
		WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &sensorsInfo.motorXStatus, sizeof(uint8_t));
		WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &sensorsInfo.motorYStatus, sizeof(uint8_t));
		WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &sensorsInfo.motorXAveCurrent, sizeof(float));
		WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &sensorsInfo.motorYAveCurrent, sizeof(float));
		WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &sensorsInfo.motorXPeakCurrent, sizeof(float));
		WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &sensorsInfo.motorYPeakCurrent, sizeof(float));
		WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &sensorsInfo.limitSwitchUp, sizeof(uint8_t));
		WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &sensorsInfo.limitSwitchDown, sizeof(uint8_t));
		WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &sensorsInfo.limitSwitchCenter, sizeof(uint8_t));
		WriteDataToBuffer(outputDataBuffer, &outputDataBufferPos, &sensorsInfo.powerSupplyFailMask, sizeof(uint8_t));
    	osMutexRelease(sensorsInfoMutex);
    	respStatus = spOK;
    	break;
    case spSetFanSpeed:
    case spSetMotorXOn:
    case spSetMotorYOn:
    	respStatus = spOK;
        break;
    case spSetDiodeOn:
    	respStatus = spOK;
    	break;
    case spSetmotorXMaxCurrent:
    case spSetmotorYMaxCurrent:
    	respStatus = spOK;
    	break;
    case spClearPeakMeasurments:
    	osMutexAcquire (resMeasurementsMutex, osWaitForever);
		for(int i = 0; i < 3; i++)
		{
			resMeasurements.voltagePeak[i] = resMeasurements.voltageRMS[i];
			resMeasurements.currentPeak[i] = resMeasurements.currentRMS[i];
		}
    	osMutexRelease(resMeasurementsMutex);
    	respStatus = spOK;
    	break;
    default:
    	respStatus = spUnknownCommand;
    	break;
    }
    dataToSend = PrepareRespFrame (uartTaskData->uartTxBuffer, spFrameData->frameHeader.frameId, spFrameData->frameHeader.frameCommand, respStatus, outputDataBuffer, outputDataBufferPos);
    if (dataToSend > 0) {
        HAL_UART_Transmit_IT (uartTaskData->huart, uartTaskData->uartTxBuffer, dataToSend);
    }
#if UART_TASK_LOGS
    printf ("Uart%d: TX bytes sent: %d\n", uartTaskData->uartNumber, dataToSend);
#endif
}


void UartTxTask (void* argument) {
    while (pdTRUE) {

            osDelay (pdMS_TO_TICKS (1000));
    }
}