Colony

/**

 * @file Command.cpp

 *

 * @author Jason Knichel

 * @date 10/9/07

 *

 * @todo make it so command doesn't rely on connection pool.  have methods

 *       that may return a response return it by getting a response array passed in to them

 *       that they fill in and then colonet server takes the contents of that response array

 *       and sends it to the appropriate client

 */

#include <ctype.h>

#include <stdio.h>

#include <stdlib.h>

#include <string.h>

#include <colonet_wireless.h>

#include <Command.h>

#include <ConnectionPool.h>

#include <map>

#include <vision.h>

using namespace std;

Command::Command(ConnectionPool * connection_pool_temp, ColonetServer* cs) {

  connection_pool = connection_pool_temp;

  colonet_server = cs;

}

Command::~Command() {}

/**

* Called by connection pool to parse command from client.

*/

int Command::parse_command(char* command, int pool_index) {

  char tokens[MAX_TOKENS][MAX_TOKEN_SIZE];

  int number_tokens = 0;

  char* end_pointer = NULL;

  int command_id;

  if (!connection_pool || !command || pool_index < 0) {

    return -1;

  }

  if ((number_tokens = tokenize_command(command, tokens)) < 0) {

    return -1;

  }

  //the 10 in the function call indicates number is base 10

  command_id = strtol(tokens[0], &end_pointer, 10);

  if (!end_pointer || *end_pointer != '\0') {

    printf("There was an error converting first token into a number.\n");

    return -1;

  }

  if (command_id == SEND_TO_ROBOT) {

    if (parse_send_to_robot(number_tokens, tokens, pool_index)) {

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

      return -1;

    }

  } else if (command_id == REQUEST_FROM_SERVER) {

    if (parse_request_from_server(number_tokens, tokens, pool_index)) {

      return -1;

    }

  }

  return 0;

}

/**

 * @brief Breaks a command up into tokens

 *

 * @param command The command to tokenize

 * @param tokens A two dimensional character array to store the tokens in

 *

 * @return 0 on success, negative error code on failure

 */

int Command::tokenize_command(char* command, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE]) {

  char* next_token = command;

  char* end_token = NULL;

  int number_tokens = 0;

  if (!command) {

    return -1;

  }

  while ((end_token = strstr(next_token, " "))) {

    *end_token = '\0';

    if (strlen(next_token) > MAX_TOKEN_SIZE-1) {

      return -1;

    }

    strcpy(tokens[number_tokens], next_token);

    number_tokens++;

    next_token = end_token + 1;

    while (isspace(*next_token) && *next_token != '\0') {

      next_token++;

    }

  }

  if (end_token == NULL && *next_token != '\0') {

    if (strlen(next_token) > MAX_TOKEN_SIZE-1) {

      return -1;

    }

    strcpy(tokens[number_tokens], next_token);

    number_tokens++;

  }

  return number_tokens;

}

/**

 * @brief checks a list of tokens to see if it's valid

 *

 * @param tokens The tokens to check

 * @param number_tokens The number of tokens contained in the tokens parameter

 *

 * @return 0 if tokens is valid

 */

int Command::check_tokens(unsigned char* tokens, int number_tokens) {

  if (number_tokens > 3 + PACKET_DATA_LEN) {

    /* Too many tokens */

    return -1;

  }

  if (number_tokens < 3) {

    /* Not enough tokens */

    return -1;

  }

  if (tokens[1] != COLONET_REQUEST && tokens[1] != COLONET_COMMAND) {

    /* Invalid message type */

    return -1;

  }

  return 0;

}

/**

* @brief Sends parsed command from server to robot(s).

*/

int Command::parse_send_to_robot(int number_int_tokens, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE], int pool_index) {

  unsigned char int_tokens[MAX_TOKENS], arguments[PACKET_DATA_LEN];

  memset(arguments, 1, PACKET_DATA_LEN);

  // Convert tokens to ints

  for (int i = ROBOT_COMMAND_OFFSET; i < number_int_tokens; i++) {

    int_tokens[i-ROBOT_COMMAND_OFFSET] = atoi(tokens[i]);

  }

  // Fill arguments buffer with arguments

  for (int i = ROBOT_COMMAND_LEN; i < number_int_tokens-ROBOT_COMMAND_OFFSET; i++) {

    arguments[i-ROBOT_COMMAND_LEN] = int_tokens[i];

  }

  // Check the tokens

  if (check_tokens(int_tokens, number_int_tokens) < 0) {

    fprintf(stderr, "%s: Error - Invalid robot command/request.\n", __FUNCTION__);

    return -1;

  }

  //printf("parsed command from internet client: ");

  for (int i = 0; i < number_int_tokens - ROBOT_COMMAND_OFFSET; i++) {

    printf("%d ", int_tokens[i]);

  }

  printf("\n");

  // Send packet to robot

  if (colonet_wl_send((short)pool_index, int_tokens[0], (ColonetRobotMessageType)int_tokens[1], int_tokens[2],

    arguments) != 0) {

    fprintf(stderr, "Error - Colonet_wl_send failed.\n");

    exit(1);

  }

  return 0;

}

/**

* 

*/

int Command::parse_request_from_server(int number_tokens, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE], int pool_index) {

  char* end_pointer = NULL;

  if (!connection_pool) {

    return -1;

  }

  if (number_tokens < 2) {

    return -1;

  }

  end_pointer=NULL;

  int second_token = strtol(tokens[1], &end_pointer, 10);

  if (!end_pointer || *end_pointer != '\0') {

    printf("There was an error converting second token into a number.\n");

    return -1;

  }

  switch (second_token) {

  case REQUEST_BOM_MATRIX:

    if (parse_request_bom_matrix(pool_index)) {

      return -1;

    }

    break;

  case REQUEST_XBEE_IDS:

    if (parse_request_xbee_ids(pool_index)) {

      return -1;

    }

    break;

  case CLIENT_REQUEST_ROBOT_POSITIONS:

    if (parse_request_robot_positions(pool_index)) {

      return -1;

    }

    break;

  case CLIENT_ASSIGN_ROBOT_ID:

    colonet_server->getPositionMonitor()->assignRealId(atoi(tokens[1]), atoi(tokens[2]));

    break;

  default:

    char* my_current_message = "Invalid request!\n";

    //printf("Sending %s\n", my_current_message);

    connection_pool->write_to_client(pool_index, my_current_message, strlen(my_current_message));

    break;

  }

  return 0;

}

int Command::parse_request_bom_matrix(int pool_index) {

  char response_bom_matrix_buffer[MAX_RESPONSE_LEN];

  char temp_bom_matrix_buffer[MAX_RESPONSE_LEN];

  if (!connection_pool) {

    return -1;

  }

  int num_robots;

  int * xbee_ids;

  int** bom_matrix = colonet_get_sensor_matrix(&num_robots, &xbee_ids);

  printf("number of robots is %d\n", num_robots);

  //TODO: make this better

  //TODO: make sure I don't need to do MAX_RESPONSE_LENGTH-1

  snprintf(response_bom_matrix_buffer, MAX_RESPONSE_LEN, "%d %d %d", RESPONSE_TO_CLIENT_REQUEST, REQUEST_BOM_MATRIX,

           num_robots);

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

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

      //TODO: don't use strcpy

      strcpy(temp_bom_matrix_buffer, response_bom_matrix_buffer);

      //TODO: put length checking in here so array doesn't go out of bounds

      //TODO: maybe use strncat?

      strcat(temp_bom_matrix_buffer," %d");

      snprintf(response_bom_matrix_buffer, MAX_RESPONSE_LEN, temp_bom_matrix_buffer, bom_matrix[i][j]);

    }

  }

  strcat(response_bom_matrix_buffer,"\n");

  connection_pool->write_to_client(pool_index, response_bom_matrix_buffer, strlen(response_bom_matrix_buffer));

  //printf("Sending %s", response_bom_matrix_buffer);

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

    free(bom_matrix[i]);

  }

  free(bom_matrix);

  free(xbee_ids);

  return 0;

}

int Command::parse_request_robot_positions(int pool_index) {

  //printf("*****parse_request_robot_positions\n");

  map<int, VisionPosition> positions = colonet_server->getPositionMonitor()->getAllRobotPositions();

  map<int, VisionPosition>::iterator iter;

  char position_buffer[256];

  position_buffer[0] = 0;

  sprintf(position_buffer, "%d %d", RESPONSE_TO_CLIENT_REQUEST, CLIENT_REQUEST_ROBOT_POSITIONS);

  for (iter = positions.begin(); iter != positions.end(); iter++) {

    char tmpbuf[80];

    sprintf(tmpbuf, " %d %d %d", iter->first, iter->second.x, iter->second.y);

    strcat(position_buffer, tmpbuf);

  }

  strcat(position_buffer, "\n");

  //printf("position buffer is: %s\n", position_buffer);

  connection_pool->write_to_client(pool_index, position_buffer, strlen(position_buffer));

  return 0;

}

int Command::parse_request_xbee_ids(int pool_index) {

  char temp_xbee_id_buffer[MAX_RESPONSE_LEN];

  char xbee_id_buffer[MAX_RESPONSE_LEN];

  int num_robots;

  int* xbee_ids = colonet_get_xbee_ids(&num_robots);

  //printf("num_robots: %d\n", num_robots);

  //printf("xbee_ids: ");

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

  //  printf("%d ", xbee_ids[i]);

  //}

  //printf("\n");

  if (!connection_pool) {

    return -1;

  }

  snprintf(xbee_id_buffer, MAX_RESPONSE_LEN, "%d %d %d", RESPONSE_TO_CLIENT_REQUEST, REQUEST_XBEE_IDS, num_robots);

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

    strcpy(temp_xbee_id_buffer, xbee_id_buffer);

    //TODO: put length checking in here so array doesn't go out of bounds

    //TODO: maybe use strncat?

    strcat(temp_xbee_id_buffer, " %d");

    snprintf(xbee_id_buffer, MAX_RESPONSE_LEN, temp_xbee_id_buffer, xbee_ids[i]);

  }

  strcat(xbee_id_buffer, "\n");

  connection_pool->write_to_client(pool_index, xbee_id_buffer, strlen(xbee_id_buffer));

  //printf("Sending %s", xbee_id_buffer);

  free(xbee_ids);

  return 0;

}