Colony

#include <stdio.h>

#include <stdlib.h>

#include <sys/time.h>

#include "data_requests.h"

#include "queue.h"

#include "math.h"

#define DEFAULT_WIDTH 512

#define DEFAULT_HEIGHT 512

#define MAX_MAPS 30

#define MAX_ID 20

typedef struct dataNode{

  struct timeval* timeStamp;

  char type;

  union{

      BomNode* u_b;

      short* u_s;

      unsigned char* u_c;

  } data;

  short robot_id;

} Data;

/** 

* Map oriented data.

*/

typedef struct Map{

  int data[DEFAULT_WIDTH][DEFAULT_HEIGHT];

  int robot_x;

  int robot_y;

  double robot_orientation;

  short robot_id;

  char correlation_group;  

} Map;

Map** map_array;

int map_count;

/**Robot oriented data.**/

Queue* dataQueue;

int robot_map[MAX_ID];

/*These functions simply add the data to the queue.*/

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

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

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

void slam_analyze(void);

void bom_analyze(BomNode* head,short robot_id);

void IR_analyze(short* data, short robot_id);

void encoder_analyze(unsigned char* data, short robot_id);

void initialize(void);

int main(void){

  data_requests_init(bom_handler,IR_handler,encoder_handler);

  dataQueue = queue_create();

  initialize();

  slam_analyze(); 

  return -1;

}

void initialize(void){

  map_array = malloc(MAX_MAPS * sizeof(Map*));

  map_count = 0;

  int i;

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

    robot_map[i] = -1;

  }

}

void slam_analyze(void){

  Data* current_data;

  while(1){

    if(!queue_is_empty(dataQueue)){

      if(queue_size(dataQueue) < 5){

        //request_all(/*The robot that most needs it*/);

      }

      current_data = (Data*)queue_remove(dataQueue);

      switch(current_data->type){

      case(BOM_TYPE):

        bom_analyze(current_data->data.u_b,current_data->robot_id);

        break;

      case(IR_TYPE):

        IR_analyze(current_data->data.u_s,current_data->robot_id);

        break;

      case(ENCODER_TYPE):

        encoder_analyze(current_data->data.u_c,current_data->robot_id);

        break;

      }

    }

  }

}

void initialize_map(int robot_id){

    int i,j;

    map_array[map_count] = (Map*)malloc(sizeof(Map));

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

        for(j=0;j<DEFAULT_HEIGHT;j++){

          map_array[map_count]->data[i][j] = 0;

        }

    }

    map_array[map_count]->robot_x = DEFAULT_WIDTH / 2;

    map_array[map_count]->robot_y = DEFAULT_HEIGHT/ 2;

    map_array[map_count]->robot_orientation = 0;

    map_array[map_count]->robot_id = robot_id;

    map_array[map_count]->correlation_group = -1;

    robot_map[robot_id] = map_count++;

}

void bom_analyze(BomNode* head, short robot_id){

  BomNode* current_node = head;

  if(robot_map[robot_id]==-1) initialize_map(robot_id);

  while(current_node!=NULL){

    current_node = current_node->next;

  }

}

void IR_analyze(short* data, short robot_id){

  if(robot_map[robot_id]==-1) initialize_map(robot_id);

}

void encoder_analyze(unsigned char* data, short robot_id){

  if(robot_map[robot_id]==-1) initialize_map(robot_id);

}

/*The packet handling functions for nata requests.*/

void bom_handler(short id, BomNode** head){

  Data* newData = malloc(sizeof(Data));

  newData->type = BOM_TYPE;

  newData->data.u_b = (*head);

  newData->robot_id = id;

  newData->timeStamp = malloc(sizeof(struct timeval));

  gettimeofday(&newData->timeStamp,NULL);

  queue_add(dataQueue,(void*)(newData));

}

void IR_handler(short id, short** data){

  Data* newData = malloc(sizeof(Data));

  newData->type = IR_TYPE;

  newData->data.u_s = (*data);

  newData->robot_id = id;

  gettimeofday(&newData->timeStamp,NULL);

  queue_add(dataQueue,(void*)(newData));

}

void encoder_handler(short id, unsigned char** data){

  Data* newData = malloc(sizeof(Data));

  newData->type = ENCODER_TYPE;

  newData->data.u_c = (*data);

  newData->robot_id = id;

  gettimeofday(&newData->timeStamp,NULL);

  queue_add(dataQueue,(void*)(newData));

}

/** 

* @brief Rotates a map by radians radians.

* Weak version...  Uses doubles and a secondary map is probably extra slow.

* Can this be done in place?

* @param map The map to be rotated.

*/

void rotate_map(int*** map, double radians){

  int i,j;

  double iprime, jprime, xoffset, yoffset, R1, R2, R3, R4, A;

  int i2,j2;

  int** new_map;

  new_map = malloc(DEFAULT_WIDTH*sizeof(int*)); 

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

      *(new_map+i) = malloc(DEFAULT_HEIGHT * sizeof(int));

      for(j=0;j<DEFAULT_WIDTH;i++){

          new_map[i][j] = 0;

      }

  } 

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

      for(j=0;j<DEFAULT_WIDTH;i++){

         iprime = i*cos(radians)-j*sin(radians);

         jprime = i*sin(radians)+j*cos(radians);

         //Smear the value over a range near i2,j2 The new values may need to be normalized TODO Figure that out.;

         i2 = (int)iprime;

         j2 = (int)jprime;

         xoffset = iprime - i2;

         yoffset = jprime - j2;

         R1 = sqrt(xoffset*xoffset + yoffset*yoffset)*(double)((*map)[i][j]);

         R2 = sqrt((1-xoffset)*(1-xoffset) + yoffset*yoffset)*(double)((*map)[i][j]);

         R3 = sqrt(xoffset*xoffset + (1-yoffset)*(1-yoffset))*(double)((*map)[i][j]);

         R4 = sqrt((1-xoffset)*(1-xoffset) + (1-yoffset)*(1-yoffset))*(double)((*map)[i][j]);

         A = 1/(R1+R2+R3+R4); 

         //If the new coordinates are legal...

         if(i2 < DEFAULT_WIDTH && i2 >= 0 && j2 < DEFAULT_HEIGHT && j2 >= 0)

             new_map[i2][j2] += (int)((R1*A)*(double)((*map)[i][j]));

         if(i2 + 1 < DEFAULT_WIDTH && i2 + 1 >= 0 && j2 < DEFAULT_HEIGHT && j2 >= 0)

             new_map[i2+1][j2] += (int)((R2*A)*(double)((*map)[i][j]));

         if(i2 < DEFAULT_WIDTH && i2 >= 0 && j2 + 1 < DEFAULT_HEIGHT && j2 + 1 >= 0)

             new_map[i2][j2+1] += (int)((R3*A)*(double)((*map)[i][j]));

         if(i2 + 1 < DEFAULT_WIDTH && i2 + 1 >= 0 && j2 + 1 < DEFAULT_HEIGHT && j2 + 1 >= 0)

             new_map[i2+1][j2+1] += (int)((R4*A)*(double)((*map)[i][j]));

      }

  }

  (*map) = new_map;

  return;

}