#include <Wire.h>

#define READ_BYTES_FROM_WIRE 1

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

struct device {
  byte address;
  String name;
};

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

int numDevices = -1;

void setup() {
  numDevices = sizeof(devices) / sizeof(device);
  
  Wire.begin();

  pinMode(LED_TX, OUTPUT);
  digitalWrite(LED_TX, HIGH);
  pinMode(LED_RX, OUTPUT);
  digitalWrite(LED_RX, HIGH);
}

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) {
            Serial.println("Altitude up");
          } else {
            Serial.println("Altitude down");
          }
        } else if (data & HEADING_TRIGGER_BIT) {
          if (data & HEADING_HIGH_BIT) {
            Serial.println("Heading right");
          } else {
            Serial.println("Heading left");
          }
        } else {
          // who are you?
        }
        Wire.requestFrom(devices[i].address, READ_BYTES_FROM_WIRE);
      }
    }
  }
}