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

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

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

#include "adc_buffers.h"
#include "interprocess_data.h"
#include "meas_tasks.h"
#include "measurements.h"
#include "mock_tasks.h"
#include "peripherial.h"
#include "uart_tasks.h"
#include "position_task.h"

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

extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart8;
extern DMA_HandleTypeDef hdma_uart8_rx;
extern CRC_HandleTypeDef hcrc;
extern DAC_HandleTypeDef hdac1;
extern osTimerId_t debugLedTimerHandle;
extern osTimerId_t fanTimerHandle;
extern osTimerId_t motorXTimerHandle;
extern osTimerId_t motorYTimerHandle;

extern TIM_HandleTypeDef htim1;
extern TIM_HandleTypeDef htim3;
extern TIM_OC_InitTypeDef fanTimerConfigOC;
extern TIM_OC_InitTypeDef motorXYTimerConfigOC;
extern COMP_HandleTypeDef hcomp1;

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 };

UartTaskData uart1TaskData = { 0 }; // Board 1
UartTaskData uart8TaskData = { 0 }; // Debug

uint8_t outputDataBuffer[OUTPUT_DATA_BUFF_SIZE];
uint16_t outputDataBufferPos = 0;

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;

#ifdef USE_UART8_INSTEAD_UART1
    UartTaskCreate (&uart8TaskData);
#else
    UartTaskCreate (&uart1TaskData);
#endif
}

void UartTaskCreate (UartTaskData* uartTaskData) {
    osThreadAttr_t osThreadAttrRxUart   = { 0 };
    osThreadAttrRxUart.stack_size       = configMINIMAL_STACK_SIZE * 2;
    osThreadAttrRxUart.priority         = (osPriority_t)osPriorityHigh;
    uartTaskData->uartRecieveTaskHandle = osThreadNew (UartRxTask, uartTaskData, &osThreadAttrRxUart);
}

void HAL_UART_RxCpltCallback (UART_HandleTypeDef* huart) {
}

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_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;
#ifdef SERIAL_PROTOCOL_DBG
                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) {
#ifdef SERIAL_PROTOCOL_DBG
                    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);
#ifdef SERIAL_PROTOCOL_DBG
                    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);
#ifdef SERIAL_PROTOCOL_DBG
                    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);
                }
#ifdef SERIAL_PROTOCOL_DBG
                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;
    uint16_t inputDataBufferPos         = 0;
    SerialProtocolRespStatus respStatus = spUnknownCommand;
    switch (spFrameData->frameHeader.frameCommand) {
    case spGetElectricalMeasurments:
        if (osMutexAcquire (resMeasurementsMutex, osWaitForever) == osOK) {
            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;
        } else {
            respStatus = spInternalError;
        }
        break;
    case spGetSensorMeasurments:
        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
            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.pvEncoderXraw, sizeof (int32_t));
            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.pvEncoderYraw, sizeof (int32_t));
            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.limitXSwitchUp, sizeof (uint8_t));
            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.limitXSwitchDown, sizeof (uint8_t));
            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.limitXSwitchCenter, sizeof (uint8_t));
            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.limitYSwitchUp, sizeof (uint8_t));
            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.limitYSwitchDown, sizeof (uint8_t));
            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.limitYSwitchCenter, sizeof (uint8_t));
            uint8_t comparatorOutput        = HAL_COMP_GetOutputLevel (&hcomp1) == COMP_OUTPUT_LEVEL_HIGH ? 1 : 0;
            sensorsInfo.powerSupplyFailMask = ~((comparatorOutput << 1) | HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_3)) & 0x01;
            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.powerSupplyFailMask, sizeof (uint8_t));
            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.currentXPosition, sizeof (float));
            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.currentYPosition, sizeof (float));
            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.positionXOffset, sizeof (float));
            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.positionYOffset, sizeof (float));
            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.positionXWeak, sizeof (uint8_t));
            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.positionYWeak, sizeof (uint8_t));
            osMutexRelease (sensorsInfoMutex);
            respStatus = spOK;
        } else {
            respStatus = spInternalError;
        }
        break;
    case spSetFanSpeed:
        osTimerStop (fanTimerHandle);
        int32_t fanTimerPeriod = 0;
        uint32_t pulse         = 0;
        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, &pulse);
        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&fanTimerPeriod);
        fanTimerConfigOC.Pulse = pulse * 10;
        if (HAL_TIM_PWM_ConfigChannel (&htim1, &fanTimerConfigOC, TIM_CHANNEL_2) != HAL_OK) {
            Error_Handler ();
        }
        if (fanTimerPeriod > 0) {
            osTimerStart (fanTimerHandle, fanTimerPeriod * 1000);
            HAL_TIM_PWM_Start (&htim1, TIM_CHANNEL_2);
        } else if (fanTimerPeriod == 0) {
            osTimerStop (fanTimerHandle);
            HAL_TIM_PWM_Stop (&htim1, TIM_CHANNEL_2);
        } else if (fanTimerPeriod == -1) {
            osTimerStop (fanTimerHandle);
            HAL_TIM_PWM_Start (&htim1, TIM_CHANNEL_2);
        }
        respStatus = spOK;
        break;
    case spSetMotorXOn:
        int32_t motorXPWMPulse    = 0;
        int32_t motorXTimerPeriod = 0;
        uint32_t motorXStatus     = 0;
        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&motorXPWMPulse);
        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&motorXTimerPeriod);
        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
            motorXStatus =
            MotorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_1, TIM_CHANNEL_2, motorXTimerHandle, motorXPWMPulse, motorXTimerPeriod, sensorsInfo.limitXSwitchUp, sensorsInfo.limitXSwitchDown);
            sensorsInfo.motorXStatus = motorXStatus;
            if (motorXStatus == 1) {
                sensorsInfo.motorXPeakCurrent = 0.0;
            }
            osMutexRelease (sensorsInfoMutex);
            respStatus = spOK;
        } else {
            respStatus = spInternalError;
        }
        break;
    case spSetMotorYOn:
        int32_t motorYPWMPulse    = 0;
        int32_t motorYTimerPeriod = 0;
        uint32_t motorYStatus     = 0;
        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&motorYPWMPulse);
        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&motorYTimerPeriod);
        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
            motorYStatus =
            MotorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_3, TIM_CHANNEL_4, motorYTimerHandle, motorYPWMPulse, motorYTimerPeriod, sensorsInfo.limitYSwitchUp, sensorsInfo.limitYSwitchDown);
            sensorsInfo.motorYStatus = motorYStatus;
            if (motorYStatus == 1) {
                sensorsInfo.motorYPeakCurrent = 0.0;
            }
            osMutexRelease (sensorsInfoMutex);
            respStatus = spOK;
        } else {
            respStatus = spInternalError;
        }
        break;
    case spSetDiodeOn:
        osTimerStop (debugLedTimerHandle);
        int32_t dbgLedTimerPeriod = 0;
        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&dbgLedTimerPeriod);
        if (dbgLedTimerPeriod > 0) {
            osTimerStart (debugLedTimerHandle, dbgLedTimerPeriod * 1000);
            DbgLEDOn (DBG_LED1);
        } else if (dbgLedTimerPeriod == 0) {
            osTimerStop (debugLedTimerHandle);
            DbgLEDOff (DBG_LED1);
        } else if (dbgLedTimerPeriod == -1) {
            osTimerStop (debugLedTimerHandle);
            DbgLEDOn (DBG_LED1);
        }
        respStatus = spOK;
        break;
    case spSetmotorXMaxCurrent:
        float motorXMaxCurrent = 0;
        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&motorXMaxCurrent);
        uint32_t dacDataCh1 = (uint32_t)(4095 * motorXMaxCurrent / (EXT_VREF_mV * 0.001));
        if (dacDataCh1 > 4095) {
            dacDataCh1 = 4095;
        }
        HAL_DAC_SetValue (&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, dacDataCh1);
        HAL_DAC_Start (&hdac1, DAC_CHANNEL_1);
        respStatus = spOK;
        break;
    case spSetmotorYMaxCurrent:
        float motorYMaxCurrent = 0;
        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&motorYMaxCurrent);
        uint32_t dacDataCh2 = (uint32_t)(4095 * motorYMaxCurrent / (EXT_VREF_mV * 0.001));
        if (dacDataCh2 > 4095) {
            dacDataCh2 = 4095;
        }
        HAL_DAC_SetValue (&hdac1, DAC_CHANNEL_2, DAC_ALIGN_12B_R, dacDataCh2);
        HAL_DAC_Start (&hdac1, DAC_CHANNEL_2);
        respStatus = spOK;
        break;
    case spClearPeakMeasurments:
        if (osMutexAcquire (resMeasurementsMutex, osWaitForever) == osOK) {
            for (int i = 0; i < 3; i++) {
                resMeasurements.voltagePeak[i] = resMeasurements.voltageRMS[i];
                resMeasurements.currentPeak[i] = resMeasurements.currentRMS[i];
            }
            osMutexRelease (resMeasurementsMutex);
            respStatus = spOK;
        } else {
            respStatus = spInternalError;
        }
        break;
    case spSetEncoderXValue:
        float enocoderXValue = 0;
        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&enocoderXValue);
        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
            sensorsInfo.positionXWeak   = 1;
            sensorsInfo.positionXOffset = enocoderXValue - sensorsInfo.currentXPosition;
            osMutexRelease (sensorsInfoMutex);
            respStatus = spOK;
        } else {
            respStatus = spInternalError;
        }
        break;
    case spSetEncoderYValue:
        float enocoderYValue = 0;
        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&enocoderYValue);
        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
            sensorsInfo.positionYWeak   = 1;
            sensorsInfo.positionYOffset = enocoderYValue - sensorsInfo.currentYPosition;
            osMutexRelease (sensorsInfoMutex);
            respStatus = spOK;
        } else {
            respStatus = spInternalError;
        }
        break;
    case spSetVoltageMeasGains:
        if (osMutexAcquire (resMeasurementsMutex, osWaitForever) == osOK) {
            for (uint8_t i = 0; i < 3; i++) {
                ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&U_MeasCorrectionData[i].gain);
            }
            osMutexRelease (resMeasurementsMutex);
            respStatus = spOK;
        } else {
            respStatus = spInternalError;
        }
        break;
    case spSetVoltageMeasOffsets:
        if (osMutexAcquire (resMeasurementsMutex, osWaitForever) == osOK) {
            for (uint8_t i = 0; i < 3; i++) {
                ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&U_MeasCorrectionData[i].offset);
            }
            osMutexRelease (resMeasurementsMutex);
            respStatus = spOK;
        } else {
            respStatus = spInternalError;
        }
        break;
    case spSetCurrentMeasGains:
        if (osMutexAcquire (resMeasurementsMutex, osWaitForever) == osOK) {
            for (uint8_t i = 0; i < 3; i++) {
                ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&I_MeasCorrectionData[i].gain);
            }
            osMutexRelease (resMeasurementsMutex);
            respStatus = spOK;
        } else {
            respStatus = spInternalError;
        }
        break;
    case spSetCurrentMeasOffsets:
        if (osMutexAcquire (resMeasurementsMutex, osWaitForever) == osOK) {
            for (uint8_t i = 0; i < 3; i++) {
                ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&I_MeasCorrectionData[i].offset);
            }
            osMutexRelease (resMeasurementsMutex);
            respStatus = spOK;
        } else {
            respStatus = spInternalError;
        }
        break;
    case spResetSystem:
    	__disable_irq();
    	NVIC_SystemReset();
    	break;
    case spSetPositonX:
    	PositionControlTaskData posXData  __attribute__ ((aligned (32))) = { 0 };
        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
            osMutexRelease (sensorsInfoMutex);
        }
    	if (positionXControlTaskInitArg.positionSettingQueue != NULL)
    	{
    		ReadFloatFromBuffer(spFrameData->dataBuffer, &inputDataBufferPos, &posXData.positionSettingValue);
			osMessageQueuePut(positionXControlTaskInitArg.positionSettingQueue, &posXData, 0, 0);
    	}
    	break;
    case spSetPositonY:
        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
            osMutexRelease (sensorsInfoMutex);
        }
    	PositionControlTaskData posYData __attribute__ ((aligned (32))) = { 0 };
    	if (positionYControlTaskInitArg.positionSettingQueue != NULL)
    	{
    		ReadFloatFromBuffer(spFrameData->dataBuffer, &inputDataBufferPos, &posYData.positionSettingValue);
			osMessageQueuePut(positionYControlTaskInitArg.positionSettingQueue, &posYData, 0, 0);
    	}
    	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);
    }
#ifdef SERIAL_PROTOCOL_DBG
    printf ("Uart%d: TX bytes sent: %d\n", uartTaskData->uartNumber, dataToSend);
#endif
}


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

        osDelay (pdMS_TO_TICKS (1000));
    }
}