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

#include <modbus.h>

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

void byteSwap16(char* bytes);

int main(int argc, char* argv[])
{
   int ret;
   int reg_addr;
   uint16_t dest;

   modbus_t *ctx;
   long baud;

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

      printf("Usage: rd_reg port address [baud]\n");
      printf("Default baud is %ld\n\n", DEFAULT_BAUD);
      printf("Example:\n");
      printf("./rd_reg com1 0x1FF\n");

      return -1;
   }

   /* get reg address */
   reg_addr = (int)strtol(argv[2], NULL, 16)-1;
    
   /* set the baud rate */    
   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);

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

   modbus_set_debug(ctx, MODBUS_DEBUG);
   modbus_set_error_recovery(ctx, MODBUS_ERROR_RECOVERY_LINK);
   modbus_set_slave(ctx, SLAVE_ID);

   printf("MODBUS_ERROR_RECOVERY_LINK = %d\n", MODBUS_ERROR_RECOVERY_LINK);



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

   /* read the holding register */
   if (MODBUS_DEBUG)
      printf("modbus_read_registers(SLAVE %d, ADDRESS 0x%04X): ", 
             SLAVE_ID, reg_addr);
    
   ret = modbus_read_registers(ctx, reg_addr, 1, &dest);
   //ret = modbus_read_input_registers(ctx, reg_addr, 1, &dest);


   /* swapping bytes here, not sure of there is a problem with
      libmodbus or wago, but the endianess is not being handled
      correctly.
    
      Note2: This is a problem with wago digital I/O modules, analog
      seems to work correctly*/

/*   byteSwap16((char*)&dest);*/

   /* check for errors */
   if (ret != 1) 
   {
       printf("FAILED\n");
       ret = -1;
   }
   else
   {
      printf("OK\n");
      printf("ADDRESS 0x%04X, REG 0x%04X\n", reg_addr, dest);
      ret = 0;
   }

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

   return ret;
}

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