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

class LcmTest {


public:
  LcmTest(int nChan, int nItems) {

    _dummyVal = 0.;
    _msg.seqNo = 0;
    _msg.doubleItem.resize(nItems);
  };

  void publish(lcm::LCM *lcm, int nChan, int nItems, char *pubPrefix);
  void handleMessage( const lcm::ReceiveBuffer *rbuf,
		      const std::string &chan,
		      const lcmMessages::DoubleItems *msg );

protected:

  lcmMessages::DoubleItems _msg;

  double _dummyVal;
};


void LcmTest::publish(lcm::LCM *lcm, int nChan, int nItems, 
			    char *pubPrefix) {

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

  _msg.nItems = nItems;
  _msg.seqNo++;
  _msg.epochUsec = 0;


  // Fill in double items
  for (int i = 0; i < _msg.nItems; i++) {
    sprintf(itemName, "item-%0d", i);
    _msg.doubleItem[i].name.assign(itemName);
    _msg.doubleItem[i].unit.assign("no-units");
    // Increment dummy value for each item
    _msg.doubleItem[i].val = _dummyVal++;
  }

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

    // Publish message on this channel
    sprintf(channelName, "%s-%03d", pubPrefix, i);
    printf("publish on channel %s\n", channelName);
    printf("msg.nItems: %d, msg.seqNo: %lld\n", _msg.nItems, _msg.seqNo);
    printf("msg values:\n");
    for (int i = 0; i < _msg.nItems; i++) {
      printf("val: %.1f\n", _msg.doubleItem[i].val);
    }

    if (lcm->publish(channelName, &_msg) != 0) {
      printf("ERROR: lcm publish failed\n");
    }
    else {
      printf("lcm publish succeeded\n");
    }
    lcm->handleTimeout(10);
  }



}


void LcmTest::handleMessage( const lcm::ReceiveBuffer *rbuf,
			     const std::string &chan,
			     const lcmMessages::DoubleItems *msg )
{
    printf("received message # %lld on channel \"%s\"\n", 
	   msg->seqNo, chan.c_str() );
}


#define LCM_DEFAULT_URL "LCM_DEFAULT_URL"

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

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

  int nLoops = 100;
  double freqHz;
  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], "%lf", &freqHz) != 1) {
    fprintf(stderr, "Invalid freqHz: %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 *pubPrefix = 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 = new lcm::LCM(lcmURL);
  if (!lcm->good()) {
    fprintf(stderr, "Invalid lcm object\n");
    return 1;
  }

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

  unsigned long loopIntervalUsec = 1000000 / freqHz;
  printf("loopIntervalUsec: %ld\n", loopIntervalUsec);
  struct timeval tval;

  // Get start time
  gettimeofday(&tval, NULL);
  suseconds_t startUsec = 1000000 * tval.tv_sec + tval.tv_usec;

  for (int n = 0; n < nLoops || nLoops == 0; n++) {

    test->publish(lcm, nChan, nItems, pubPrefix);

    // Compute sleep interval
    gettimeofday(&tval, NULL);

    suseconds_t nowUsec = 1000000 * tval.tv_sec + tval.tv_usec;

    useconds_t sleepUsec = (startUsec + (n+1) * loopIntervalUsec) - nowUsec;

    if (sleepUsec <= 0.) {
      printf("Missed deadline: sleepUsec=%u\n", sleepUsec);
    }
    usleep(sleepUsec);
  }

  gettimeofday(&tval, NULL);
  printf("Executed %d loops at %.2fHz in %.3f seconds\n",
	 nLoops, freqHz, 
	 (1000000 * tval.tv_sec + tval.tv_usec - startUsec) / 1e6);

  printf("done - destroy LCM object\n");

  delete lcm;

  return 0;
}