/*
	This file contains a simple example of using the A* routine
	
	The task is coded up to find a path on a regular grid between
	any two points.  The grid values can be integers from 0 to 100.
	A specified number of rectangular obstacles can be defined.
	
	You can modify the Astar to be breadth-first (make h zero) or greedy search
	(make g zero). Options are also available for using Manhattan or Euclidean
	distance as the heuristic in the h function.
*/

// Include files
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "astar.h"

// global variables: goal configuration, initial configuration, obstacles
int gblGoal[2] = {50, 50};
int gblInit[2] = {25, 20};
char gblImage[101][101];

// define the number of obstacles here
#define gblNumObstacles 3

// define the obstacle rectangles here: (top, left, bottom, right), origin in lower left
int gblObstacle[gblNumObstacles][4] = {40, 5, 30, 55,
				       40, 50, 10, 65,
				       40, 5, 10, 10};

// NodeInfo structure
typedef struct {
	int	x;
	int y;
} NodeInfo;

// define the g function as parent plus 1 step
double gfunction(Node *p);
double gfunction(Node *p) {
	Node *q;
	NodeInfo *ni;
	double dx, dy;
	
	if(p == NULL)
		return(0.0);
		
	// Uncomment this to do greedy search
	// return(0.0);
		
	// This uses steps from the initial state (Manhattan distance)
	if(p->parent == NULL)
		return(0.0);
	else {
		q = (Node *)p->parent;
		return(q->g + 1.0);
	}
	
}

// define the h function as Manhattan/Euclidean distance to the goal
double hfunction(Node *p);
double hfunction(Node *p) {
	NodeInfo *ni;
	double h, dx, dy;
	
	if(p == NULL)
		return(1e+7);
	
	ni = (NodeInfo *)p->nodeInfo;
	
	// Uncomment this to execute breadth-first search
	// return(0);
	
	
	// Uncomment this to use Manhattan distance
	
	dx = gblGoal[0] - ni->x;
	dx = dx < 0.0 ? -dx : dx;
	dy = gblGoal[1] - ni->y;
	dy = dy < 0.0 ? -dy : dy;
	h = dx + dy;
	
	
	// Uncomment this to use Euclidean distance
	/*
	dx = gblGoal[0] - ni->x;
	dy = gblGoal[1] - ni->y;
	h = sqrt(dx * dx + dy * dy);
	*/
	
	return(h);
}
	

// define the goalNode function
int goal(Node *p);
int goal(Node *p) {
	NodeInfo *ni;
	
	ni = (NodeInfo *)p->nodeInfo;
	
	if(ni->x == gblGoal[0] & ni->y == gblGoal[1])
		return(1);
	
	return(0);
}

// Test for whether a point is in an obstacle or not
int inObstacle(int x, int y);
int inObstacle(int x, int y) {
	int i;
	
	// uncomment this to remove the obstacles
	// return(0);
	
	for(i=0;i<gblNumObstacles;i++) {
		if(y <= gblObstacle[i][0] && y >= gblObstacle[i][2] && 
			x >= gblObstacle[i][1] && x <= gblObstacle[i][3])
			return(1);
	}
	
	return(0);
}

#define NORTH 0
#define SOUTH 1
#define EAST 2
#define WEST 3

// define the children function
Node *makeChildren(Node *parent);
Node *makeChildren(Node *parent) {
	NodeInfo *ni, *gpni, *pni;
	Node *gp, *p, *q;
	int gpDirection;
	
	ni = (NodeInfo *)parent->nodeInfo;
	
	// create the three children that are not the parent of this node
	if(parent->parent != NULL) {
		gp = (Node *)parent->parent;
		gpni = (NodeInfo *)gp->nodeInfo;
		if(gpni->x < ni->x)
			gpDirection = WEST;
		else if(gpni->x > ni->x)
			gpDirection = EAST;
		else if(gpni->y < ni->y)
			gpDirection = SOUTH;
		else
			gpDirection = NORTH;
	}
	else
		gpDirection = -1;
		
	// initialize the return list
	q = NULL;
	
	// create the North child
	if(gpDirection != NORTH && ni->y < 100) {
		if(!inObstacle(ni->x, ni->y + 1)) {
			p = (Node *)malloc(sizeof(Node));
			pni = (NodeInfo *)malloc(sizeof(NodeInfo));
			
			pni->x = ni->x;
			pni->y = ni->y + 1;
			p->nodeInfo = pni;
			p->parent = (void *)parent;
			p->next = q;
			q = p;
		}
	}
	
	// create the South child
	if(gpDirection != SOUTH && ni->y > 0) {
		if(!inObstacle(ni->x, ni->y - 1)) {
			p = (Node *)malloc(sizeof(Node));
			pni = (NodeInfo *)malloc(sizeof(NodeInfo));
			
			pni->x = ni->x;
			pni->y = ni->y - 1;
			p->nodeInfo = pni;
			p->parent = (void *)parent;
			p->next = q;
			q = p;
		}
	}

	// create the East child
	if(gpDirection != EAST && ni->x < 100) {
		if(!inObstacle(ni->x + 1, ni->y)) {
			p = (Node *)malloc(sizeof(Node));
			pni = (NodeInfo *)malloc(sizeof(NodeInfo));
			
			pni->x = ni->x + 1;
			pni->y = ni->y;
			p->nodeInfo = pni;
			p->parent = (void *)parent;
			p->next = q;
			q = p;
		}
	}
	
	// create the West child
	if(gpDirection != WEST && ni->x > 0) {
		if(!inObstacle(ni->x - 1, ni->y)) {
		p = (Node *)malloc(sizeof(Node));
			pni = (NodeInfo *)malloc(sizeof(NodeInfo));
			
			pni->x = ni->x - 1;
			pni->y = ni->y;
			p->nodeInfo = pni;
			p->parent = (void *)parent;
			p->next = q;
			q = p;
		}
	}
	
	return(q);
}

// Free node function
void freeNode(Node *p);
void freeNode(Node *p) {
	NodeInfo *ni;
	
	ni = (NodeInfo *)p->nodeInfo;
	free(ni);
	free(p);
}

// Test for node equality
int nodeEqual(Node *a, Node *b);
int nodeEqual(Node *a, Node *b) {
	int diff, x1, y1, x2, y2;
	
	if(a == NULL && b == NULL)
		return(1);
	else if(a == NULL)
		return(0);
	else if(b == NULL)
		return(0);
	
	x1 = ((NodeInfo *)(a->nodeInfo))->x;
	x2 = ((NodeInfo *)(b->nodeInfo))->x;
	y1 = ((NodeInfo *)(a->nodeInfo))->y;
	y2 = ((NodeInfo *)(b->nodeInfo))->y;
	
	diff = (((NodeInfo *)(a->nodeInfo))->x - ((NodeInfo *)(b->nodeInfo))->x) == 0 ? 0 : 1;
	diff += (((NodeInfo *)(a->nodeInfo))->y - ((NodeInfo *)(b->nodeInfo))->y) == 0 ? 0 : 1;
	
	return(diff == 0);
}

// simple function to print a node
void printNode(Node *p);
void printNode(Node *p) {
	printf("Node %05d: f %.2lf (%4d, %4d)\n", p->id, p->f, ((NodeInfo *)p->nodeInfo)->x, ((NodeInfo *)p->nodeInfo)->y);
}

void graphNode(Node *p, char c) {
  NodeInfo *ni;

  ni = (NodeInfo *)p->nodeInfo;
  if(ni->x>= 0 && ni->x <= 100 && ni->y >= 0 && ni->y <= 100)
    gblImage[ni->y][ni->x] = c;
}

// main function
main() {
	Node *root;
	NodeInfo *ni;
	Node *path, *p;
	int step;
	int i, j, k;
	FILE *fp;

	// Now build a character representation of the search
	for(i=0;i<100;i++) {
		for(j=0;j<100;j++)
			gblImage[i][j] = ' ';
		gblImage[i][j] = '\0';
	}
	
	// allocate and setup the root node
	root = (Node *)malloc(sizeof(Node));
	ni = (NodeInfo *)malloc(sizeof(NodeInfo));
		
	ni->x = gblInit[0];
	ni->y = gblInit[1];
	
	root->nodeInfo = (void *)ni;
	root->parent = NULL;
	root->next = NULL;
	root->prev = NULL;
	root->g = 0;
	root->h = hfunction(root);
	root->f = root->g + root->h;
	root->id = 0;
	root->depth = 0;
	root->state = OPEN;

	// call the A* algorithm
	path = AStarSearch(root, gfunction, hfunction, goal, makeChildren, freeNode, nodeEqual, printNode, graphNode);
	
	// A* returned failure
	if(path == NULL) {
		printf("No path found, terminating\n");
		return(0);
	}
	
	// otherwise, we had a successful search
	p = path;
	step = 0;
	while(p != NULL) {
		ni = (NodeInfo *)p->nodeInfo;
		printf("Step %03d: (%4d, %4d)\n", step++, ni->x, ni->y);
		p = (Node *)p->next;
	}
	
	
	// Put in the obstacle
	for(k=0;k<gblNumObstacles;k++) {
		for(i=gblObstacle[k][2];i<gblObstacle[k][0];i++) {
			for(j=gblObstacle[k][1];j<gblObstacle[k][3];j++)
				gblImage[i][j] = '#';
		}
	}
	
	p = path;
	while(p != NULL) {
		ni = (NodeInfo *)p->nodeInfo;
		gblImage[ni->y][ni->x] = '@';
		p = (Node *)p->next;
	}
	
	gblImage[gblInit[1]][gblInit[0]] = '0';
	gblImage[gblGoal[1]][gblGoal[0]] = 'X';
	
	fp = fopen("image.txt", "w");
	for(i=99;i>=0;i--) {
		fprintf(fp, "%s\n", gblImage[i]);
	}
	fclose(fp);
	
	// delete the nodes on the path (which should delete root as well)
	while(path != NULL) {
		p = (Node *)path->next;
		freeNode(path);
		path = p;
	}
	
	return(0);
}