#include "modbusTCP_contex.h"

int encode_U8(uint8_t *stream, int idx, uint8_t units, uint8_t* values)
{
    stream[idx++] = 0x81;
    stream[idx++] = units;
    int i;
    for (i=0; i<units; i++)
    {
        stream[idx++] = values[i];
    }
    return idx;
}

int decode_U8(uint8_t *stream, int idx, uint8_t units, uint8_t* values)
{
    if (stream[idx]==0x81 && stream[idx+1]<=units)
    {
        uint8_t count = stream[idx+1];
        idx += 2;
        int i;
        for (i=0; i<count; i++)
        {
            values[i] = stream[idx++];
        }
        if (count < units)
        {
            values[i] = 0x0;
        }
    }
    return idx;
}

int encode_U16(uint8_t *stream, int idx, uint8_t units, uint16_t* values)
{
    stream[idx++] = 0x82;
    stream[idx++] = units;
    int i;
    for (i=0; i<units; i++)
    {
        stream[idx++] = (values[i]>>8)&0xFF;
        stream[idx++] = (values[i]>>0)&0xFF;
    }
    return idx;
}

int decode_U16(uint8_t *stream, int idx, uint8_t units, uint16_t* values)
{
    if (stream[idx]==0x82 && stream[idx+1]<=units)
    {
        uint8_t count = stream[idx+1];
        idx += 2;
        int i;
        for (i=0; i<count; i++)
        {
            values[i]  = stream[idx++]<<8;
            values[i] |= stream[idx++];
        }
        if (count < units)
        {
            values[i] = 0x0;
        }
    }
    return idx;
}

int encode_U32(uint8_t *stream, int idx, uint8_t units, uint32_t* values)
{
    stream[idx++] = 0x84;
    stream[idx++] = units;
    int i;
    for (i=0; i<units; i++)
    {
        stream[idx++] = (values[i]>>24)&0xFF;
        stream[idx++] = (values[i]>>16)&0xFF;
        stream[idx++] = (values[i]>>8)&0xFF;
        stream[idx++] = (values[i]>>0)&0xFF;
    }
    return idx;
}

int decode_U32(uint8_t *stream, int idx, uint8_t units, uint32_t* values)
{
    if (stream[idx]==0x84 && stream[idx+1]<=units)
    {
        uint8_t count = stream[idx+1];
        idx += 2;
        int i;
        for (i=0; i<count; i++)
        {
            values[i]  = stream[idx++]<<24;
            values[i] |= stream[idx++]<<16;
            values[i] |= stream[idx++]<<8;
            values[i] |= stream[idx++];
        }
        if (count < units)
        {
            values[i] = 0x0;
        }
    }
    return idx;
}

int encode_U64(uint8_t *stream, int idx, uint8_t units, uint64_t* values)
{
    stream[idx++] = 0x88;
    stream[idx++] = units;
    int i;
    for (i=0; i<units; i++)
    {
        uint32_t* pvalue = (uint32_t*)&values[i];

        stream[idx++] = (pvalue[1]>>24)&0xFF;
        stream[idx++] = (pvalue[1]>>16)&0xFF;
        stream[idx++] = (pvalue[1]>>8)&0xFF;
        stream[idx++] = (pvalue[1]>>0)&0xFF;

        stream[idx++] = (pvalue[0]>>24)&0xFF;
        stream[idx++] = (pvalue[0]>>16)&0xFF;
        stream[idx++] = (pvalue[0]>>8)&0xFF;
        stream[idx++] = (pvalue[0]>>0)&0xFF;
    }
    return idx;
}

int decode_U64(uint8_t *stream, int idx, uint8_t units, uint64_t* values)
{
    if (stream[idx]==0x88 && stream[idx+1]<=units)
    {
        uint8_t count = stream[idx+1];
        idx += 2;
        int i;
        
        for (i=0; i<count; i++)
        {
            uint32_t* pvalue = (uint32_t*)&values[i];
            pvalue[1]  = stream[idx++]<<24;
            pvalue[1] |= stream[idx++]<<16;
            pvalue[1] |= stream[idx++]<<8;
            pvalue[1] |= stream[idx++];

            pvalue[0] |= stream[idx++]<<24;
            pvalue[0] |= stream[idx++]<<16;
            pvalue[0] |= stream[idx++]<<8;
            pvalue[0] |= stream[idx++];
        }
        if (count < units)
        {
            values[i] = 0x0;
        }
    }
    return idx;
}

int encode_HEADER(uint8_t* stream, uint16_t transID, uint8_t slaveID, uint8_t command, uint8_t* values, int length)
{
    int idx = 0;
    stream[idx++] = (transID>>8)&0xFF;
    stream[idx++] = (transID>>0)&0xFF;
    stream[idx++] = 0x00;
    stream[idx++] = 0x00;
    stream[idx++] = (length>>8)&0xFF;
    stream[idx++] = (length>>0)&0xFF;
    stream[idx++] = slaveID;
    stream[idx++] = command;
    
    if (values != NULL)
    {
        if (values != &stream[8])
        {
            memcpy(&stream[8], values, length);
        }
    }
    return idx+length;
}

int decode_HEADER(uint8_t* stream, uint16_t *transID, uint16_t *protoID, uint16_t *datalen, uint8_t *slaveID, uint8_t *command)
{
    int idx = 0;
    if (transID) *transID = (stream[idx+0]<<8) | stream[idx+1];
    if (protoID) *protoID = (stream[idx+2]<<8) | stream[idx+3];
    if (datalen) *datalen = (stream[idx+4]<<8) | stream[idx+5];
    if (slaveID) *slaveID = stream[idx+6];
    if (command) *command = stream[idx+7];
    return idx+8;
}

int decode_FRAME(uint8_t* stream, modbusTCP_frame_t* frame)
{
    int idx = 0;
    frame->transID = (stream[idx+0]<<8) | stream[idx+1];
    frame->protoID = (stream[idx+2]<<8) | stream[idx+3];
    frame->datalen = (stream[idx+4]<<8) | stream[idx+5];
    frame->slaveID = stream[idx+6];
    frame->command = stream[idx+7];
    return idx+8;    
}