/****************************************************************************/
/* 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    FALSE
#define DBG             0

#define MAX_BYTES 128
#define TOTAL_REG 128

int main(int argc, char *argv[])
{
    modbus_t *ctx;
    uint16_t reg;
    uint16_t input[TOTAL_REG];
    int i, bytes_still_queued, bytes_available;
    uint16_t regQ, regDS;
    long baud;

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

        printf("Usage: ser_read port instport [baudrate]\n");
        printf("Example:\n");
        printf("./ser_read com1 0 19200\n");

        return -1;
    }

    baud = DEFAULT_BAUD;
    if (argc == 4)
    {
        baud = atol(argv[3]);
        
        switch ( baud )
        {
            case 9600:   break;
            case 19200:  break;
            case 38400:  break;
            case 57600:  break;
            case 115200: break;
            default: 
                printf("Invalid baud: %ld\n", baud);
                printf("Supported bauds: 9600, 19200, 38400, 57600, 115200\n");
                return -1;
        }
    }

    /* init the modbus interface */
    ctx = modbus_new_rtu(argv[1], baud, 'N', 8, 2);
    
    // set addresses
    if (*argv[2] == '0')
    {
        regQ = 0x0306;
        regDS = 0x0307;
    } else if (*argv[2] == '1') {
        regQ = 0x0406;
        regDS = 0x0407;
    }
    else {
        printf("Error: invalid port value, must be 0 or 1.\n"); 
        return -1;
    }

    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;
    }

    /* read number of queued bytes */
    modbus_read_registers(ctx, regQ, 1, &reg);

    /* if nothing is queued, bail */
    if ( reg <= 1 )
    {
        /* Close the connection */
        modbus_close(ctx);
        modbus_free(ctx);

        if (DBG) printf("NO BYTES AVAILABLE TO READ, EXITING SER_READ\n");
        return 0;
    }

    if (DBG) {if (reg> 0) printf("\n\t\t\tRX_BYTES_QUEUED = %d\n", (int)reg);}
    /* start reading out all queued bytes and dumping them to sreen */
    do
    {
        /* if there are more bytes than you can read
           at once, break up the transaction*/
        if (reg > MAX_BYTES)
        {
            bytes_still_queued = TRUE;
            reg = MAX_BYTES;
        }
        else
        {
            bytes_still_queued = FALSE;
        }

        /* read out the bytes */
        modbus_read_registers(ctx, regDS, ((reg/2)) , input);
        bytes_available = (reg/2)*2;
        if (DBG) printf("\n\t\t\tBYTES_TO_READ = %d\n",bytes_available);

        /* print out serial data prompt */
        if (DBG) printf("SERIAL_DATA = [");

        /* dump register content to the sreen */
        for (i = 0; i < (reg/2); ++i)
        {
            putchar(input[i] >> 8);
            //if ((input[i] >> 8) == '\r' )
            //   putchar('\n');

            /* decrement byte count for every putchar */
            --bytes_available;

            //{
            //if ((input[i] & 0x00FF) != '\0' )
            if ( bytes_available > 0 )
            {    putchar(input[i] & 0x00FF);
                if (bytes_available <=0)
                    printf("\n\t\t\tEXTRA CHAR\n");
            }
            //else if (bytes_available <=0)
            //    printf("\n\t\t\tGot NULL!\n");
            /* decrement byte count for every putchar */
            --bytes_available;
            //}
        }

        /* read number of queued bytes */
        if ( bytes_still_queued )
        {
            /* spit out serial data close bracket and LF */
            if (DBG) printf("]\n");
            /* read the next batch of data */
            modbus_read_registers(ctx, regQ, 1, &reg);
            if (DBG) printf("RX_BYTES_QUEUED = %d\n", (int)reg);
        }

        //putchar(12);
    }
    while (bytes_still_queued); 

    /* spit out serial data close bracket and LF */
    if (DBG) printf("]\n");
    /* Close the connection */
    modbus_close(ctx);
    modbus_free(ctx);

    return 0;
}