/*
	Bruce A. Maxwell
	Spring 2017
	CS 363 Project 6

	Functions for managing a particle filter

	This is intended for localization, it is not a general-purpose PF

	The map is set up so that the (x, y, theta) are to be interpreted as
	a right-handed coordinate system with x positive to the right and y
	positive up.

	The map indexes, or course, are in image coordinates, which are x
	positive to the right and y positive down.  

	map_get takes in world x,y coordinates and returns the map value at
	that location

	map_getraw takes in image coordinates

	map_xy2pix converts from world coordinates to image coordinates.
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include "ppmIO.h"
#include "particle.h"

static void map_xy2pix( Map *map, float x, float y, int *col, int *row );

// read a map from a PGM file
Map *map_read(char *filename) {
	Map *map;

	map = malloc(sizeof(Map));
	if(!map) {
		printf("map_read: unable to malloc map\n");
		return(NULL);
	}

	map->grid = readPGM( &(map->rows), &(map->cols), &(map->intensities), filename );
	if( !map->grid ) {
		printf("map_read: unable to read map %s\n", filename);
		free(map);
		return(NULL);
	}

	// need to read these from the yaml file
	map->gridSize = 0.05;
	map->origin[0] = -5.0;
	map->origin[1] = 5.0; // convert the map to Cartesian coords so (x, y) in world space is x right, y up
	map->size[0] = map->cols * map->gridSize;
	map->size[1] = map->rows * map->gridSize;

	return( map );
}

// free all malloc'd memory in the map structure
void map_free( Map *map ) {
	if( map ) {
		if( map->grid )
			free(map->grid);
		free(map);
	}
}

/*
	Takes in an (x, y) in world coords and returns the corresponding pixel value
 */
int map_get( Map *map, float x, float y ) {
	int r, c;
	map_xy2pix( map, x, y, &c, &r );
	return( map->grid[ r * map->cols + c ] );
}

/*
	Takes in the raw row, column values
 */
int map_getraw( Map *map, int c, int r ) {
	return( map->grid[ r * map->cols + c ] );
}

/*
	Takes in an (x, y) in world coords and sets the corresponding pixel value
 */
void map_set( Map *map, float x, float y, unsigned char val ) {
	int r, c;
	map_xy2pix( map, x, y, &c, &r );
	map->grid[ r * map->cols + c ] = val;
}

/*
	Takes in an (x, y) in world coords and returns the corresponding row/col of the map
 */
static void map_xy2pix( Map *map, float x, float y, int *col, int *row ) {
	*col = (int)((x - map->origin[0]) / map->gridSize  + 0.5);
	*row = (int)((map->origin[1] - y) / map->gridSize  + 0.5);
}

// copy the map data over to the pixels
void map_setPix( Map *map, Pixel *pix ) {
	int i;
	
	for(i=0;i<map->rows*map->cols;i++) {
		pix[i].r = pix[i].g = pix[i].b = map->grid[i];
	}
}

// takes in two (x, y) locations in world coords and draws a line in the src image
// draws a line that includes the two endpoint pixels
// draws into src using the color value
void map_line( Map *map, float xf0, float yf0, float xf1, float yf1, Pixel *src, Pixel value) {
	int x0, y0, x1, y1;
	int x, y;
	int p;
	int dx, dy;
	int twoDx, twoDy;
	int xstep, ystep;
	int i;

	map_xy2pix( map, xf0, yf0, &x0, &y0 );
	map_xy2pix( map, xf1, yf1, &x1, &y1 );
	
	if(x0 < 0 || x0 >= map->cols || x1 < 0 || x1 >= map->cols || y0 < 0 || y0 >= map->rows || y1 < 0 || y1 >= map->rows)
		return;
	
	dx = x1 - x0;
	dy = y1 - y0;
	x = x0;
	y = y0;
	xstep = dx < 0 ? -1 : 1;
	ystep = dy < 0 ? -1 : 1;
	

	// horizontal and vertical lines
	if(dx == 0) {
		if(dy == 0) {
			src[y*map->cols + x] = value;
			return;
		}
		for(; y != y1;y+=ystep) {
			src[y*map->cols + x] = value;
		}
		return;
	}
	if(dy == 0) {
		for(;x!=x1;x+=xstep) {
			src[y*map->cols + x] = value;
		}
		return;
	}

	twoDx = abs(dx*2);
	twoDy = abs(dy*2);
	
	if( twoDx > twoDy ) {
		p = twoDy - abs(dx);
		for(;x!=x1;x+=xstep) {
			src[y*map->cols + x] = value;
			if(p > 0) {
				y += ystep;
				p -= twoDx;
			}
			p += twoDy;
		}
	}
	else {
		p = twoDx - abs(dy);
		for(;y!=y1;y+=ystep) {
			src[y*map->cols + x] = value;
			if(p > 0) {
				x += xstep;
				p -= twoDy;
			}
			p += twoDx;
		}
	}
	
	return;
}


// sensor model
// returns likelihod that the actual sensor reading matches the expected sensor reading
// uses a Gaussian centered on the expected sensor reading with stdev sigma
double pf_sensorModel( float actual_sensor, float expected_sensor, float sigma ) {
	const double divisor = sqrt( 2.0 * M_PI );
	const double diff = actual_sensor - expected_sensor;

	return( (1.0 / (sigma * divisor)) * exp( -0.5 * (diff*diff) / (sigma * sigma) ) );
}

// calc the expected value of the laser at the given angle (rad) at position p
// this needs fixing
float pf_calcExpected( ParticleFilter *pf, Particle *p, float angle, Map *map, Pixel *test ) {
	int x0;
	int y0;
	float a;
	float dx;
	float dy;
	float tx, ty;
	int x, y;
	int i;
	const int maxDistance = (int)(4.0/0.05);
	
	// calculate the start location in pixels
	tx = p->x + pf->dlaser[0] * cos(p->t) - pf->dlaser[1] * sin(p->t);
	ty = p->y + pf->dlaser[0] * sin(p->t) + pf->dlaser[1] * cos(p->t);

	if(tx < map->origin[0] || tx >= map->origin[0] + map->size[0] ||
		 ty > map->origin[1] || ty <= map->origin[1] - map->size[1]) {
		return(0.0);
	}
		 
	map_xy2pix( map, tx, ty, &x0, &y0 );

	if( map_getraw( map, x0, y0 ) < 250 ) { // unknown or inside an obstacle, discard particle
		return(0.0);
	}

	// calculate the angle
	a = p->t + angle;

	// calculate the gradients, these are in pixel space on the map
	// use a negative angle to adjust from LHS to RHS on the map
	dx = cos(-a);
	dy = sin(-a);

	x = 0;
	y = 0;
	if( fabs(dx) > fabs(dy) ) { // step in x
		for(i=0;i <maxDistance;i++) {
			x = dx >= 0.0 ? i : -i;
			y = (int)((fabs(dy)/fabs(dx)) * i +  0.5);
			y = dy >= 0.0 ? y : -y;

			if( x0 +x < 0 || x0+x >= map->cols || y0+y < 0 || y0+y >= map->rows ) {
				return(0.0);
			}

			if(test != NULL) {
				test[(y0+y)*map->cols + (x0+x)].r = 255;
				test[(y0+y)*map->cols + (x0+x)].g = 10;
				test[(y0+y)*map->cols + (x0+x)].b = 10;
			}

			if( map_getraw( map, x0+x, y0+y ) < 20 ) { // obstacle
				return( sqrt( x*x + y*y ) * map->gridSize );
			}
		}
	}
	else { // step in y
		for(i=0;i<maxDistance;i++) {
			y = dy >= 0.0 ? i : -i;
			x = (int)((fabs(dx) / fabs(dy)) * i + 0.5);
			x = dx >= 0.0 ? x : -x;

			if( x0 +x < 0 || x0+x >= map->cols || y0+y < 0 || y0+y >= map->rows ) {
				return(0.0);
			}

			if(test != NULL) {
				test[(y0+y)*map->cols + (x0+x)].r = 255;
				test[(y0+y)*map->cols + (x0+x)].g = 10;
				test[(y0+y)*map->cols + (x0+x)].b = 10;
			}

			if( map_getraw( map, x0+x, y0+y ) < 20) { // obstacle
				return( sqrt( x*x + y*y ) * map->gridSize );
			}
		}
	}
	// no obstacle along the line so return 0

	return(0.0);
}