#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>
#include <gtest/gtest.h>

#include <lcm/lcm.h>

#include "common.hpp"

#define info(...) do { printf("cpp_client: "); printf(__VA_ARGS__); \
  printf("\n"); } while(0)

class EchoTest {
 public:
  EchoTest()
      : lcm_(),
        num_trials_(100),
        test_channel_("TEST_ECHO"),
        response_count_(0),
        echo_data_(NULL),
        echo_msg_len_(0),
        subscription_() {
  }

  bool Run(void) {
    int maxlen = 10000;
    int minlen = 10;
    echo_data_ = (uint8_t*) malloc(maxlen);
    subscription_ = lcm_.subscribe(test_channel_ + "_REPLY",
                                    &EchoTest::Handler, this);
    response_count_ = 0;

    for (int iter = 0; iter < num_trials_; iter++) {
      echo_msg_len_ = rand() % (maxlen - minlen) + minlen;
      for (int i = 0; i < echo_msg_len_; i++)
        echo_data_[i] = rand() % 256;

      lcm_.publish(test_channel_, echo_data_, echo_msg_len_);

      if (lcm_.handleTimeout(500) <= 0 || (response_count_ != iter + 1)) {
        info("echo test failed to receive response on iteration %d", iter);
        free(echo_data_);
        return false;
      }
    }

    lcm_.unsubscribe(subscription_);
    free(echo_data_);
    return true;
  }

 private:
  void Handler(const lcm::ReceiveBuffer *rbuf, const std::string& channel) {
    if (rbuf->data_size != echo_msg_len_)
      return;
    if (memcmp(rbuf->data, echo_data_, rbuf->data_size))
      return;
    response_count_++;
  }

  lcm::LCM lcm_;
  int num_trials_;
  int echo_msg_len_;
  uint8_t* echo_data_;
  int response_count_;
  lcm::Subscription* subscription_;
  std::string test_channel_;
};

TEST(LCM_CPP, Echo) {
  srand(time(NULL));

  EXPECT_TRUE(EchoTest().Run());
}

template<class LcmType>
class TypedTest {
 public:
  TypedTest(const std::string test_name, int num_trials)
      : lcm_(),
      test_channel_(test_name),
      response_count_(0),
      num_trials_(num_trials),
      subscription_() {
  }

  bool Run(void) {
    LcmType msg;
    lcm::Subscription* subscription = lcm_.subscribe(
        test_channel_ + "_reply", &TypedTest<LcmType>::Handler, this);
    bool result = true;
    for (int trial = 0; trial < num_trials_ && result; trial++) {
      FillLcmType(trial, &msg);
      lcm_.publish(test_channel_, &msg);
      if (lcm_.handleTimeout(500) <= 0) {
        info("%s test: Timeout waiting for reply", test_channel_.c_str());
        result = false;
        break;
      } else if (response_count_ != trial + 1) {
        info("%s test: failed on iteration %d", test_channel_.c_str(), trial);
        result = false;
        break;
      }
      ClearLcmType(&msg);
    }
    lcm_.unsubscribe(subscription);
    return result;
  }

 private:
  void Handler(const lcm::ReceiveBuffer* rbuf, const std::string& channel,
               const LcmType* msg) {
    if (CheckLcmType(msg, response_count_ + 1)) {
      response_count_++;
      return;
    }
  }

  lcm::LCM lcm_;
  int response_count_;
  int num_trials_;
  lcm::Subscription* subscription_;
  std::string test_channel_;
};

TEST(LCM_CPP, primitives_t) {
    EXPECT_TRUE(TypedTest<lcmtest::primitives_t>("test_lcmtest_primitives_t",
                                          1000).Run());
}

TEST(LCM_CPP, primitives_list_t) {
    EXPECT_TRUE(TypedTest<lcmtest::primitives_list_t>(
                "test_lcmtest_primitives_list_t", 100).Run());
}

TEST(LCM_CPP, node_t) {
    EXPECT_TRUE(TypedTest<lcmtest::node_t>("test_lcmtest_node_t",
                                          7).Run());
}

TEST(LCM_CPP, multidim_array_t) {
    EXPECT_TRUE(TypedTest<lcmtest::multidim_array_t>(
          "test_lcmtest_multidim_array_t", 5).Run());
}

TEST(LCM_CPP, cross_package_t) {
    EXPECT_TRUE(TypedTest<lcmtest2::cross_package_t>(
          "test_lcmtest2_cross_package_t", 100).Run());
}
