/* CameraImage_publisher.cxx

A publication of data of type CameraImage

This file uses code automatically generated by the rtiddsgen command:

rtiddsgen -language C++11 -example <arch> CameraImage.idl

(Ignore messages warning about CameraImage_publisher.cxx,
CameraImage_subscriber.cxx, and USER_QOS_PROFILES.xml already existing. 
These files contain the example code.)

Instructions:

(1) Compile this file and the example subscription.

(2) Start the subscription on the same domain used for RTI Data Distribution
Service with the command
objs/<arch>/CameraImage_subscriber <domain_id> <sample_count>

(3) Start the publication on the same domain used for RTI Data Distribution
Service with the command
objs/<arch>/CameraImage_publisher <domain_id> <sample_count>

(4) [Optional] Specify the list of discovery initial peers and 
multicast receive addresses via an environment variable or a file 
(in the current working directory) called NDDS_DISCOVERY_PEERS. 

You can run any number of publishers and subscribers programs, and can 
add and remove them dynamically from the domain.

Example:

To run the example application on domain <domain_id>:

On Unix: 

objs/<arch>/CameraImage_publisher <domain_id> 
objs/<arch>/CameraImage_subscriber <domain_id> 

On Windows:

objs\<arch>\CameraImage_publisher <domain_id>  
objs\<arch>\CameraImage_subscriber <domain_id>   
*/

#include <iostream>

#include <dds/pub/ddspub.hpp>
#include <rti/util/util.hpp> // for sleep()
#include <rti/config/Logger.hpp>

#include "CameraImage.hpp"

const int PIXEL_COUNT = 10;

// Simplest way to create the data sample
void build_data_sample(CameraImageBuilder& builder, int seed)
{
    builder.add_format(Format::RGB);

    auto resolution_offset = builder.add_resolution();
    resolution_offset.height(100);
    resolution_offset.width(200);

    auto string_builder = builder.build_source();
    string_builder.set_string("CAM-1");
    string_builder.finish();
    
    // Method 1 - Build the pixel sequence element by element
    auto pixels_builder = builder.build_pixels();
    for (int i = 0; i < PIXEL_COUNT; i++) {
        auto pixel = pixels_builder.add_next();
        pixel.red((seed + i) % 100);
        pixel.green((seed + i + 1) % 100);
        pixel.blue((seed + i + 2) % 100);
    }
    pixels_builder.finish();
}

// Creates the same data sample using a faster method
// to populate the pixel sequence
void build_data_sample_fast(CameraImageBuilder& builder, int seed)
{
    // The initialization of these members doesn't change
    builder.build_source().set_string("CAM-1");
    builder.add_format(Format::RGB);

    auto resolution_offset = builder.add_resolution();
    resolution_offset.height(100);
    resolution_offset.width(200);


    // Populate the pixel sequence accessing the byte buffer directly, instead
    // of iterating through each element Offset

    // 1) Use the builder to add all the elements at once. This operation is
    // cheap (it just advances the underlying buffer without initializing anything)
    auto pixels_builder = builder.build_pixels();
    pixels_builder.add_n(PIXEL_COUNT);
    auto pixels = pixels_builder.finish();

    // 2) Use plain_cast to access the buffer as an array of regular C++ Pixels
    // (pixel_array's type is PixelPlainHelper*)
    auto pixel_array = rti::flat::plain_cast(pixels);
    for (int i = 0; i < PIXEL_COUNT; i++) {
        auto& pixel = pixel_array[i];
        pixel.red((seed + i) % 100);
        pixel.green((seed + i + 1) % 100);
        pixel.blue((seed + i + 2) % 100);
    }
}

void publisher_main(int domain_id, int sample_count)
{
    // Create a DomainParticipant with default Qos
    dds::domain::DomainParticipant participant (domain_id);

    // Create a Topic -- and automatically register the type
    dds::topic::Topic<CameraImage> topic (participant, "Example CameraImage");

    // Create a DataWriter with default Qos (Publisher created in-line)
    dds::pub::DataWriter<CameraImage> writer(dds::pub::Publisher(participant), topic);

    for (int count = 0; count < sample_count || sample_count == 0; count++) {
        CameraImageBuilder builder = rti::flat::build_data(writer);

        // Build the CameraImage data sample using the builder
        if (count % 2 == 0) {
            build_data_sample(builder, count); // method 1
        } else {
            build_data_sample_fast(builder, count); // method 2
        }

        // Create the sample
        CameraImage *sample = builder.finish_sample();

        std::cout << "Writing CameraImage, count " << count << std::endl;

        writer.write(*sample);

        rti::util::sleep(dds::core::Duration(4));
    }    
}

int main(int argc, char *argv[])
{

    int domain_id = 0;
    int sample_count = 0; // infinite loop

    if (argc >= 2) {
        domain_id = atoi(argv[1]);
    }
    if (argc >= 3) {
        sample_count = atoi(argv[2]);
    }

    // To turn on additional logging, include <rti/config/Logger.hpp> and
    // uncomment the following line:
    rti::config::Logger::instance().verbosity(rti::config::Verbosity::WARNING);

    try {
        publisher_main(domain_id, sample_count);
    } catch (const std::exception& ex) {
        // This will catch DDS exceptions
        std::cerr << "Exception in publisher_main(): " << ex.what() << std::endl;
        return -1;
    }

    // RTI Connext provides a finalize_participant_factory() method
    // if you want to release memory used by the participant factory singleton.
    // Uncomment the following line to release the singleton:
    //
    // dds::domain::DomainParticipant::finalize_participant_factory();

    return 0;
}

