RoboidControl-cpp/Participants/ParticipantMQTT.cpp

#include "ParticipantMQTT.h"

#if defined(__unix__) || defined(__APPLE__)
#include <arpa/inet.h>
#include <fcntl.h>  // For fcntl
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>
#endif

#include "Participants/ParticipantUDP.h"

namespace RoboidControl {

MQTTParticipantBase::MQTTParticipantBase(const char* ipAddress, int port)
    : RemoteParticipantUDP("127.0.0.1", port) {
  this->name = "ParticipantUDP";
  this->remoteSite = new RemoteParticipantUDP(ipAddress, port);
  // Participant::registry.Add(this);

  this->root = Thing::LocalRoot();  // Participant::LocalParticipant->root;
  this->root->owner = this;
  this->root->name = "UDP Root";
  this->Add(this->root);

  Participant::ReplaceLocalParticipant(*this);
}

void MQTTParticipantBase::SetupTCP() {
  this->connected = false;
}

#define MQTT_CONNECT 0x10
#define MQTT_CONNECT_ACK 0x20

void MQTTParticipantBase::send_mqtt_connect(const char* client_id) {
  MQTTConnectPacket packet;
  packet.fixed_header = MQTT_CONNECT;
  packet.remaining_length =  // 14;
      2 + 4 + 2 + strlen(client_id) +
      3;                            // Protocol name + protocol level + connect
                                    // flags + keep alive + client ID
  packet.protocol_name_length = 4;  // "MQTT"
  packet.protocol_name = "MQTT";
  packet.protocol_level = 4;      // MQTT version 3.1.1
  packet.connect_flags = 2;       // Clean session
  packet.keep_alive = htons(15);  // Keep alive time in seconds

  // Create the MQTT connect packet
  int index = 0;

  this->buffer[index++] = packet.fixed_header;
  this->buffer[index++] = packet.remaining_length;
  this->buffer[index++] = 0;  // MSB protocol_name_length
  this->buffer[index++] = packet.protocol_name_length;
  memcpy(&this->buffer[index], packet.protocol_name,
         packet.protocol_name_length);
  index += packet.protocol_name_length;
  this->buffer[index++] = packet.protocol_level;
  this->buffer[index++] = packet.connect_flags;
  this->buffer[index++] = packet.keep_alive & 0xFF;         // LSB
  this->buffer[index++] = (packet.keep_alive >> 8) & 0xFF;  // MSB
  size_t client_id_length = strlen(client_id);
  this->buffer[index++] = (client_id_length >> 8) & 0xFF;  // MSB
  this->buffer[index++] = client_id_length & 0xFF;         // LSB
  memcpy(&this->buffer[index], client_id, client_id_length);
  index += client_id_length;

  for (int ix = 0; ix < index; ix++)
    std::cout << std::hex << "0x" << (int)this->buffer[ix] << " ";
  std::cout << std::dec << std::endl;

  // Send the MQTT connect packet
  SendTCP(index);

  std::cout << "Send connect, client ID = " << client_id << std::endl;
}

void MQTTParticipantBase::sendSubscribe(const char* topic) {
  // Packet Identifier (2 bytes)
  static unsigned short packetId =
      1;  // Increment this for each new subscription

  // Calculate the length of the topic name
  size_t topicLength = strlen(topic);

  // Remaining length = Packet Identifier (2 bytes) + Topic Length (2 bytes) +
  // Topic Name + QoS (1 byte)
  size_t remainingLength = 1 + 2 + 2 + topicLength + 1;

  // Construct the SUBSCRIBE packet
  // unsigned char subscribePacket[3 + topicLength];  // 3 bytes for fixed
  // header
  //                                                  // and packet ID
  this->buffer[0] = (char)0x82;                 // Packet type and flags
  this->buffer[1] = (char)remainingLength;      // Remaining length
  this->buffer[2] = (packetId >> 8) & 0xFF;     // Packet Identifier MSB
  this->buffer[3] = packetId & 0xFF;            // Packet Identifier LSB
  this->buffer[4] = (topicLength >> 8) & 0xFF;  // Topic Length MSB
  this->buffer[5] = topicLength & 0xFF;         // Topic Length LSB

  // Copy the topic name into the packet
  memcpy(&this->buffer[6], topic, topicLength);
  this->buffer[6 + topicLength] = 0x00;  // QoS level (0)

  int index = 7 + topicLength;
  //   for (int ix = 0; ix < index; ix++)
  //     std::cout << std::hex << (int)this->buffer[ix] << std::dec << "\n";

  // Send the SUBSCRIBE packet
  SendTCP(7 + topicLength);

  std::cout << "Send subscribe to topic: " << topic << std::endl;
}

void MQTTParticipantBase::Update(bool recurse) {
  int packetSize = ReceiveTCP();
  if (packetSize > 0) {
    ReceiveData(packetSize);
  }
}
void MQTTParticipantBase::SendTCP(int bufferSize) {}

int MQTTParticipantBase::ReceiveTCP() {
  return 0;
}

void MQTTParticipantBase::ReceiveData(unsigned char bufferSize) {
  std::cout << " receivemsg\n";
  // std::cout << "receive msg " << (int)msgId << "\n";
  // std::cout << "  buffer size = " <<(int) bufferSize << "\n";
};

// void ParticipantMQTT::receiveMessages(int sock) {
//   unsigned char buffer[1024];
//   // Check for incoming messages without blocking
//   int bytesRead = recv(sock, buffer, sizeof(buffer), MSG_DONTWAIT);
//   if (bytesRead > 0) {
//     // Process the incoming MQTT message
//     std::cout << "Received message: ";
//     for (int i = 0; i < bytesRead; ++i) {
//       std::cout << std::hex << (int)buffer[i] << " ";
//     }
//     std::cout << std::dec << std::endl;  // Reset to decimal output
//   } else if (bytesRead < 0) {
//     // If no messages are available, check for errors
//     if (errno != EAGAIN && errno != EWOULDBLOCK) {
//       std::cerr << "Error receiving message: " << strerror(errno) <<
//       std::endl;
//     }
//   }
// }

}  // namespace RoboidControl