#include "AQTPackageMachinePlug.hpp"
#include <QDebug>
#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
#include "HelloWorld.hpp"
#include "HelloWorldPubSubTypes.hpp"

AQTPackageMachinePlug::AQTPackageMachinePlug(QObject *parent)
    : QObject(parent)
    , participant_(nullptr)
    , publisher_(nullptr)
    , topic_(nullptr)
    , writer_(nullptr)
    , message_index_(0)
{
    type_ = TypeSupport(new HelloWorldPubSubType());
}

AQTPackageMachinePlug::~AQTPackageMachinePlug()
{
    if (writer_ != nullptr) {
        publisher_->delete_datawriter(writer_);
    }
    if (topic_ != nullptr) {
        participant_->delete_topic(topic_);
    }
    if (publisher_ != nullptr) {
        participant_->delete_publisher(publisher_);
    }
    if (participant_ != nullptr) {
        DomainParticipantFactory::get_instance()->delete_participant(participant_);
    }
}

bool AQTPackageMachinePlug::init()
{
    DomainParticipantQos participantQos;
    participantQos.name("Participant_publisher");
    participant_ = DomainParticipantFactory::get_instance()->create_participant(0, participantQos);

    if (participant_ == nullptr) {
        qDebug() << "Error creating participant";
        return false;
    }

    type_.register_type(participant_);

    publisher_ = participant_->create_publisher(PUBLISHER_QOS_DEFAULT);
    if (publisher_ == nullptr) {
        qDebug() << "Error creating publisher";
        return false;
    }

    topic_ = participant_->create_topic(
        "HelloWorldTopic",
        "HelloWorld",
        TOPIC_QOS_DEFAULT);

    if (topic_ == nullptr) {
        qDebug() << "Error creating topic";
        return false;
    }

    writer_ = publisher_->create_datawriter(
        topic_,
        DATAWRITER_QOS_DEFAULT);

    if (writer_ == nullptr) {
        qDebug() << "Error creating data writer";
        return false;
    }

    qDebug() << "DDS initialization completed";
    return true;
}

void AQTPackageMachinePlug::publishOnce()
{
    HelloWorld st;
    st.index(message_index_);
    st.message("Hello World " + std::to_string(message_index_));

    if (writer_ != nullptr) {
        writer_->write(&st);
        emit messagePublished(QString::fromStdString(st.message()), message_index_);
    } else {
        qDebug() << "DataWriter not initialized";
    }

    message_index_++;
}

QString AQTPackageMachinePlug::echo(const QString& message)
{
    return QString("Plugin Echo: %1").arg(message);
}

int AQTPackageMachinePlug::calculate(int a, int b)
{
    return a + b;
} 