/****************************************************************************/
/* 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 value;

   modbus_t *ctx;
   long baud;

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

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

      return -1;
   }

   /* get reg address */
   reg_addr = (int)strtol(argv[2], NULL, 16)-1;//adjust for libmodbus address shift

   /* get reg value */
   value = (uint16_t)strtol(argv[3], NULL, 16);

   /* set the baud rate */
   baud = DEFAULT_BAUD;
   if (argc == 5)
   {
      baud = atol(argv[4]);
      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);

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

   /* write the holding register */
   if (MODBUS_DEBUG)
      printf("modbus_write_register(SLAVE %d, ADDRESS 0x%04X, VALUE 0x%04X,): ",
             SLAVE_ID, reg_addr, value);

   /* 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*)&value);*/

   ret = modbus_write_register(ctx, reg_addr, value);

/*
16 (0x10) Write Multiple registers - int modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *src);

23 (0x17) Read/Write Multiple registers - int modbus_write_and_read_registers(modbus_t *ctx, int write_addr, int write_nb, const uint16_t *src, int read_addr, int read_nb, const uint16_t *dest);
*/

   /* check for errors */
   if (ret != 1) 
   {
       printf("FAILED\n");
       ret = -1;
   }
   else
   {
      printf("OK\n");
      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];
}