#include "qt_pi_serial/serial_driver.h"

unsigned char com_buf[buf_size];
int com_buf_len;

int sig = 1;

pthread_t thread_id;
int fd;

unsigned char out_buf[buf_size];
int payload_len;
com_state state = IDLE;

pthread_cond_t data_wait_cond;
pthread_mutex_t data_wait_mutex;

struct timespec ts;

int open_port(const char *);
void *serialDataIn(void *);

void (*IMU_DATA_PUBLISH_CB)(unsigned char *) = NULL;

void set_CB(void (*pub)(unsigned char *)){
  IMU_DATA_PUBLISH_CB = pub;
}

int init_comm(const char *port) {

  if ((fd = open_port(port)) < 0)
    return -1;

  pthread_create(&thread_id, NULL, serialDataIn, NULL);

  pthread_mutex_init(&data_wait_mutex, NULL);
  pthread_cond_init(&data_wait_cond, NULL);

  return 0;
}

int open_port(const char *port) {
  int fd;

  fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
  if (fd == -1) {
    return -1;
  } else
    fcntl(fd, F_SETFL, 0);

  struct termios options;

  tcgetattr(fd, &options);

  cfsetispeed(&options, B115200);
  cfsetospeed(&options, B115200);

  options.c_cflag |= (CLOCAL | CREAD);
  options.c_cflag &= ~PARENB;
  options.c_cflag &= ~CSIZE;
  options.c_cflag &= ~CSTOPB;
  options.c_cflag |= CS8;
  options.c_cflag &= ~CRTSCTS;

  options.c_cc[VTIME] = 5;
  options.c_cc[VMIN] = 1;

  options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);

  options.c_iflag &= ~(IXON | IXOFF | IXANY);

  options.c_oflag &= ~OPOST;
  
  tcsetattr(fd, TCSANOW, &options);
  if (tcsetattr(fd, TCSAFLUSH, &options)) {
    close(fd);
    return -1;
  }

  return (fd);
}

void runRecvCom(uint16_t com_len, unsigned char *com) {

  switch (com[COMB]) {

  case GET_BAUDRATE:
  case GET_ENCODERS:
  case GET_PID_P:
  case GET_BATTERY_LVL:
  case EN_ODOM:
  case EN_IMU:
  case SET_PID_L:
  case SET_PID_R:
  case RESET_ENCODERS:
  case SET_SPEEDS:
    if (state != COM_SENT)
      return;

    pthread_mutex_lock(&data_wait_mutex);

    com_buf_len = com_len;
    memcpy(com_buf, com, com_len);
    state = COM_RECEIVED;

    pthread_cond_signal(&data_wait_cond);

    pthread_mutex_unlock(&data_wait_mutex);

    break;

  case IMU_DATA:

    IMU_DATA_PUBLISH_CB(com + DATASTRB);

    break;

  case ODOM_DATA:

    ODOM_DATA_PUBLISH(com + DATASTRB);

    break;

  default:
    
    com_buf_len = -1;
    
  }
}

void *serialDataIn(void *vargp) {

  enum PACKET_STATE {
    WAITING_FOR_NEW_COM,
    FIRST_BYTE_VERIFIED,
    SECOND_BYTE_VERIFIED,
    LENGTH_INPUT,
    READING_PAYLOAD,
    READING_COMPLETE,
    COM_RECEIVED
  };

  int inL = 0;

  unsigned char com_recv[25];
  unsigned char in_buf[25];
  unsigned char inChar;
  int in_buf_i = 0;
  uint16_t com_len;

  PACKET_STATE curState = WAITING_FOR_NEW_COM;
  pthread_yield();

  while (sig) {

    inL = read(fd, &inChar, 1);

    switch (curState) {

    case WAITING_FOR_NEW_COM:

      if (inChar == magic_start1) {
        curState = FIRST_BYTE_VERIFIED;
      }

      break;

    case FIRST_BYTE_VERIFIED:

      if (inChar == magic_start2) {
        curState = SECOND_BYTE_VERIFIED;
      } else {
        curState = WAITING_FOR_NEW_COM;
      }

      break;

    case SECOND_BYTE_VERIFIED:

      com_len = inChar;
      curState = LENGTH_INPUT;

      break;

    case LENGTH_INPUT:

      com_len = com_len | (inChar << 8);

      if (com_len < 20) {
        in_buf_i = 0;
        curState = READING_PAYLOAD;
      } else {
        curState = WAITING_FOR_NEW_COM;
      }

      break;

    case READING_PAYLOAD:

      in_buf[in_buf_i] = inChar;
      in_buf_i++;

      if (in_buf_i == com_len) {
        curState = READING_COMPLETE;
      }
      break;

    case READING_COMPLETE:

      in_buf_i = 0;

      if (inChar == magic_end) {

        memcpy(com_recv, in_buf, com_len);

        runRecvCom(com_len, com_recv);
      }

      curState = WAITING_FOR_NEW_COM;

      break;
    }
  }
}

int sendCom() {
  int data_len = com_buf_len - com_preamble_len;

  out_buf[MB1P] = magic_start1;
  out_buf[MB2P] = magic_start2;
  memcpy(out_buf + LB1P, &com_buf_len, len_len);
  out_buf[CIDBP] = com_buf[CIDB];
  out_buf[COMBP] = com_buf[COMB];
  memcpy(out_buf + DATASTRBP, com_buf + DATASTRB, data_len);
  out_buf[preamble_len + com_buf_len] = magic_end;

  pthread_mutex_lock(&data_wait_mutex);

  state = COM_SENT;

  write(fd, out_buf, net_extra_len + com_buf_len);

  clock_gettime(CLOCK_REALTIME, &ts);
  ts.tv_sec += 2;

  while (state != COM_RECEIVED) {
    if (pthread_cond_timedwait(&data_wait_cond, &data_wait_mutex, &ts) ==
        ETIMEDOUT) {

      pthread_mutex_unlock(&data_wait_mutex);
      state = IDLE;
      return 1;
    }
  }

  pthread_mutex_unlock(&data_wait_mutex);  

  state = IDLE;
  
  if(com_buf_len == -1)
    return -1;  

  return 0;
}

void close_comm() {
  sig = 0;
  pthread_yield();
  pthread_cancel(thread_id);
  pthread_join(thread_id, NULL);
  pthread_mutex_destroy(&data_wait_mutex);
  pthread_cond_destroy(&data_wait_cond);
  close(fd);
}
