#include <unistd.h>
#include <stdexcept>
#include <lcm/lcm-cpp.hpp>
#include "lcmMessages/DoubleItem.hpp"
#include "lcmMessages/DoubleItems.hpp"

class PublisherTest {


public:
  PublisherTest(int nChan, int nItems) {
    // Create array of DoubleItem objects
    _itemArray = new lcmMessages::DoubleItem*[nItems];
    for (int i = 0; i < nItems; i++) {
      _itemArray[i] = new lcmMessages::DoubleItem();
    }
  };

  void publish(char *channelNamePrefix, lcm::LCM *lcm, int nChan, int nItems);


protected:

  lcmMessages::DoubleItem **_itemArray;
};


void PublisherTest::publish(char *channelNamePrefix, lcm::LCM *lcm, 
			    int nChan, int nItems) {

  char channelName[256];
  char itemName[256];

  lcmMessages::DoubleItems msg;

  // Assign values to each DoubleItem
  for (int i = 0; i < nItems; i++) {
    sprintf(itemName, "item-%0d", i);
    _itemArray[i]->name.assign(itemName);
    _itemArray[i]->val = 9999.;
    _itemArray[i]->unit.assign("no-units");
  }

  // Add items to each channel
  printf("publish to %d channels\n", nChan);
  for (int i = 0; i < nChan; i++) {

    sprintf(channelName, "%s-%0d", channelNamePrefix, i);
    msg.nItems = nItems;
    msg.epochUsec = 0;

    for (int j = 0; j < nItems; j++) {
      msg.doubleItem.push_back(*_itemArray[j]);
    }


    // Publish message on this channel
    printf("publish on channel %s\n", channelName);
    printf("msg.nItems: %d\n", msg.nItems);
    if (lcm->publish(channelName, &msg) != 0) {
      printf("ERROR: lcm publish failed\n");
    }
    else {
      printf("lcm publish succeeded\n");
    }
  }
}

#define LCM_DEFAULT_URL "LCM_DEFAULT_URL"

int main(int argc, char **argv) {

  if (argc != 6) {
    fprintf(stderr, 
	    "usage: %s nLoops sleepMicrosec nChan nItems chanNamePrefix\n", 
	    argv[0]);
    fprintf(stderr, "nLoops=0 means loop forever\n");
    return 1;
  }

  int nLoops = 100;
  int sleepUsec = 1000000;
  int nChan = 10;
  int nItems = 10;


  // Process command line arguments
   bool error = false;
  if (sscanf(argv[1], "%d", &nLoops) != 1) {
    fprintf(stderr, "Invalid nLoops: %s\n", argv[1]);
    error = true;
  }

  if (sscanf(argv[2], "%d", &sleepUsec) != 1) {
    fprintf(stderr, "Invalid sleepUsec: %s\n", argv[2]);
    error = true;
  }

  if (sscanf(argv[3], "%d", &nChan) != 1) {
    fprintf(stderr, "Invalid nChan: %s\n", argv[3]);
    error = true;
  }

  if (sscanf(argv[4], "%d", &nItems) != 1) {
    fprintf(stderr, "Invalid nItems: %s\n", argv[4]);
    error = true;
  }

  char *channelNamePrefix = argv[5];

  if (error) {
    return -1;
  }

  printf("Checking environment variable %s\n", LCM_DEFAULT_URL);
  const char *lcmURL = getenv(LCM_DEFAULT_URL);
  if (lcmURL == NULL) {
    lcmURL = "udpm://239.255.76.67:7667?ttl=0";
    printf("Environment variable %s not set\n", LCM_DEFAULT_URL);
  }

  printf("Using LCM URL %s\n", lcmURL);

  lcm::LCM lcm;
  if (!lcm.good()) {
    fprintf(stderr, "Invalid lcm object\n");
    return 1;
  }

  PublisherTest *test = new PublisherTest(nChan, nItems);

  printf("sleep for %d microsec on each loop\n", sleepUsec);
  bool done = false;
  int n = 0;
  while (!done) {
    if (nLoops > 0 && n >= nLoops) {
      break;
    }
    n++;
    test->publish(channelNamePrefix, &lcm, nChan, nItems);
    usleep(sleepUsec);
  }
}