Colony

#include "data_requests.h"

#include <stdlib.h>

#include <stdio.h>

#include "../../../libwireless/lib/wireless.h"

PacketGroupHandler pgh;

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

void (*bom_handle)(BomNode* head);

void (*IR_handle)(unsigned char* data);

void (*encoder_handle)(unsigned char* data);

void data_requests_init(void(*bom_data_handler)(BomNode* head),

                        void(*IR_data_handler)(unsigned char* data),

                        void(*encoder_data_handler)(unsigned char* data)){

  bom_handle = bom_data_handler;

  IR_handle = IR_data_handler;

  encoder_handle = encoder_data_handler;

  pgh.groupCode = DATA_REQUEST_GROUP;

  pgh.timeout_handler = NULL;

  pgh.handle_response = NULL;

  pgh.handle_receive = receive_handle;

  pgh.unregister = NULL;

  wl_register_packet_group(&pgh);

}

void request_bom_data(int robot_id){

  wl_send_robot_to_robot_packet(DATA_REQUEST_GROUP,BOM_TYPE,NULL,0,robot_id,0);

}

void request_IR_data(int robot_id){

  wl_send_robot_to_robot_packet(DATA_REQUEST_GROUP,IR_TYPE,NULL,0,robot_id,0);

}

void request_encoder_data(int robot_id){

  wl_send_robot_to_robot_packet(DATA_REQUEST_GROUP,ENCODER_TYPE,NULL,0,robot_id,0);

}

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

  int i=0;

  BomNode* head;

  switch(type){

    case  BOM_TYPE:

      //BomNode* head;

      head = malloc(sizeof(BomNode));

      BomNode* iter = head;

      while(i<length){

        iter->robot_id = ((short)packet[i]<<8) + ((short)packet[i+1]);

        i+=2;

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

        if(i<length){

            iter-> next = malloc(sizeof(BomNode));

            iter = iter->next;

        }

      }

      bom_handle(head);

      break;

    case IR_TYPE:

      /*An IR packet consists of just the 5 bytes representing the IR

      values, in order.*/

      if(length != 4){

          //The packet is incomplete... bail.

          break;

      }

      else{

        unsigned char data[4];

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

          data[i] = packet[i];

        }

        IR_handle(data);

      }

      break;

    case ENCODER_TYPE:

      /*I have no idea, unsupported.*/

      break;

  }

}