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

#include "common.h"

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

static lcm_t *g_lcm = NULL;

// ====================================== node_t test
#define MAKE_CLIENT_TEST(type, num_iters)                                                     \
                                                                                              \
    static int g_##type##_response_count = 0;                                                 \
                                                                                              \
    static void type##_handler(const lcm_recv_buf_t *, const char *, const type *msg, void *) \
    {                                                                                         \
        if (!check_##type(msg, g_##type##_response_count + 1)) {                              \
            return;                                                                           \
        }                                                                                     \
        g_##type##_response_count++;                                                          \
    }                                                                                         \
                                                                                              \
    static int do_##type##_test(void)                                                         \
    {                                                                                         \
        type msg;                                                                             \
        type##_subscription_t *subs =                                                         \
            type##_subscribe(g_lcm, "test_" #type "_reply", type##_handler, NULL);            \
        g_##type##_response_count = 0;                                                        \
        int result = 1;                                                                       \
        int iter;                                                                             \
        for (iter = 0; iter < num_iters && result; iter++) {                                  \
            fill_##type(iter, &msg);                                                          \
            type##_publish(g_lcm, "test_" #type, &msg);                                       \
            if (!lcm_handle_timeout(g_lcm, 500)) {                                            \
                info(#type " test: Timeout waiting for reply");                               \
                result = 0;                                                                   \
            } else if (g_##type##_response_count != iter + 1) {                               \
                info(#type " test: failed on iteration %d", iter);                            \
                result = 0;                                                                   \
            }                                                                                 \
            clear_##type(&msg);                                                               \
        }                                                                                     \
        type##_unsubscribe(g_lcm, subs);                                                      \
        return result;                                                                        \
    }

MAKE_CLIENT_TEST(lcmtest2_cross_package_t, 100);
MAKE_CLIENT_TEST(lcmtest_multidim_array_t, 5);
MAKE_CLIENT_TEST(lcmtest_node_t, 7);
MAKE_CLIENT_TEST(lcmtest_primitives_list_t, 100);
MAKE_CLIENT_TEST(lcmtest_primitives_t, 1000);

// ================================= echo test
int g_echo_response_count = 0;
unsigned int g_echo_msg_len = 0;
uint8_t *g_echo_data = NULL;

static void echo_handler(const lcm_recv_buf_t *rbuf, const char *, void *)
{
    if (rbuf->data_size != g_echo_msg_len)
        return;
    if (memcmp(rbuf->data, g_echo_data, rbuf->data_size))
        return;
    g_echo_response_count++;
}

TEST(LCM_C, EchoTest)
{
    srand(time(NULL));
    g_lcm = lcm_create(NULL);
    ASSERT_TRUE(g_lcm != NULL);

    int maxlen = 10000;
    int minlen = 10;
    g_echo_data = (uint8_t *) malloc(maxlen);
    lcm_subscription_t *subs = lcm_subscribe(g_lcm, "TEST_ECHO_REPLY", echo_handler, NULL);
    g_echo_response_count = 0;

    int iter;
    for (iter = 0; iter < 100; iter++) {
        g_echo_msg_len = rand() % (maxlen - minlen) + minlen;
        unsigned int i;
        for (i = 0; i < g_echo_msg_len; i++)
            g_echo_data[i] = rand() % 256;

        lcm_publish(g_lcm, "TEST_ECHO", g_echo_data, g_echo_msg_len);

        ASSERT_GT(lcm_handle_timeout(g_lcm, 500), 0);
        ASSERT_EQ(g_echo_response_count, iter + 1);

        if (g_echo_response_count != iter + 1) {
            info("echo test failed to receive response on iteration %d", iter);
            lcm_unsubscribe(g_lcm, subs);
            free(g_echo_data);
            return;
        }
    }

    lcm_unsubscribe(g_lcm, subs);
    free(g_echo_data);
}

// Typed tests
TEST(LCM_C, primitives_t)
{
    ASSERT_TRUE(g_lcm != NULL);
    EXPECT_EQ(1, do_lcmtest_primitives_t_test());
}

TEST(LCM_C, primitives_list_t)
{
    ASSERT_TRUE(g_lcm != NULL);
    EXPECT_EQ(1, do_lcmtest_primitives_list_t_test());
}

TEST(LCM_C, node_t)
{
    ASSERT_TRUE(g_lcm != NULL);
    EXPECT_EQ(1, do_lcmtest_node_t_test());
}

TEST(LCM_C, multidim_array_t)
{
    ASSERT_TRUE(g_lcm != NULL);
    EXPECT_EQ(1, do_lcmtest_multidim_array_t_test());
}

TEST(LCM_C, cross_package)
{
    ASSERT_TRUE(g_lcm != NULL);
    EXPECT_EQ(1, do_lcmtest2_cross_package_t_test());
    lcm_destroy(g_lcm);
}
