Colony

#include "data_requests.h"

#include <stdlib.h>

#include <stdio.h>

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

#include <string.h>

PacketGroupHandler pgh;

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

void (*bom_handle)(short id, BomNode** head);

void (*IR_handle)(short id, short** data);

void (*encoder_handle)(short id, unsigned char** data);

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

                        void(*IR_data_handler)(short id, short** data),

                        void(*encoder_data_handler)(short id, 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_global_packet(DATA_REQUEST_GROUP,BOM_TYPE,NULL,0,robot_id,0);

}

void request_IR_data(int robot_id){

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

}

void request_encoder_data(int robot_id){

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

}

void request_all(int robot_id){

  wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,ALL_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:

      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(source,&head);

      break;

    case IR_TYPE:

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

      values as shorts, in order.*/

      if(length != 10) break;

      short* data = malloc(5*sizeof(short));

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

        data[i]=  ((((short)packet[2*i])<<8)&0xff00 ) | ( (short)(packet[2*i+1])&255);

      }

      IR_handle(source, &data);

      break;

    case ENCODER_TYPE:

      /*I have no idea yet, unsupported.*/

      break;

    case ALL_TYPE:

      short* data = malloc(5*sizeof(short));

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

        data[i]=  ((((short)packet[2*i])<<8)&0xff00 ) | ( (short)(packet[2*i+1])&255);

      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(source,&head);

      IR_handle(source, &data);

      encoder_handle(source,NULL);

      break;

  }

}