Colony

/** @file colonet_wireless.cpp 

 *

 * @brief Implementation of server-side colonet wireless library

 *

 * @author Eugene Marinelli

 *

 * @bug Only reads when gtkterm is open

 * @bug Fails to tokenize

 * @bug Deconstructor not implemented

 * @bug Nonsense initializations in constructor

 */

/********************************* Includes **********************************/

#include <string>

#include <pthread.h>

#include <iostream>

#include <fstream>

#include <unistd.h>

#include <fcntl.h>

#include "colonet_wireless.h"

/******************************* Definitions *********************************/

#define TIMEOUT_MAX 100

#define WL_SEND_DELAY 5 //microseconds

//for debug printouts

#define DEBUG 1

#ifdef DEBUG

#define dbg_printf(...) printf(__VA_ARGS__)

#define dbg_fprintf(...) fprintf(__VA_ARGS__)

#else

#define dbg_printf(...)

#define dbg_fprintf(...)

#endif

typedef struct {

  MsgHandlerFunction handler;

  ColonetWireless* cw;

} ListenerArgs;

/************************* Internal prototypes *******************************/

static void* listen(void* args);

/**************************** Public functions *******************************/

ColonetWireless::ColonetWireless(char* wl_port, 

                                 MsgHandlerFunction message_handler_, 

                                 char* log_filename_, bool join_token_ring_,

                                 bool ignore_token_)

{

  have_token = false;

  first_packet_sent = false;

  //FIX THESE INITIALIZATIONS

  token_src = 255;

  token_dest = 255;

  num_robots = 0;

  connected = false;

  ignore_token = ignore_token_;

  sprintf(wireless_port, wl_port);

  if (log_filename) {

    logging_enabled = true;

    sprintf(log_filename, log_filename_);

  }

  join_token_ring = join_token_ring_;

  message_handler = message_handler_;

}

ColonetWireless::~ColonetWireless()

{

  //kill listener thread

  //free packets in queues

}

int ColonetWireless::handle_packet(ColonetPacket* pkt)

{

  if (logging_enabled) {

    /* Log the packet */

    log_packet(pkt);

  }

  /* Update what we know about the network based on the packet */

  update_network_props(pkt);

  /* Received a message */

  if (message_handler(pkt) < 0) {

    fprintf(stderr, "%s: message_handler failed\n", __FUNCTION__);

    return -1;

  }

  if ((unsigned char) pkt->token_dest == WL_ERROR_DEST) {

    /* token_dest is an error - handle it */

    if (handle_error(pkt) < 0) {

      fprintf(stderr, "%s: handle_error failed\n", __FUNCTION__);

      return -1;

    }

  } else if(join_token_ring && !connected) {

    /* We want to join the token ring but we are not connected - join robot 

     * network */

    dbg_printf("%s: Trying to join robot network...\n", __FUNCTION__);

    if (join_network(pkt) < 0) {

      dbg_fprintf(stderr, "%s: join_network failed\n", __FUNCTION__);

      return -1;

    }

  } else {

    if (pkt->token_dest == token_src && join_token_ring && !ignore_token) {

      /* Received the token - send queued message(s) */

      dbg_printf("%s: token received\n", __FUNCTION__);

      have_token = true;

      if (transmit_queued_message() < 0) {

        fprintf(stderr, "%s: transmit_queued_message failed\n", __FUNCTION__);

        return -1;

      } else {

        if (!first_packet_sent) {

          dbg_printf("First packet sent.\n");

          first_packet_sent = true;

        }

      }

    }

  }

  return 0;

}

pthread_t* ColonetWireless::run_listener_thread()

{

  // pthread_t listener_thread;

  pthread_t* listener_thread = (pthread_t*)malloc(sizeof(pthread_t));

  dbg_printf("Spawning listener thread...\n");

  ListenerArgs* args = new ListenerArgs;

  args->cw = this;

  if (pthread_create(listener_thread, NULL, listen, args) != 0) {

    perror("pthread_create");

    return NULL;

  }

  return listener_thread;

}

int ColonetWireless::send(unsigned char msg_dest, unsigned char msg_type, 

                          unsigned char msg_code, unsigned char* data)

{

  /* Create a new packet it and queue it to be sent */

  ColonetPacket* newpkt = create_packet(token_src, token_dest, msg_dest, 

                                        msg_type, msg_code, data, num_robots);

  if(newpkt == NULL){

    fprintf(stderr, "Error: %s - create_packet failed\n", __FUNCTION__);

    return -1;

  }

  if (ignore_token) {

    send_packet(newpkt);

  } else {

    outbox.push(newpkt);                

  }

  return 0;

}

void ColonetWireless::clear_message_queue(void)

{

  while (!outbox.empty()) {

    outbox.pop();

  }

}

/**************************** Private functions ******************************/

/** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.

 * Listens for bytes on the wireless port and constructs packets based on them.

 * Not part of the ColonetWireless class since a function pointer must be

 * passed in starting the thread that runs it (tricky or impossible if it's 

 * a class function).

 *

 * @param args Pointer to arguments.  Can be safely casted to ListenerArgs type

 * @return NULL

 */

static void* listen(void* args)

{

  int read_len;

  int portfd;

  unsigned char inbuf[128];

  int no_packet_timeout_count = 0;

  ColonetPacket* in_pkt;

  /* Extract arguments from args */

  ColonetWireless* cw = ((ListenerArgs*)args)->cw;

  /* Open connection to wireless port */

  if((portfd = open(cw->wireless_port, O_RDWR)) < 0){

    perror("open");

    pthread_exit(NULL);

  }

  /* loop waiting for messages */

  while(1){

    /* Read in some bytes (blocks until bytes are read) */

    read_len = read(portfd, inbuf, 3);

    if(read_len > 0){

      /* Read in some bytes - this will always be true if portfd is blocking */

      /* parse the packet or part of packet */

      in_pkt = cw->build_packet(inbuf, read_len);

      if(in_pkt == NULL){

        /* Packet is not ready yet - go back and read some more bytes */

        continue;

      }else{

        /* Packet is complete */

        /* close fd so that we can safely write to it in the handler */

        close(portfd);

        if (cw->handle_packet(in_pkt)) {

          fprintf(stderr, "%s: Error - handle_packet failed\n", __FUNCTION__);

        }

        /* reopen wireless port */

        if(!(portfd = open(cw->wireless_port, O_RDWR))){

          perror("open");

          pthread_exit(NULL);

        }

      }

    }else if(read_len == 0){

      // Note - this won't actually happen if read blocks

      if(no_packet_timeout_count++ > TIMEOUT_MAX){

        //Nothing received for a while - try to start a new token ring

        dbg_printf("Attempting to start a token ring...\n");

        cw->token_src = 0;

        cw->token_dest = 1;

        cw->num_robots = 0;

        ColonetPacket* tmp_pkt = cw->create_packet(cw->token_src, 

                                                   cw->token_dest, 0, 0, 0, 

                                                   (unsigned char*) "",

                                                   cw->num_robots);

        cw->send_packet(tmp_pkt);

        free(tmp_pkt);

      }

    }else{ //(read_len < 0)

      //Error

      perror("read");

    }

  }

  pthread_exit(NULL);

  return NULL;

}

/** @brief Takes some number of bytes and tries to build a packet with them

 * State of the function is saved between callings, so parts of packets 

 * can be provided as available

 *

 * @param buf Data buffer 

 * @param len Number of bytes to build with

 */

ColonetPacket* ColonetWireless::build_packet(unsigned char* buf, int len)

{

  static unsigned char packet_buf[PACKET_SIZE];

  static int buf_pos = 0;

  /* If this is pointer is returned - will either point to NULL or a complete

   * packet */

  ColonetPacket* complete_packet = NULL;

  if (buf_pos >= PACKET_SIZE) {

    buf_pos = 0;

  }

  for (int i = 0; i < len; i++) {

    dbg_printf("%d ", buf[i]);

    switch(buf_pos){

    case 0: //prefix0

      if(buf[i] == 'R'){

        memset(packet_buf, 0, PACKET_SIZE);

        packet_buf[0] = 'R';

        buf_pos = 1;

      }

      break;

    case 1: //prefix1

      if(buf[i] == 'C'){

        packet_buf[1] = 'C';

        buf_pos = 2;

      }else{

        buf_pos = 0;

      }

      break;

    default: 

      /* anything except prefix0, prefix1, or checksum */

      /* Check for R,C - in this case, very likely that we missed a byte */

      if (i > 0 && buf[i] == 'C' && buf[i-1] == 'R') {

        packet_buf[0] = 'R';

        packet_buf[1] = 'C';

        buf_pos = 2;

      } else {

        packet_buf[buf_pos] = buf[i];

        buf_pos++;

      }

      break;

    case (PACKET_SIZE-1):

      /* At checksum location */

      buf_pos = 0;

      packet_buf[15] = buf[i];

      /* Compute desired checksum */

      unsigned char sum = 0;

      for(int j = 2; j < PACKET_SIZE; j++){

        sum += packet_buf[j];

      }

      if(!sum){

        //Checksum passed

        dbg_printf("%s: checksum passed\n", __FUNCTION__);

        complete_packet = create_packet(packet_buf[2], packet_buf[3], 

                                        packet_buf[4], packet_buf[5], 

                                        packet_buf[6], (packet_buf+7), 

                                        packet_buf[PACKET_SIZE-2]);

      }else{

        dbg_printf("%s: checksum failed\n", __FUNCTION__);

      }

      break;

    }

  }

  return complete_packet;

}

void ColonetWireless::update_network_props(ColonetPacket* pkt)

{

  if(first_packet_sent && pkt->token_dest == token_dest){

    /* Another robot is sending to our dest - therefore the network

     * doesn't know about us yet.  Try to register again. */

    dbg_printf("Disconnected from network\n");

    dbg_printf("%s: failed to register in network; trying again...\n", 

               __FUNCTION__);

    connected = false;

  }

  if(num_robots < pkt->num_robots){    

    if(token_dest == 0){

      /* If we were the last robot, we now point to newly added robot */

      token_dest = num_robots;

      dbg_printf("%s: token_dest updated to %d\n", __FUNCTION__, token_dest);

    }

    num_robots = pkt->num_robots;

    dbg_printf("%s: num_robots updated to %d\n", __FUNCTION__, num_robots);

  }else if(num_robots > pkt->num_robots && first_packet_sent){

    /* A robot has left the network... hopefully not us */

    dbg_printf("%s: number of robots decreased to %d\n", __FUNCTION__, 

               pkt->num_robots);

    num_robots = pkt->num_robots;

  }

}

int ColonetWireless::join_network(ColonetPacket* pkt)

{

  dbg_printf("Attempting to join network...\n");

  token_src = pkt->num_robots;

  token_dest = 0;

  num_robots = pkt->num_robots+1;

  ColonetPacket* newpkt = create_packet(token_src, token_dest, 0, 0, 0, 

                                        (unsigned char*)"", num_robots);

  if(newpkt == NULL){

    fprintf(stderr, "Error: %s - create_packet failed\n", __FUNCTION__);

    return -1;

  }

  dbg_printf("JOIN PACKET:\n"

             "token_src:%d\n"

             "token_dest:%d\n"

             "msg_dest:%d\n"

             "msg_type:%d\n"

             "msg_code:%d\n"

             "num_robots:%d\n",

             newpkt->token_src, newpkt->token_dest, newpkt->msg_dest, 

             newpkt->msg_type, newpkt->msg_code, newpkt->num_robots);

  dbg_printf("data: ");

  for(int i = 0; i < PACKET_DATA_LEN; i++){

    dbg_printf("%d ", newpkt->data[i]);

  }

  dbg_printf("\n");

  send_packet(newpkt);

  connected = true;

  dbg_printf("Connected to network.\n");

  free(newpkt);

  return 0;

}

int ColonetWireless::transmit_queued_message()

{

  ColonetPacket* top_pkt;

  int ret;

  if(have_token){

    top_pkt = outbox.front();

    if(top_pkt == NULL){

      dbg_printf("%s: no messages to send -- Passing the token.\n", 

                 __FUNCTION__);

      /* If no packets need to be sent, just send a blank packet */

      top_pkt = create_packet(token_src, token_dest, 0, 0, 0, 

                              (unsigned char*)"", num_robots);

    }else{

      outbox.pop();

    }

    ret = send_packet(top_pkt);

    free(top_pkt);

    if(ret < 0){

      dbg_fprintf(stderr, "%s: send_packet failed\n", __FUNCTION__);

      return -1;

    }

    dbg_printf("%s: packet sent\n", __FUNCTION__);

    have_token = false;

  }

  return 0;

}

int ColonetWireless::send_packet(ColonetPacket* pkt)

{

  int portfd = open(wireless_port, O_RDWR);

  dbg_printf("\nsend packet: \n");

  if (portfd == 0) {

    return -1;

  }

  //write(portfd, (unsigned char*)pkt, 16);

  for (int i = 0; i < PACKET_SIZE; i++) {

    write(portfd, (unsigned char*)pkt+i, 1);

    usleep(50); //delay here - robot wireless can't handle full data rate...

    dbg_fprintf(stderr, "%x ", ((unsigned char*)pkt)[i]);

  }

  dbg_printf("\n");

  close(portfd);

  return 0;

}

unsigned char ColonetWireless::compute_checksum(ColonetPacket* pkt)

{

  char checksum = 0;

  checksum -= pkt->token_src;

  checksum -= pkt->token_dest;

  checksum -= pkt->msg_dest;

  checksum -= pkt->msg_type;

  checksum -= pkt->msg_code;

  for(int i = 0; i < PACKET_DATA_LEN; i++){

    checksum -= pkt->data[i];

  }

  checksum -= pkt->num_robots;

  return checksum;

}

ColonetPacket* ColonetWireless::create_packet(unsigned char token_src_, 

                                              unsigned char token_dest_,

                                              unsigned char msg_dest, 

                                              unsigned char msg_type, 

                                              unsigned char msg_code, 

                                              unsigned char* data,

                                              unsigned char num_robots_)

{

  ColonetPacket* dest_packet = new ColonetPacket;

  dest_packet->prefix[0] = 'R';

  dest_packet->prefix[1] = 'C';

  dest_packet->token_src = token_src_;

  dest_packet->token_dest = token_dest_;

  dest_packet->msg_dest = msg_dest;

  dest_packet->msg_type = msg_type;

  dest_packet->msg_code = msg_code;

  memset(dest_packet->data, 0, PACKET_DATA_LEN);

  memcpy(dest_packet->data, (char*)data, PACKET_DATA_LEN);

  dest_packet->num_robots = num_robots_;

  dest_packet->checksum = compute_checksum(dest_packet);

  //printf("Made this packet: \n");

  //print_packet(dest_packet);

  return dest_packet;

}

void ColonetWireless::print_packet(ColonetPacket* pkt)

{

  int i;

  printf("Prefix: %c%c\n", pkt->prefix[0], pkt->prefix[1]);

  printf("Token src: %d\n", pkt->token_src);

  printf("Token dest: %d\n", pkt->token_dest);

  printf("Msg dest: %d\n", pkt->msg_dest);

  printf("Msg type: %d\n", pkt->msg_type);

  printf("Msg code: %d\n", pkt->msg_code);

  printf("Data: ");

  for(i = 0; i < PACKET_DATA_LEN; i++){

    printf("%d ", pkt->data[i]);

  }

  printf("\n");

  printf("Num robots: %d\n", pkt->num_robots);

  printf("Checksum: %d\n", pkt->checksum);

}

int ColonetWireless::handle_error(ColonetPacket* pkt)

{

  int dead_robot;

  int errcode = ((char*)pkt)[WL_ERROR_LOC];

  dbg_printf("%s: Handling error %d\n", __FUNCTION__, pkt->token_dest);

  switch(errcode){ //token_dest 

  case WL_ERRCODE_ROBOTDEATH:

    dead_robot = ((char*)pkt)[WL_ERR_DEAD_LOC];

    fprintf(stderr, "Dead robot: %d\n", dead_robot);

    num_robots--;

    if(token_src > dead_robot){

      if(token_src == num_robots){

        token_src--;

        token_dest = 0;

      }else{

        token_src--;

        token_dest--;

      }

    }

    //ensure that the last robot talks to the 0th one

    //this won't be set if the LAST robot dies (all robots are < dead)

    if(token_src == num_robots - 1){

      token_dest = 0;

    }

    if(token_src == dead_robot){

      //you took the place of the dead robot

      //you start the token ring again

      ColonetPacket* tmp_pkt = create_packet(token_src, token_dest, 0, 0, 0,

                                             (unsigned char*)"", num_robots);

      send_packet(tmp_pkt);

      free(tmp_pkt);

    }

    break;

  case WL_ERRCODE_NEEDTOCHARGE:

    fprintf(stderr, "Need to charge error received\n");

    break;

  case WL_ERRCODE_CHARGESTATION:

    fprintf(stderr, "Charge station error received\n");

    break;

  }

  return 0;

}

/** @brief 

 *

 * @param pkt Packet to be logged

 *

 * @return 0 on success, -1 on failure

 */

int ColonetWireless::log_packet(ColonetPacket* pkt)

{

  FILE* logfile = fopen(log_filename, "a");

  if (logfile == NULL) {

    dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);

    return -1;

  }

  for (int i = 0; i < PACKET_SIZE; i++) {

    fprintf(logfile, "%d ", *((unsigned char*)pkt+i));

  }

  fprintf(logfile, "\n");

  fclose(logfile);

  return 0;

}