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

#include <lcm/lcm.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* rbuf, const char* channel, \
        const type* msg, void* user) \
{ \
    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;
int g_echo_msg_len = 0;
uint8_t* g_echo_data = NULL;

static void
echo_handler(const lcm_recv_buf_t *rbuf, const char * channel, void * user)
{
    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;
        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);
}
