Colony

#include "data_requests.h"

#include <stdlib.h>

#include <stdio.h>

#include <pthread.h>

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

#include <string.h>

PacketGroupHandler pgh;

pthread_t* dr_thread;

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

void (*bom_handle)(BomNode** head);

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

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

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

                        void(*IR_data_handler)(short** 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_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* pthread_handle(void* arg){

  RawData* data = (RawData*)arg;

  unsigned char* packet = data->packet;

  int length = data->length;

  char type = data->type;

  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 != 10) break;

      short data[10];

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

        data[i] = ((short)packet[2*i]<<8)+((short)packet[2*i+1]);

      }

      IR_handle(data);

      break;

    case ENCODER_TYPE:

      I have no idea, unsupported.

      break;

  }

  free(data->packet);

  free(data);

  return NULL;

}*/

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

  int i=0;

  BomNode* head;

  printf("Packet contents: [ ");

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

    printf("%i ",packet[i]);

  }

  switch(type){

    case  BOM_TYPE:

      printf("I shouldn't be here during testing\n");

      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 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(&data);

      break;

    case ENCODER_TYPE:

      /*I have no idea, unsupported.*/

      break;

  }

}

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

  RawData* data = malloc(sizeof(nawData));

  if(!data){

    return;

  }

  unsigned char* newPacket = malloc(length);

  memcpy(newPacket, packet,length);

  data->packet=newPacket;

  data->source=source;

  data->length=length;

  data->type=type;

  dr_thread = malloc(sizeof(pthread_t));  

  pthread_create(dr_thread,NULL,pthread_handle,(void*)data);

}*/