Colony

#include <stdio.h>

#include <wireless.h>

#include <unistd.h>

#include <slam_defs.h>

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);

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);

      //head_bot will not be modified by the analyzer.

      usleep(50000);

    }

    slam_analyze(head_bot);

  }

        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; //Works even if head_bot is null.

      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.

    BomNode* bom_node;

    current_bot->bom_head = bom_node;

    //i still points to the first bom information spot at 5.

    while(i<length){

      bom_node = malloc(sizeof BomNode);

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

      if(i>=length){

        printf("Packet not constructed correctly. Terminate. \n");

        exit(EXIT_FAILURE);

      }

      bom_node->value = packet[i++];

      if(i>=length){

        printf("Packet not constructed correctly. Terminate. \n");

        exit(EXIT_FAILURE);

      }

      bom_node = bom_node->next;

    }

  }

}

RobotData* getRobot(int id){

  RobotData* next = head_bot;

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

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

  else return next;

}