#include "hw_impl_unix.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <pthread.h>
#include "iic_utils.h"

#define NUM_INPUTS 7

void * update_i2c_input_cache(void *);

union val {
  unsigned char b[2];
  unsigned short s;
};

static char *input_names[NUM_INPUTS];
static int fifos[NUM_INPUTS];
static union val cache[NUM_INPUTS];

static short last_dev;
static short last_reg;
static short last_val;

static int nums[] = {0, 1, 2, 3, 4, 5};
static pthread_t workers[NUM_INPUTS];

int unix_i2c_reset(struct I2CDriver *self) {
  input_names[0] = "i2c-mpu-accel-x";
  input_names[1] = "i2c-mpu-accel-y";
  input_names[2] = "i2c-mpu-accel-z";
  input_names[3] = "i2c-mpu-gryo-x";
  input_names[4] = "i2c-mpu-gryo-y";
  input_names[5] = "i2c-mpu-gyro-z";
  input_names[6] = "i2c-lidar";

  mkdir(VIRT_QUAD_FIFOS_DIR, 0777);

  // Start up worker thread whose job is to update the caches
  int i;
  for (i = 0; i < NUM_INPUTS; i += 1) {
    pthread_create(&workers[i], 0, update_i2c_input_cache, &nums[i]);
  }

  cache[0].s = 0;
  cache[1].s = 0;
  cache[2].s = 0;
  cache[3].s = 0;
  cache[4].s = 0;
  cache[5].s = 0;
  cache[6].s = 0;

  return 0;
}

int unix_i2c_write(struct I2CDriver *self,
                   unsigned short device_addr,
                   unsigned char *data,
                   unsigned int length) {
  if (length == 2) {
    last_dev = device_addr;
    last_reg = data[0];
    last_val = data[1];
  }
  return 0;
}

int unix_i2c_read(struct I2CDriver *self,
                  unsigned short device_addr,
                  unsigned char *buff,
                  unsigned int length) {
  if (last_dev != device_addr) {
    return -1;
  }

  switch (device_addr) {
  case MPU9150_DEVICE_ADDR:
    if (last_reg == ACCEL_GYRO_BASE_ADDR) {
      buff[0] = cache[0].b[0];
      buff[1] = cache[0].b[1];
      buff[2] = cache[1].b[0];
      buff[3] = cache[1].b[1];
      buff[4] = cache[2].b[0];
      buff[5] = cache[2].b[1];
      buff[6] = 0;
      buff[7] = 0;
      buff[8] = cache[3].b[0];
      buff[9] = cache[3].b[1];
      buff[10] = cache[4].b[0];
      buff[11] = cache[4].b[1];
      buff[12] = cache[5].b[0];
      buff[13] = cache[5].b[1];
    }
    else if (last_reg == LIDARLITE_DEVICE_ADDR) {
      buff[0] = cache[6].b[0];
      buff[1] = cache[6].b[0];
    }
    else {
      return -1;
    }
  }
  return 0;
}

void * update_i2c_input_cache(void *arg) {
  int *cache_index = arg;
  int i = *cache_index;
  char buff[16];

  // Setup FIFO
  unlink(input_names[i]);
  char fifoname[64];
  sprintf(fifoname, "%s/%s", VIRT_QUAD_FIFOS_DIR, input_names[i]);
  mkfifo(fifoname, 0666);
  fifos[i] = open(fifoname, O_RDONLY);

  // Block while waiting for reads
  while (1) {
    int bytes_read = read(fifos[i], buff, 15);
    if (bytes_read > 0) {
      buff[bytes_read] = '\0';
      unsigned long val = strtoll(buff, NULL, 10);
      cache[i].s = val;
      printf("%s: %ld\n", input_names[i], val);
    }
    pthread_yield();
  }
  return NULL;
}