/* comm.nqh
 * version 0.0
 *
 * Written by Rachel Gockley <rgockley@andrew.cmu.edu>. This code
 * is provided free-of-charge to anyone who wants it. Mess with it
 * all you want, but please leave this notice intact.
 * 
 * Oh, and if you do use this code, let me know at the above email
 * address, okay? Thanks.
 * 
 * Copyright 2003, Rachel Gockley.
 */

#ifndef _COMM_NQH_
#define _COMM_NQH_

/* An RCX timer required for message timing. Do not try to use
 * this timer in any of the rest of your code!
 */
#define MESSAGE_TIMER 0

/* The packet protocol is simple: send STARTBYTE followed by the
 * bytes you wish to send (the payload), followed by STOPBYTE.
 * All messages are responded to with either ACK or NCK, in packet
 * form (with the exception of ACK and NCK themselves, which get 
 * no response).
 */

#define STARTBYTE 254
#define STOPBYTE  255

/* Highest message byte that can be sent */
#define HIGHEST   253

/* The maximum packet length. */
#define MAX_LENGTH 4

/* A timeout on reads, in 10s of milliseconds. After sending a message,
 * and while waiting to receive the last byte of the packet, the bot
 * will wait this long before giving up. (50 is half a second.)
 */
#define TIMEOUT 50

/* These are possible responses to common packets. */

#define ACK  1  /* acknowledgement; the receiving 'bot understood */
#define NCK  2  /* non-acknowledgement; the packet didn't arrive or the 
		 * receiving 'bot couldn't parse it */

/* Redefine your own messages here. Specify only numbers from
 * 1 through 253, inclusive. (Message 0 is discouraged; 254 and
 * 255 are used in the packet protocol.)
 */

#define GO   3  /* tells the other 'bot to start moving */
#define STOP 4  /* tells the other 'bot to stop moving */
#define PING 5  /* just make sure the other guy is still alive and in range */
#define FWD  6  /* moving forward */
#define REV  7  /* moving backward */

/* Error conditions. */

#define GOT_NCK   -1 /* received a NCK */
#define NO_RSP    -2 /* did not get a response from the other robot */
#define PARTIAL   -3 /* timed out after receiving part of a packet */
#define WRONG_RSP -4 /* received something other than an ACK or NCK */
#define TOO_LONG  -5 /* message length is longer than MAX_LENGTH */

/* A buffer for the most recently received packet. */
int packet_buffer[MAX_LENGTH];

/* The number of bytes received in the last packet. */
int packetlen;

/* Whether something went wrong in the last transmission. Set to 0
 * if all went well.
 */
int error = 0;

/* Send a single byte message (in packet form). */
void SendMessagePacket (const int & message) {
  int msg = message;

  /* this makes sure that the message is a positive number,
   * from 0 to 256.
   */
  msg = (msg + 256) % 256;
  
  /* however, we can't send 0 or above 253, so we simply
   * don't send anything. 
   */
  if (msg == 0 || msg > HIGHEST)
    return;

  SendByte(STARTBYTE);
  SendByte(msg);
  SendByte(STOPBYTE);
}

/* Send an "encoder" packet. The idea is to send four bytes in each packet:
 * the direction of each wheel, and the change in encoder values since the
 * last packet was sent.
 *
 * If you want other specialized multi-byte messages, model them after this
 * function.
 */
void SendEncoderPacket (const int & ldir, 
			const int & lchange,
			const int & rdir,
			const int & rchange) {
  /* Error checking: make sure that ldir and rdir are both either
   * FWD or REV, and that lchange and rchange are both between 0 and
   * 253.
   */
  if ((ldir != FWD && ldir != REV) || (rdir != FWD && rdir != REV) ||
      (lchange < 1) || (lchange > HIGHEST) || (rchange < 1) || (rchange > HIGHEST))
    return;

  SendByte(STARTBYTE);
  SendByte(ldir);
  SendByte(lchange);
  SendByte(rdir);
  SendByte(rchange);
  SendByte(STOPBYTE);
}

/* Send an acknowledgement. This is the same as calling SendMessagePacket(ACK). */
void SendACK() {
  SendMessagePacket(ACK);
}

/* Send a non-acknowledgement. This is the same as calling SendMessagePacket(NCK). */
void SendNCK() {
  SendMessagePacket(NCK);
}

/* Look for an acknowledgement. This function should be called after any
 * Send*Packet function.
 */
void LookForACK() {
  int msg;

  ReadByte(msg);

  /* might have gotten a stray message. Keep reading until the start of
   * a packet, or a timeout.
   */
  while (msg != STARTBYTE) {
    if (msg == 0) {
      error = NO_RSP;
      return;
    }
    ReadByte(msg);
  }

  /* this is the message - should be ACK */
  ReadByte(msg);

  /* did we really get an ACK or NCK? */
  if (msg != ACK && msg != NCK) {
    error = WRONG_RSP;
    return;
  }

  if (msg == NCK)
    error = GOT_NCK;
  else
    error = 0;  /* so far, so good! */

  /* look for the stop byte */
  ReadByte(msg);

  /* make sure we got the stop byte! */
  if (msg != STOPBYTE) {
    if (msg == 0)
      error = PARTIAL;
    else
      error = WRONG_RSP;
  }

  /* that's it! */
}

/* Read the next incoming packet. */
void ReadPacket() {
  int byte;
  int i;

  error = 0;
  packetlen = 0;
  
  /* zero out the packet buffer */
  for (i = 0; i < MAX_LENGTH; i++)
    packet_buffer[i] = 0;

  /* look for start byte */
  ReadByte(byte);
  while (byte != STARTBYTE) {
    if (byte == 0) {
      /* timeout case */
      error = NO_RSP;
      return;
    }
    ReadByte(byte);
  }

  /* read until a timeout or the stop byte is received */
  ReadByte(byte);
  while (byte != STOPBYTE) {
    if (byte == 0) {
      /* timeout */
      error = PARTIAL;
      return;
    }

    if (packetlen > MAX_LENGTH) {
      error = TOO_LONG;
      return;
    }

    packet_buffer[packetlen] = byte;
    packetlen++;

    ReadByte(byte);
  }

  /* either by now we've timed out and returned, or just got a stop byte.
   * in either case, nothing more to do!
   */
}

/* Read a single byte from the IR receiver. If no messages are available,
 * keep trying until TIMEOUT time is reached.
 *
 * Assumes that the last byte read has been cleared from the buffer.
 */
void ReadByte(int & byte) {
  ClearTimer(MESSAGE_TIMER);
  byte = Message();
  while (byte == 0) {
    /* if there was no message, then Message() returns 0. Wait until
     * we reach the timeout before giving up.
     */
    if (FastTimer(MESSAGE_TIMER) > TIMEOUT)
      return;

    Wait(1);
    byte = Message();
  }
  ClearMessage();
}

/* Sends a single byte, and waits briefly (because sending too many
 * bytes at a time makes the other RCX VERY likely to miss some).
 */
void SendByte(const int & byte) {
  SendMessage(byte);
  Wait(5);
}

#endif