#include <U8g2lib.h>

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

#define ROT_PORT_A A
#define ROT_ADDR_1 0x20

#define HEADING_CLK 0b00000001
#define HEADING_DT 0b00000010
#define HEADING_SW 0b00000100
#define SPEED_CLK 0b00100000
#define SPEED_DT 0b01000000
#define SPEED_SW 0b10000000


// https://github.com/olikraus/u8g2/wiki/fntgrpfreeuniversal

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

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

EthernetClient client;
MqttClient mqttClient(client);

byte lastRotInput_1A;

MCP23017 mcp1 = MCP23017(ROT_ADDR_1, 100);
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* clock=*/ SCL, /* data=*/ SDA, /* reset=*/ U8X8_PIN_NONE);  // High speed I2C

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
  }
  
  // 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:
    for(;;)
      ;
  }
  // print your local IP address:
  Serial.println(Ethernet.localIP());

  mqttClient.setUsernamePassword(MQTT_USER, 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");
  }

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

  Wire.begin();
  if(!mcp1.Init()){
    Serial.println("MCP not connected");
    while(1){} 
  } else {
    Serial.println("MCP connected");
  }
  
  mcp1.setPortMode(0b00000000, ROT_PORT_A);
  mcp1.setPortPullUp(0b00000000, ROT_PORT_A);

  lastRotInput_1A = mcp1.getPort(ROT_PORT_A);

  u8g2.begin();

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

bool isBitDown(byte input, byte reference) {
  return !(input & reference);
}

int getRotation(bool newClk, bool lastClk, bool newDt, bool lastDt) {
  if (lastClk && !newClk) {
    if (newDt) {
      // CW
      return 1;
    } else {
      // CCW
      return -1;
    }
  } else {
    return 0;
  }
}

void sendMqttByRotation(int rotation, char *topic, char *cw, char *ccw) {
  if (rotation != 0) {
    mqttClient.beginMessage(topic);
    if (rotation == 1) {
      mqttClient.print(cw);
    } else {
      mqttClient.print(ccw);
    }
    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();

  byte newRotInput_1A = mcp1.getPort(ROT_PORT_A);

  if (isBitDown(newRotInput_1A, HEADING_SW) && !isBitDown(lastRotInput_1A, HEADING_SW)) {
    mqttClient.beginMessage("/xplane/meta/cmnd");
    mqttClient.print("sim/autopilot/heading_push");
    mqttClient.endMessage();
  }

  int rotation = 0;

  bool newHeadingClk = isBitDown(newRotInput_1A, HEADING_CLK);
  bool lastHeadingClk = isBitDown(lastRotInput_1A, HEADING_CLK);
  bool newHeadingDt = isBitDown(newRotInput_1A, HEADING_DT);
  bool lastHeadingDt = isBitDown(lastRotInput_1A, HEADING_DT);
  rotation = getRotation(newHeadingClk, lastHeadingClk, newHeadingDt, lastHeadingDt);
  sendMqttByRotation(rotation, "/xplane/meta/cmnd", "sim/autopilot/heading_up", "sim/autopilot/heading_down");

  bool newSpeedClk = isBitDown(newRotInput_1A, SPEED_CLK);
  bool lastSpeedClk = isBitDown(lastRotInput_1A, SPEED_CLK);
  bool newSpeedDt = isBitDown(newRotInput_1A, SPEED_DT);
  bool lastSpeedDt = isBitDown(lastRotInput_1A, SPEED_DT);
  rotation = getRotation(newSpeedClk, lastSpeedClk, newSpeedDt, lastSpeedDt);
  sendMqttByRotation(rotation, "/xplane/meta/cmnd", "sim/autopilot/airspeed_up", "sim/autopilot/airspeed_down");

  lastRotInput_1A = newRotInput_1A;
}

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';
    }
  
    u8g2.clearBuffer();
    u8g2.setFont(u8g2_font_fub11_tf);
    u8g2.drawStr(0, 11, "HDG");
    u8g2.setFont(u8g2_font_fub42_tn);
    u8g2.drawStr(10, 63, buf);
    u8g2.sendBuffer();
  }

  // TODO: speed display

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