/*
 * FreeModbus Libary: BARE Demo Application
 * Copyright (C) 2006 Christian Walter <wolti@sil.at>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * File: $Id$
 */

/* ----------------------- Modbus includes ----------------------------------*/
#include "mb.h"
#include "mbport.h"
#include "mbutils.h"

/* ----------------------- Defines ------------------------------------------*/
#define REG_INPUT_START 2
#define REG_INPUT_NREGS 8

#define REG_HOLDING_START 10
#define REG_HOLDING_NREGS 10

#define REG_COILS_START 100
#define REG_COILS_SIZE 80

#define REG_DISC_START 200
#define REG_DISC_SIZE 80
/* ----------------------- Static variables ---------------------------------*/
static USHORT usRegInputBuf[REG_INPUT_NREGS] = { 0x0401,0x0402,0x0403,0x0404,0x0405,0x0406,0x0407,0x0408 };
static USHORT usRegHoldingBuf[REG_HOLDING_NREGS] = { 0x0300,0x0301,0x0302,0x0303,0x0304,0x0305,0x0306,0x0307,0x0308,0x0309 };
// static UCHAR ucRegCoilsBuf[REG_COILS_SIZE / 8] = { 0x10,0x11 };
static UCHAR ucRegCoilsBuf[REG_COILS_SIZE / 8] = { 0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,0x18,0x19 };
static UCHAR ucRegDiscBuf[REG_DISC_SIZE / 8] = { 0x20,0x21,0x22,0x23,0x24,0x25,0x26,0x27,0x28,0x29 };


/* ----------------------- Start implementation -----------------------------*/
// int
// main( void )
// {
//     eMBErrorCode    eStatus;

//     eStatus = eMBInit( MB_RTU, 0x0A, 0, 38400, MB_PAR_EVEN );

//     /* Enable the Modbus Protocol Stack. */
//     eStatus = eMBEnable(  );

//     for( ;; )
//     {
//         ( void )eMBPoll(  );

//         /* Here we simply count the number of poll cycles. */
//         usRegInputBuf[0]++;
//     }
// }

eMBErrorCode
eMBRegInputCB(UCHAR* pucRegBuffer, USHORT usAddress, USHORT usNRegs)
{
    eMBErrorCode    eStatus = MB_ENOERR;
    int             iRegIndex;

    usAddress--;

    if((usAddress >= REG_INPUT_START)
        && (usAddress + usNRegs <= REG_INPUT_START + REG_INPUT_NREGS))
    {
        iRegIndex = (int)(usAddress - REG_INPUT_START);
        while(usNRegs > 0)
        {
            *pucRegBuffer++ =
                (UCHAR)(usRegInputBuf[iRegIndex] >> 8);
            *pucRegBuffer++ =
                (UCHAR)(usRegInputBuf[iRegIndex] & 0xFF);
            iRegIndex++;
            usNRegs--;
        }
    }
    else
    {
        eStatus = MB_ENOREG;
    }

    return eStatus;
}

eMBErrorCode
eMBRegHoldingCB(UCHAR* pucRegBuffer, USHORT usAddress, USHORT usNRegs,
    eMBRegisterMode eMode)
{
    eMBErrorCode    eStatus = MB_ENOERR;
    int             iRegIndex;

    /* it already plus one in modbus function method. */
    usAddress--;

    /* Check if we have registers mapped at this block. */
    if((usAddress >= REG_HOLDING_START)
        && (usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS))
    {
        iRegIndex = (int)(usAddress - REG_HOLDING_START);
        switch(eMode)
        {
        /* Read current values and pass to protocol stack. */
        case MB_REG_READ:
            while(usNRegs > 0)
            {
                *pucRegBuffer++ = (UCHAR)(usRegHoldingBuf[iRegIndex] >> 8);
                *pucRegBuffer++ = (UCHAR)(usRegHoldingBuf[iRegIndex] & 0xFF);
                iRegIndex++;
                usNRegs--;
            }
            break;
        /* Update current register values. */
        case MB_REG_WRITE:
            while(usNRegs > 0)
            {
                usRegHoldingBuf[iRegIndex] = *pucRegBuffer++ << 8;
                usRegHoldingBuf[iRegIndex] |= *pucRegBuffer++;
                iRegIndex++;
                usNRegs--;
            }
        }
    }
    else
    {
        eStatus = MB_ENOREG;
    }
    return eStatus;
}



/*
0x12:
100 = 0
101 = 1
102 = 0
103 = 0
104 = 1
105 = 0
106 = 0
107 = 0
*/
eMBErrorCode
eMBRegCoilsCB(UCHAR* pucRegBuffer, USHORT usAddress, USHORT usNCoils,
    eMBRegisterMode eMode)
{
    eMBErrorCode    eStatus = MB_ENOERR;
    short           iNCoils = (short)usNCoils;
    unsigned short  usBitOffset;

    usAddress--;

    if((usAddress >= REG_COILS_START) &&
        (usAddress + usNCoils <= REG_COILS_START + REG_COILS_SIZE))
    {
        usBitOffset = (unsigned short)(usAddress - REG_COILS_START);
        switch(eMode)
        {
        case MB_REG_READ:
            while(iNCoils > 0)
            {
                *pucRegBuffer++ =
                    xMBUtilGetBits(ucRegCoilsBuf, usBitOffset,
                        (UCHAR)(iNCoils > 8 ? 8 : iNCoils));
                iNCoils -= 8;
                usBitOffset += 8;
            }
            break;

        case MB_REG_WRITE:
            while(iNCoils > 0)
            {
                xMBUtilSetBits(ucRegCoilsBuf, usBitOffset,
                    (UCHAR)(iNCoils > 8 ? 8 : iNCoils), *pucRegBuffer++);
                iNCoils -= 8;
            }
            break;
        }
    }
    else
    {
        eStatus = MB_ENOREG;
    }
    return eStatus;
}


eMBErrorCode
eMBRegDiscreteCB(UCHAR* pucRegBuffer, USHORT usAddress, USHORT usNDiscrete)
{
    eMBErrorCode    eStatus = MB_ENOERR;
    short           iNDiscrete = (short)usNDiscrete;
    unsigned short  usBitOffset;

    usAddress--;

    if((usAddress >= REG_DISC_START) &&
        (usAddress + usNDiscrete <= REG_DISC_START + REG_DISC_SIZE))
    {
        usBitOffset = (unsigned short)(usAddress - REG_DISC_START);
        while(iNDiscrete > 0)
        {
            *pucRegBuffer++ =
                xMBUtilGetBits(ucRegDiscBuf, usBitOffset,
                    (UCHAR)(iNDiscrete > 8 ? 8 : iNDiscrete));
            iNDiscrete -= 8;
            usBitOffset += 8;
        }
    }
    else
    {
        eStatus = MB_ENOREG;
    }
    return eStatus;
}
