#include <iostream>
#include "ndds/ndds_cpp.h"

DDS_TypeCode *createTypeCode()
{
    DDS_TypeCodeFactory * factory = NULL;
    DDS_TypeCode * structTc = NULL;
    DDS_ExceptionCode_t ex = DDS_NO_EXCEPTION_CODE;
    DDS_StructMemberSeq structMembers;
    factory = DDS_TypeCodeFactory::get_instance();

    structTc = factory->create_struct_tc("test_type", structMembers, ex);
    structTc->add_member("bool_1", DDS_TYPECODE_MEMBER_ID_INVALID, factory->get_primitive_tc(DDS_TK_BOOLEAN),
                    DDS_TYPECODE_NONKEY_REQUIRED_MEMBER, ex);
    structTc->add_member("bool_2", DDS_TYPECODE_MEMBER_ID_INVALID, factory->get_primitive_tc(DDS_TK_BOOLEAN),
                    DDS_TYPECODE_NONKEY_REQUIRED_MEMBER, ex);
    structTc->add_member("bool_3", DDS_TYPECODE_MEMBER_ID_INVALID, factory->get_primitive_tc(DDS_TK_BOOLEAN),
                    DDS_TYPECODE_NONKEY_REQUIRED_MEMBER, ex);
    structTc->add_member("double_1", DDS_TYPECODE_MEMBER_ID_INVALID, factory->get_primitive_tc(DDS_TK_DOUBLE),
                    DDS_TYPECODE_NONKEY_REQUIRED_MEMBER, ex);
    structTc->add_member("ulong_1", DDS_TYPECODE_MEMBER_ID_INVALID, factory->get_primitive_tc(DDS_TK_ULONG),
                    DDS_TYPECODE_NONKEY_REQUIRED_MEMBER, ex);
    structTc->add_member("string_1", DDS_TYPECODE_MEMBER_ID_INVALID, factory->create_string_tc(60, ex),
                    DDS_TYPECODE_NONKEY_REQUIRED_MEMBER, ex);
    return structTc;
}

#define BUFFER_SIZE (1000)

int main(int argc, char *argv[])
{
    DDS_ExceptionCode_t ex;
    DDS_TypeCode* typeCode;
    DDSDynamicDataTypeSupport* typeSupport;

    char buffer[BUFFER_SIZE];
    int bufferLen = BUFFER_SIZE;
    memset(buffer, 0, bufferLen);

    typeCode = createTypeCode();
    typeSupport = new DDSDynamicDataTypeSupport(typeCode, DDS_DYNAMIC_DATA_TYPE_PROPERTY_DEFAULT);
    typeCode->print_IDL(0, ex);

    int serializedSize = 0;

    RTICdrStream serializationStream;
    RTICdrStream_init(&serializationStream);

    RTICdrStream deserializationStream;
    RTICdrStream_init(&deserializationStream);

    DDS_DynamicData* dynamicData = typeSupport->create_data();
    DDS_DynamicData* deserializedData = typeSupport->create_data();

    for (int i = 0;; i++) {
        /* Prepare data */
        char str_buf[60];
        sprintf(str_buf, "String: %u", i);
        dynamicData->set_double("double_1", DDS_DYNAMIC_DATA_MEMBER_ID_UNSPECIFIED, i);
        dynamicData->set_boolean("bool_1", DDS_DYNAMIC_DATA_MEMBER_ID_UNSPECIFIED, 1);
        dynamicData->set_boolean("bool_2", DDS_DYNAMIC_DATA_MEMBER_ID_UNSPECIFIED, 0);
        dynamicData->set_boolean("bool_3", DDS_DYNAMIC_DATA_MEMBER_ID_UNSPECIFIED, 1);
        dynamicData->set_ulong("ulong_1", DDS_DYNAMIC_DATA_MEMBER_ID_UNSPECIFIED, i);
        dynamicData->set_string("string_1", DDS_DYNAMIC_DATA_MEMBER_ID_UNSPECIFIED, str_buf);
        std::cout << "============== Original data =================" << std::endl;
        typeSupport->print_data(dynamicData);
        /* Serialize data */
        RTICdrStream_set(&serializationStream, buffer, bufferLen);
        dynamicData->to_stream(serializationStream);
        serializedSize = RTICdrStream_getCurrentPositionOffset(&serializationStream);
        std::cout << "serializedSize: " << serializedSize << std::endl;
        /* Deserialize data */
        RTICdrStream_set(&deserializationStream, buffer, bufferLen);
        deserializedData->from_stream(deserializationStream);
        /* Print deserialized data */
        std::cout << "============== Deserialized data =================" << std::endl;
        typeSupport->print_data(deserializedData);
        sleep(1);
    }
}