Colony

#include <stdio.h>

#include <stdlib.h>

#include <time.h>

#include "data_requests.h"

#include "queue.h"

#include "math.h"

typedef struct dataNode{

  struct timeval* timeStamp;

  char type;

  union{

      BomNode* u_b;

      short* u_s;

      unsigned char* u_c;

  } data;

  short robot_id;

} Data;

/*

This is the game plan if anyone is interested.

Each robot will go for an unspecified amount of time,

accruing encoder, BOM, and IR data.  The IR and encoders

will be used to generate heat maps for each robot, starting

at the center, decreasing in certainty (i.e. the amount of

'color' put on during a time unit will be spread over a greater

area) over time according to the physical limitations of 

the measuring devices.

When the uncertainty gets too high, a new map will be allocated for

the robot.

The BOM data will be used to orient individual maps to one another.

Along with physical limitations on the size of the environment, and

hopefully some brainstorming to do with triangularization and what not,

the heat maps will become increasingly well oriented.

Finally, after a currently unspecified amount of time, the maps which have

been properly oriented (this could probably just be all of them due to the nature

of the heat maps,) will be summed up into one meta map! Metamap will undergo some

signal processing, probably in the form of a second order gradient, to get a map

of magnitudes, which we will try to further reduce to lines.  These lines are the

walls of the environment.

In time, it is possible that this "unspecfied amount of time" could become a continuous

computation, but I haven't put much thought into how this will scale.  It will almost 

definitely depend upon the chosen resolution of the heat map.

I do want people working with me, this should be a lot of fun,

I'm coming to accept the fact that I'm not a very good programmer,

but I think this is a solid method of doing SLAM.

*/

/** 

* @brief An array of maps.

*/

#define DEFAULT_WIDTH 50

#define DEFAULT_HEIGHT 50

//Indexed x,y

int* map_array[DEFAULT_WIDTH][DEFAULT_HEIGHT];

/** 

* @brief An array of short's indicating which robot each map belongs to.

*/

short* map_ids;

/** 

* @brief Describes whether a map has been correlated to previous maps with a BOM reading.

*/

char* map_correlated;

Queue* dataQueue;

/*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 bom_analyze(BomNode* head);

void IR_analyze(short* data);

void encoder_analyze(unsigned char* data);

void slam_analyze(void){

  Data* current_data;

  while(1){

    if(!queue_is_empty(dataQueue)){

      current_data = (Data*)queue_remove(dataQueue);

      switch(current_data->type){

      case(BOM_TYPE):

        bom_analyze(current_data->data.u_b);

        break;

      case(IR_TYPE):

        IR_analyze(current_data->data.u_s);

        break;

      case(ENCODER_TYPE):

        encoder_analyze(current_data->data.u_c);

        break;

      }

    }

  }

}

int main(void){

  data_requests_init(bom_handler,IR_handler,encoder_handler);

  dataQueue = queue_create();

  slam_analyze(); 

  return -1;

}

void bom_analyze(BomNode* head){

}

void IR_analyze(short* data){

}

void encoder_analyze(unsigned char* data){

}

/*The packet handling functions for data 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;

  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;

  int i2,j2;

  int new_map[DEFAULT_WIDTH][DEFAULT_HEIGHT];

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

      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;

         //If the new coordinates are legal...

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

             new_map[i2][j2] += (int)(sqrt(xoffset*xoffset + yoffset*yoffset)*(double)((*map)[i][j]));

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

             new_map[i2+1][j2] += (int)(sqrt((1-xoffset)*(1-xoffset) + yoffset*yoffset)*(double)((*map)[i][j]));

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

             new_map[i2][j2+1] += (int)(sqrt(xoffset*xoffset + (1-yoffset)*(1-yoffset))*(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)(sqrt((1-xoffset)*(1-xoffset) + (1-yoffset)*(1-yoffset))*(double)((*map)[i][j]));

      }

  }

  //Incompatible types? FIXME

  (*map) = new_map;

  return;

}