/***************************************************************************
 *                                                                         *
 *   Spherical Map code for Nomad 200                                      *
 *   Written By : Brian Hand, Stephen Hutchins, and Simon Evelyn           *
 *   Changes by J.Zytkow                                                   *
 ***************************************************************************/
/* WARNING:  perhaps some of the (map_size  / 2) should be changed to  */
/*           ((map_size - 1) / 2)  */

/* preprocessor statements */
#include "client.h"           /* for Nomad functions */
#include "mapcode.h"          /* header for map function */
#include <bios.h>
#include <conio.h>
#include <stdio.h>
#include <stdlib.h>
#include <dos.h>
#include <math.h>

#define ST_SONAR_SIZE 16         /* constant for the size of the sonar array */
#define MAP_CELL_SIZE 4          /* square size will be 4 inches */
#define N_LENGTH_INCH 10      /* nomad's representation of 1 inch */
                              /* replace by a straightforward unit conversion */
#define PI 3.1415926535       /* the value of pi */
#define MOTORSON              /* according to Brian: even if defined in */
                              /* client.h problems appear if not repeated here */
#define ST_SONAR_BASE 17     /* base index for sonar index from the state variable */
#define SURE_DIST 60         /* cut off distance for accurate reading */
#define ST_NOMAD_STEER 36    /* index from state variable for steering angle */
#define ST_NOMAD_X 34        /* nomad's x coordinate from the state variable */
#define ST_NOMAD_Y 35        /* nomad's y coordinate from the state variable */

/* global declarations */
int map[map_size][map_size];     /* this array will hold all of the sonar readings */
int sonar_array[ST_SONAR_SIZE];  /* used to hold the raw sonar readings */
int robotx = (map_size - 1) / 2;         /* robot's initial coordinate on map */
int roboty = (map_size - 1) / 2;         /* robot's initial coordinate on map */
FILE *outfile;                /* file pointer to output file */
double tot_disp = 0.0;        /* total displacement used to determine when to dump to file */

void map_area() {
	st();               /* ensure stop */
	ws( 1, 1, 1, 2);   /* wait for the stop */
	get_sonar_data();           /* get sonar data */
	mapper();                /* map the data to the spherical array */
}

void get_sonar_data() {
	int count;                  /* index variable for looping */
   /*   outfile = fopen ("MAP.DAT", "a");  */
	gs();                       /* update the global sonar state */
	for ( count = 0 ; count < ST_SONAR_SIZE ; count++ ) {
		sonar_array[count] = State[ST_SONAR_BASE + count]; /* get sonar readings */
	}
   /*   fclose(outfile); */
}

/* mapper maintains the spherical array and nomad location in that array */
void mapper () {
	double steering = (State[ST_NOMAD_STEER] / 10.0) * PI / 180.0; /* steering angle */
                                            /* from tenths of inch to radians */
	double angle = steering;            /* for polar to rect. conversion */
	int count;                          /* index variable for looping */
	int x, y;                           /* index for placement in map */
	double displacement;                /* displacement value to keep track of motion */
	outfile = fopen("MAP.DAT", "a");
	for ( count = 0; count < ST_SONAR_SIZE; count++ ) {
	   if ( sonar_array[count] < SURE_DIST ) {
	     x = (int) (robotx + ((sonar_array[count] * (cos(angle))) / MAP_CELL_SIZE));
	     y = (int) (roboty + ((sonar_array[count] * (sin(angle))) / MAP_CELL_SIZE));
	     x = x % map_size;
	     if ( x < 0 ) x = map_size + x;             /* fell off map going left */
	     y = y % map_size;
	     if ( y < 0 ) y = map_size + y;             /* fell off map going up */
	     map[x][y]++;
	   }
	   angle = angle + (2 * PI / 16);            /* add angle for next sonar reading */
	}
	/* update nomad's position in the map */
	displacement = sqrt((State[ST_NOMAD_X] * State[ST_NOMAD_X]) + (State[ST_NOMAD_Y] * State[ST_NOMAD_Y]));
	tot_disp = tot_disp + displacement;
	robotx = (int) robotx + ((displacement / N_LENGTH_INCH * MAP_CELL_SIZE) * cos(steering));
	if ( robotx < 0 ) robotx = map_size + robotx;
	else robotx = robotx % map_size;
	roboty = (int) roboty + ((displacement / N_LENGTH_INCH * MAP_CELL_SIZE) * sin(steering));
	if ( roboty < 0 ) roboty = map_size + roboty;
	else roboty = roboty % map_size;
	/* erase data */
	if ( State[ST_NOMAD_STEER] >= 900 && State[ST_NOMAD_STEER] <= 2700 ) {
	  x = robotx + (map_size  / 2);
	} else {
	  x = robotx - (map_size / 2);
	}
	if ( x < 0 ) x = map_size + x;
	else x = x % map_size;
	if ( State[ST_NOMAD_STEER] >= 0 && State[ST_NOMAD_STEER] <= 1800 ) {
	  y = roboty + (map_size / 2);
	} else {
	  y = roboty - (map_size / 2);
	}
	if ( y < 0 ) y = map_size + y;
	else y = y % map_size;
	for ( count = 0; count < map_size; count++ )    /* improve to erase the right number */
	  map[x][count] = 0;                            /* of rows and columns */
	for ( count = 0; count < map_size; count++ )
	  map[count][y] = 0;
	fclose(outfile);
	if (tot_disp >= 720 ) {
	  tk("print map");
	  print_map();
	  tot_disp = 0.0;
	}
	if ( displacement >= 40.0 ) {
	  displacement = 0.0;
	  dp(0, 0);
	}
}

/* improve print_map to include robotx and roboty and print the array */
/* with the robot in the middle                                       */

void print_map() {
   int countx, county;            /* indices for traversing through array */
   outfile = fopen( "MAP.DAT", "a" );
   fprintf( outfile, "\n\n");
   for ( county = 0 ; county < (map_size - 1); county++ ) {
       for ( countx = 0 ; countx < (map_size - 1) ; countx++ ) {
	   if (map[countx][county] > 0 )
	      fprintf( outfile, "%d", map[countx][county] );
	   else
	      fprintf(outfile, ".");
       }
       fprintf(outfile, "\n");
   }
   fprintf(outfile, "\n\n");
   fclose(outfile);
}