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

#define RECEIVER 1
#define SBBPIN 25


// put function declarations here:

void publishMqtt(PJONAny, uint8_t, String, String );
long getDistance();

// WLAN Daten
const char* ssid = "binde";
const char* password = "bindi1518!";

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

// Strategy
//StrategyLink<SoftwareBitBang> linkstrategy;
StrategyLink<DualUDP> linkstrategy;
// Bus Address
PJONAny bus(1);

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

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);
  Serial.println("connected");
  while (WiFi.status() != WL_CONNECTED) {
    Serial.println(".");
    delay(500);
  }
}
/*
void reconnect() {
  while (!client.connected()) {
    if (client.connect("UltrasoundClient")) {
      // Verbunden
    } else {
      delay(500);
    }
  }
}
*/
void receiver_function(uint8_t *payload, uint16_t length, const PJON_Packet_Info &packet_info) {
  /* Copy received data in buffer */
  Serial.println("received pjon message");
  /*
  memcpy(&receiveBuffer, payload, sizeof(receiveBuffer));
  receivedFlag = true;
  Serial.println(receiveBuffer.topic);
  */
  /*
  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 with device ID ");
    Serial.print(bus.packets[data].content[0], DEC);
    Serial.println(" is lost.");
  }
  if(code == PJON_PACKETS_BUFFER_FULL) {
    Serial.print("Packet buffer is full, has now a length of ");
    Serial.println(data, DEC);
    Serial.println("Possible wrong bus configuration!");
    Serial.println("higher PJON_MAX_PACKETS if necessary.");
  }
  if(code == PJON_CONTENT_TOO_LONG) {
    Serial.print("Content is too long, length: ");
    Serial.println(data);
  }
};
/*
void callback(char* topic, byte* payload, unsigned int length) {
  Serial.println("received Mqtt Message");
  Set tempset;
  memcpy(&tempset,subs.get(topic),sizeof(subs.get(topic)));
  int n = tempset.first();
  String msg;
  for (unsigned int i = 0; i < length; i++) {
    msg += (char)payload[i]; // Zeichenweise anhängen
  }
  while (n != -1)
  {
    publishMqtt(bus, n, topic,msg);
    n = tempset.next();
  }
  
}
  */

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  setup_wifi();
  //linkstrategy.strategy.set_pin(12);
  bus.set_error(error_handler);
  bus.strategy.set_link(&linkstrategy);
  bus.set_receiver(receiver_function);
  bus.begin();

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

unsigned long previousMillis = 0;  // Zwischenspeicher für Zeitstempel
const long interval = 1000;        // Intervall in Millisekunden (1 Sekunde)
void loop() {
  // put your main code here, to run repeatedly:
  bus.update();
  bus.receive();

  //client.loop();
  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    publishMqtt(bus,44, "test","42");
  }

  if (receivedFlag) {
    receivedFlag = false;
  }

}



// JSONMqtt funktions

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