#include <termios.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <tgmath.h>
#include <fcntl.h>
#define BUFFER_SIZE 100
#define BAUDRATE B9600
#define SERIALDEVICE "/dev/ttyS0"
#define FALSE 0
#define TRUE 1
#define ACK 6 
#define MAX_BOTS 10
#define MAX_ARRAY 21
//This is the adjustment for how many bots need to see an obstical or bad spot
// the real threshold will be the number of bots / threshold

#define THRESHOLD 2


unsigned char get_serial_char(int fd, int debug_level);
int * read_array(int *size, int fd);
int get_int(int fd);
void data_reduce(int *dist[MAX_BOTS], int *degree[MAX_BOTS],
		int master[MAX_ARRAY][MAX_ARRAY], 
		int size[MAX_BOTS], int bot);
void print_map(int master[MAX_ARRAY][MAX_ARRAY]);

int main(int argc, char *argv[]) {
   int num, j, dig, dig_cnt, fd, debug_level = 0;
   int *dist[MAX_BOTS], *degree[MAX_BOTS];
   int size[MAX_BOTS];
   int master[MAX_ARRAY][MAX_ARRAY];
   int bot;
   char com;
   struct termios oldtio, newtio, oldstdtio, newstdtio;
 
   /* The serial code writen below was orginally written by Lew and Charlie for
    * the Davis Weather Station.
    *
    * begin credit for Lew and Charlie 
    */

   /* Open serial port for reading and writing and not as controlling tty
      because we don't want to get killed if linenoise sends CTRL-C.  
   */

   fd = open(SERIALDEVICE, O_RDONLY | O_NOCTTY);
   if (fd < 0) {
      printf("error opening serial device.\n");
      perror(SERIALDEVICE); 
      exit(-1); 
   }

 
   /* Save the current serial port settings.
   */
   tcgetattr(fd, &oldtio); 

   /* Set bps rate and hardware flow control and 8n1 (8bit,no parity,1 stopbit).
      Also don't hangup automatically and ignore modem status.  Finally enable 
      receiving characters.
   */
   newtio.c_cflag = BAUDRATE | CS8 | CLOCAL | CREAD;
 
   /* Don't echo characters, we just want output from the device.  Don't 
      generate signals.  This is the quick and dirty way to do this, we should 
      use the specific masks for ECHO and ISIG but since we don't care about 
      having any of the items in this vector enabled we can just zero it.
   */
   newtio.c_lflag = 0;

  
   /* Ignore bytes with parity errors and leave port raw and dumb.
   */
   newtio.c_iflag = IGNPAR;

   /* Flush the I/O queue for the port and enable the new settings.
   */
   tcflush(fd, TCIFLUSH);
   tcsetattr(fd, TCSANOW, &newtio);

   /* Save the old settings and then stop echo and buffering on stdin.
   */
   tcgetattr(0, &oldstdtio);
   tcgetattr(0, &newstdtio); /* get working stdtio definition */
   newstdtio.c_lflag &= ~(ICANON | ECHO); /* change the flags */ 
   tcsetattr(0, TCSANOW, &newstdtio); /* change the configuration */ 

   /* end credit */

   /* 
      Read in the info from the bots.
   */
   bot=0;
   while(1){
     printf("press start on handyboard before enter on acl side 
ready for next bot, enter for next, q for quit\n");
     com = getc(stdin);
     if((com == 'q') || (bot > MAX_BOTS)){
       break;
     }
     num = 0;
     dig_cnt = 1;
     printf("Starting bot %d\n", bot);
     while(1){
       dig = (int)get_serial_char(fd, debug_level);  
       //printf("dig=%d\n", dig);
       if(dig=='a'){
	 dist[bot]= read_array(&size[bot], fd);
	 degree[bot]= read_array(&size[bot], fd);
	 printf("size[%d]=%d\n", bot, size[bot]);
	 for(j=0;j<size[bot];j++){
	   printf("dist[%d]=%d\n", j, dist[bot][j]);
	 }
	 for(j=0;j<size[bot];j++){
	   printf("degree[%d]=%d\n", j, degree[bot][j]);
	 }
       }else if(dig=='x'){
	 printf("bot done\n\n");
	 bot++;
	 break;
       }
     }
   }
   
   /*begin credit*/
   /* Put the serial port and the terminal back the way the were when
      we started.
   */
   tcsetattr(fd, TCSANOW, &oldtio); 
   tcsetattr(0, TCSANOW, &oldstdtio); 

   close(fd);

   /*end credit*/

   /*do analization and reporting*/
   memset(master, 0, sizeof(int[MAX_ARRAY][MAX_ARRAY]));
   printf("starting analasys\n");
   data_reduce(dist, degree, master, size, bot);
   //printing out map
   print_map(master);

   exit(0);
}

/*
 *This function just prints out the double array master in a nice
 *format.
 *Arguments:
 *master[][]: two dimintional array representing the map of the space.
 *
 *returns: none
 */

void print_map(int master[MAX_ARRAY][MAX_ARRAY]){
  int i, j;

  printf("\n");
  printf("-10            -5              0              5              10\n");
  for(j=0;j<MAX_ARRAY;j++){
    if(j==10){
      printf(" + ");
    }else{
      printf("---");
    }
  }
  printf("\n");
  for(i=0; i<(MAX_ARRAY/2)+1;i++){
    for(j=0;j<MAX_ARRAY;j++){
      printf(" %d ", master[j][i]);
    }
    printf("\n");
  }
  for(j=0;j<MAX_ARRAY;j++){
    printf("---");
  }
  printf("\n");
  printf("Key:\n 1=obstacles\n 2=bad spot\n 3=both\n");
}

/*
 *This function takes the bot data (dist and degree) and converts them
 *into obst_spot and bad_spot which are in cartesian.  These are then
 *analized and put into master[][].
 *Arguments:
 *dist[]: array of pointers to the distances for each bot.
 *degree[]: array of pointers to the degrees for each bot.
 *master[][]: two dimintional array representing the map of the space.
 *size[]: array of the sizes of the arrays dist and degree for each bot.
 *bot: total number of bots that sent data.
 *
 *returns: none
 */

void data_reduce(int *dist[MAX_BOTS], int *degree[MAX_BOTS],
		int master[MAX_ARRAY][MAX_ARRAY], 
		int size[MAX_BOTS], int bot){
  
  int bad_spots[MAX_ARRAY][MAX_ARRAY]; 
  int obst_spots[MAX_ARRAY][MAX_ARRAY]; 
  int i, j;
  int x, y;
  int deg;
  float pi=3.14159;
  int error=0;
  
  memset(bad_spots, 0, sizeof(int[MAX_ARRAY][MAX_ARRAY]));
  memset(obst_spots, 0, sizeof(int[MAX_ARRAY][MAX_ARRAY]));

  printf("filling obst&bad\n");  
  for(i=0; i<bot;i++){
    for(j=0;j<size[i];j++){
      
      /*We have to count the spots on the right side and left side
       differently because of the way sin and cos behave when the degree
      is over 90*/
      
      if(degree[i][j]<=90 && degree[i][j]>=0){
	if(dist[i][j]>0){
	  x= dist[i][j]/30*(cos((float)degree[i][j]*(pi/180)));
	  // we have to shift this over to the center of the array, 
	  // where the light is.
	  x=x+10;
	  y= dist[i][j]/30*(sin((float)degree[i][j]*(pi/180)));
	  obst_spots[x][y]++;
	  printf("obst=x %d, y %d\n", x, y);
	}else if(dist[i][j]<0){
	  x= -(dist[i][j])/30*(cos((float)degree[i][j]*(pi/180)));
	  x=x+10;
	  y= -(dist[i][j])/30*(sin((float)degree[i][j]*(pi/180)));
	  bad_spots[x][y]++;
	  printf("bad=x %d, y %d\n", x, y);
	}else{
	  error++;
	}
      }else if(degree[i][j]>90){ // degree is > 90
	if(dist[i][j]>0){
	  deg= 180-degree[i][j];
	  x= dist[i][j]/30*(cos((float)deg*(pi/180)));
	  y= dist[i][j]/30*(sin((float)deg*(pi/180)));
	  // cos gives us the x distance from the light here and we need
	  // the distance from the left 'corner'.
	  x=10-x;
	  obst_spots[x][y]++;
	  printf("obst=x %d, y %d\n", x, y);
	}else if(dist[i][j]<0){
	  x= -(dist[i][j])/30*(cos((float)deg*(pi/180)));
	  y= -(dist[i][j])/30*(sin((float)deg*(pi/180)));
	  x=10-x;
	  bad_spots[x][y]++;
	  printf("bad=x %d, y %d\n", x, y);
	}else{
	  error++;
	}
      }else{
	error++;
      }
    }
  }

  // done combining all the data from the bots together

  printf("possible errors=%d\n", error);
  printf("filling master\n");  
  for(i=0; i<MAX_ARRAY;i++){
    for(j=0;j<MAX_ARRAY;j++){
      if(obst_spots[i][j]>(bot/THRESHOLD)){
	if(master[i][j]!=0){
	  master[i][j]=3;
	}else{
	  master[i][j]=1;
	}
      }
      if(bad_spots[i][j]>(bot/THRESHOLD)){
	if(master[i][j]!=0){
	  master[i][j]=3;
	}else{
	  master[i][j]=2;
	}
      }
    }
  }
  /*print out stuff*/
  for(i=0; i<MAX_ARRAY/2;i++){
    for(j=0;j<MAX_ARRAY;j++){
      printf(" %d ", bad_spots[j][i]);
    }
    printf("\n");
  }
  printf("\n");
  printf("\n");
  for(i=0; i<MAX_ARRAY/2;i++){
    for(j=0;j<MAX_ARRAY;j++){
      printf(" %d ", obst_spots[j][i]);
    }
    printf("\n");
  }
  printf("\n");
}

/* built up from get_int() function
 * Arguments:
 *size: pointer to the size of the array.
 *fd: file discripter to the serial port
 *
 *return: pointer to malloced array
 */

int * read_array(int *size, int fd){
  int i;
  int *temp;

  *size = get_int(fd);
  temp = (int*)malloc(sizeof(int[*size]));
  if(temp==NULL){
    printf("malloc failed, bailing out\n");
    exit(1);
  }
  for(i=0;i<*size;i++){
    temp[i]=get_int(fd);
  }
  return(temp);
}

/* ---- Serial I/O functions based on templates from Davis ---- */
//function to get single int
int get_int(int fd){
  int dig;
  int sign=1;
  int num = 0;
  int dig_cnt = 1;

  while(1){
    dig = (int)get_serial_char(fd, 0);  
    
    if(dig==10){
      num = num *sign;
      return(num);
    }else if(dig == '-'){
      sign = -1;
    }else{
      dig = dig*dig_cnt;
      num = num + dig;
      dig_cnt = dig_cnt*10; 
    }
    
  }
}

unsigned char get_serial_char(int fd, int debug_level) {
   unsigned char  c;
  
   read(fd, &c, 1);
   return(c);
}