Colony

#include <stdio.h>

#include <wireless.h>

#include <unistd.h>

#define SLAM_PACKET_GROUP 11

#define CONTROL_PACKET_TYPE 'C'

#define REQUEST_PACKET_TYPE 'R'

#define ROBOT_RESPONSE 'I'

#define MAX_ROBOTS 50 

void handle_timeout(void);

void handle_response(int frame, int received);

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

void unregister(void);

void RobotData* getRobot(int id);

typedef struct bom_node{

  char value;

  short int id;

  BomNode* next;

} BomNode;

//If at some point we have more than 50 robots, this will fail.

typedef struct robot_data{

  int id; 

  unsigned char IR[5]; 

  RobotData* next;

  BomNode* bom_head;

} RobotData;

RobotData* head_bot;

RobotData* current_bot;

int main(void)

{

  PacketGroupHandler* pgh = malloc(sizeof(PacketGroupHandler)); 

  pgh->groupCode = SLAM_PACKET_GROUP;

  pgh->timeout_handler = handle_timeout;

  pgh->handle_response = handle_response;

  pgh->handle_receive = handle_receive;

  pgh->unregister = unregister;

  wl_init();

  wl_register_packet_group(pgh);

  wl_token_ring_register();

  wl_token_ring_join();

  wl_token_ring_set_bom_functions(NULL,NULL,NULL);

  while(1){

    wl_do();

    wl_token_iterator_begin() 

    //Iterate through the robots in the token ring. 

    while(wl_token_iterator_has_next()){

      wl_send_robot_to_robot_packet(SLAM_PACKET_GROUP,REQUEST_PACKET_TYPE,

        NULL,0,wl_token_iterator_next(),0);   

      usleep(50000); 

    }

  }

        return 0;

}

void handle_timeout(void){}

void handle_response(int frame, int received){}

void handle_receive(char type, int source, unsigned char* packet, int length){

  if(type == ROBOT_RESPONSE){

    int i; 

    current_bot = getRobot(source);

    if(current_bot == NULL){

      //Add the new robot to the front of the list.

      current_bot = malloc(sizeof(RobotData));

      current_bot->next = head_bot;

      head_bot = current_bot;

    }

    for(i=0;i<5;i++)

      current_bot->IR[i] = packet[i];

    i++; //i=5 

    //Bom Handling.

    //Clear the current information.

    BomNode* head = current_bot->bom_head;

    if(head!=NULL){

      BomNode* next = head-next;

      while(next!=NULL){

        free(head);

        head = next;

        next = head->next;

      }

      free(head);

    }

    //All of the memory allocated for the BomList is free.

    //The remainder of the packet is bom data. 

    //TODO Finish this shit.

    BomNode* bom_node = malloc(sizeof(BomNode));

    current_bot->bom_head = bom_node;

    while(i<length){

      bom_node->id = (short int)packet[i];

      i+=2;

      bom_node->value = packet[i];

      i++;

    }

  }

}

RobotData* getRobot(int id){

  RobotData* next = head_bot; 

  while(next->id != id && next->next!=NULL);

  if(next->id != id) return NULL;

  return next;

}