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

#define HEADING_PORT A
#define HEADING_ADDR 0x20
#define HEADING_RESET 9
#define HEADING_CLK 0b00000001
#define HEADING_DT 0b00000010
#define HEADING_SW 0b00000100

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 lastHeadingInput;

MCP23017 mcp1 = MCP23017(HEADING_ADDR, HEADING_RESET);

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, HEADING_PORT);
  mcp1.setPortPullUp(0b00000000, HEADING_PORT);

  lastHeadingInput = mcp1.getPort(HEADING_PORT);
}

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

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 newHeadingInput = mcp1.getPort(HEADING_PORT);

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

  bool newHeadingClk = isBitDown(newHeadingInput, HEADING_CLK);
  bool lastHeadingClk = isBitDown(lastHeadingInput, HEADING_CLK);

  if (lastHeadingClk && !newHeadingClk) {
    mqttClient.beginMessage("/xplane/meta/cmnd");
    if (isBitDown(newHeadingInput, HEADING_DT)) {
      // CW
      mqttClient.print("sim/autopilot/heading_up");
    } else {
      // CCW
      mqttClient.print("sim/autopilot/heading_down");
    }
    mqttClient.endMessage();
  }

  lastHeadingInput = newHeadingInput;
}

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