#include <Arduino.h>
#include<PJON.h>
#include <PJONAny.h>
#include <PJONSoftwareBitBang.h>
#include <PubSubClient.h>
#include <WiFi.h>
#include <Hashtable.h>
#include <set.h>

#define RECEIVER 1
#define SBBPIN 12


// put function declarations here:

void publishMqtt(PJONAny, uint8_t, String, String );
void receiver_function(uint8_t , uint16_t length, const PJON_Packet_Info);
void error_handler(uint8_t, uint16_t);
long getDistance();

// WLAN Daten
const char* ssid = "DEIN_SSID";
const char* password = "DEIN_PASSWORT";

// MQTT Daten
const char* mqtt_server = "DEIN_MQTT_BROKER";
const int mqtt_port = 1883;

// Strategy
StrategyLink<SoftwareBitBang> link;


// Bus Address
PJONAny bus(1);

struct mqttMessage {
  uint8_t method; // 0: publish 1: subscribe 2: unsubscribe
  char topic[250];
  char message[512];
};

mqttMessage receiveBuffer;
bool receivedFlag = false;

WiFiClient espClient;
PubSubClient client(espClient);


Hashtable<String,Set> subs;


void setup_wifi() {
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
  }
}

void reconnect() {
  while (!client.connected()) {
    if (client.connect("UltrasoundClient")) {
      // Verbunden
    } else {
      delay(500);
    }
  }
}

void setup() {
  // put your setup code here, to run once:

  link.strategy.set_pin(SBBPIN);
  bus.strategy.set_link(&link);
  bus.set_error(error_handler);
  bus.set_receiver(receiver_function);
  bus.begin();

  setup_wifi();
  client.setServer(mqtt_server, mqtt_port);
  client.setCallback(callback);
}

void loop() {
  // put your main code here, to run repeatedly:
  bus.update();

  if (receivedFlag) {
    receivedFlag = false;
  }

}



// JSONMqtt funktions

void publishMqtt(PJONAny bus, uint8_t destAddress, String topic, String message ){
  mqttMessage mqMessage;
  mqMessage.method = 0;
  topic.toCharArray(mqMessage.topic, topic.length());
  message.toCharArray(mqMessage.message, message.length());
  bus.send(destAddress,&mqMessage,sizeof(mqMessage));
}

void callback(char* topic, byte* payload, unsigned int length) {

  Serial.println();
  Set tempset;
  memcpy(&tempset,subs.get(topic),sizeof(subs.get(topic)));
  int n = tempset.first();
  while (n != -1)
  {
    publishMqtt(bus, n, topic,String((char)payload));
    n = tempset.next();
  }
  
}

void receiver_function(uint8_t *payload, uint16_t length, const PJON_Packet_Info &packet_info) {
  /* Copy received data in buffer */
  memcpy(&receiveBuffer, payload, sizeof(receiveBuffer));
  receivedFlag = true;

  if (receiveBuffer.method == 0){
    client.publish(receiveBuffer.topic,receiveBuffer.message);
  } else if (receiveBuffer.method == 1){
    client.subscribe(receiveBuffer.topic);
    if (!subs.exists(receiveBuffer.topic)){
      subs.put(receiveBuffer.topic,Set());
    }
    subs.get(receiveBuffer.topic)->add(packet_info.tx.id);
    
  }else if (receiveBuffer.method == 2){
    subs.get(receiveBuffer.topic)->sub(packet_info.tx.id);
  }

};
void error_handler(uint8_t code, uint16_t data, void *custom_pointer) {
  if(code == PJON_CONNECTION_LOST) {
    Serial.print("Connection lost with device id ");
    Serial.println(bus.packets[data].content[0], DEC);
  }
};