#include <Wire.h>
#include <ArduinoMqttClient.h>
#include <Ethernet.h>
#include "config.h"

#define READ_BYTES_FROM_WIRE 1

// needs global define
#define DATA_STOP_BYTE 0x00
#define DATA_RESET_BYTE 0xFF
#define HEADING_TRIGGER_BIT 1
#define HEADING_HIGH_BIT 2
#define ALTITUDE_TRIGGER_BIT 4
#define ALTITUDE_HIGH_BIT 8

#define HEADING_SIM_UP "sim/autopilot/heading_up"
#define HEADING_SIM_DOWN "sim/autopilot/heading_down"
#define ALTITUDE_SIM_UP "sim/autopilot/altitude_up"
#define ALTITUDE_SIM_DOWN "sim/autopilot/altitude_down"
#define AIRPSEED_SIM_UP "sim/autopilot/airspeed_up"
#define AIRSPEED_SIM_DOWN "sim/autopilot/airspeed_down"

#define MQTT_SIM_COMMAND_TOPIC "/xplane/meta/cmnd"
#define MQTT_SIM_DEVICE_TOPIC "/xplane/meta/device"
#define MQTT_SIM_VALUE_TOPIC "/xplane/rref/#"

#define MQTT_KEEPALIVE_INTERVAL_MS 15000

struct device {
  byte address;
  String name;
};

struct device devices[] = {
  {0x00, "althead"}
};

int numDevices = -1;

byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };

const char broker[] = "openhab.sugarland.lan";
int        port     = 1883;
const char topic[]  = MQTT_SIM_VALUE_TOPIC;

EthernetClient client;
char localIP[16];
MqttClient mqttClient(client);
unsigned long mqttLastKeepAlive = millis();

void onMqttMessage(int messageSize) {
//  // we received a message, print out the topic and contents
//  Serial.println("Received a message with topic '");
//  Serial.print(mqttClient.messageTopic());
//  Serial.print("', length ");
//  Serial.print(messageSize);
//  Serial.println(" bytes:");
//
//  // use the Stream interface to print the contents
//  while (mqttClient.available()) {
//    Serial.print((char)mqttClient.read());
//  }
//  Serial.println();
//
//  Serial.println();
}

void sendMqttMessage(char *topic, char *message) {
  mqttClient.beginMessage(topic);
  mqttClient.print(message);
  mqttClient.endMessage();
}

void setup() {
  // count devices for loop
  numDevices = sizeof(devices) / sizeof(device);

  // start the Ethernet connection:
  if (Ethernet.begin(mac) == 0) {
    Serial.println("Failed to configure Ethernet using DHCP");
    // no point in carrying on, so do nothing forevermore:
    while (1);
  }
  // print your local IP address:
  sprintf(localIP, "%d.%d.%d.%d", Ethernet.localIP()[0], Ethernet.localIP()[1], Ethernet.localIP()[2], Ethernet.localIP()[3]);
  Serial.println(localIP);

  // MQTT auth
  mqttClient.setUsernamePassword(MQTT_USER, MQTT_PASS);

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

    while (1);
  } else {
    Serial.println("MQTT connected");
    sendMqttMessage(MQTT_SIM_DEVICE_TOPIC, "Master online");
  }

  // subscribe to MQTT topic
  mqttClient.onMessage(onMqttMessage);
  mqttClient.subscribe(topic);
  mqttClient.setKeepAliveInterval(10 * 1000L);

  // start I2C
  Wire.begin();

  // for debugging
  pinMode(LED_TX, OUTPUT);
  digitalWrite(LED_TX, HIGH);
  pinMode(LED_RX, OUTPUT);
  digitalWrite(LED_RX, HIGH);

  // initialize devices
  for (int i = 0; i < numDevices; i++) {
    Wire.beginTransmission(devices[i].address);
    Wire.write(DATA_RESET_BYTE);
    Wire.endTransmission(devices[i].address);
  }
}

void loop() {
  // loop through devices and ask for data
  for (int i = 0; i < numDevices; i++) {
    Wire.requestFrom(devices[i].address, READ_BYTES_FROM_WIRE);
    while (Wire.available()) {
      // device has data, ask as long for data as it sends
      // if it never stops sending, we are fucked :)
      byte data = Wire.read();
      if (data != DATA_STOP_BYTE) {
        if (data & ALTITUDE_TRIGGER_BIT) {
          if (data & ALTITUDE_HIGH_BIT) {
            sendMqttMessage(MQTT_SIM_COMMAND_TOPIC, ALTITUDE_SIM_UP);
          } else {
            sendMqttMessage(MQTT_SIM_COMMAND_TOPIC, ALTITUDE_SIM_DOWN);
          }
        } else if (data & HEADING_TRIGGER_BIT) {
          if (data & HEADING_HIGH_BIT) {
            sendMqttMessage(MQTT_SIM_COMMAND_TOPIC, HEADING_SIM_UP);
          } else {
            sendMqttMessage(MQTT_SIM_COMMAND_TOPIC, HEADING_SIM_DOWN);
          }
        } else {
          // who are you?
        }
        Wire.requestFrom(devices[i].address, READ_BYTES_FROM_WIRE);
      }
    }
  }

  // transmit MQTT keepalive message if MQTT_KEEPALIVE_INTERVAL_MS is reached
  // or millis() is wrapping to 0
  if (mqttLastKeepAlive + MQTT_KEEPALIVE_INTERVAL_MS < millis() || mqttLastKeepAlive > millis()) {
    sendMqttMessage(MQTT_SIM_DEVICE_TOPIC, localIP);
    mqttLastKeepAlive = millis();
  }
}