/****************************************************************************/
/* Copyright 2014 MBARI.                                                    */
/* Monterey Bay Aquarium Research Institute Proprietary Information.        */
/* All rights reserved.                                                     */
/****************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>

#include <modbus.h>

#define SLAVE_ID        0x01
#define DEFAULT_BAUD    19200L
#define MODBUS_DEBUG    TRUE

#define TX_BUFF_SZ  128
#define TX_REG_CNT  ((TX_BUFF_SZ / 2) + 1)

typedef struct 
{
    uint16_t txReq;
    char txBuff[TX_BUFF_SZ];
} TxData;

typedef union TxMsg
{
    TxData txData;
    uint16_t txReg[TX_REG_CNT];
} TxMsg;

void byteSwap16(char* bytes);
int serWriteData(modbus_t *ctx, char* data);
uint16_t regWR, regRD;

int main(int argc, char *argv[])
{
    modbus_t *ctx;
    char buff[TX_BUFF_SZ];

    if (argc < 3)
    {
        printf("SER_WRITE compiled with libmodbus version %s on %s at %s\n\n", 
                LIBMODBUS_VERSION_STRING, __DATE__, __TIME__);

        printf("Usage: ser_write port inst_port 'string to write'\n");
        printf("Example:\n");
        printf("./ser_write com1 0 'Howdy, modbus!'\n");

        return -1;
    }

    if ( strlen(argv[3]) > 100 )
    {
        fprintf(stderr, "ERROR write string should be 100 characters or less\n");
        return -1;
    }

    if (*argv[2] == '0') {
        regWR = 0x0386;
        regRD = 0x0382;
    }
    else if ( *argv[2] == '1') {
        regWR = 0x0486;
        regRD = 0x0482;
    }
    else {
        printf("ERROR: bad instrument port input, must be 0 or 1.\n");
        return -1;
    }
    /* init the modbus interface */
    ctx = modbus_new_rtu(argv[1], DEFAULT_BAUD, 'N', 8, 2);

    if (ctx == NULL)
    {
        fprintf(stderr, "Unable to allocate libmodbus context\n");
        return -1;
    }

    /* configure the modbus interface */
    modbus_set_debug(ctx, MODBUS_DEBUG);
    modbus_set_error_recovery(ctx, MODBUS_ERROR_RECOVERY_LINK);
    modbus_set_slave(ctx, SLAVE_ID);

    if (modbus_connect(ctx) == -1)
    {
        fprintf(stderr, "ERROR Connection failed: %s\n",
                modbus_strerror(errno));
        modbus_free(ctx);
        return -1;
    }

    /* copy the CLI arg to a buff and cat a CR LF on to it */
    strcpy(buff, argv[3]);
    strcat(buff, "\r");
    serWriteData(ctx, buff);

    /* Close the connection */
    modbus_close(ctx);
    modbus_free(ctx);

    return 0;
}

int serWriteData(modbus_t *ctx, char* data)
{
    int i;
    int reg_cnt;
    TxMsg txMsg;
    uint16_t reg;
    int bytes_queued, queue_available;

    /* get the number of bytes to write*/
    txMsg.txData.txReq = (uint16_t)strlen(data);

    /* make sure you're not writing more than the buffer can handle */
    if ( txMsg.txData.txReq > TX_BUFF_SZ )
        txMsg.txData.txReq = TX_BUFF_SZ;

    /* copy the bytes to the msg buffer */
    strncpy(txMsg.txData.txBuff, data, TX_BUFF_SZ);

    /* swap the bytes (assumes little endian machine) */
    for (i = 0; i < TX_REG_CNT; ++i)
    {
        /* swap the char bytes back as reading them from the union swaps them */
        if ( i > 0 )
            byteSwap16((char*)&txMsg.txReg[i]);
    }

    /* calculate the total registers needed for a write */
    reg_cnt = ((txMsg.txData.txReq + 1) / 2) + 1;

#define WR_ADDR   0x0386   
#define RD_ADDR   0x0382   

    /* write the message to the remote port and
       read back the bytes sucessfully queued */
    modbus_write_and_read_registers(ctx, regWR, reg_cnt, txMsg.txReg, 
            regRD, 1, &reg);

    bytes_queued = (0x00FF & reg);
    queue_available = (reg >> 8);

    /* display the send stats */
    printf("REG RD_ADDR      = 0x%04X\n", reg);
    printf("TX_WRITE_ACCEPT = %d\n", bytes_queued);
    printf("TX_QUEUE_FREE   = %d\n", queue_available);

    /* if you didn't queue all the bytes, let the user know */
    if ( txMsg.txData.txReq != bytes_queued )
    {
        printf("Error: serWriteData(...) failed.\n");
        printf("requested write of  %hu\n", txMsg.txData.txReq);
        printf("successfully queued %d\n", bytes_queued);

        return -1;
    }

    return 0;
}

void byteSwap16(char* bytes)
{
    char swap_buff[2];

    swap_buff[0] = bytes[0];
    swap_buff[1] = bytes[1];

    bytes[0] = swap_buff[1];
    bytes[1] = swap_buff[0];
}