// file : datalog.c
//
// A program to upload datalog entries from an RCX brick.
// Compile using: gcc datalog.c -o datalog
// Set TTY to the serial device you want to use.

// An extensively modified version of Kekoa Proudfoot's 'send' program.
// D J Rees, 28/3/02

// Copyright (C) 1998, Kekoa Proudfoot.  All Rights Reserved.
// License to copy, use, and modify this software is granted provided that
// this notice is retained in any copies of any part of this software.
// The author makes no guarantee that this software will compile or
// function correctly.  Also, if you use this software, you do so at your
// own risk.  Some additional documentation is available at:
//   http://graphics.stanford.edu/~kekoa/rcx/tools.html
//
// Kekoa Proudfoot
// kekoa@graphics.stanford.edu
// 10/3/98

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdlib.h>
#include <unistd.h>
#include <termios.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <ctype.h>

#define LOGSIZE 500    /* max datalog size - could be bigger */
#define LOGCHUNK 48    /* must be less than 50 */

#define FALSE 0
#define TRUE 1

#define TTY "/dev/ttyS0"  /* linux - COM1 */

#define BUFFER_SIZE   4096
#define RETRIES      5

#define TIMEOUT 500000  /* response time-out in microsecs */

#define DUMP_LINE_SIZE   16
#define DUMP_GROUP_SIZE  4

// dump buffer in hex
void hexdump(unsigned char *buf, int len) {
  int i;
    
  for (i=0; i<len; i++) {
    if (i % DUMP_LINE_SIZE == 0) printf("\n%04x:", i);
    if (i % DUMP_GROUP_SIZE == 0) putchar(' ');
    printf(" %02x", *buf++);
  }
  printf("\n\n");
}

// initialise RCX brick
int rcx_init(char *tty) {
  int fd;
  struct termios ios;

  // open tty
  if ((fd = open(tty, O_RDWR)) < 0) {
    fprintf(stderr, "open %s failed\n", tty);
    exit(1);
  }

  // check it's a tty
  if (!isatty(fd)) {
    fprintf(stderr, "%s is not a tty\n", tty);
    exit(1);
  }

  // set terminal attributes
  memset(&ios, 0, sizeof(ios));    // clear termios structure
  ios.c_cflag = CREAD      /* enable receiver */
                | CLOCAL   /* ignore modem control lines */
                | CS8      /* character size mask */
                | PARENB   /* enable parity generation & checking */
                | PARODD;  /* odd parity */
  cfsetispeed(&ios, B2400);  // 2400 baud input
  cfsetospeed(&ios, B2400);  // 2400 baud output

  // set attributes NOW
  if (tcsetattr(fd, TCSANOW, &ios) == -1) {
    fprintf(stderr, "tcsetattr failed on %s\n", tty);
    exit(1);
  }

  return fd;
}

// close access to RCX brick
void rcx_close(int fd) {
  close(fd);
}

// transmit command to brick and get response
int rcx_retrans(int fd, unsigned char *sbuf, int slen,
                        unsigned char *rbuf, int rlen) {
  unsigned char tbuf[BUFFER_SIZE];
  unsigned char vbuf[BUFFER_SIZE];
  struct timeval tv;    // time value in secs and microsecs
  fd_set fds;           // set of descriptors
  int tlen, checksum, i, c;
  int vlen, vpos, rpos, count;

  // build message protocol
  tlen = 0;

  // header bytes
  tbuf[tlen++] = 0x55;
  tbuf[tlen++] = 0xff;
  tbuf[tlen++] = 0x00;

  // each char followed by its complement
  // checksum is sum of the chars
  checksum = 0;
  for (i=0; i<slen; i++) {
    c = *sbuf++;
    tbuf[tlen++] = c;
    tbuf[tlen++] = ~c;
    checksum = checksum+c;
  }
  tbuf[tlen++] = checksum;
  tbuf[tlen++] = ~checksum;

  // write prepared message to brick
  if (write(fd, tbuf, tlen) != tlen) {
    fprintf(stderr, "write to RCX brick failed");
    return -1;
  }

  // read any response within time-out into buffer
  vlen = 0;
  while (TRUE) {
    FD_ZERO(&fds);     // clear descriptor set
    FD_SET(fd, &fds);  // add tty descriptor to set
    tv.tv_sec = 0;     // set time-out to 300 millisecs
    tv.tv_usec = TIMEOUT;

    // wait until descriptor ready or time-out has elapsed
    if (select(FD_SETSIZE, &fds, NULL, NULL, &tv) == -1) {
      fprintf(stderr, "descriptor set select failed");
      exit(1);
    }

    // test if this descriptor ready
    if (!FD_ISSET(fd, &fds)) break;    // no response

    // read will not block since something ready
    if ((count = read(fd, &vbuf[vlen], BUFFER_SIZE-vlen)) == -1) {
      fprintf(stderr, "read response failed");
      exit(1);
    }
    vlen = vlen+count;
  }

  // Check first part of response is echo of message sent
  if (vlen < tlen) {
    fprintf(stderr, "invalid command echo");
    return -1;
  }
  for (vpos=0; vpos<tlen; vpos++) {
    if (tbuf[vpos] != vbuf[vpos]) {
      fprintf(stderr, "invalid command echo");
      return -1;
    }
  }

  // Check rest of response
  if (vpos == vlen) return 0;    // no response beyond echo - OK

  // check for valid extra response
  if (vlen-vpos < 5 || vbuf[vpos] != 0x55
      || vbuf[vpos+1] != 0xff || vbuf[vpos+2] != 0x00) {
    fprintf(stderr, "invalid response header");
    return -1;
  }
  vpos = vpos+3;

  // check rest of message
  checksum = 0;
  for (rpos = 0; vpos < vlen-2; rpos++) {
    if (vbuf[vpos] != ((~vbuf[vpos+1]) & 0xff)) {
      fprintf(stderr, "invalid response complements");
      return -1;
    }
    checksum = checksum+vbuf[vpos];
    if (rpos >= rlen) {
      fprintf(stderr, "response too long");
      return -1;
    }
    rbuf[rpos] = vbuf[vpos];
    vpos = vpos+2;
  }
  if (vbuf[vpos] != ((~vbuf[vpos+1]) & 0xff)) {
    fprintf(stderr, "invalid response checksum complement");
    return -1;
  }
  if ((checksum & 0xff) != vbuf[vpos]) {
    fprintf(stderr, "invalid response checksum");
    return -1;
  }

  // all OK
  return rpos;
}

// try command RETRY times
int rcx_trans(int fd, unsigned char *sbuf, int slen,
                      unsigned char *rbuf, int rlen) {
  int retry, len;

  for (retry=0; retry<RETRIES; retry++) {
    len = rcx_retrans(fd, sbuf, slen, rbuf, rlen);
    if (len >= 0) return len;
    fprintf(stderr, " - retrying\n");
  }
  fprintf(stderr, "command %02x failed\n", sbuf[0]);
  exit(1);
}

// main program to upload datalog entries
int main (int argc, char **argv) {
  unsigned char sendbuf[BUFFER_SIZE];
  unsigned char recvbuf[BUFFER_SIZE];
  int datalog[LOGSIZE];
  int fd, sendlen, len, logsize, l, m, n;

  if (argc == 1) {
    fprintf(stderr, "Need number of datalog entries to be uploaded\n");
    exit(1);
  }
  logsize = strtol(argv[1], NULL, 10);
  if (logsize > LOGSIZE) {
    fprintf(stderr, "Too many datalog entries requested\n");
    exit(1);
  }

  // open the serial port
  fd = rcx_init(TTY);

  // uncomment the following if the brick program
  // needs to be run to fill its datalog entries

  // clear command
  // sendbuf[0] = 0xd2;    // remote command
  // sendbuf[1] = 0x00;    // clear
  // sendbuf[2] = 0x00;
  // sendlen = 3;
  // len = rcx_trans(fd, sendbuf, sendlen, recvbuf, BUFFER_SIZE);

  // start command
  // sendbuf[0] = 0xd2;    // remote command
  // sendbuf[1] = 0x02;    // start program 1
  // sendbuf[2] = 0x00;
  // sendlen = 3;
  // len = rcx_trans(fd, sendbuf, sendlen, recvbuf, BUFFER_SIZE);

  // wait until all datalog entries generated
  // l = 0;
  // while (l <= logsize) {
  //   sleep(10);    // wait a bit
  //   sendbuf[0] = 0xa4;
  //   sendbuf[1] = 0;
  //   sendbuf[2] = 0;
  //   sendbuf[3] = 1;
  //   sendbuf[4] = 0;
  //   sendlen = 5;
  //   len = rcx_trans(fd, sendbuf, sendlen, recvbuf, BUFFER_SIZE);
  //   l = recvbuf[3]<<8 | recvbuf[2];
  // }

  // upload 'logsize' datalog points, LOGCHUNK points at a time
  for (l=0; l<logsize+1; l=l+LOGCHUNK) {
    sendbuf[0] = 0xa4;
    sendbuf[1] = l&0xff;
    sendbuf[2] = l>>8;
    if (l+LOGCHUNK >= logsize) n = logsize-l+1; else n = LOGCHUNK;
    sendbuf[3] = n;
    sendbuf[4] = 0;
    sendlen = 5;

    // send it and get any response
    len = rcx_trans(fd, sendbuf, sendlen, recvbuf, BUFFER_SIZE);
    hexdump(recvbuf, len);

    for (m=0; m<n; m++) {
      datalog[l+m] = recvbuf[3*m+3]<<8 | recvbuf[3*m+2];
    }
  }

  rcx_close(fd);

  // print out datalog values
  for (l=0; l<logsize+1; l++) {
    printf("%4x ", datalog[l]);
    if ((l+1)%DUMP_LINE_SIZE == 0) putchar('\n');
  }
  putchar('\n');

}