#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"
#include "ringbuffer.h"

#define IIC_DEVICE "/dev/i2c-7"
#define BUFFER_BLOCKS 128

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

struct ringBuffer i2cDataRead;
struct ringBuffer i2cDataWrite;

struct blockData {
  char data[DATA_SIZE];
  int triggerBit;
};

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

int tempCounter = 0;
int tempGauge = 0;
void* mqttHandler(void* arg) {
  struct blockData *block = malloc(sizeof(struct blockData));

  while (1) {
    sprintf(block->data, "%03d", tempCounter);
    block->triggerBit = TRIGGER_BIT_1;
    ringBufferWrite(&i2cDataWrite, block);
    tempCounter++;
    sleep(1);
  }

  free(block);
}

void* i2cHandler(void* arg) {
  int i2c, tmp, buzy;
  __s32 i2cResponse;
  struct blockData *block = malloc(sizeof(struct blockData));

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

  while (1) {
    buzy = 0;
    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);
        continue;
      }

      if (i2cResponse == DATA_RESET_BYTE) {
        sendI2CInit(i2c, devices[i]);
        buzy = 1;
      } else if (i2cResponse == DATA_STOP_BYTE) {
        // expect no forther data from device
        // send data to device
        do {
          tmp = ringBufferRead(&i2cDataWrite, block);
          if (tmp == 0) {
            // we got something to send
            sendI2CData(i2c, devices[i], block->triggerBit, block->data, 3);
            buzy = 1;
          }
        } while (tmp == 0);
        continue;
      } else {
        // real data
        _log("data: 0x%02x\n", i2cResponse);
        if (i2cResponse & TRIGGER_BIT_2) {
          if (i2cResponse & HIGH_BIT_2) {
            tempGauge++;
            if (tempGauge > 999) {
              tempGauge = 999;
            }
          } else {
            tempGauge--;
            if (tempGauge < 0) {
              tempGauge = 0;
            }
          }
          sprintf(block->data, "%03d", tempGauge);
          block->triggerBit = TRIGGER_BIT_2;
          ringBufferWrite(&i2cDataWrite, block);
        }
        buzy = 1;
      }
    }

    if (buzy == 0) {
      usleep(50 * 1000);
    }
  }

  close(i2c);
  free(block);
  pthread_exit(NULL);
}

int main() {
  pthread_t i2cThread, mqttThread;

  // create the ring buffers for i2c devices
  ringBufferCreate(BUFFER_BLOCKS, sizeof(struct blockData), &i2cDataRead);
  ringBufferCreate(BUFFER_BLOCKS, sizeof(struct blockData), &i2cDataWrite);

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

  // start and join threads
  pthread_create(&i2cThread, NULL, i2cHandler, NULL);
  pthread_create(&mqttThread, NULL, mqttHandler, NULL);
  pthread_join(i2cThread, NULL);
  pthread_join(mqttThread, NULL);

  // free resources
  free(rrefSubscriptions);
  ringBufferDestroy(&i2cDataRead);
  ringBufferDestroy(&i2cDataWrite);

  return 0;
}