/*
 * serial_protocol.c
 *
 *  Created on: Aug 16, 2024
 *      Author: jakubski
 */
#include <stdint.h>
#include <string.h>

#include "serial_protocol.h"
#include <main.h>

extern CRC_HandleTypeDef hcrc;

void WriteShortWordToBuffer (uint8_t* buff, uint8_t* buffPos, uint16_t data) {
    uint8_t i          = 0;
    uint8_t newBuffPos = *buffPos;
    for (i = 0; i < sizeof (data); i++) {
        buff[newBuffPos++] = (uint8_t)((data >> (i * 8)) & 0xFF);
    }
    *buffPos = newBuffPos;
}

void WriteWordToBuffer (uint8_t* buff, uint8_t* buffPos, uint32_t data) {
    uint8_t i          = 0;
    uint8_t newBuffPos = *buffPos;
    for (i = 0; i < sizeof (data); i++) {
        buff[newBuffPos++] = (uint8_t)((data >> (i * 8)) & 0xFF);
    }
    *buffPos = newBuffPos;
}

void WriteFloatToBuffer (uint8_t* buff, uint8_t* buffPos, float data) {
    uint32_t* uDataPtr = (uint32_t*)&data;
    uint32_t uData     = *uDataPtr;
    uint8_t i          = 0;
    uint8_t newBuffPos = *buffPos;
    for (i = 0; i < sizeof (uData); i++) {
        buff[newBuffPos++] = (uint8_t)((uData >> (i * 8)) & 0xFF);
    }
    *buffPos = newBuffPos;
}

void WriteDataToBuffer (uint8_t* buff, uint16_t* buffPos, void* data, uint8_t dataSize) {
    uint32_t* uDataPtr = data;
    uint32_t uData     = *uDataPtr;
    uint8_t i          = 0;
    uint8_t newBuffPos = *buffPos;
    for (i = 0; i < dataSize; i++) {
        buff[newBuffPos++] = (uint8_t)((uData >> (i * 8)) & 0xFF);
    }
    *buffPos = newBuffPos;
}

void ReadFloatFromBuffer(uint8_t* buff, uint16_t* buffPos, float* data)
{
	uint32_t* word =  (uint32_t *)data;
	*word = CONVERT_BYTES_TO_WORD(&buff[*buffPos]);
	*buffPos += sizeof(float);
}

void ReadShortWordFromBufer(uint8_t* buff, uint16_t* buffPos, uint16_t* data)
{
	*data = CONVERT_BYTES_TO_SHORT_WORD(&buff[*buffPos]);
	*buffPos += sizeof(uint16_t);
}

void ReadWordFromBufer(uint8_t* buff, uint16_t* buffPos, uint32_t* data)
{
	*data = CONVERT_BYTES_TO_SHORT_WORD(&buff[*buffPos]);
	*buffPos += sizeof(uint32_t);
}

void ReadByteFromBufer(uint8_t* buff, uint16_t* buffPos, uint8_t* data)
{
	*data = CONVERT_BYTES_TO_SHORT_WORD(&buff[*buffPos]);
	*buffPos += sizeof(uint8_t);
}

uint16_t PrepareReqFrame (uint8_t* txBuffer, uint16_t frameId, SerialProtocolCommands frameCommand, uint8_t* dataBuffer, uint16_t dataLength) {
    uint16_t crc         = 0;
    uint16_t txBufferPos = 0;
    uint16_t frameCmd    = ((uint16_t)frameCommand);

    memset (txBuffer, 0x00, dataLength);
    txBuffer[txBufferPos++] = FRAME_INDICATOR;
    txBuffer[txBufferPos++] = GET_SHORT_WORD_FIRST_BYTE (frameId);
    txBuffer[txBufferPos++] = GET_SHORT_WORD_SECOND_BYTE (frameId);
    txBuffer[txBufferPos++] = GET_SHORT_WORD_FIRST_BYTE (frameCmd);
    txBuffer[txBufferPos++] = GET_SHORT_WORD_SECOND_BYTE (frameCmd);
    txBuffer[txBufferPos++] = GET_SHORT_WORD_FIRST_BYTE (dataLength);
    txBuffer[txBufferPos++] = GET_SHORT_WORD_SECOND_BYTE (dataLength);
    txBuffer[txBufferPos++] = 0x00;
    if (dataLength > 0) {
        memcpy (&txBuffer[txBufferPos], dataBuffer, dataLength);
        txBufferPos += dataLength;
    }
    crc = HAL_CRC_Calculate (&hcrc, (uint32_t*)txBuffer, txBufferPos);
    txBuffer[txBufferPos++] = GET_SHORT_WORD_FIRST_BYTE (crc);
    txBuffer[txBufferPos++] = GET_SHORT_WORD_SECOND_BYTE (crc);

    return txBufferPos;
}

uint16_t PrepareRespFrame (uint8_t* txBuffer, uint16_t frameId, SerialProtocolCommands frameCommand, SerialProtocolRespStatus respStatus, uint8_t* dataBuffer, uint16_t dataLength) {
    uint16_t crc         = 0;
    uint16_t txBufferPos = 0;
    uint16_t frameCmd = ((uint16_t)frameCommand) | 0x8000; // MSB set means response

    memset (txBuffer, 0x00, dataLength);
    txBuffer[txBufferPos++] = FRAME_INDICATOR;
    txBuffer[txBufferPos++] = GET_SHORT_WORD_FIRST_BYTE (frameId);
    txBuffer[txBufferPos++] = GET_SHORT_WORD_SECOND_BYTE (frameId);
    txBuffer[txBufferPos++] = GET_SHORT_WORD_FIRST_BYTE (frameCmd);
    txBuffer[txBufferPos++] = GET_SHORT_WORD_SECOND_BYTE (frameCmd);
    txBuffer[txBufferPos++] = GET_SHORT_WORD_FIRST_BYTE (dataLength);
    txBuffer[txBufferPos++] = GET_SHORT_WORD_SECOND_BYTE (dataLength);
    txBuffer[txBufferPos++] = (uint8_t)respStatus;
    if (dataLength > 0) {
        memcpy (&txBuffer[txBufferPos], dataBuffer, dataLength);
        txBufferPos += dataLength;
    }
    crc = HAL_CRC_Calculate (&hcrc, (uint32_t*)txBuffer, txBufferPos);
    txBuffer[txBufferPos++] = GET_SHORT_WORD_FIRST_BYTE (crc);
    txBuffer[txBufferPos++] = GET_SHORT_WORD_SECOND_BYTE (crc);

    return txBufferPos;
}