#include <fcntl.h>
#include <pthread.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <qt_pi_serial/payload_specs.h>
#include <math.h>
int sig = 1;
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);
  usleep(2000000);
  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_cc[VTIME] = 5;
  options.c_cc[VMIN] = 1;

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

  if (tcsetattr(fd, TCSAFLUSH, &options)) {
    close(fd);
    return -1;
  }

  return (fd);
}

pthread_t thread_id;
int fd;

void *serialDataIn(void *);

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

  int16_t tmp[6];
  float imu[6];

  memcpy(tmp, in_buf, 2);
  memcpy(tmp+1, in_buf + 2, 2);
  memcpy(tmp+2, in_buf + 4, 2);

  memcpy(tmp+3, in_buf + 8, 2);
  memcpy(tmp+4, in_buf + 10, 2);
  memcpy(tmp+5, in_buf + 12, 2);

for(int i = 0; i < 3;i++)
{
    imu[i] = tmp[i] * 9.8 / 16384.0;
    imu[i + 3] = tmp[i + 3] * (M_PI / 180.0) / 131.0;
}

    printf("%f \t %f \t %f \t %f \t %f \t %f\n", imu[0], imu[1], imu[2], imu[3], imu[4], imu[5]);
      }

      curState = WAITING_FOR_NEW_COM;

      break;
    }
  }
}


int main(int argc, char **argv) {
  fd = open_port(argv[1]);
  if (fd < 0)
    return 0;

  pthread_create(&thread_id, NULL, serialDataIn, NULL);
  pthread_yield();
  int l = 8;
  unsigned char buffer[] = {0xee, 0xae, 0x03, 0x00, 0x59, 0x59, 0x01, 0xfc};

    int n = write(fd, buffer, l);
  char in_string[64];
  while (1) {

    fgets(in_string, 64, stdin);
    if (in_string[0] == 'q') {
      sig = 0;
      pthread_cancel(thread_id);
      close(fd);
      return 0;
    }

  }
}
