#include <stdio.h>
#include <ndds/ndds_c.h>

#define MAX_SEQUENCE_LEN 8

DDS_TypeCode * Example_createTypecode(
        struct DDS_TypeCodeFactory *typeCodeFactory)
{
    struct DDS_TypeCode *typeCode = NULL;
    struct DDS_TypeCode *msgStructTypeCode = NULL;
    struct DDS_TypeCode *sequenceStructTypeCode = NULL;
    struct DDS_TypeCode *numberListTypeCode = NULL;
    struct DDS_StructMemberSeq msgStructMembers = DDS_SEQUENCE_INITIALIZER;
    struct DDS_StructMemberSeq basicDataMembers = DDS_SEQUENCE_INITIALIZER;
    int numberListLen = 10;
    int sequenceStructLen = 8;

    DDS_TypeCode * seqElementTC = NULL;
    DDS_ExceptionCode_t ex;

    /* ------------------------------------------------
     *  Create msgStructTypeCode
     * ------------------------------------------------*/
    msgStructTypeCode = DDS_TypeCodeFactory_create_struct_tc(
            typeCodeFactory,
            "SNAExamples::MsgStruct",
            &msgStructMembers,
            &ex);
    if (ex != DDS_NO_EXCEPTION_CODE) {
        fprintf(stderr, "! Unable to create struct MsgStruct\n");
        goto done;
    }

    /* Add MsgStruct.doubleA */
    DDS_TypeCode_add_member(
            msgStructTypeCode,
            "doubleA",
            DDS_TYPECODE_MEMBER_ID_INVALID,
            DDS_TypeCodeFactory_get_primitive_tc(
                    typeCodeFactory,
                    DDS_TK_DOUBLE),
            DDS_TYPECODE_NONKEY_REQUIRED_MEMBER,
            &ex);
    if (ex != DDS_NO_EXCEPTION_CODE) {
        fprintf(stderr, "! Unable to add member 'doubleA' to 'MsgStruct'\n");
        goto done;
    }

    /* Add MsgStruct.intB */
    DDS_TypeCode_add_member(
            msgStructTypeCode,
            "intB",
            DDS_TYPECODE_MEMBER_ID_INVALID,
            DDS_TypeCodeFactory_get_primitive_tc(
                    typeCodeFactory,
                    DDS_TK_ULONG),
            DDS_TYPECODE_NONKEY_REQUIRED_MEMBER,
            &ex);
    if (ex != DDS_NO_EXCEPTION_CODE) {
        fprintf(stderr, "! Unable to add member 'intB' to 'MsgStruct'\n");
        goto done;
    }

    /* ------------------------------------------------
     *  Create numberListTypeCode
     * ------------------------------------------------*/
    numberListTypeCode = DDS_TypeCodeFactory_create_sequence_tc(
            typeCodeFactory,
            numberListLen,
            DDS_TypeCodeFactory_get_primitive_tc(
                    typeCodeFactory,
                    DDS_TK_ULONG),
            &ex);
    if (ex != DDS_NO_EXCEPTION_CODE) {
        fprintf(stderr, "! Unable to create numberListTypeCode\n");
        goto done;
    }

    /* ------------------------------------------------
     *  Create sequenceStructTypeCode
     * ------------------------------------------------*/
    sequenceStructTypeCode = DDS_TypeCodeFactory_create_sequence_tc(
            typeCodeFactory,
            sequenceStructLen,
            msgStructTypeCode,
            &ex);
    if (ex != DDS_NO_EXCEPTION_CODE) {
        fprintf(stderr, "! Unable to create sequenceStructTypeCode\n");
        goto done;
    }


    /* ------------------------------------------------
     *  Create DDSBasicData_msg typeCode
     * ------------------------------------------------*/
    typeCode = DDS_TypeCodeFactory_create_struct_tc(
            typeCodeFactory,
            "SNAExamples::DDSBasicData_msg",
            &basicDataMembers,
            &ex);
    if (ex != DDS_NO_EXCEPTION_CODE) {
        fprintf(stderr, "! Unable to create struct DDSBasicData_msg\n");
        goto done;
    }

    DDS_TypeCode_add_member(
            typeCode,
            "a",
            DDS_TYPECODE_MEMBER_ID_INVALID,
            DDS_TypeCodeFactory_get_primitive_tc(
                    typeCodeFactory,
                    DDS_TK_DOUBLE),
            DDS_TYPECODE_NONKEY_REQUIRED_MEMBER,
            &ex);
    if (ex != DDS_NO_EXCEPTION_CODE) {
        fprintf(stderr, "! Unable to add member 'a' to 'DDSBasicData_msg'\n");
        goto done;
    }

    DDS_TypeCode_add_member(
            typeCode,
            "b",
            DDS_TYPECODE_MEMBER_ID_INVALID,
            numberListTypeCode,
            DDS_TYPECODE_NONKEY_REQUIRED_MEMBER,
            &ex);
    if (ex != DDS_NO_EXCEPTION_CODE) {
        fprintf(stderr, "! Unable to add member 'b' to 'DDSBasicData_msg'\n");
        goto done;
    }

    DDS_TypeCode_add_member(
            typeCode,
            "seq",
            DDS_TYPECODE_MEMBER_ID_INVALID,
            sequenceStructTypeCode,
            DDS_TYPECODE_NONKEY_REQUIRED_MEMBER,
            &ex);
    if (ex != DDS_NO_EXCEPTION_CODE) {
        fprintf(stderr, "! Unable to add member 'b' to 'DDSBasicData_msg'\n");
        goto done;
    }

done:
    if (msgStructTypeCode != NULL) {
        DDS_TypeCodeFactory_delete_tc(
                typeCodeFactory,
                msgStructTypeCode,
                &ex);
        msgStructTypeCode = NULL;
    }

    if (numberListTypeCode != NULL) {
        DDS_TypeCodeFactory_delete_tc(
                typeCodeFactory,
                numberListTypeCode,
                &ex);
        numberListTypeCode = NULL;
    }

    if (sequenceStructTypeCode != NULL) {
        DDS_TypeCodeFactory_delete_tc(
                typeCodeFactory,
                sequenceStructTypeCode,
                &ex);
        sequenceStructTypeCode = NULL;

    }

    DDS_StructMemberSeq_finalize(&msgStructMembers);
    DDS_StructMemberSeq_finalize(&basicDataMembers);

    return typeCode;
}

void Example_deleteTypeCode(
        DDS_TypeCodeFactory *typeCodeFactory,
        DDS_TypeCode *typeCode)
{
    DDS_ExceptionCode_t ex;

    if (typeCode != NULL) {
        DDS_TypeCodeFactory_delete_tc(
                typeCodeFactory,
                typeCode,
                &ex);
        typeCode = NULL;
    }
}

DDS_ReturnCode_t Example_takeSamples(DDS_DynamicDataReader *dynamicDataReader)
{
    int i = 0;
    DDS_ReturnCode_t retCode = DDS_RETCODE_ERROR;
    struct DDS_DynamicDataSeq dataSeq = DDS_SEQUENCE_INITIALIZER;
    struct DDS_SampleInfoSeq infoSeq = DDS_SEQUENCE_INITIALIZER;

    retCode = DDS_DynamicDataReader_take(
            dynamicDataReader,
            &dataSeq,
            &infoSeq,
            DDS_ANY_SAMPLE_STATE,
            DDS_ANY_SAMPLE_STATE,
            DDS_LENGTH_UNLIMITED,
            DDS_ANY_INSTANCE_STATE);
    if (retCode == DDS_RETCODE_OK) {
        for (i = 0;  i < DDS_DynamicDataSeq_get_length(&dataSeq); ++i) {
            struct DDS_DynamicData sample = DDS_DynamicDataSeq_get(&dataSeq, i);
            printf("Sample %d\n", i);
            DDS_DynamicData_print(&sample, stdout, 2);
        }
    } else {
        goto done;
    }

done:
    DDS_DynamicDataReader_return_loan(dynamicDataReader, &dataSeq, &infoSeq);
    DDS_DynamicDataSeq_finalize(&dataSeq);
    DDS_SampleInfoSeq_finalize(&infoSeq);

    return retCode;
}

DDS_ReturnCode_t Example_writeSample(
        DDS_DynamicDataWriter *dynamicDataWriter,
        DDS_TypeCode * typeCode)
{
    DDS_ReturnCode_t retCode = DDS_RETCODE_ERROR;
    DDS_DynamicData *sample = NULL;

    sample = DDS_DynamicData_new(
            typeCode,
            &DDS_DYNAMIC_DATA_PROPERTY_DEFAULT);
    if (sample == DDS_RETCODE_OK) {
        fprintf(stderr, "! Unable write\n");
        goto done;
    }

    /* Modify sample here */

    retCode = DDS_DynamicDataWriter_write(
            dynamicDataWriter,
            sample,
            &DDS_HANDLE_NIL);

done:
    if (sample != NULL) {
        DDS_DynamicData_delete(sample);
        sample = NULL;
    }

    return retCode;
}

int main()
{
    DDS_DomainParticipant *participant = NULL;
    DDS_DataWriter *writer = NULL;
    DDS_DataReader *reader = NULL;
    DDS_DynamicDataWriter *dynamicDataWriter = NULL;
    DDS_DynamicDataReader *dynamicDataReader = NULL;
    DDS_Topic *topic = NULL;
    DDS_DynamicData * sample = NULL;
    struct DDS_TypeCode *typeCode = NULL;
    DDS_Publisher *publisher = NULL;
    DDS_Subscriber *subscriber = NULL;
    int domainId = 0;
    struct DDS_DynamicDataTypeSupport *typeSupport = NULL;
    struct DDS_DynamicDataTypeProperty_t props =
            DDS_DynamicDataTypeProperty_t_INITIALIZER;
    struct DDS_TypeCodeFactory *typeCodeFactory = NULL;
    const char * typeName = "SNAExamples::DDSBasicData_msg";
    const char * topicName = "Example SNAExamples_DDSBasicData_msg";
    DDS_ReturnCode_t retCode = DDS_RETCODE_ERROR;
    int i = 0;

    participant = DDS_DomainParticipantFactory_create_participant(
            DDS_TheParticipantFactory,
            domainId,
            &DDS_PARTICIPANT_QOS_DEFAULT,
            NULL /* listener */,
            DDS_STATUS_MASK_NONE);
    if (participant == NULL) {
        fprintf(stderr, "! Error creating Participant\n");
        goto done;
    }

    publisher = DDS_DomainParticipant_get_implicit_publisher(participant);
    if (publisher == NULL) {
        fprintf(stderr, "! Unable to get implicit publisher\n");
        goto done;
    }

    subscriber = DDS_DomainParticipant_get_implicit_subscriber(participant);
    if (subscriber == NULL) {
        fprintf(stderr, "! Unable to get implicit subscriber\n");
        goto done;
    }


    typeCodeFactory = DDS_TypeCodeFactory_get_instance();
    if (typeCodeFactory == NULL) {
        fprintf(stderr, "! Error getting typeCodeFactory\n");
        goto done;
    }

    typeCode = Example_createTypecode(typeCodeFactory);
    if (typeCode == NULL) {
        fprintf(stderr, "! Error getting typeCode\n");
        goto done;
    }

    /* Create the Dynamic Data TypeSupport object */
    typeSupport = DDS_DynamicDataTypeSupport_new(typeCode, &props);
    if (typeSupport == NULL) {
        fprintf(stderr, "! Unable to create dynamic data type support\n");
        goto done;
    }

    retCode = DDS_DynamicDataTypeSupport_register_type(
            typeSupport,
            participant,
            typeName);
    if (retCode != DDS_RETCODE_OK) {
        fprintf(stderr, "! Unable to register type %s\n", typeName);
        goto done;
    }

    topic = DDS_DomainParticipant_create_topic(
            participant,
            topicName,
            typeName,
            &DDS_TOPIC_QOS_DEFAULT,
            NULL,
            DDS_STATUS_MASK_NONE);
    if (topic == NULL) {
        fprintf(stderr, "! Unable to create topic\n");
        goto done;
    }

    writer = DDS_Publisher_create_datawriter(
            publisher,
            topic,
            &DDS_DATAWRITER_QOS_DEFAULT,
            NULL,
            DDS_STATUS_MASK_NONE);
    if (writer == NULL) {
        fprintf(stderr, "! Unable to create writer\n");
        goto done;
    }

    dynamicDataWriter = DDS_DynamicDataWriter_narrow(writer);
    if (dynamicDataWriter == NULL) {
        fprintf(stderr, "! Unable to create dynamicDataWriter\n");
        goto done;
    }

    reader = DDS_Subscriber_create_datareader(
            subscriber,
            DDS_Topic_as_topicdescription(topic),
            &DDS_DATAREADER_QOS_DEFAULT,
            NULL,
            DDS_STATUS_MASK_NONE);
    if (reader == NULL) {
        fprintf(stderr, "! Unable to create reader\n");
        return -1;
    }

    dynamicDataReader = DDS_DynamicDataReader_narrow(reader);

    /* If you want to write
     * Example_writeSample(dynamicDataWriter);
     */

    while (DDS_BOOLEAN_TRUE) {
        Example_takeSamples(dynamicDataReader);
        sleep(2);
    }

    retCode = DDS_RETCODE_OK;
done:
    if (typeCode != NULL) {
        Example_deleteTypeCode(typeCodeFactory, typeCode);
        typeCode = NULL;
    }

    if (participant != NULL) {
        DDS_DomainParticipant_delete_contained_entities(participant);

        DDS_DomainParticipantFactory_delete_participant(
                DDS_TheParticipantFactory, participant);
        participant = NULL;
    }

    DDS_DomainParticipantFactory_finalize_instance();

    return retCode;
}