/*
 *  rcx_comm.c
 *
 *  RCX communication routines.
 *
 *  The contents of this file are subject to the Mozilla Public License
 *  Version 1.0 (the "License"); you may not use this file except in
 *  compliance with the License. You may obtain a copy of the License at
 *  http://www.mozilla.org/MPL/
 *
 *  Software distributed under the License is distributed on an "AS IS"
 *  basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
 *  License for the specific language governing rights and limitations
 *  under the License.
 *
 *  The Original Code is Firmdl code, released October 3, 1998.
 *
 *  The Initial Developer of the Original Code is Kekoa Proudfoot.
 *  Portions created by Kekoa Proudfoot are Copyright (C) 1998, 1999
 *  Kekoa Proudfoot. All Rights Reserved.
 *
 *  Contributor(s): Kekoa Proudfoot <kekoa@graphics.stanford.edu>
 */

/*  2002.04.01
 *
 *  Modifications to the original loader.c file in LegOS 0.2.4 include:
 *
 *  Hary D. Mahesan's update to support USB IR firmware downloading 
 *  using RCX 2.0's USB tower under WIN32 on Cygwin.
 *	<hdmahesa@engmail.uwaterloo.ca>
 *	<hmahesan@hotmail.com>
 *
 *  CVS inclusion, revision and modification by Paolo Masetti.
 *	<paolo.masetti@itlug.org>
 *
 */

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

#if defined(_WIN32)
  #include <windows.h>
#else
  #include <sys/ioctl.h>
  #include <errno.h>
#endif

#include "rcx_comm.h"

/* Defines */

#define BUFFERSIZE  4096

/* Globals */

int __comm_debug = 0;
extern int tty_usb; 

/* Timer routines */

typedef struct timeval timeval_t;

#define tvupdate(tv)  gettimeofday(tv,NULL)
#define tvsec(tv)     ((tv)->tv_sec)
#define tvmsec(tv)    ((tv)->tv_usec * 1e-3)

static float
timer_reset(timeval_t *timer)
{
    tvupdate(timer);
    return 0;
}

static float
timer_read(timeval_t *timer)
{
    timeval_t now;
    tvupdate(&now);
    return tvsec(&now) - tvsec(timer) + (tvmsec(&now) - tvmsec(timer)) * 1e-3;
}

void myperror(char *str) {
#if defined(_WIN32)
    fprintf(stderr, "Error %lu: %s\n", (unsigned long) GetLastError(), str);
#else
    perror(str);
#endif
}

/* Timeout read routine */

static int nbread (FILEDESCR fd, void *buf, int maxlen, int timeout)
{
    char *bufp = (char *)buf;
    int len = 0;
#ifndef _WIN32
   int count;
   fd_set fds;
   struct timeval tv;
#endif

    while (len < maxlen) {

#if defined(_WIN32)
      if(tty_usb) {
	  // USB Stuff here
	  // We can't use Serial timeouts.
	  DWORD count = 0;
	  struct timeval timebegin ,timenow;
	  unsigned long elapsed; // for timeout values

	  gettimeofday(&timebegin,0); 
	  while(count==0) {
		ReadFile( fd, &bufp[len], maxlen - len, &count, NULL);
		gettimeofday(&timenow,0);
		elapsed = (timenow.tv_sec - timebegin.tv_sec ) + (timenow.tv_usec - timebegin.tv_usec);				
		if(elapsed > timeout) 
			break;
	  }
	  if(count==0) {
		if(__comm_debug) 
			printf("Hary Mahesan - USB mode: nbread(len=%d, maxlen=%d) break...timed out\n", len, maxlen);
		break;
	  }
	  len += count; //update len
      } else {
	  // Serial port stuff now.
	  DWORD count = 0;
	  COMMTIMEOUTS CommTimeouts;

	  GetCommTimeouts (fd, &CommTimeouts);

        // Change the COMMTIMEOUTS structure settings.
        CommTimeouts.ReadIntervalTimeout = MAXDWORD;
        CommTimeouts.ReadTotalTimeoutMultiplier = 0;
        CommTimeouts.ReadTotalTimeoutConstant = timeout;
        CommTimeouts.WriteTotalTimeoutMultiplier = 10;
        CommTimeouts.WriteTotalTimeoutConstant = 1000;

        // Set the time-out parameters for all read and write operations
        // on the port.
        SetCommTimeouts(fd, &CommTimeouts);

        if (ReadFile(fd, &bufp[len], maxlen - len, &count, NULL) == FALSE) {
            myperror("ReadFile");
	      fprintf(stderr, "nb_read - error reading tty: %lu\n", (unsigned long) GetLastError());
	      exit(1);
	  }

        len += count;

        if (count == 0) {
		if(__comm_debug) 
			printf("Serial mode: nbread(len=%d, maxlen=%d) break...timed out\n", len, maxlen);
		break;
        }
	}
#else
     if (tty_usb == 1)
	 {
	    // LegoUSB doesn't work with select(), so just set a read
	    // timeout and then later check to see if the read timed out
	    // without reading data.

	    ioctl(fd, _IOW('u', 0xc8, int), timeout);
	 }
     else
	 {
		FD_ZERO(&fds);
		FD_SET(fd, &fds);

		tv.tv_sec = timeout / 1000;
		tv.tv_usec = (timeout % 1000) * 1000;

		if (select(fd+1, &fds, NULL, NULL, &tv) < 0) {
		    perror("select");
		    exit(1);
		}
		if (!FD_ISSET(fd, &fds))
		    break;
		}

		count = read(fd, &bufp[len], maxlen - len);

	    // If no data was read from a USB tower and the read() timed out,
		// go ahead and assume we are done reading data.
        if (tty_usb == 1 && count == -1 && errno == ETIMEDOUT) 
		{
			break;
		}
	
		if (count < 0) {
		    perror("read");
		    exit(1);
		}

        len += count;
#endif

    }

    return len;
}

/* discard all characters in the input queue of tty */
static void rx_flush(FILEDESCR fd)
{
#if defined(_WIN32)
    if (tty_usb == 0) {
	  PurgeComm(fd, PURGE_RXABORT | PURGE_RXCLEAR);
    } else {
      char echo[BUFFERSIZE];
      nbread(fd, echo, BUFFERSIZE, 200);
    }
#else
    char echo[BUFFERSIZE];
    nbread(fd, echo, BUFFERSIZE, 200);
#endif
}

int mywrite(FILEDESCR fd, const void *buf, size_t len) {
#if defined(_WIN32)
    DWORD nBytesWritten=0;
    WriteFile(fd, buf, len, &nBytesWritten, NULL);
    return nBytesWritten;
#else
   /* For usb tower, the driver, legousbtower, uses interrupt  */
   /* urb which can only carry up to 8 bytes at a time. To     */
   /* transmit more we have to check and send the rest.  It is */
   /* a good thing to check, so I'll make it general.          */

   int actual = 0;
   int rc;
   char * cptr;
   int retry = 1000;

   if (len < 1) return len;
   cptr = (char *) buf;
   while (actual < len) {
     rc = (long) write(fd, cptr+actual, len-actual);
     if (rc == -1) {
       if ((errno == EINTR) || (errno == EAGAIN))  {
         rc = 0;
         usleep(10);
         retry --;
       } else return -1;
     }
     actual += rc;
     if (retry < 1) return actual;
   }
   return len;
#endif
}

/* RCX routines */

FILEDESCR rcx_init(char *tty, int is_fast)
{
    FILEDESCR fd;

#if defined(_WIN32)
    DCB dcb;
#else
    struct termios ios;
#endif

    if (__comm_debug) printf("mode = %s\n", is_fast ? "fast" : "slow");

#if defined(_WIN32)
	// have windows platform I/O
    if ((fd = CreateFile(tty, GENERIC_READ | GENERIC_WRITE,
                              0, NULL, OPEN_EXISTING,
                              0, NULL)) == INVALID_HANDLE_VALUE) {
		fprintf(stderr, "Error %lu: Opening %s\n", (unsigned long) GetLastError(), tty);
		exit(1);
    }

    //These settings don't apply to the USB tower, so if{} them out.
    if(tty_usb==0) {
	// Serial settings
	FillMemory(&dcb, sizeof(dcb), 0);
	if (!GetCommState(fd, &dcb)) {	// get current DCB
        // Error in GetCommState
	  myperror("GetCommState");
        exit(1);
	} else {
	  dcb.ByteSize = 8;
	  dcb.Parity   = (is_fast ? 0 : 1);	// 0-4=no,odd,even,mark,space
	  dcb.StopBits = 0;			// 0,1,2 = 1, 1.5, 2
	  dcb.fBinary  = TRUE ;
	  dcb.fParity  = (is_fast ? FALSE : TRUE) ;
	  dcb.fAbortOnError = FALSE ;
	  dcb.BaudRate = (is_fast ? CBR_4800 : CBR_2400);	// Update DCB rate.

	  // Set new state.
	  if (!SetCommState(fd, &dcb)) {
	    // Error in SetCommState. Possibly a problem with the communications
          // port handle or a problem with the DCB structure itself.
	    myperror("SetCommState");
	    exit(1);
        }
	} // GetCommState
    } // usb

#else
	// have linux platform I/O
    if ((fd = open(tty, O_RDWR)) < 0) {
		fprintf(stderr,"ERROR: tty=%s, ",tty);
		perror("open");
		exit(1);
    }

    if (tty_usb == 0){
	    if (!isatty(fd)) {
			close(fd);
			fprintf(stderr, "%s: not a tty\n", tty);
			exit(1);
	    }

	    memset(&ios, 0, sizeof(ios));

	    ios.c_cflag = CREAD | CLOCAL | CS8 | (is_fast ? 0 : PARENB | PARODD);
	
	    cfsetispeed(&ios, is_fast ? B4800 : B2400);
	    cfsetospeed(&ios, is_fast ? B4800 : B2400);

	    if (tcsetattr(fd, TCSANOW, &ios) == -1) {
			perror("tcsetattr");
			exit(1);
	    }
    }
#endif

    return fd;
}

void rcx_close(FILEDESCR fd)
{
#if defined(_WIN32)
    CloseHandle(fd);
#else
    close(fd);
#endif
}

int rcx_wakeup_tower (FILEDESCR fd, int timeout)
{
    char msg[] = { 0x10, 0xfe, 0x10, 0xfe };
    char keepalive = 0xff;
    char buf[BUFFERSIZE];
    timeval_t timer;
    int count = 0;
    int len;

    // First, I send a KeepAlive Byte to settle IR Tower...
    mywrite(fd, &keepalive, 1);
    usleep(20000);
    rx_flush(fd);

    timer_reset(&timer);

    do {
	if (__comm_debug) {
	    printf("writelen = %d\n", sizeof(msg));
	    hexdump("W", msg, sizeof(msg));
	}
	if (mywrite(fd, msg, sizeof(msg)) != sizeof(msg)) {
	    myperror("write");
	    exit(1);
	}
	count += len = nbread(fd, buf, BUFFERSIZE, 50);
	if (len == sizeof(msg) && !memcmp(buf, msg, sizeof(msg)))
	    return RCX_OK; /* success */
	if (__comm_debug) {
	    printf("recvlen = %d\n", len);
	    hexdump("R", buf, len);
	}
	rx_flush(fd);
    } while (timer_read(&timer) < (float)timeout / 1000.0f);

    if (!count)
	return RCX_NO_TOWER; /* tower not responding */
    else
	return RCX_BAD_LINK; /* bad link */
}

/* Hexdump routine */

#define LINE_SIZE   16
#define GROUP_SIZE  4
#define UNPRINTABLE '.'

void hexdump(char *prefix, void *buf, int len)
{
    unsigned char *b = (unsigned char *)buf;
    int i, j, w;

    for (i = 0; i < len; i += w) {
        w = len - i;
        if (w > LINE_SIZE)
            w = LINE_SIZE;
	if (prefix)
	    printf("%s ", prefix);
        printf("%04x: ", i);
        for (j = 0; j < w; j++, b++) {
            printf("%02x ", *b);
            if ((j + 1) % GROUP_SIZE == 0)
                putchar(' ');
        }
        putchar('\n');
    }
}


int rcx_send (FILEDESCR fd, void *buf, int len, int use_comp)
{
    char *bufp = (char *)buf;
    char buflen = len;
    char msg[BUFFERSIZE];
    char echo[BUFFERSIZE];
    int msglen, echolen;
    int sum;
    int tty_usb_echo = 0; // USB do not echos

    /* Encode message */

    msglen = 0;
    sum = 0;

    if (use_comp) {
	msg[msglen++] = 0x55;
	msg[msglen++] = 0xff;
	msg[msglen++] = 0x00;
	while (buflen--) {
	    msg[msglen++] = *bufp;
	    msg[msglen++] = (~*bufp) & 0xff;
	    sum += *bufp++;
	}
	msg[msglen++] = sum;
	msg[msglen++] = ~sum;
    }
    else {
	msg[msglen++] = 0xff;
    	while (buflen--) {
	    msg[msglen++] = *bufp;
	    sum += *bufp++;
	}
	msg[msglen++] = sum;
    }

    /* Send message */

    if (mywrite(fd, msg, msglen) != msglen) {
	myperror("write");
	exit(1);
    }

    /* Receive echo */

    // USB Tower doesn't echo
    if(tty_usb == tty_usb_echo) {

      echolen = nbread(fd, echo, msglen, 100);

      if (__comm_debug) {
	  printf("msglen = %d, echolen = %d\n", msglen, echolen);
	  hexdump("C", echo, echolen);
      }

      /* Check echo */
      /* Ignore data, since rcx might send ack even if echo data is wrong */

      if (echolen != msglen /* || memcmp(echo, msg, msglen) */ ) {
	  /* Flush connection if echo is bad */
	  rx_flush(fd);
	  return RCX_BAD_ECHO;
      }
    } // USB

    return len;
}

int rcx_recv (FILEDESCR fd, void *buf, int maxlen, int timeout, int use_comp)
{
    char *bufp = (char *)buf;
    unsigned char msg[BUFFERSIZE];
    int msglen;
    int sum;
    int pos;
    int len;

    /* Receive message */

    msglen = nbread(fd, msg, BUFFERSIZE, timeout);

    if (__comm_debug) {
	printf("recvlen = %d\n", msglen);
	hexdump("R", msg, msglen);
    }

    /* Check for message */

    if (!msglen)
	return RCX_NO_RESPONSE;

    /* Verify message */

    if (use_comp) {
	if (msglen < 5 || (msglen - 3) % 2 != 0)
	    return RCX_BAD_RESPONSE;

	if (msg[0] != 0x55 || msg[1] != 0xff || msg[2] != 0x00)
	    return RCX_BAD_RESPONSE;

	for (sum = 0, len = 0, pos = 3; pos < msglen - 2; pos += 2) {
	    if (msg[pos] != ((~msg[pos+1]) & 0xff))
		return RCX_BAD_RESPONSE;
	    sum += msg[pos];
	    if (len < maxlen)
		bufp[len++] = msg[pos];
	}

	if (msg[pos] != ((~msg[pos+1]) & 0xff))
	    return RCX_BAD_RESPONSE;

	if (msg[pos] != (sum & 0xff))
	    return RCX_BAD_RESPONSE;

	/* Success */
	return len;
    }
    else {
	if (msglen < 4)
	    return RCX_BAD_RESPONSE;

	if (msg[0] != 0x55 || msg[1] != 0xff || msg[2] != 0x00)
	    return RCX_BAD_RESPONSE;

	for (sum = 0, len = 0, pos = 3; pos < msglen - 1; pos++) {
	    sum += msg[pos];
	    if (len < maxlen)
		bufp[len++] = msg[pos];
	}

	/* Return success if checksum matches */
	if (msg[pos] == (sum & 0xff))
	    return len;

	/* Failed.  Possibly a 0xff byte queued message? (unlock firmware) */
	for (sum = 0, len = 0, pos = 3; pos < msglen - 2; pos++) {
	  sum += msg[pos];
	  if (len < maxlen)
	    bufp[len++] = msg[pos];
	}

	/* Return success if checksum matches */
	if (msg[pos] == (sum & 0xff))
	  return len;

	/* Failed.  Possibly a long message? */
	/* Long message if opcode is complemented and checksum okay */
	/* If long message, checksum does not include opcode complement */
	for (sum = 0, len = 0, pos = 3; pos < msglen - 1; pos++) {
	    if (pos == 4) {
		if (msg[3] != ((~msg[4]) & 0xff))
		    return RCX_BAD_RESPONSE;
	    }
	    else {
		sum += msg[pos];
		if (len < maxlen)
		    bufp[len++] = msg[pos];
	    }
	}

	if (msg[pos] != (sum & 0xff))
	    return RCX_BAD_RESPONSE;

	/* Success */
	return len;
    }
}

int rcx_sendrecv (FILEDESCR fd, void *send, int slen, void *recv, int rlen,
		  int timeout, int retries, int use_comp)
{
    int status = 0;

    if (__comm_debug) printf("sendrecv %d:\n", slen);

    while (retries--) {
	if ((status = rcx_send(fd, send, slen, use_comp)) < 0) {
	    if (__comm_debug) printf("status = %s\n", rcx_strerror(status));
	    continue;
	}
	if ((status = rcx_recv(fd, recv, rlen, timeout, use_comp)) < 0) {
	    if (__comm_debug) printf("status = %s\n", rcx_strerror(status));
	    continue;
	}
	break;
    }

    if (__comm_debug) {
	if (status > 0)
	    printf("status = %s\n", rcx_strerror(0));
	else
	    printf("status = %s\n", rcx_strerror(status));
    }

    return status;
}

int rcx_is_alive (FILEDESCR fd, int use_comp)
{
    unsigned char send[1] = { 0x10 };
    unsigned char recv[1];

    return (rcx_sendrecv(fd, send, 1, recv, 1, 50, 5, use_comp) == 1);
}

char *rcx_strerror (int error)
{
    switch (error) {
    case RCX_OK: return "no error";
    case RCX_NO_TOWER: return "tower not responding";
    case RCX_BAD_LINK: return "bad ir link";
    case RCX_BAD_ECHO: return "bad ir echo";
    case RCX_NO_RESPONSE: return "no response from rcx";
    case RCX_BAD_RESPONSE: return "bad response from rcx";
    default: return "unknown error";
    }
}