/****************************************************************************/
/* Copyright 2014 MBARI.                                                    */
/* Monterey Bay Aquarium Research Institute Proprietary Information.        */
/* All rights reserved.                                                     */
/****************************************************************************/
/* speed test for write->read->write time of digitial in and out            */
/* modules running at 115200 baud.                                          */
/*                                                                          */
/*   Test results from 11/6/2014                                            */
/*                                                                          */
/*   best case ~ 20 ms                                                      */ 
/*   worst case ~ 38 ms                                                     */
/*                                                                          */
/****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <errno.h>

#include <modbus.h>

#define SLAVE_ID        0x01
#define DEFAULT_BAUD    115200L
#define MODBUS_DEBUG    FALSE

#define RD_REG          0x0000
#define WR_REG          0x0204
#define OUTPUT_1        0x0100
#define OUTPUT_2        0x0200

int main(int argc, char *argv[])
{
   long i, baud;

   modbus_t *ctx;

   uint16_t output, old_output;
   uint16_t input;

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

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

      return -1;
   }

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

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

   /* clear the output register */
   output = 0x0000;
   old_output = output;

   modbus_write_register(ctx, WR_REG, output);

   for (i = 0; i < 1000000L; ++i)
   {
      /* toggle BIT 1 every 100 times */
      if ((i % 100) == 0)
      {
         output ^= OUTPUT_1;
         modbus_write_register(ctx, WR_REG, output);
      }
      /* read the input register*/
      modbus_read_registers(ctx, RD_REG, 1, &input);

      /* check the input state */
      if ( input )
         output |= OUTPUT_2;
      else
         output &= ~OUTPUT_2;

      if ( output != old_output )
      {
         old_output = output;
         /* write BIT 2 to reflect the input state */
         modbus_write_register(ctx, WR_REG, output);
      }
   }

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

   return 0;
}