/*****************************************************************************
*evgrid.c                                                                    *
*                                                                            *
*Tom Stepleton and Stephanie Wojtkowski                                      *
*                                                                            *
* This is a program that constructs the probability map based using HPM's    *
* evidence grid code.  It has been edited to create a tiff image of the      *
*evidence grid.                                                              *
*2/15/00 Stephanie Wojtkowski                                                *
*****************************************************************************/

#include <stdio.h>
#include "sonar_head.h"
#include "cmacs.h"
#include <tiffio.h>
#include <sys/types.h>

double *po1 = NULL;
double *po2 = NULL;

/*More tiff stuff*/
uint32 *tiff_read2raster(char filename[], uint32 *nrow, uint32 *ncol);
int tiff_raster2file(char filename[], uint32 *raster, uint32 nrow, uint32 ncol);

/*
  This prints a very simple map showing where
  high probability obstacles are.
*/
void PrintCharMap(EvGrid *ev, char *filename) {
  /*Tiff stuff*/
  uint32 *image;
  uint32 rows=120, cols=160;
  double grey = 0;

  char *map;
  int i, j, k;

  image = (uint32 *)malloc(sizeof(uint32) * rows * cols);

  map = (char *)malloc(sizeof(char) * ev->ysize * (ev->xsize + 1));
  printf("xsize = %d, ysize = %d\n", ev->xsize, ev->ysize);
  if(map) {

    for(i=0, k=0;i<ev->ysize;i++) {
      for(j=0;j<ev->xsize;j++, k++) {
	if(ev->map[k] < 0.2)
	  map[i*(ev->xsize+1) + j] = ' ';
	else if(ev->map[k] < 0.6)
	  map[i*(ev->xsize+1) + j] = '.';
	else if(ev->map[k] < 0.8)
	  map[i*(ev->xsize+1) + j] = 'o';
	else if(ev->map[k] < 0.95)
	  map[i*(ev->xsize+1) + j] = 'x';
	else
	  map[i*(ev->xsize+1) + j] = 'X';
      }
      map[i*(ev->xsize+1) + j] = '\0';
    }
    
    for(i=ev->ysize-1;i>=0;i--) {
      printf("%s\n", &(map[i*(ev->xsize+1)]));
    }
    
    for(i=0, k=0;i<ev->ysize;i++) {
      for(j=0;j<ev->xsize;j++, k++) {
	grey = 255 - (ev->map[k]*255);
	printf("grey = %f\n", grey);
	image[i*ev->xsize + j] = (uint32)255*256*256*256  +
	  (uint32)grey*256*256 + (uint32)grey*256 + (uint32)grey;	
      }
    }  
    
    tiff_raster2file(filename, image, rows, cols);
    free(map);
  }

  return;
}

#define MAXSIZE 2000

EvGrid *CreateMap(int xsize, int ysize, double x0, double y0, double cellSize) {
  EvGrid *ev;
  long i;

  ev = (EvGrid *)malloc(sizeof(EvGrid));
  if(!ev) {
    fprintf(stderr, "CreateMap(): Unable to allocate memory\n");
    return(NULL);
  }

  ev->xsize = xsize > MAXSIZE ? MAXSIZE : xsize;
  ev->ysize = ysize > MAXSIZE ? MAXSIZE : ysize;
  ev->mapsize = ev->xsize * ev->ysize;
  ev->map = (double *)malloc(sizeof(double) * ev->mapsize);
  if(!(ev->map)) {
    fprintf(stderr, "CreateMap(): Unable to allocate memory (2)\n");
    free(ev);
    return(NULL);
  }

  ev->x0 = x0;
  ev->y0 = y0;
  ev->cellsize = cellSize;
  ev->back_occ = 0.2;
  ev->free_back_occ = 0.2;
  ev->xmax = ev->x0 + ev->cellsize * ev->xsize;
  ev->ymax = ev->y0 + ev->cellsize * ev->ysize;

  for(i=0;i<ev->mapsize;i++)
    ev->map[i] = ev->back_occ;

  /* create the rest of the global variables*/
  if(po1 != NULL)
    free(po1);

  po1 = (double *) malloc(ev->mapsize*sizeof(double));

  if(po2 != NULL)
    free(po2);

  po2 = (double *) malloc(ev->mapsize*sizeof(double));

  return(ev);
}

void DeleteEvGrid(EvGrid *ev) {
  if(ev) {
    if(ev->map)
      free(ev->map);
    free(ev);
  }
  if(po1)
    free(po1);
  if(po2)
    free(po2);
}

/*
  This routine takes a sonar reading--sonar position, sonar angle, and
  sonar value--and puts it into an evidence grid using Hans Moravec's
  code.  The evidence grid itself is maintained and accessed by a set
  of routines also in this file.  
*/
int InsertSonarReading(EvGrid *ev, double bmx, double bmy, double bmangle, double bmrange) {
  int i, j, k, reading, i_reading, sw;
  double B, C, D, E;
  double odd, odd1, odd2;  float tf;
  int xl, xh, yl, yh;
  FILE *readbeam;
  char filename[50], tstr[50];

  /* error check*/
  if(ev == NULL) {
    fprintf(stderr, "InsertSonarReading(): Evidence grid pointer is NULL\n");
    return(-1);
  }
     
  /* if the reading is in a valid range*/
  if(CHECKREADING(bmrange)) {
    /* fills in a second map with uniform values*/
    for(j=0; j<ev->mapsize; j++) {
      po1[j] = ev->free_back_occ;
    }

    /* fill in a wedge in po1 according to the new reading*/
    Fillwedge(ev, bmrange, bmx,bmy, bmangle);

    /* update the evidence grid (old_prob) any place that the
       po1 grid has been changed*/
    for(j=0; j<ev->mapsize; j++) {
      if (ev->map[j]==1.0 || po1[j]==1.0) { /* if either map is 1, 
					       leave it there*/
	ev->map[j]=1.0;
      }
      else if (po1[j] != ev->free_back_occ) { /* this executes the 
						 Bayesian updating*/
	odd1 = ev->map[j]/(1-ev->map[j]);
	odd2 = po1[j]/(1-po1[j]);
	odd = odd1*odd2*(1.0-(ev->free_back_occ))/(ev->free_back_occ);
	ev->map[j] = odd/(1+odd);
      }
    }
  }

  /* return that everything went ok*/
  return(0);
}