Colony

/** @file colonet_wireless.c

 *

 * @brief Implementation of server-side colonet wireless library

 *

 * @author Eugene Marinelli

 */

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

#include <string>

#include <pthread.h>

#include <iostream>

#include <fstream>

#include <unistd.h>

#include <fcntl.h>

#include <signal.h>

#include <ColonetServer.h>

#include <wireless.h> // Colonet wireless library.

#include <wl_token_ring.h>

#include <colonet_defs.h>

#include <colonet_wireless.h>

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

//Enable 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

extern ColonetServer colonet_server;

static bool logging_enabled;

static char log_filename[80];

static pthread_t listener_thread;

static char wl_port[40];

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

static void* listen(void* args);

static void timeout_handler(void);

static void handle_response(int frame, int received);

static void handle_receive(char type, int source, unsigned char* packet, int len);

static void unregister(void);

static int log_packet(unsigned char* packet, int len);

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

int colonet_wl_init(char* wl_port_, char* log_filename_) {

  if (log_filename_ != NULL) {

    logging_enabled = true;

    strcpy(log_filename, log_filename_);

  }

  strncpy(wl_port, wl_port_, 40);

  wl_set_com_port(wl_port);

  printf("Calling wl_init(%s)...\n", wl_port);

  if (wl_init() != 0) {

    fprintf(stderr, "wl_init failed.\n");

    return -1;

  }

  wl_token_ring_register();

  printf("Joining token ring...\n");

  if (wl_token_ring_join() != 0) {

    fprintf(stderr, "Failed to join token ring.\n");

    return -1;

  }

  printf("Joined token ring.\n");

  return 0;

}

void colonet_wl_kill_listener_thread() {

  pthread_kill(listener_thread, 9);

}

int colonet_wl_run_listener_thread() {

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

  if (pthread_create(&listener_thread, NULL, listen, NULL)) {

    perror("pthread_create");

    return -1;

  }

  return 0;

}

int colonet_wl_send(short client_source, short dest, ColonetRobotMessageType msg_type, unsigned char msg_code,

  unsigned char* args) {

  //printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", client_source, dest, msg_code);

  ColonetRobotServerPacket pkt;

  pkt.client_id = client_source;

  pkt.msg_code = msg_code;

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

    //printf("data %i: %u\n", i, args[i]);

    pkt.data[i] = args[i];

  }

  if (dest == GLOBAL_DEST) {

    //printf("sending to global dest\n");

    if (wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt),

                              sizeof(ColonetRobotServerPacket), 0) != 0) {

      return -1;

    }

  } else {

    //("sending to specific robot: %d.\n", dest);

    if (wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt),

      sizeof(ColonetRobotServerPacket), dest, COLONET_RESPONSE_PACKET_FRAME_ID) != 0) {

      return -1;

    }

  }

  return 0;

}

int colonet_get_num_robots(void) {

  return wl_token_get_num_robots();

}

int* colonet_get_xbee_ids(int* numrobots) {

  int num_robots = wl_token_get_num_robots();

  int* ids = (int*)malloc(num_robots * sizeof(int));

  wl_token_iterator_begin();

  int i = 0;

  while (wl_token_iterator_has_next()) {

    ids[i] = wl_token_iterator_next();

    i++;

  }

  *numrobots = num_robots;

  return ids;

}

// Returns int**; should be freed

int** colonet_get_sensor_matrix(int* numrobots, int** ids_) {

  int num_robots;

  int* ids = colonet_get_xbee_ids(&num_robots);

  int** m = (int**)malloc(num_robots * sizeof(int*));

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

    m[i] = (int*)malloc(num_robots * sizeof(int*));

  }

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

    for (int j = 0; j < num_robots; j++) {

      m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]);

    }

  }

  *numrobots = num_robots;

  *ids_ = ids;

  return m;

}

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

static void timeout_handler() {

  // printf("colonet wireless - timeout!\n");

}

static void handle_response(int frame, int received) {

  //TODO: These are just here to get rid of compiler warnings

  frame = frame;

  received = received;

  //printf("got response.\n");

}

static void handle_receive(char type, int source, unsigned char* data, int len) {

  //TODO: These are just here to get rid of compiler warnings

  type = type;

  //printf("!!!***Got packet from robot***!!!\n");

  ColonetRobotServerPacket* pkt = (ColonetRobotServerPacket*)data;

  if (logging_enabled) {

    log_packet(data, len);

  }

  if (pkt->msg_code == ROBOT_REQUEST_POSITION_FROM_SERVER) {

    /* Robot has requested its position. */

    int robot_x, robot_y;

    PositionMonitor* pm = colonet_server.getPositionMonitor();

    int num_robots = pm->getNumVisibleRobots();

    int ret = pm->getRobotPosition(source, &robot_x, &robot_y);

    if (ret != 0 && num_robots != 1) {

      fprintf(stderr, "Robot %d requested position, but its position is not known.\n", source);

    } else {

      if (ret != 0 /* && num_robots == 1 */) {

        VisionPosition pos = pm->getFirstPosition();

        robot_x = pos.x;

        robot_y = pos.y;

      }

      unsigned char response[80];

      response[0] = (robot_x >> 8) & 0xFF;

      response[1] = robot_x & 0xFF;

      response[2] = (robot_y >> 8) & 0xFF;

      response[3] = robot_y & 0xFF;

      fprintf(stderr, "Robot %d requested position.  Its position is %d,%d.\n", source, robot_x, robot_y);

      if (colonet_wl_send(-1, source, COLONET_COMMAND, SERVER_REPORT_POSITION_TO_ROBOT, response) != 0) {

        fprintf(stderr, "colonet_wl_send failed!\n");

        exit(1);

      }

    }

  } else {

    char processed_data[80];

    sprintf(processed_data, "%d %d %d %s\n", RESPONSE_TO_CLIENT_REQUEST, pkt->msg_code, source, pkt->data);

    colonet_server.process_received_wireless_message(pkt->client_id, processed_data, strlen(processed_data));

  }

}

static void unregister(void) {

  printf("unregister\n");

}

/** @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) {

  //TODO: These are just here to get rid of compiler warnings

  args = args;

  printf("Called listen.\n");

  PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID,

                            timeout_handler,

                            handle_response,

                            handle_receive,

                            unregister};

  wl_register_packet_group(&pgh);

  while (1) {

    wl_do();

    usleep(1000);

  }

  wl_terminate();

  return NULL;

}

/** @brief

 *

 * @param pkt Packet to be logged

 *

 * @return 0 on success, -1 on failure

 */

int log_packet(unsigned char* packet, int len) {

  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 < len; i++) {

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

  }

  fprintf(logfile, "\n");

  fclose(logfile);

  return 0;

}