#include <linux/i2c-dev.h>
#include <i2c/smbus.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <stdint.h>
#include <stdarg.h>

#include "global.h"

#define I2C_DEVICE "/dev/i2c-7"

#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"

int i2c;

struct device {
  uint8_t 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 rrefSubscription {
  uint8_t address;
  uint8_t triggerBit;
};

const 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"
  }
};

struct rrefSubscription rrefSubscriptions[2];

void _log(char *format, ...) {
  va_list args;
  va_start(args, format);
  vprintf(format, args);
  va_end(args);
}

int setI2CAddress(uint8_t address) {
  if (ioctl(i2c, I2C_SLAVE, address) < 0) {
    _log("setting periphal address 0x%02X failed\n", address);
    return 0;
  } else {
    return 1;
  }
}

void sendI2CInit(struct device d) {
  _log("Sending init to %s\n", d.name);
  
  int bufLen = 1 + 1 + sizeof(d.name1) + 1 + sizeof(d.name2);
  char buf[bufLen];
  int i = 0;
  buf[i++] = DATA_RESET_BYTE;
  buf[i++] = DATA_INIT_BYTE_1;
  buf[i++] = d.name1[0];
  buf[i++] = d.name1[1];
  buf[i++] = d.name1[2];
  buf[i++] = DATA_INIT_BYTE_2;
  buf[i++] = d.name2[0];
  buf[i++] = d.name2[1];
  buf[i++] = d.name2[2];

  setI2CAddress(d.address);
  i2c_smbus_write_block_data(i2c, 0, bufLen, buf);
}

void sendI2CData(struct device d, uint8_t triggerBit, char *data, uint8_t dataLen) {
  int bufLen = 1 + 1 + 1 + dataLen;
  char buf[bufLen];
  int i = 0;
  buf[i++] = DATA_BYTE;
  buf[i++] = triggerBit;
  buf[i++] = dataLen;
  for (int j = 0; j < dataLen; j++) {
    buf[i++] = data[j];
  }

  setI2CAddress(d.address);
  i2c_smbus_write_block_data(i2c, 0, bufLen, buf);
}

int main() {
  __s32 i2cResponse;

  i2c = open(I2C_DEVICE, O_RDWR);
  if (i2c < 0) {
    _log("Unable to open I2C device %s\n", I2C_DEVICE);
    return 1;
  }

  while (1) {
    int numDevices = sizeof(devices) / sizeof(struct device);
    for (int i = 0; i < numDevices; i++) {
      setI2CAddress(devices[i].address);
      i2cResponse = i2c_smbus_read_byte_data(i2c, 0);
      if (i2cResponse < 0) {
        _log("I2C read failed: %s (%d)\n", strerror(errno), errno);
      }

      if (i2cResponse == DATA_RESET_BYTE) {
        sendI2CInit(devices[i]);
        sendI2CData(devices[i], TRIGGER_BIT_1, "xxx", 3);
        sendI2CData(devices[i], TRIGGER_BIT_2, "xxx", 3);
      } else if (i2cResponse == DATA_STOP_BYTE) {
        // expect no forther data from device
        continue;
      } else {
        // real data
        _log("data: 0x%02x\n", i2cResponse);
      }
    }
    usleep(10 * 1000);
  }

  close(i2c);
}