#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 <stdlib.h>
#include <pthread.h>

#include "global.h"

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

#define ERROR_IIC_OPEN "open failed"

pthread_mutex_t i2cLock;

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"
  }
};
const int numDevices = sizeof(devices) / sizeof(struct device);
struct rrefSubscription *rrefSubscriptions;

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

int setI2CAddress(int i2c, 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(int i2c, 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(i2c, d.address);
  i2c_smbus_write_block_data(i2c, 0, bufLen, buf);
}

void sendI2CData(int i2c, 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(i2c, d.address);
  i2c_smbus_write_block_data(i2c, 0, bufLen, buf);
}

void* i2cSend(void* arg) {
  int i2c;
}

void* i2cReceive(void* arg) {
  int i2c;
  __s32 i2cResponse;

  i2c = *((int*)arg);

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

      if (i2cResponse == DATA_RESET_BYTE) {
        sendI2CInit(i2c, devices[i]);
        sendI2CData(i2c, devices[i], TRIGGER_BIT_1, "111", 3);
        sendI2CData(i2c, devices[i], TRIGGER_BIT_2, "222", 3);
      } else if (i2cResponse == DATA_STOP_BYTE) {
        // expect no forther data from device
        usleep(50 * 1000);
        continue;
      } else {
        // real data
        _log("data: 0x%02x\n", i2cResponse);
        /*
        XXX: put data into cache and handle with another core / loop / fork / whatever
        */
        if (i2cResponse & TRIGGER_BIT_1) {
          if (i2cResponse & HIGH_BIT_1) {
            sendI2CData(i2c, devices[i], TRIGGER_BIT_1, "+1+", 3);
          } else {
            sendI2CData(i2c, devices[i], TRIGGER_BIT_1, "-1-", 3);
          }
        }

        if (i2cResponse & TRIGGER_BIT_2) {
          if (i2cResponse & HIGH_BIT_2) {
            sendI2CData(i2c, devices[i], TRIGGER_BIT_2, "+2+", 3);
          } else {
            sendI2CData(i2c, devices[i], TRIGGER_BIT_2, "-2-", 3);
          }
        }
      }
    }
  }

  close(i2c);
  pthread_exit(NULL);
}

int main() {
  int i2c;
  pthread_t i2cReceiver;

  rrefSubscriptions = calloc(sizeof(struct device), numDevices);

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

  pthread_mutex_init(&i2cLock, NULL);

  pthread_create(&i2cReceiver, NULL, i2cReceive, &i2c);
  pthread_join(i2cReceiver, NULL);

  pthread_mutex_destroy(&i2cLock);

  return 0;
}