#include "xmodem.h"
#include <termios.h>
#include <sys/mman.h>

#define X_STX 0x02
#define X_ACK 0x06
#define X_NAK 0x15
#define X_EOF 0x04

#define min(a, b) ((a) < (b) ? (a) : (b))

struct xmodem_chunk {
  uint8_t start;
  uint8_t block;
  uint8_t block_neg;
  uint8_t payload[1024];
  uint16_t crc;
} __attribute__((packed));

#define CRC_POLY 0x1021

int outputFd;

int writeAndIntercept(int fd, const void* buf, size_t count) {
  return write(fd, buf, count);
}

static uint16_t crc_update(uint16_t crc_in, int incr) {
  uint16_t xor = crc_in >> 15;
  uint16_t out = crc_in << 1;

  if (incr)
    out++;

  if (xor)
    out ^= CRC_POLY;

  return out;
}

static uint16_t crc16(const uint8_t* data, uint16_t size) {
  uint16_t crc, i;

  for (crc = 0; size > 0; size--, data++)
    for (i = 0x80; i; i >>= 1)
      crc = crc_update(crc, *data & i);

  for (i = 0; i < 16; i++)
    crc = crc_update(crc, 0);

  return crc;
}

static uint16_t swap16(uint16_t in) {
  return (in >> 8) | ((in & 0xff) << 8);
}

int xymodem_send(int serial_fd, const char* filename, int protocol, int wait) {
  size_t len;
  int ret, fd;
  uint8_t answer;
  struct stat stat;
  const uint8_t* buf;
  uint8_t eof = X_EOF;
  struct xmodem_chunk chunk;
  int skip_payload = 0;

  fd = open(filename, O_RDONLY);
  if (fd < 0) {
    perror("open");
    return -errno;
  }

  fstat(fd, &stat);
  len = stat.st_size;
  buf = mmap(NULL, len, PROT_READ, MAP_PRIVATE, fd, 0);
  if (!buf) {
    perror("mmap");
    return -errno;
  }

  if (wait) {
    printf("Waiting for receiver ping ...");
    fflush(stdout);

    do {
      ret = read(serial_fd, &answer, sizeof(answer));
      if (ret != sizeof(answer)) {
        perror("read");
        return -errno;
      }
    } while (answer != 'C');

    printf("done.\n");
  }

  printf("Sending %s ", filename);

  if (protocol == PROTOCOL_YMODEM) {
    strncpy((char*)chunk.payload, filename, sizeof(chunk.payload));
    chunk.block = 0;
    skip_payload = 1;
  } else {
    chunk.block = 1;
  }

  chunk.start = X_STX;

  while (len) {
    printf("block block: %d", chunk.block);
    size_t z = 0;
    int next = 0;
    char status;

    if (!skip_payload) {
      z = min(len, sizeof(chunk.payload));
      memcpy(chunk.payload, buf, z);
      memset(chunk.payload + z, 0x1a, sizeof(chunk.payload) - z);
    } else {
      skip_payload = 0;
    }

    chunk.crc = swap16(crc16(chunk.payload, sizeof(chunk.payload)));
    chunk.block_neg = 0xff - chunk.block;

    ret = writeAndIntercept(serial_fd, &chunk, sizeof(chunk));
    if (ret != sizeof(chunk))
      return -errno;

    ret = read(serial_fd, &answer, sizeof(answer));
    if (ret != sizeof(answer))
      return -errno;
    switch (answer) {
      case X_NAK:
        status = 'N';
        break;
      case X_ACK:
        status = '.';
        next = 1;
        break;
      default:
        status = answer;
        break;
    }

    printf("%c", status);
    fflush(stdout);

    if (next) {
      chunk.block++;
      len -= z;
      buf += z;
    }
  }

  ret = writeAndIntercept(serial_fd, &eof, sizeof(eof));
  if (ret != sizeof(eof))
    return -errno;

  /* send EOT again for YMODEM */
  if (protocol == PROTOCOL_YMODEM) {
    ret = writeAndIntercept(serial_fd, &eof, sizeof(eof));
    if (ret != sizeof(eof))
      return -errno;
  }

  printf("done.\n");

  return 0;
}

int open_serial(const char* path, int baud) {
  struct termios tty;

  int fd = open(path, O_RDWR | O_SYNC);
  if (fd < 0) {
    perror("open");
    return -errno;
  }

  memset(&tty, 0, sizeof(tty));
  if (tcgetattr(fd, &tty) != 0) {
    perror("tcgetattr");
    return -errno;
  }

  cfsetospeed(&tty, baud);
  cfsetispeed(&tty, baud);

  tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8;  // 8-bit chars
  tty.c_iflag &= ~IGNBRK;                      // disable break processing
  tty.c_lflag = 0;                             // no signaling chars, no echo,
                                               // no canonical processing
  tty.c_oflag = 0;                             // no remapping, no delays
  tty.c_cc[VMIN] = 1;                          // read doesn't block
  tty.c_cc[VTIME] = 5;                         // 0.5 seconds read timeout

  tty.c_iflag &= ~(IXON | IXOFF | IXANY);  // shut off xon/xoff ctrl

  tty.c_cflag |= (CLOCAL | CREAD);    // ignore modem controls,
                                      // enable reading
  tty.c_cflag &= ~(PARENB | PARODD);  // shut off parity
  tty.c_cflag &= ~CSTOPB;
  tty.c_cflag &= ~CRTSCTS;

  if (tcsetattr(fd, TCSANOW, &tty) != 0) {
    perror("tcsetattr");
    return -errno;
  }

  return fd;
}