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

#include "config.h"
#include "global.h"

#define READ_BYTES_FROM_WIRE 1

#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 AIRSPEED_SIM_UP "sim/autopilot/airspeed_up"
#define AIRSPEED_SIM_DOWN "sim/autopilot/airspeed_down"

#define MQTT_SIM_COMMAND_TOPIC "/xplane/meta/cmnd"
#define MQTT_LOG_TOPIC "/xplane/meta/log"
#define MQTT_SIM_VALUE_TOPIC "/xplane/rref/#"
#define MQTT_SIM_VALUE_SUBSCRIBE_TOPIC_PREFIX "/xplane/rref/"
#define MQTT_SIM_VALUE_SUBSCRIBE_TOPIC "/xplane/meta/rref"

#define MQTT_KEEPALIVE_INTERVAL_MS 15000

#define SLAVE_RESET_PIN 7

struct device {
  byte address;       // I2C address of device
  char *name;         // name of this device
  char name1[3];      // first name to send on init of device
  char *highCommand1; // command to use if TRIGGER_BIT_1 is set and HIGH_BIT_1 is set
  char *lowCommand1;  // command to use if TRIGGER_BIT_1 is set and HIGH_BIT_1 is not set
  int rrefRefresh1;   // refresh rate of subscribing rref
  char *rref1;        // rref to subscribe to
  char name2[3];      // second name to send on init of device
  char *highCommand2; // command to use if TRIGGER_BIT_2 is set and HIGH_BIT_2 is set
  char *lowCommand2;  // command to use if TRIGGER_BIT_2 is set and HIGH_BIT_2 is not set
  int rrefRefresh2;   // refresh rate of subscribing rref
  char *rref2;        // rref to subscribe to
};

struct device devices[] = {
  {
    0x08,
    "Heading Speed",
    {'H', 'D', 'G'},
    HEADING_SIM_UP,
    HEADING_SIM_DOWN,
    200,
    "sim/cockpit/autopilot/heading_mag",
    {'S', 'P', 'D'},
    AIRSPEED_SIM_UP,
    AIRSPEED_SIM_DOWN,
    200,
    "sim/cockpit/autopilot/airspeed"
  }
};
const int numDevices = sizeof(devices) / sizeof(device);

struct rrefSubscription {
  byte address;
  byte triggerBit;
};

struct rrefSubscription rrefSubscriptions[numDevices * 2];

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

MqttClient mqttClient(client);

void onMqttMessage(int messageSize) {
  Serial.println("Received MQTT message");
  
  // assume, that we always receive topic beginning with MQTT_SIM_VALUE_SUBSCRIBE_TOPIC_PREFIX
  // otherwise, this will _HORRIBLY_ fail

  char *rrefNoStr = &mqttClient.messageTopic()[13];
  int rrefNo = atoi(rrefNoStr);
  struct rrefSubscription rref = rrefSubscriptions[rrefNo - 1];

  Wire.beginTransmission(rref.address);
  Wire.write(DATA_BYTE);
  Wire.write(rref.triggerBit);
  Wire.write(messageSize);
  while (mqttClient.available()) {
    Wire.write((char)mqttClient.read());
  }
  Wire.endTransmission(rref.address);
}

void sendMqttMessage(char *topic, int n, ...) {
  mqttClient.beginMessage(topic);
  va_list args;
  va_start(args, n);
  for (int i = 0; i < n; i++) {
    mqttClient.print(va_arg(args, char*));
  }
  va_end(args);
  mqttClient.endMessage();
}

void sendI2CInit(struct device d) {
  sendMqttMessage(MQTT_LOG_TOPIC, 2, "Sending init to ", d.name);
  Wire.beginTransmission(d.address);
  Wire.write(DATA_RESET_BYTE);
  Wire.write(DATA_INIT_BYTE_1);
  Wire.write(d.name1, 3);
  Wire.write(DATA_INIT_BYTE_2);
  Wire.write(d.name2, 3);
  Wire.endTransmission(d.address);
}

void subscribeRrefs(struct device *d, int deviceNo) {
  char subscribeMsg[256];

  // subscribe no 1
  sprintf(subscribeMsg, "%03d %03d %s", deviceNo + 1, d->rrefRefresh1, d->rref1); // subscription no is index-1 based
  sendMqttMessage(MQTT_SIM_VALUE_SUBSCRIBE_TOPIC, 1, subscribeMsg);
  rrefSubscriptions[deviceNo * 2] = { d->address, TRIGGER_BIT_1 };
  
  // subscribe no 2
  sprintf(subscribeMsg, "%03d %03d %s", deviceNo + 2, d->rrefRefresh2, d->rref2); // subscription no is index-1 based
  sendMqttMessage(MQTT_SIM_VALUE_SUBSCRIBE_TOPIC, 1, subscribeMsg);
  rrefSubscriptions[(deviceNo * 2) + 1] = { d->address, TRIGGER_BIT_2 };
}

void setup() {
  // setup slave reset pin and pull it down for a reset
  pinMode(SLAVE_RESET_PIN, OUTPUT);
  digitalWrite(SLAVE_RESET_PIN, LOW);
  
  // 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:
  char localIP[16];
  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(MQTT_SERVER, MQTT_PORT)) {
    Serial.print("MQTT connection failed! Error code = ");
    Serial.println(mqttClient.connectError());

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

  // subscribe to MQTT topic
  mqttClient.onMessage(onMqttMessage);
  mqttClient.subscribe(MQTT_SIM_VALUE_TOPIC);
  mqttClient.setKeepAliveInterval(MQTT_KEEPALIVE_INTERVAL_MS);

  // switch on the slaves and give them a moment to boot
  digitalWrite(SLAVE_RESET_PIN, HIGH);
  delay(5000);

  // start I2C
  Wire.begin();
}

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) {
        // do not request further data
        continue;
      } else if (data == DATA_RESET_BYTE) {
        // device needs initialization
        sendMqttMessage(MQTT_LOG_TOPIC, 2, "got reset byte from ", devices[i].name);
        sendI2CInit(devices[i]);
        subscribeRrefs(&devices[i], i);
      } else {
        // handle as payload
        if (data & TRIGGER_BIT_1) {
          if (data & HIGH_BIT_1) {
            sendMqttMessage(MQTT_SIM_COMMAND_TOPIC, 1, devices[i].highCommand1);
          } else {
            sendMqttMessage(MQTT_SIM_COMMAND_TOPIC, 1, devices[i].lowCommand1);
          }
        } else if (data & TRIGGER_BIT_2) {
          if (data & HIGH_BIT_2) {
            sendMqttMessage(MQTT_SIM_COMMAND_TOPIC, 1, devices[i].highCommand2);
          } else {
            sendMqttMessage(MQTT_SIM_COMMAND_TOPIC, 1, devices[i].lowCommand2);
          }
        } else {
          // who are you?
        }
      }

      Wire.requestFrom(devices[i].address, READ_BYTES_FROM_WIRE);
    }
  }

  // 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();
}