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

static unsigned short calculate_crc16(unsigned char *pData, unsigned short iLen)
{
    unsigned short iCrc = 0xFFFF;
    unsigned short j = 0, i = 0;
    for (j = 0; j < iLen; j++)
    {
        iCrc = iCrc ^ *pData++;
        for (i = 0; i < 8; i++)
        {
            if ((iCrc & 0x0001) > 0)
            {
                iCrc = iCrc >> 1;
                iCrc = iCrc ^ 0xa001;
            }
            else
            {
                iCrc = iCrc >> 1;
            }
        }
    }
    return iCrc;
}

int _modbus_head_init(unsigned char *buf, unsigned char cmd)
{
    modbus_head_t *head = (modbus_head_t*)buf;
    int len;
    head->slave = 0xff;
    head->cmd = cmd;
    len = 2;
    return len;
}

int _modbus_addr_set(unsigned char *buf, unsigned short addr)
{
    buf[0] = addr >> 8;
    buf[1] = addr & 0xff;
    return 2;
}

int _modbus_size_set(unsigned char *buf, unsigned short size)
{
    buf[0] = size >> 8;
    buf[1] = size & 0xff;
    return 2;
}

int _modbus_write_head(unsigned char *pkt, unsigned char cmd, unsigned short regAddr, unsigned short regCnt)
{
    if (pkt == NULL)
    {
        return -1;
    }
    unsigned char *ppkt = pkt;
    unsigned char len;
    int bytecnt;
    
    *ppkt++ = 0xff;
    *ppkt++ = cmd;
    *ppkt++ = (regAddr >> 8) & 0xff;
    *ppkt++ = (regAddr) & 0xff;
    *ppkt++ = (regCnt >> 8) & 0xff;
    *ppkt++ = (regCnt) & 0xff;
    len = 6;
    if (cmd == MB_FUNC_MULIT_WR)
    {
        bytecnt = regCnt * 2;
        *ppkt++ = bytecnt;
        len++;
    }
    return len;
}



int _modbus_write_body()
{
    return 0;
}

void _modbus_checksum(unsigned char *buf, int len)
{
    unsigned short crc = calculate_crc16(buf,len);
    buf[len] = crc & 0xff;
    buf[len+1] = (crc>>8) & 0xff;
}

int _modbus_verify_crc(unsigned char *buf, int len)
{
    unsigned short crc = calculate_crc16(buf,len - 2);
    unsigned short recv_crc = buf[len - 2] | (buf[len - 1] << 8);
    //printf("crc=0x%x recv_crc=0x%x\n", crc, recv_crc);
    if (crc == recv_crc)
    {
        return 1;
    }
    return 0;
}

#if 0
int _modbus_packet(unsigned char *pkt, unsigned char cmd, unsigned short addr, unsigned short size)
{
    unsigned char *pdat;
    int len = 0, ret = 0;
    ret = _modbus_head_init(pkt, cmd);
    pdat = pkt + ret;
    len += ret;
    
    ret = _modbus_addr_set(pdat, addr);
    pdat += ret;
    len += ret;
    
    ret = _modbus_size_set(pdat, size);
    pdat += ret;
    len += ret;
    
    ret = _modbus_checksum(pkt, len);
    len += ret;
    return len;
}
#else

void _modbus_add_float(unsigned char *hex,float val)
{
    unsigned int *pint = (unsigned int *)&val;
    hex[0] = (*pint>>8)&0xff;
    hex[1] = (*pint>>0)&0xff;
    hex[2] = (*pint>>24)&0xff;
    hex[3] = (*pint>>16)&0xff;
    //printf("[%s:%d]%f %x %x %x %x %x\n",__FUNCTION__, __LINE__, val,*pint,hex[0],hex[1],hex[2],hex[3]);
}


int _modbus_packet(unsigned char *pkt, unsigned char cmd, unsigned short regAddr, unsigned short regCnt, unsigned char data[])
{
    if (pkt == NULL)
    {
        return -1;
    }
    unsigned char *ppkt = pkt;
    unsigned char len;
    unsigned short crc;
    int bytecnt;
    
    *ppkt++ = 0xff;
    *ppkt++ = cmd;
    *ppkt++ = (regAddr >> 8) & 0xff;
    *ppkt++ = (regAddr) & 0xff;
    *ppkt++ = (regCnt >> 8) & 0xff;
    *ppkt++ = (regCnt) & 0xff;
    len = 6;
    if (cmd == MB_FUNC_MULIT_WR)
    {
        bytecnt = regCnt * 2;
        *ppkt++ = bytecnt;
        len++;
        memcpy(ppkt, data, bytecnt);
        len += bytecnt;
    }
    
    crc = calculate_crc16(pkt, len);
    *ppkt++ = (crc >> 8) & 0xff;
    len++;
    *ppkt++ = (crc) & 0xff;
    len++;
    return len;
}   
#endif