/*
 * 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 "mock_tasks.h"
#include "uart_tasks.h"

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

extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart2;
extern UART_HandleTypeDef huart3;
extern UART_HandleTypeDef huart6;
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 uart2RxBuffer[UART2_RX_BUFF_SIZE]        = { 0 };
uint8_t uart2TxBuffer[UART2_TX_BUFF_SIZE]        = { 0 };
uint8_t uart2TaskFrameData[INPUT_DATA_BUFF_SIZE] = { 0 };

uint8_t uart3RxBuffer[UART3_RX_BUFF_SIZE]        = { 0 };
uint8_t uart3TxBuffer[UART3_TX_BUFF_SIZE]        = { 0 };
uint8_t uart3TaskFrameData[INPUT_DATA_BUFF_SIZE] = { 0 };

uint8_t uart6RxBuffer[UART6_RX_BUFF_SIZE]        = { 0 };
uint8_t uart6TxBuffer[UART6_TX_BUFF_SIZE]        = { 0 };
uint8_t uart6TaskFrameData[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 };

#ifdef USE_UART8_INSTEAD_UART1
uint8_t boardToUartNumberMap[SLAVES_COUNT] = { 8, 3, 6, 2 };
#else
uint8_t boardToUartNumberMap[SLAVES_COUNT] = { 1, 3, 6, 2 };
#endif

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

#ifdef USE_UART8_INSTEAD_UART1
UartTaskData* uartTasks[] = { &uart8TaskData, &uart3TaskData, &uart6TaskData, &uart2TaskData, NULL, };
#else
UartTaskData* uartTasks[] = { &uart1TaskData, &uart3TaskData, &uart6TaskData, &uart2TaskData, NULL, };
#endif

uint8_t outputDataBuffer[OUTPUT_DATA_BUFF_SIZE];
uint16_t outputDataBufferPos = 0;

RESMeasurements resMeasurements[SLAVES_COUNT] = { 0 };
SesnorsInfo sensorsInfo[SLAVES_COUNT]         = { 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;

    uart2TaskData.uartRxBuffer    = uart2RxBuffer;
    uart2TaskData.uartRxBufferLen = UART2_RX_BUFF_SIZE;
    uart2TaskData.uartTxBuffer    = uart2TxBuffer;
    uart2TaskData.uartRxBufferLen = UART2_TX_BUFF_SIZE;
    uart2TaskData.frameData       = uart2TaskFrameData;
    uart2TaskData.frameDataLen    = UART2_RX_BUFF_SIZE;
    uart2TaskData.huart           = &huart2;
    uart2TaskData.uartNumber      = 2;

    uart3TaskData.uartRxBuffer    = uart3RxBuffer;
    uart3TaskData.uartRxBufferLen = UART3_RX_BUFF_SIZE;
    uart3TaskData.uartTxBuffer    = uart3TxBuffer;
    uart3TaskData.uartRxBufferLen = UART3_TX_BUFF_SIZE;
    uart3TaskData.frameData       = uart3TaskFrameData;
    uart3TaskData.frameDataLen    = UART3_RX_BUFF_SIZE;
    uart3TaskData.huart           = &huart3;
    uart3TaskData.uartNumber      = 3;

    uart6TaskData.uartRxBuffer    = uart6RxBuffer;
    uart6TaskData.uartRxBufferLen = UART6_RX_BUFF_SIZE;
    uart6TaskData.uartTxBuffer    = uart6TxBuffer;
    uart6TaskData.uartRxBufferLen = UART6_TX_BUFF_SIZE;
    uart6TaskData.frameData       = uart6TaskFrameData;
    uart6TaskData.frameDataLen    = UART6_RX_BUFF_SIZE;
    uart6TaskData.huart           = &huart6;
    uart6TaskData.uartNumber      = 6;

    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;

    UartTaskCreate (&uart1TaskData);
    UartTaskCreate (&uart2TaskData);
    UartTaskCreate (&uart3TaskData);
    UartTaskCreate (&uart6TaskData);
    UartTaskCreate (&uart8TaskData);
}

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

    uartTaskData->processRxDataMsgBuffer = xMessageBufferCreate (INPUT_DATA_BUFF_SIZE);
    uartTaskData->processDataCb          = NULL;

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


    uartTaskData->uartRecieveTaskHandle = osThreadNew (UartRxTask, uartTaskData, &osThreadAttrRxUart);


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

    //    osThreadAttrTxUart.name              = "os_thread_uart1_tx";
    osThreadAttrTxUart.stack_size        = configMINIMAL_STACK_SIZE * 4;
    osThreadAttrTxUart.priority          = (osPriority_t)osPriorityNormal;
    uartTaskData->uartTransmitTaskHandle = osThreadNew (UartTxTask, uartTaskData, &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 == USART2) {
        HandleUartRxCallback (&uart2TaskData, huart, Size);
    } else if (huart->Instance == USART3) {
        HandleUartRxCallback (&uart3TaskData, huart, Size);
    } else if (huart->Instance == USART6) {
        HandleUartRxCallback (&uart6TaskData, 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);
    while (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;
                printf ("Uart%d: RX bytes received: %ld\n", uartTaskData->uartNumber, bytesRec);
            } 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) {
                    printf ("Uart%d: Frame CRC PASS\n", uartTaskData->uartNumber);
                    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);
                    printf ("Uart%d: RX data receiver timeout!\n", uartTaskData->uartNumber);
                } else if (!crcPass) {
                    dataToSend = PrepareRespFrame (uartTaskData->uartTxBuffer, spFrameData.frameHeader.frameId, spFrameData.frameHeader.frameCommand, spCrcFail, NULL, 0);
                    printf ("Uart%d: Frame CRC FAIL\n", uartTaskData->uartNumber);
                }
                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);
                }
                printf ("Uart%d: TX bytes sent: %d\n", dataToSend, uartTaskData->uartNumber);
                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 ReadMeasSetFromBuffer(uint8_t* buff, uint16_t* buffPos, float* dataSet)
{
	for(uint8_t i = 0; i < 3; i++)
	{
		ReadFloatFromBuffer(buff, buffPos, &dataSet[i]);
	}
}

void UartTxTask (void* argument) {
    UartTaskData* const uartTaskData  = (UartTaskData*)argument;
    InterProcessData data             = { 0 };
    SerialProtocolFrameData frameData = { 0 };
    size_t bytesInMsg;
    uint16_t frameId                    = 0;
    uint32_t rndVal                     = 0;
    uint16_t bytesToSend                = 0;
    SerialProtocolCommands frameCommand = spUnknown;
    uint16_t inputDataBufferPos         = 0;
    uint8_t boardNumber                 = 0;

    while (pdTRUE) {
        if (uartTaskData->sendCmdToSlaveQueue != NULL) {
            osMessageQueueGet (uartTaskData->sendCmdToSlaveQueue, &data, 0, osWaitForever);
            HAL_RNG_GenerateRandomNumber (&hrng, &rndVal);
            frameId             = (uint16_t)(rndVal & 0xFFFF);
            frameCommand        = data.spCommand;
            outputDataBufferPos = 0;
            memset (outputDataBuffer, 0x00, OUTPUT_DATA_BUFF_SIZE);
            switch (frameCommand) {
            case spSetFanSpeed:
            case spSetMotorXOn:
            case spSetMotorYOn:
                WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &data.values.integerValues.value[0], sizeof (uint32_t));
                WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &data.values.integerValues.value[1], sizeof (uint32_t));
                break;
            case spSetDiodeOn: WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &data.values.integerValues.value[0], sizeof (uint32_t)); break;
            case spSetmotorXMaxCurrent:
            case spSetmotorYMaxCurrent: WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &data.values.flaotValues.value[0], sizeof (float)); break;
            case spGetElectricalMeasurments:
            case spGetSensorMeasurments: break;
            case spClearPeakMeasurments: break;
            case spSetEncoderXValue: WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &data.values.flaotValues.value[0], sizeof (float)); break;
            case spSetEncoderYValue: WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &data.values.flaotValues.value[0], sizeof (float)); break;
            case spSetVoltageMeasGains:
            case spSetVoltageMeasOffsets:
            case spSetCurrentMeasGains:
            case spSetCurrentMeasOffsets:
                WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &data.values.flaotValues.value[0], sizeof (float));
                WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &data.values.flaotValues.value[1], sizeof (float));
                WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &data.values.flaotValues.value[2], sizeof (float));
                break;
            case spResetSystem: break;
            case spSetPositonX:
            case spSetPositonY:
                WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &data.values.flaotValues.value[0], sizeof (float));
                break;
                break;
            default: continue; break;
            }

            bytesToSend = PrepareReqFrame (uartTaskData->uartTxBuffer, frameId, frameCommand, outputDataBuffer, outputDataBufferPos);
            HAL_UART_Transmit_IT (uartTaskData->huart, uartTaskData->uartTxBuffer, bytesToSend);
            bytesInMsg = xMessageBufferReceive (uartTaskData->processRxDataMsgBuffer, &frameData, INPUT_DATA_BUFF_SIZE, pdMS_TO_TICKS (1000));
            for (boardNumber = 0; boardNumber < SLAVES_COUNT; boardNumber++) {
                if (boardToUartNumberMap[boardNumber] == uartTaskData->uartNumber) {
                    break;
                }
            }
            if (bytesInMsg == 0) {
                if (frameCommand == spGetElectricalMeasurments) {
                    osMutexAcquire (resMeasurementsMutex, osWaitForever);
                    slaveLastSeen[boardNumber]++;
                    osMutexRelease (resMeasurementsMutex);
                }
                printf ("Uart%d: Response timeout for frameId 0x%x\n", uartTaskData->uartNumber, frameId);
            } else {
                if ((frameId == frameData.frameHeader.frameId) && (frameData.frameHeader.respStatus == spOK)) {
                    printf ("Uart%d: Response for frameId 0x%x OK\n", uartTaskData->uartNumber, frameId);
                    slaveLastSeen[boardNumber] = 0;
                    switch (frameData.frameHeader.frameCommand) {
                    case spGetElectricalMeasurments:
                        osMutexAcquire (resMeasurementsMutex, osWaitForever);
                        RESMeasurements* resMeas = &resMeasurements[boardNumber];
                        inputDataBufferPos       = 0;
                        ReadMeasSetFromBuffer (frameData.dataBuffer, &inputDataBufferPos, resMeas->voltageRMS);
                        ReadMeasSetFromBuffer (frameData.dataBuffer, &inputDataBufferPos, resMeas->voltagePeak);
                        ReadMeasSetFromBuffer (frameData.dataBuffer, &inputDataBufferPos, resMeas->currentRMS);
                        ReadMeasSetFromBuffer (frameData.dataBuffer, &inputDataBufferPos, resMeas->currentPeak);
                        ReadMeasSetFromBuffer (frameData.dataBuffer, &inputDataBufferPos, resMeas->power);
                        osMutexRelease (resMeasurementsMutex);
                        break;
                    case spGetSensorMeasurments:
                        osMutexAcquire (sensorsInfoMutex, osWaitForever);
                        inputDataBufferPos   = 0;
                        SesnorsInfo* sensors = &sensorsInfo[boardNumber];
                        ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->pvTemperature[0]);
                        ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->pvTemperature[1]);
                        ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->fanVoltage);
                        ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->pvEncoderX);
                        ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->pvEncoderY);
                        ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->motorXStatus);
                        ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->motorYStatus);
                        ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->motorXAveCurrent);
                        ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->motorYAveCurrent);
                        ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->motorXPeakCurrent);
                        ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->motorYPeakCurrent);
                        ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->limitXSwitchUp);
                        ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->limitXSwitchDown);
                        ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->limitXSwitchCenter);
                        ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->limitYSwitchUp);
                        ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->limitYSwitchDown);
                        ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->limitYSwitchCenter);
                        ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->powerSupplyFailMask);
                        ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->currentXPosition);
                        ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->currentYPosition);
                        ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->positionXWeak);
                        ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->positionYWeak);
                        osMutexRelease (sensorsInfoMutex);
                        break;
                    default: break;
                    }
                }
            }
        } else {
            osDelay (pdMS_TO_TICKS (1000));
        }
    }
}

void MeasurmentsReqSchedulerTaskInit (void) {
    osThreadAttr_t osThreadAttrMeasurmentsReqSchedulerTask = { 0 };

    osThreadAttrMeasurmentsReqSchedulerTask.name       = "os_thread_XXX";
    osThreadAttrMeasurmentsReqSchedulerTask.stack_size = configMINIMAL_STACK_SIZE * 2;
    osThreadAttrMeasurmentsReqSchedulerTask.priority   = (osPriority_t)osPriorityNormal;

    osThreadNew (MeasurmentsReqSchedulerTask, uartTasks, &osThreadAttrMeasurmentsReqSchedulerTask);
}

void MeasurmentsReqSchedulerTask (void* argument) {
    while (pdTRUE) {
        __uintptr_t* ptr = (__uintptr_t*)argument;
        while (*ptr != 0) {
            UartTaskData* uartTask = (UartTaskData*)*ptr;
            if (uartTask->sendCmdToSlaveQueue != NULL) {
                InterProcessData data = { 0 };
                uint8_t boardNumber = 0;
            	for(boardNumber = 0; boardNumber < SLAVES_COUNT; boardNumber++)
            	{
            		if(boardToUartNumberMap[boardNumber] == uartTask->uartNumber)
            		{
            			break;
            		}
            	}
                data.spCommand        = spGetElectricalMeasurments;
                osMessageQueuePut (uartTask->sendCmdToSlaveQueue, &data, 0, (TickType_t)100);
            	osMutexAcquire (resMeasurementsMutex, osWaitForever);
            	if(slaveLastSeen[boardNumber] == 0)
            	{
            		data.spCommand = spGetSensorMeasurments;
            		osMessageQueuePut (uartTask->sendCmdToSlaveQueue, &data, 0, (TickType_t)100);
            	}
            	osMutexRelease(resMeasurementsMutex);
            }
            ptr++;
        }
        osDelay (pdMS_TO_TICKS (MEASURMENTS_SCHEDULER_INTERVAL_MS));
    }
}