#include <U8g2lib.h>
#include <ArduinoMqttClient.h>
#include <ESP8266WiFi.h>
#include "config.h"

WiFiClient client;
MqttClient mqttClient(client);

//U8G2_SSD1306_128X64_NONAME_F_HW_I2C headingDisplay(U8G2_R0, /* clock=*/ SCL, /* data=*/ SDA, /* reset=*/ U8X8_PIN_NONE);  // High speed I2C
//U8G2_SSD1306_128X64_NONAME_F_HW_I2C speedDisplay(U8G2_R0, /* clock=*/ SCL, /* data=*/ SDA, /* reset=*/ U8X8_PIN_NONE);

U8G2_SSD1306_128X64_NONAME_F_HW_I2C headingDisplay(U8G2_R0);  // High speed I2C
U8G2_SSD1306_128X64_NONAME_F_HW_I2C speedDisplay(U8G2_R0);


const char broker[] = "mqtt.sugarland.lan";
int        port     = 1883;
const char topic[]  = "/xplane/rref/#";

void onMqttMessage(int messageSize) {
  if (mqttClient.messageTopic() == "/xplane/rref/1") {
    char buf[3] = "";
    char c = '\0';
    for (int i = 0; i < 3 && mqttClient.available() && c != '.'; i++) {
      c = (char)mqttClient.read();
      if (c != '.') {
        buf[i] = c;
      }
    }

    if (buf[1] == '\0') {
      buf[1] = buf[0];
      buf[0] = '0';
    }
  
    if (buf[2] == '\0') {
      buf[2] = buf[1];
      buf[1] = buf[0];
      buf[0] = '0';
    }
  
    headingDisplay.clearBuffer();
    headingDisplay.setFont(u8g2_font_fub11_tf);
    headingDisplay.drawStr(0, 11, "HDG");
    headingDisplay.setFont(u8g2_font_fub42_tn);
    headingDisplay.drawStr(10, 63, buf);
    headingDisplay.sendBuffer();
  } else if (mqttClient.messageTopic() == "/xplane/rref/2") {
    char buf[3] = "";
    char c = '\0';
    for (int i = 0; i < 3 && mqttClient.available() && c != '.'; i++) {
      c = (char)mqttClient.read();
      if (c != '.') {
        buf[i] = c;
      }
    }

    if (buf[1] == '\0') {
      buf[1] = buf[0];
      buf[0] = '0';
    }
  
    if (buf[2] == '\0') {
      buf[2] = buf[1];
      buf[1] = buf[0];
      buf[0] = '0';
    }
  
    speedDisplay.clearBuffer();
    speedDisplay.setFont(u8g2_font_fub11_tf);
    speedDisplay.drawStr(0, 11, "SPD");
    speedDisplay.setFont(u8g2_font_fub42_tn);
    speedDisplay.drawStr(10, 63, buf);
    speedDisplay.sendBuffer();
    
  }
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }

  WiFi.begin(WLAN_SSID, WLAN_PASS);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println(WiFi.localIP());

  mqttClient.setUsernamePassword(MQTT_USER3, MQTT_PASS);
  if (!mqttClient.connect(broker, port)) {
    Serial.print("MQTT connection failed! Error code = ");
    Serial.println(mqttClient.connectError());

    while (1);
  } else {
    Serial.println("MQTT connected");
  }

  headingDisplay.begin();
  speedDisplay.setI2CAddress(0x7A);
  speedDisplay.begin();

  mqttClient.onMessage(onMqttMessage);
  mqttClient.subscribe(topic);

  mqttClient.beginMessage("/xplane/meta/rref");
  mqttClient.print("001 200 sim/cockpit/autopilot/heading_mag");
  mqttClient.endMessage();
  mqttClient.beginMessage("/xplane/meta/rref");
  mqttClient.print("002 200 sim/cockpit/autopilot/airspeed");
  mqttClient.endMessage();
}

void loop() {
  // call poll() regularly to allow the library to receive MQTT messages and
  // send MQTT keep alives which avoids being disconnected by the broker
  mqttClient.poll();
}